Stageros + roscpp + controlling robot through a C++ program

The objective is to control a simulated robot within the stage environment. This will help us in writing our own control algorithms for controlling the simulated robot. Please follow the steps given below:

#1 Follow the instructions on this page. This package demonstrates how one can run a stage simulation through stageros. Basically tells you to download the package stage_controllers.tar.gz and compile and build the package within a folder where ROS can find it.

$ rosmake stage_controllers

$ rospack find stage_controllers

$ rosrun stage stageros `rospack find stage_controllers`/world/roomba-wander.world

You should be able to see a robot moving around in a stage environment as shown in the picture.

#2 Now we want to stop this robot moving and write a C++ program to provide the necessary motion command.  In order to achieve this, open the file ‘/world/roomba-wander.world’ inside the stage_controllers folder and comment out the line that contains the word “wander” as shown below:

roomba

(              

  # can refer to the robot by this name

  name "roomba"

  pose [ -8 6 0 0 ]

  sicklaser()

#ctrl "wander"

)

If you now execute the above commands you will see a stationary robot within the stage environment. Now we need to write a C++ program to get the robot moving again.

#3 Create a file ‘robctrl.cpp’ inside ‘stage_controllers/src/’ folder that contains the following piece of code:

#include "ros/ros.h" #include "std_msgs/String.h" #include "geometry_msgs/Twist.h" #include int main(int argc, char **argv) {   ros::init(argc, argv, "RobCtrl");   ros::NodeHandle n;   ros::Publisher cmd_pub = n.advertise("cmd_vel",1);   geometry_msgs::Twist cmd_msg;   ros::Rate cycle(10.0);   //Specify the velocity   cmd_msg.linear.x = 0.1;   cmd_msg.linear.y = 0;   cmd_msg.linear.z = 0;   cmd_msg.angular.x = 0;   cmd_msg.angular.y = 0;   cmd_msg.angular.z = 0.2;   while(ros::ok())   {         cmd_pub.publish(cmd_msg);         cycle.sleep();   }   return 0; }

#4 Edit the stage_controllers/manifest.xml to reflect dependency on stage package.

#5 Add the following line to the end of the ‘CMakeLists.txt’ file

rosbuild_add_executable(stage_controllers src/robctrl.cpp)

#6 Build

$ rosmake stage_controllers stage_controllers

# Execute

$ roscore (on one window)

$ rosrun stage stageros `rospack find stage_controllers`/world/roomba-wander.world (on another window)

$ rosrun stage_controllers stage_controllers

You should see the robot rotating in a circle within the stage environment.

This program publishes the velocity values to the topic ‘/cmd_vel’. To know about the available topics where the data could be published, use ‘rostopic’ and ‘rxgraph’ command.

$ rostopic list -v

Published topics:

 * /base_scan [sensor_msgs/LaserScan] 1 publisher

 * /rosout [rosgraph_msgs/Log] 2 publishers

 * /tf [tf/tfMessage] 1 publisher

 * /clock [rosgraph_msgs/Clock] 1 publisher

 * /odom [nav_msgs/Odometry] 1 publisher

 * /rosout_agg [rosgraph_msgs/Log] 1 publisher

 * /cmd_vel [geometry_msgs/Twist] 1 publisher

 * /base_pose_ground_truth [nav_msgs/Odometry] 1 publisher

Subscribed topics:

 * /clock [rosgraph_msgs/Clock] 2 subscribers

 * /rosout [rosgraph_msgs/Log] 1 subscriber

 * /cmd_vel [geometry_msgs/Twist] 1 subscriber

$ rxgraph