Author: Jordi Pages < jordi.pages@pal-robotics.com >

Maintainer: Jordi Pages < jordi.pages@pal-robotics.com >

Support: tiago-support@pal-robotics.com

Source: https://github.com/pal-robotics/tiago_tutorials.git

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

Moving the base through velocity commands

Description: This tutorial shows how to move the base by sending velocity commands to the appropriate topic.

Keywords: teleoperation, mobile base, key_teleop

Tutorial Level: BEGINNER

Next Tutorial: Joint Trajectory Controller

cmd_vel_linear+angular.gif

Purpose

This tutorial shows how to send velocity commands to the mobile base controller through the topic

  • /mobile_base_controller/cmd_vel

which is of type geometry_msgs/Twist. A twist is composed of:

  • geometry_msgs/Vector3 linear
    geometry_msgs/Vector3 angular

therefore, it is specified by a linear velocity vector and an angular velocity vector.

Pre-requisites

First make sure that the tutorials are properly installed along with the TIAGo simulation, as shown in the Tutorials Installation Section.

Execution

First open two consoles and source the public simulation workspace as follows:

  • $ cd /tiago_public_ws/
    $ source ./devel/setup.bash

Launching the simulation

In the first console launch for example the following simulation

  • roslaunch tiago_gazebo tiago_gazebo.launch public_sim:=true end_effector:=pal-gripper world:=empty

Gazebo will show up with TIAGo in an empty world

gazebo_empty_world.jpg

Sending velocity commands through command line

This section shows how to command the differential drive base of TIAGo through ROS topics.

Moving forward and backward

In the second console run the following instruction which will publish a constant linear velocity of 0.5 m/s along the X axis, i.e. the axis of the robot pointing forward

  • rostopic pub /mobile_base_controller/cmd_vel geometry_msgs/Twist "linear:
      x: 0.5
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0" -r 3

or

  • rostopic pub /mobile_base_controller/cmd_vel geometry_msgs/Twist -r 3 -- '[0.5,0.0,0.0]' '[0.0, 0.0, 0.0]'

Note that the argument -r 3 specifies that the message will be published 3 times per second. This is necessary to maintain a constant speed as the mobile_base_controller only applies a given velocity for less than a second for safety reasons. The idea is that client that wants to move the mobile base keeps publishing the desired speeds, otherwise, if the client dies, the latest sent velocities would still be applied possibly leading to a collision with the environment.

cmd_vel_forward.gif

In order to move backwards it is only necessary to set a negative sign on the x component of the linear velocity vector of the Twist message.

Turning left an right

In order to make TIAGo rotate leftwards the following command has to be published:

  • rostopic pub /mobile_base_controller/cmd_vel geometry_msgs/Twist "linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.3" -r 3

or

  • rostopic pub /mobile_base_controller/cmd_vel geometry_msgs/Twist -r 3 -- '[0.0,0.0,0.0]' '[0.0, 0.0, 0.3]'

This will make TIAGo rotate about itself leftwards at 0.3 rad / s.

cmd_vel_leftwards.gif

In order to turn rightwards the sign of the z angular velocity component needs to be inverted.

Wiki: Robots/TIAGo/Tutorials/motions/cmd_vel (last edited 2023-02-24 10:28:02 by thomaspeyrucain)