两轮差速机器人ROS下速度解算

订阅cmd_val下的geometry_msgs::Twist消息 ，并且实际转化为左右轮的速度，以下是转换的源码。

geometry_msgs::Twist twist = twist_aux;
double vel_x = twist_aux.linear.x;
double vel_th = twist_aux.angular.z;
double right_vel = 0.0;
double left_vel = 0.0;
if(vel_x == 0)
{
	right_vel = vel_th * width_robot / 2.0;
	left_vel = (-1) * right_vel;
}
else if(vel_th == 0)
{
	left_vel = right_vel = vel_x;
}
else
{
	left_vel = vel_x - vel_th * width_robot / 2.0;
	right_vel = vel_x + vel_th * width_robot / 2.0;
}

right_vel = right_vel*MS_RADMIN;
left_vel = left_vel*MS_RADMIN;