Introduction

The mobile robot we are using at these tutorials has two DC motors that enable moving the mobile robot. One DC motor drives the left and the other the right wheel. This part of tutorials will present basic examples of how commands can be sent to a mobile robot to start (and stop) moving.

Sending commands to a robot via terminal

Task: Send the commands (one-by-one) to a mobile robot directly via terminal, which will cause, that the mobile robot will first start to slowly move forward, then it will stop after few seconds and start to rotate slowly in place. The descriptions of the individual steps of the procedure that can help you with this task can be found below.

Step 1. Switch ON the mobile robot and do the initial setup (WiFI settings, ROS Master IP, etc. Follow the initial setup instructions and instructions on how to connect the mobile robot to ROS under the section MOBILE ROBOT DOCUMENTATION/Getting-started).

Step 2. Launch all nodes, that are needed for the mobile robot to operate. In the first terminal run:

roscore

in the second terminal:

rosrun rosserial_server socket_node

and in the third terminal:

rosrun rosserial_python message_info_service.py

Alternatively, if you e.g. don't have a real mobile robot, you can do these tasks also with a simulated one. To do this with a simulated robot, you need to launch the simulation:

cd .../rosbot_ros

source devel/setup.bash

roslaunch rosbot simulation.launch

Step 3. Send command to start moving the robot forward. This can be done by publishing a message to the /cmd_vel topic. The publisheg message needs to be of type Twist from the ROS geometry_msgs collection. Twist message expresses velocity in free space broken into its linear and angular parts:

geometry_msgs/Vector3 linear
  float64 x
  float64 y
  float64 z
geometry_msgs/Vector3 angular
  float64 x
  float64 y
  float64 z

To publish a Twist message to the /cmd_vel topic, you should open a new terminal (or a new tab in a terminal) and start writing the following command (DO NOT PRESS ENTER BUTTON YET!):

rostopic pub /cmd_vel

To complete the command press Tab key two times to auto-complete the type definition and default contents as shown below:

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"

You can now edit the message (use arrow keys and edit the values). For example, for the robot to start moving in the forward direction with a speed of 0.05 m/s you should modify the message contents in the following way:

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.05
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"

The message is now complete. Press Enter key to publish the message. The robot should start moving.

Step 4. Stop the robot. After the previous step, the mobile robot is moving forward. In this step, we will stop the robot by publishing another Twist message to the /cmd_vel topic with values of all linear and angular components set to zero:

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0"

Step 5. Rotating slowly in place. The last step of this task is that the mobile robot starts rotating slowly in place. For this to happen, you need to publish the following (or similar) message to the /cmd_vel topic:

rostopic pub /cmd_vel geometry_msgs/Twist "linear:
x: 0.0
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.5"

Before you move forward to the next task, stop the mobile robot to save with the battery.

Sending commands to a robot via custom ROS node

Another way of how to send commands to a robot is by creating a node that will publish the Twist messages on topic /cmd_vel. Below is a demo program and then a task where you will have to program a new node that will send commands to the robot.

Create a new package motor_control inside workospace rosbot_ros:

cd .../rosbot_ros/src

catkin_create_pkg motor_control std_msgs roscpp

Demo program

Below is a program for the ROS node that causes the mobile robot to start moving forward and then stops it after three seconds.

#include "ros/ros.h"
#include <geometry_msgs/Twist.h>

int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_robot_demo");
  ros::NodeHandle nh;
  ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/cmd_vel", 1000);

  ros::Rate loop_rate(10);

  double timeStart =  ros::Time::now().toSec();

  while (ros::ok()) {

    double timeNow = ros::Time::now().toSec();

    geometry_msgs::Twist msg;
    if (timeNow - timeStart < 3.0) {
      msg.linear.x = 0.05;
    } else {
      msg.linear.x = 0.0;
    }
    pub.publish(msg);

    ros::spinOnce();

    loop_rate.sleep();
  }

  return 0;
}

To try how this demo program works on a real mobile robot, follow the steps below.

Step 1. Navigate to .../rosbot_ros/src/motor_control/src directory and create a new file move_robot_demo.cpp:

cd .../rosbot_ros/src/motor_control/src

touch move_robot_demo.cpp

Step 2. Open the file move_robot_demo.cpp and paste the C++ programing code presented above in it. (Don't forget to save changes.)

Step 3. In the CMakeLists.txt file (which in the folder .../rosbot_ros/src/motor_control), add the following lines in the appropriate places:

...
add_executable(move_robot_demo src/move_robot_demo.cpp)
...

target_link_libraries(move_robot_demo ${catkin_LIBRARIES})
...

Step 4. Navigate to the workspace directory, build the executable files using catkin_make, and run the command source setup.bash:

cd .../rosbot_ros

catkin_make

source devel/setup.bash

Step 5. Switch ON the mobile robot and do the initial setup (WiFI settings, ROS Master IP, etc. Follow the initial setup instructions and instructions on how to connect the mobile robot to ROS under the section MOBILE ROBOT DOCUMENTATION/Getting-started) and in seperate terminals launch all nodes, that are needed for the mobile robot to operate (roscore, socket_node, message_info_service).

Step 5. Run the move_robot_demo node (run it in the terminal, where you have navigated to the directory ~/rosbot_ros and executed the command source devel/setup.bash):

rosrun motor_control move_robot_demo

Additional task

Create another custom node named move_robot_task that will send commands to a mobile robot in a way that the mobile robot will drive in a square.

Other ways to control a mobile robot

There are other ways of controlling a mobile robot. For example, you can use a standard node teleop_twist_keyboard or mouse_teleop and control a mobile robot with teleoperation (you already did this in Day 1). There is another more advanced way to control a mobile robot in ROS: This is for example the way using the move_base node. The move_base node performs navigation tasks. Navigation is the ability of a mobile robot to determine its position and orientation in the environment where it is located (localization) and to autonomously plan and execute the path to a target location. It enables autonomous avoiding both static and dynamic obstacles. We just need to tell the robot the target location, and it will find and execute a path to that target location without any additional help and instructions.