Question of the video:

How to move to a certain point in space using Twist /cmd_vel? (https://answers.ros.org/question/273384/how-to-move-to-a-certain-point-in-space-using-twist-cmd_vel/)

Answer:

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 an 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 within 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 build 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. 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

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).

Step 0. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. After logging in, click on My ROSjects. You’ll find the public simulations provided by the Construct. We’ll use the Husky simulation today.

Step 1. Move the robot

At first, let’s create a package for the code.

cd catkin_ws/src
catkin_create_pkg simple_controller rospy

To move the robot from A to B, we can apply a controller. Then we create a controller.py file under the src folder in the package with the following content.

import rospy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from math import atan2

x = 0.0
y = 0.0 
theta = 0.0

def newOdom(msg):
    global x
    global y
    global theta

    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y

    rot_q = msg.pose.pose.orientation
    (roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])

rospy.init_node("speed_controller")

sub = rospy.Subscriber("/odometry/filtered", Odometry, newOdom)
pub = rospy.Publisher("/cmd_vel", Twist, queue_size = 1)

speed = Twist()

r = rospy.Rate(4)

goal = Point()
goal.x = 5
goal.y = 5

while not rospy.is_shutdown():
    inc_x = goal.x -x
    inc_y = goal.y -y

    angle_to_goal = atan2(inc_y, inc_x)

    if abs(angle_to_goal - theta) > 0.1:
        speed.linear.x = 0.0
        speed.angular.z = 0.3
    else:
        speed.linear.x = 0.5
        speed.angular.z = 0.0

    pub.publish(speed)
    r.sleep()    

You can also launch the rviz tool for visualization with

rosrun rviz rviz

Then you can launch the program with

rosrun simple_controller controller.py

You should see the robot turns and move to the point we set by comparing the difference between the set goal and the odometry.

What to learn more?

If you are interested in learning ROS, please visit

1. 1. • ROS Courses Library
      • ROS Development Studio

 

Edit by: Tony Huang