How to move to a certain point in space using Twist /cmd_vel

edit

Hi, what you are talking about is called Robot navigation, that is, the ability of the robot to go from one place to another.

Then, in order to achieve that, you need to decide at least the following three things:

1. In which frame are you measuring the coordinates (0,0) and (5,5)?
2. Do you need to have obstacle avoidance (that is, avoid any obstacle that may appear between (0,0) and (5,5) when your robot is moving towards the end location)?
3. Is it the location where the robot moves known or unknown?

Let's assume the simplest answer to the questions above:

1. We are measuring the coordinates in the odometry frame (I'm assuming you know what odometry is). The problem with this frame is that has a sift (an error) which increases over time. But for your example is good enough.
2. Let's say, there are no obstacles in the path. Hence, we do not have to implement an obstacle avoidance algorithm like potential fields or else.
3. Let's say it is and unknown environment, of which we are not going to build a map.

Even if this is a very simple setup, that setup can be used, for example, to move the robot to the position of a detected target. Imagine that the robot has a camera and a program that detects people. Then, once the program detects a person in 5 meters in front of the robot, it means that the robot can go to the person location using that simple framework we defined in the questions above.

Now, you need to built a controller that converts the distances from the current position AND orientation of the robot into velocity commands that are sent to the /cmd_vel of the robot to make it move towards the location. That controller can be built in many ways.

A simple controller to do that would be the following:

1. If the robot (odometry) orientation is not towards the target, then rotate only the robot until its orientation is towards the target.

   speed = Twist()
   speed.linear.x = 0.0
   speed.angular.z = 0.2
   

2. Once the robot is headed towards the target, just move straight towards the goal until reached

   speed = Twist()
   speed.linear.x = 0.5
   speed.angular.z = 0.0
   

I have created this video that shows how that controller works with a Husky robot in a Gazebo simulated environment.

More complex controllers can be implemented that optimize the time to reach the goal, or other factors like moving faster when far away from goal and moving slower when getting closer.

In case you want to make the robot include obstacle avoidance, navigation in bigger environments of which a map can be built, then you need to use the Navigation Stack of ROS which includes all that (and more). Check this video tutorial for a good overview of what the ROS Navigation stack can do.

Hope it helps, and welcome to the robot navigation world!

One Idea: Subscribe to current pose topic and publish to /cmd_vel.

Using your current robot pose and desired pose you can write a simple P controller. Based on how far they are apart and how your reference frame of you robot.

Random example:

geometry_msgs::Twist twist_;
twist_.linear.x = 2.0; // or based on distance between(current pose, desired pose)
twist_.linear.y = 0.0;
twist_.linear.z = 0.0;
twist_.angular.x = 0.0;
twist_.angular.y = 0.0;
twist_.angular.z = -pGain*sin();