!
Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags. |
Command your robot with simple motion commands
Description: Command your robot with simple motion commandsTutorial Level: BEGINNER
Next Tutorial: Remote monitoring and control
Contents
Driving the robot mobile platform
Driving the robot is done simply by publishing a geometry_msgs/Twist message to the cmd_vel topic.
By default, all robots use the twist_mux, which provides a multiplexer for geometry_msgs:Twist messages. It takes 3 input twist topics and outputs the messages to a single one. This available input topics are:
joy_vel - This topic is used for teleoperation using a joystick and have the highest priority (priority: 100) so you can use the joystick to takeover control while the robot is in autonomous mode.
cmd_vel - The main user interface topic (priority: 90).
nav_vel - This topic is used is used by the move_base to send navigation commands (priority: 80).
The output topic is mobile_base_controller/cmd_vel, it's not recommended to publish Twist messages directly to this topic.
For more configuration details see the komodo2/komodo2_utils/ros_twist_mux/config/twist_mux.yaml.
The geometry_msgs/Twist expanded definition looks like:
Vector3 linear float64 x float64 y float64 z Vector3 angular float64 x float64 y float64 z
However, our robot can't drive sideways (linear.y), or rotate about the x and y axes! Therefore, we will only be using linear.x and angular.z to control our robot.
One option is to use the rostopic command line tool to publish a driving message, for example:
$ rostopic pub /cmd_vel geometry_msgs/Twist -r 10 -- '[0.3, 0.0, 0.0]' '[0.0, 0.0, -0.9]'
The above command will command the robot to drive with a forward velocity of 0.3 m/s and turn right (positive values will cause the robot to turn left) with angular velocity of 0.9 rad/s. The "-r 10" argument will cause this message to be sent in a rate of 10 Hz, This is necessary because the robot controller have a 0.25 seconds safety stop timeout watch. After a period of 0.25 seconds without any coming message, the robot will stop.
If you want to use a graphical tool for publishing this command you can use the rqt Robot Steering tool or the rqt Message Publisher tool.
Lets, give it a try together. First, lets open a new terminal and launch the komodo2 robot in the gazebo simulation:
$ roslaunch komodo2 komodo2.launch gazebo:=true
Now, in a new terminal, type the following command for driving the robot:
$ rostopic pub /cmd_vel geometry_msgs/Twist -r 10 -- '[0.2, 0.0, 0.0]' '[0.0, 0.0, 0.6]'
Open the Gazebo window and watch the komodo2 robot drive.
To stop the robot from driving click Ctrl+C in order to stop publishing.
Now, Lets use the Robot Steering tool. In your second terminal launch rqt by typing:
$ rqt
From the upper toolbar, select Plugins->Robot Tools->Robot Steering.
Enter the cmd_vel topic at the top text box.
Your window should look like this:
Now, play with the two sliders to steer the komodo2 robot.