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

ROSCPP + OpenCV Program

Basically, here I am demonstrating how you can start writing your own program with ROS.

Create ROS package. Follow the tutorial link here. I am assuming that ros_workspace folder is added into ROS_PACKAGE_PATH

$ cd ~/ros_workspace

$ roscreate-pkg tbsimctrl std_msgs roscpp opencv2 sensor_msgs cv_bridge image_transport

You may not all the dependencies mentioned above.

$ cd tbsimctrl

$ rospack profile

$ rospack find tbsimctrl

/home/turtlebot/ros_workspace/tbsimctrl

Now create a file “camera_test.cpp” under src folder. The file contains the following piece of code:

#include

#include    //may not be needed

#include  //may not be needed

#include  //may not be needed

#include

#include

#include

#include

using namespace std;

using namespace cv;

int main(int argc, char **argv)

{

  ros::init(argc, argv, "captureimg");

  ros::NodeHandle n;

  CvCapture* capture = cvCaptureFromCAM(-1);

  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH, 320);

  cvSetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT, 240);

  IplImage *img = 0;

  img = cvQueryFrame(capture);

  int frameH = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_HEIGHT);

  int frameW = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FRAME_WIDTH);

  int fps = (int) cvGetCaptureProperty(capture, CV_CAP_PROP_FPS);

  cout << frameW << “\t” << frameH << “\t” << fps << endl;

  cvNamedWindow("Left", CV_WINDOW_AUTOSIZE);

  for(int n = 1; n= 30; n++)

  {

        img = cvQueryFrame(capture);

        cvShowImage("Left", img);  //show frames while capturing

        int c = cvWaitKey(20);               // wait 20 ms

        if(c == 27)

          break;

  }

  cvDestroyWindow( "Left" );

  cvReleaseCapture(&capture);

  return 0;

}

The above program captures video from a webcam. Save the file and insert the following statement inside the CMakeLists.txt file inside the tbsimctrl package folder. 

rosbuild_add_executable(${PROJECT_NAME} src/camera_test.cpp)

Now compile and build the package using rosmake

$ cmake .

$ rosmake

Make sure that the package is built with 0 failures. Once it is done, you can test the code as follows:

$ rosrun tbsimctrl tbsimctrl

You should be able to see the video from your webcam.