11

I'm trying to convert Twist to the left and right wheels' speed with the formula:

float speed_wish_right = (cmd_vel.angle*WHEEL_DIST)/2 + cmd_vel.speed;
float speed_wish_left = cmd_vel.speed*2-speed_wish_right;

Twist.angular is a vector [x, y, z] and so is Twist.linear. What do x, y, z mean in the vector and how can I get angles and speed out of the two vectors?

This is my callback function in Arduino

const int WHEEL_DIST = 16;
void velCallback(geometry_msgs::Twist vel) {
  float linear = vel.linear.x;
  float angle = vel.angular.z;

  float speed_wish_right = (angle * WHEEL_DIST) / 2 + linear;
  float speed_wish_left = linear * 2 - speed_wish_right;
  motors.setSpeeds(speed_wish_left, speed_wish_right);
}
akshayk07
  • 2,092
  • 1
  • 20
  • 32
desperatecoder
  • 187
  • 1
  • 3
  • 13

1 Answers1

19

Consider that you are in some space, then there are 3 axes - x, y and z which are mutually perpendicular to each other and their point of intersection is called the origin (x = 0, y = 0, z = 0). This can be a frame of reference i.e. you can define various points and directions w.r.t. them.

The x, y, and z in Twist.linear are the linear velocities in x, y and z directions w.r.t. that frame of reference.

Similarly, the x, y, and z in Twist.angular are the angular velocities about the x, y and z directions respectively w.r.t. the same frame of reference.

Since you have a ground robot, most probably your angular velocity will be in z i.e. robot's turning speed. And your linear velocity will be mostly in x i.e. robot's moving straight speed. This is the case for the Turtlebot 2 at least.

akshayk07
  • 2,092
  • 1
  • 20
  • 32
  • Thank you so much. What is the type of the x, y, and z? float? int? – desperatecoder Jun 22 '18 at 13:19
  • 3
    You can check using `rosmsg show geometry_msgs/Twist`. It is `float64` by the way. And if this answer solved your issue, then click the green tick and accept it, so that this question can be closed. – akshayk07 Jun 22 '18 at 14:49