SHARE
TWEET

playerros_node.cpp

bricerebsamen May 30th, 2011 456 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. /**
  2.  * A simple driver to interface with a player robot (stage or other).
  3.  * This was written as an exercise to learn ROS. Adapted from stageros.cpp.
  4.  *
  5.  * It is conform to the requirements of the navigation stack:
  6.  * http://www.ros.org/wiki/navigation/Tutorials/RobotSetup
  7.  *  - it publishes information about the relationships between coordinate
  8.  *    frames using tf. Frames used are odom, base_link and base_laser.
  9.  *  - sensor_msgs/LaserScan messages are published
  10.  *  - odometry information are published using tf and the nav_msgs/Odometry
  11.  *    message.
  12.  *  - it accepts geometry_msgs/Twist messages in the base coordinate frame of
  13.  *    the robot on the "cmd_vel" topic.
  14.  *
  15.  * To test (use one terminal per command):
  16.  * - start a player robot (i.e. in stage)
  17.  * - start roscore
  18.  * - start this program
  19.  * - to send vel cmd: rostopic pub cmd_vel geometry_msgs/Twist '[0.2,0,0]' '[0,0,0]'
  20.  *   where the six digits are the linear vel (x,y,z) and the angular vel (x,y,z),
  21.  *   or run the teleop node.
  22.  * - echo odometry msgs: rostopic echo odom
  23.  *   or visualize with rviz
  24.  */
  25.  
  26. #include <iostream>
  27. #include <string>
  28.  
  29. // roscpp
  30. #include <ros/ros.h>
  31. #include <tf/transform_broadcaster.h>
  32. #include <sensor_msgs/LaserScan.h>
  33. #include <nav_msgs/Odometry.h>
  34. #include <geometry_msgs/Twist.h>
  35.  
  36. // libstage
  37. #include <libplayerc++/playerc++.h>
  38.  
  39. #include <assert.h>
  40.  
  41.  
  42.  
  43. class PlayerRosNode {
  44.   public:
  45.     PlayerRosNode(std::string host);
  46.  
  47.   private:
  48.     //  Player objects
  49.     PlayerCc::PlayerClient *client;
  50.     PlayerCc::Position2dProxy *posProx;
  51.     PlayerCc::LaserProxy *lasProx;
  52.     pthread_mutex_t mutex;
  53.  
  54.     // ROS topics
  55.     ros::NodeHandle n;
  56.     ros::Publisher laser_pub;
  57.     ros::Publisher odom_pub;
  58.     ros::Subscriber vel_sub;
  59.     tf::TransformBroadcaster tfb;
  60.  
  61.     void odomReceived(void);
  62.     void laserReceived(void);
  63.     void cmdvelReceived(const boost::shared_ptr<geometry_msgs::Twist const>& msg);
  64. };
  65.  
  66.  
  67. /// Connects to Player on the specified host and create the bridge between
  68. /// Player and ROS.
  69. PlayerRosNode::PlayerRosNode(std::string host)
  70. {
  71.   // Connect to player and get the proxies for position and laser
  72.   try {
  73.     client = new PlayerCc::PlayerClient(host);
  74.     posProx = new PlayerCc::Position2dProxy(client);
  75.     posProx->SetMotorEnable(true);
  76.     lasProx = new PlayerCc::LaserProxy(client);
  77.   }
  78.   catch( PlayerCc::PlayerError e) {
  79.     std::cerr << e << std::endl;
  80.     abort();
  81.   }
  82.  
  83.   // A mutex to protect concurrent access to posProx, because it is accessed in
  84.   // 2 different threads (odomReceived and cmdvelReceived)
  85.   pthread_mutex_init(&mutex,NULL);
  86.  
  87.   // Connect the handlers to treat player updates
  88.   posProx->ConnectReadSignal(boost::bind(&PlayerRosNode::odomReceived,this));
  89.   lasProx->ConnectReadSignal(boost::bind(&PlayerRosNode::laserReceived,this));
  90.  
  91.   // Advertize the ROS topics for odometry and laser data
  92.   odom_pub = n.advertise<nav_msgs::Odometry>("/odom", 10);
  93.   laser_pub = n.advertise<sensor_msgs::LaserScan>("/base_scan", 10);
  94.  
  95.   // Subscribe to the topic to receive velocity commands
  96.   vel_sub = n.subscribe<geometry_msgs::Twist>("/cmd_vel", 10,
  97.                         boost::bind(&PlayerRosNode::cmdvelReceived,this,_1));
  98.  
  99.   // Start the player client
  100.   client->StartThread();
  101. }
  102.  
  103.  
  104. /// A callback function called by player when an odometry packet is received.
  105. /// Read the packet and create the corresponding ROS message, then publish it.
  106. void PlayerRosNode::odomReceived() {
  107.   // safety net
  108.   assert(posProx);
  109.  
  110.   // lock access to posProx
  111.   pthread_mutex_lock( &mutex );
  112.  
  113.   // Retrieve odo info from Player
  114.   float x = posProx->GetXPos();
  115.   float y = posProx->GetYPos();
  116.   float th = posProx->GetYaw(); //theta
  117.   float vx = posProx->GetXSpeed();
  118.   float vth = posProx->GetYawSpeed();
  119.  
  120.   // release access to posProx
  121.   pthread_mutex_unlock( &mutex );
  122.  
  123.   // current time
  124.   ros::Time current_time = ros::Time::now();
  125.  
  126.   //first, we'll publish the transform over tf
  127.   //position and velocity of frame base_link w.r.t. frame odom (fixed frame)
  128.   geometry_msgs::TransformStamped odom_trans;
  129.   odom_trans.header.stamp = current_time;
  130.   odom_trans.header.frame_id = "odom";
  131.   odom_trans.child_frame_id = "base_link";
  132.   odom_trans.transform.translation.x = x;
  133.   odom_trans.transform.translation.y = y;
  134.   odom_trans.transform.translation.z = 0.0;
  135.   odom_trans.transform.rotation = tf::createQuaternionMsgFromYaw(th);
  136.   tfb.sendTransform(odom_trans);
  137.  
  138.   //next, we'll publish the odometry message over ROS
  139.   nav_msgs::Odometry odom;
  140.   odom.header.stamp = current_time;
  141.   odom.header.frame_id = "odom";
  142.   odom.child_frame_id = "base_link";
  143.   odom.pose.pose.position.x = x;
  144.   odom.pose.pose.position.y = y;
  145.   odom.pose.pose.position.z = 0.0;
  146.   odom.pose.pose.orientation = tf::createQuaternionMsgFromYaw(th);
  147.   odom.twist.twist.linear.x = vx;
  148.   odom.twist.twist.linear.y = 0;
  149.   odom.twist.twist.angular.z = vth;
  150.   odom_pub.publish(odom);
  151. }
  152.  
  153.  
  154. /// A callback function called by player when a laser packet is received.
  155. /// Read the packet and create the corresponding ROS message, then publish it.
  156. void PlayerRosNode::laserReceived() {
  157.   //safety net
  158.   assert(lasProx);
  159.  
  160.   // retrieve laser data
  161.   unsigned nPoints = lasProx->GetCount();
  162.   float resolution = lasProx->GetScanRes();
  163.   float fov = (nPoints-1)*resolution;
  164.  
  165.   // current time
  166.   ros::Time now = ros::Time::now();
  167.  
  168.   // first publish the transform over tf. In our case, the position of the laser
  169.   // is fixed.
  170.   tfb.sendTransform( tf::StampedTransform(
  171.       tf::Transform( tf::createIdentityQuaternion(), //orientation of the laser on the robot
  172.                      tf::Vector3(0.0, 0.0, 0.0) ),   //Position of the laser on the robot
  173.       now, "base_link", "base_laser"));
  174.  
  175.   // then send the laser message
  176.   sensor_msgs::LaserScan laserMsg;
  177.   laserMsg.header.frame_id = "/base_laser";
  178.   laserMsg.header.stamp = now;
  179.   laserMsg.angle_min = -fov/2.0;
  180.   laserMsg.angle_max = +fov/2.0;
  181.   laserMsg.angle_increment = resolution;
  182.   laserMsg.range_min = 0.0;
  183.   laserMsg.range_max = 20; //arbitrary value (can we get the real value from Player?)
  184.   laserMsg.ranges.resize(nPoints);
  185.   laserMsg.intensities.resize(nPoints);
  186.   for(unsigned int i=0;i<nPoints;i++) {
  187.     laserMsg.ranges[i] = lasProx->GetRange(i);
  188.     laserMsg.intensities[i] = lasProx->GetIntensity(i);
  189.   }
  190.   laser_pub.publish(laserMsg);
  191. }
  192.  
  193.  
  194. /// A callback function called by ROS when a velocity command message is received.
  195. /// Read the message and send the command to player.
  196. void PlayerRosNode::cmdvelReceived(const boost::shared_ptr<geometry_msgs::Twist const>& msg) {
  197.   // safety net
  198.   assert(posProx);
  199.  
  200.   // lock access to posProx
  201.   pthread_mutex_lock( &mutex );
  202.  
  203.   // Pass the velocity command to Player
  204.   posProx->SetSpeed(msg->linear.x, msg->angular.z);
  205.  
  206.   // release access to posProx
  207.   pthread_mutex_unlock( &mutex );
  208. }
  209.  
  210.  
  211.  
  212. int main(int argc, char **argv)
  213. {
  214.   // Init ROS
  215.   ros::init(argc, argv, "playerros");
  216.  
  217.   // Create the PlayerROS node
  218.   PlayerRosNode p("localhost");
  219.  
  220.   // Start the ROS message loop
  221.   ros::spin();
  222.  
  223.   return 0;
  224. }
RAW Paste Data
Pastebin PRO Autumn Special!
Get 40% OFF on Pastebin PRO accounts!
Top