stageros.cpp
Go to the documentation of this file.
00001 /*
00002  *  stageros
00003  *  Copyright (c) 2008, Willow Garage, Inc.
00004  *
00005  *  This program is free software; you can redistribute it and/or modify
00006  *  it under the terms of the GNU General Public License as published by
00007  *  the Free Software Foundation; either version 2 of the License, or
00008  *  (at your option) any later version.
00009  *
00010  *  This program is distributed in the hope that it will be useful,
00011  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  *  GNU General Public License for more details.
00014  *
00015  *  You should have received a copy of the GNU General Public License
00016  *  along with this program; if not, write to the Free Software
00017  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00018  */
00019 
00027 #include <stdio.h>
00028 #include <stdlib.h>
00029 #include <string.h>
00030 
00031 #include <sys/types.h>
00032 #include <sys/stat.h>
00033 #include <unistd.h>
00034 #include <signal.h>
00035 
00036 
00037 // libstage
00038 #include <stage.hh>
00039 
00040 // roscpp
00041 #include <ros/ros.h>
00042 #include <boost/thread/mutex.hpp>
00043 #include <boost/thread/thread.hpp>
00044 #include <sensor_msgs/LaserScan.h>
00045 #include <nav_msgs/Odometry.h>
00046 #include <geometry_msgs/Twist.h>
00047 #include <rosgraph_msgs/Clock.h>
00048 
00049 #include "tf/transform_broadcaster.h"
00050 
00051 #define USAGE "stageros <worldfile>"
00052 #define ODOM "odom"
00053 #define BASE_SCAN "base_scan"
00054 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
00055 #define CMD_VEL "cmd_vel"
00056 
00057 // Our node
00058 class StageNode
00059 {
00060   private:
00061     // Messages that we'll send or receive
00062     sensor_msgs::LaserScan *laserMsgs;
00063     nav_msgs::Odometry *odomMsgs;
00064     nav_msgs::Odometry *groundTruthMsgs;
00065     rosgraph_msgs::Clock clockMsg;
00066 
00067     // roscpp-related bookkeeping
00068     ros::NodeHandle n_;
00069 
00070     // A mutex to lock access to fields that are used in message callbacks
00071     boost::mutex msg_lock;
00072 
00073     // The models that we're interested in
00074     std::vector<Stg::ModelRanger *> lasermodels;
00075     std::vector<Stg::ModelPosition *> positionmodels;
00076     std::vector<ros::Publisher> laser_pubs_;
00077     std::vector<ros::Publisher> odom_pubs_;
00078     std::vector<ros::Publisher> ground_truth_pubs_;
00079     std::vector<ros::Subscriber> cmdvel_subs_;
00080     ros::Publisher clock_pub_;
00081 
00082     // A helper function that is executed for each stage model.  We use it
00083     // to search for models of interest.
00084     static void ghfunc(Stg::Model* mod, StageNode* node);
00085 
00086     static bool s_update(Stg::World* world, StageNode* node)
00087     {
00088       node->WorldCallback();
00089       // We return false to indicate that we want to be called again (an
00090       // odd convention, but that's the way that Stage works).
00091       return false;
00092     }
00093 
00094     // Appends the given robot ID to the given message name.  If omitRobotID
00095     // is true, an unaltered copy of the name is returned.
00096     const char *mapName(const char *name, size_t robotID);
00097 
00098     tf::TransformBroadcaster tf;
00099 
00100     // Last time that we received a velocity command
00101     ros::Time base_last_cmd;
00102     ros::Duration base_watchdog_timeout;
00103 
00104     // Current simulation time
00105     ros::Time sim_time;
00106 
00107   public:
00108     // Constructor; stage itself needs argc/argv.  fname is the .world file
00109     // that stage should load.
00110     StageNode(int argc, char** argv, bool gui, const char* fname);
00111     ~StageNode();
00112 
00113     // Subscribe to models of interest.  Currently, we find and subscribe
00114     // to the first 'laser' model and the first 'position' model.  Returns
00115     // 0 on success (both models subscribed), -1 otherwise.
00116     int SubscribeModels();
00117 
00118     // Our callback
00119     void WorldCallback();
00120     
00121     // Do one update of the world.  May pause if the next update time
00122     // has not yet arrived.
00123     bool UpdateWorld();
00124 
00125     // Message callback for a MsgBaseVel message, which set velocities.
00126     void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);
00127 
00128     // The main simulator object
00129     Stg::World* world;
00130 };
00131 
00132 // since stageros is single-threaded, this is OK. revisit if that changes!
00133 const char *
00134 StageNode::mapName(const char *name, size_t robotID)
00135 {
00136   if (positionmodels.size() > 1)
00137   {
00138     static char buf[100];
00139     snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
00140     return buf;
00141   }
00142   else
00143     return name;
00144 }
00145 
00146 void
00147 StageNode::ghfunc(Stg::Model* mod, StageNode* node)
00148 {
00149   if (dynamic_cast<Stg::ModelRanger *>(mod))
00150     node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
00151   if (dynamic_cast<Stg::ModelPosition *>(mod))
00152     node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
00153 }
00154 
00155 void
00156 StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg)
00157 {
00158   boost::mutex::scoped_lock lock(msg_lock);
00159   this->positionmodels[idx]->SetSpeed(msg->linear.x, 
00160                                       msg->linear.y, 
00161                                       msg->angular.z);
00162   this->base_last_cmd = this->sim_time;
00163 }
00164 
00165 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
00166 {
00167   this->sim_time.fromSec(0.0);
00168   this->base_last_cmd.fromSec(0.0);
00169   double t;
00170   ros::NodeHandle localn("~");
00171   if(!localn.getParam("base_watchdog_timeout", t))
00172     t = 0.2;
00173   this->base_watchdog_timeout.fromSec(t);
00174 
00175   // We'll check the existence of the world file, because libstage doesn't
00176   // expose its failure to open it.  Could go further with checks (e.g., is
00177   // it readable by this user).
00178   struct stat s;
00179   if(stat(fname, &s) != 0)
00180   {
00181     ROS_FATAL("The world file %s does not exist.", fname);
00182     ROS_BREAK();
00183   }
00184 
00185   // initialize libstage
00186   Stg::Init( &argc, &argv );
00187 
00188   if(gui)
00189     this->world = new Stg::WorldGui(800, 700, "Stage (ROS)");
00190   else
00191     this->world = new Stg::World();
00192 
00193   // Apparently an Update is needed before the Load to avoid crashes on
00194   // startup on some systems.
00195   // As of Stage 4.1.1, this update call causes a hang on start.
00196   //this->UpdateWorld();
00197   this->world->Load(fname);
00198 
00199   // We add our callback here, after the Update, so avoid our callback
00200   // being invoked before we're ready.
00201   this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
00202 
00203   this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
00204   if (lasermodels.size() != positionmodels.size())
00205   {
00206     ROS_FATAL("number of position models and laser models must be equal in "
00207               "the world file.");
00208     ROS_BREAK();
00209   }
00210   size_t numRobots = positionmodels.size();
00211   ROS_INFO("found %u position/laser pair%s in the file", 
00212            (unsigned int)numRobots, (numRobots==1) ? "" : "s");
00213 
00214   this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
00215   this->odomMsgs = new nav_msgs::Odometry[numRobots];
00216   this->groundTruthMsgs = new nav_msgs::Odometry[numRobots];
00217 }
00218 
00219 
00220 // Subscribe to models of interest.  Currently, we find and subscribe
00221 // to the first 'laser' model and the first 'position' model.  Returns
00222 // 0 on success (both models subscribed), -1 otherwise.
00223 //
00224 // Eventually, we should provide a general way to map stage models onto ROS
00225 // topics, similar to Player .cfg files.
00226 int
00227 StageNode::SubscribeModels()
00228 {
00229   n_.setParam("/use_sim_time", true);
00230 
00231   for (size_t r = 0; r < this->positionmodels.size(); r++)
00232   {
00233     if(this->lasermodels[r])
00234     {
00235       this->lasermodels[r]->Subscribe();
00236     }
00237     else
00238     {
00239       ROS_ERROR("no laser");
00240       return(-1);
00241     }
00242     if(this->positionmodels[r])
00243     {
00244       this->positionmodels[r]->Subscribe();
00245     }
00246     else
00247     {
00248       ROS_ERROR("no position");
00249       return(-1);
00250     }
00251     laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN,r), 10));
00252     odom_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(ODOM,r), 10));
00253     ground_truth_pubs_.push_back(n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH,r), 10));
00254     cmdvel_subs_.push_back(n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL,r), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1)));
00255   }
00256   clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock",10);
00257   return(0);
00258 }
00259 
00260 StageNode::~StageNode()
00261 {
00262   delete[] laserMsgs;
00263   delete[] odomMsgs;
00264   delete[] groundTruthMsgs;
00265 }
00266 
00267 bool
00268 StageNode::UpdateWorld()
00269 {
00270   return this->world->UpdateAll();
00271 }
00272 
00273 void
00274 StageNode::WorldCallback()
00275 {
00276   boost::mutex::scoped_lock lock(msg_lock);
00277 
00278   this->sim_time.fromSec(world->SimTimeNow() / 1e6);
00279   // We're not allowed to publish clock==0, because it used as a special
00280   // value in parts of ROS, #4027.
00281   if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
00282   {
00283     ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
00284     return;
00285   }
00286 
00287   // TODO make this only affect one robot if necessary
00288   if((this->base_watchdog_timeout.toSec() > 0.0) &&
00289       ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
00290   {
00291     for (size_t r = 0; r < this->positionmodels.size(); r++)
00292       this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
00293   }
00294 
00295   // Get latest laser data
00296   for (size_t r = 0; r < this->lasermodels.size(); r++)
00297                 {
00298                         const std::vector<Stg::ModelRanger::Sensor>& sensors = this->lasermodels[r]->GetSensors();
00299                 
00300                 if( sensors.size() > 1 )
00301                         ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
00302 
00303                 // for now we access only the zeroth sensor of the ranger - good
00304                 // enough for most laser models that have a single beam origin
00305                 const Stg::ModelRanger::Sensor& s = sensors[0];
00306                 
00307     if( s.ranges.size() )
00308                         {
00309       // Translate into ROS message format and publish
00310       this->laserMsgs[r].angle_min = -s.fov/2.0;
00311       this->laserMsgs[r].angle_max = +s.fov/2.0;
00312       this->laserMsgs[r].angle_increment = s.fov/(double)(s.sample_count-1);
00313       this->laserMsgs[r].range_min = s.range.min;
00314       this->laserMsgs[r].range_max = s.range.max;
00315       this->laserMsgs[r].ranges.resize(s.ranges.size());
00316       this->laserMsgs[r].intensities.resize(s.intensities.size());
00317                         
00318       for(unsigned int i=0; i<s.ranges.size(); i++)
00319                                 {
00320                                         this->laserMsgs[r].ranges[i] = s.ranges[i];
00321                                         this->laserMsgs[r].intensities[i] = (uint8_t)s.intensities[i];
00322                                 }
00323                         
00324       this->laserMsgs[r].header.frame_id = mapName("base_laser_link", r);
00325       this->laserMsgs[r].header.stamp = sim_time;
00326       this->laser_pubs_[r].publish(this->laserMsgs[r]);
00327                         }
00328 
00329     // Also publish the base->base_laser_link Tx.  This could eventually move
00330     // into being retrieved from the param server as a static Tx.
00331     Stg::Pose lp = this->lasermodels[r]->GetPose();
00332     tf::Quaternion laserQ;
00333     laserQ.setRPY(0.0, 0.0, lp.a);
00334     tf::Transform txLaser =  tf::Transform(laserQ,
00335                                             tf::Point(lp.x, lp.y, 0.15));
00336     tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
00337                                           mapName("base_link", r),
00338                                           mapName("base_laser_link", r)));
00339 
00340     // Send the identity transform between base_footprint and base_link
00341     tf::Transform txIdentity(tf::createIdentityQuaternion(),
00342                              tf::Point(0, 0, 0));
00343     tf.sendTransform(tf::StampedTransform(txIdentity,
00344                                           sim_time,
00345                                           mapName("base_footprint", r),
00346                                           mapName("base_link", r)));
00347 
00348     // Get latest odometry data
00349     // Translate into ROS message format and publish
00350     this->odomMsgs[r].pose.pose.position.x = this->positionmodels[r]->est_pose.x;
00351     this->odomMsgs[r].pose.pose.position.y = this->positionmodels[r]->est_pose.y;
00352     this->odomMsgs[r].pose.pose.orientation = tf::createQuaternionMsgFromYaw(this->positionmodels[r]->est_pose.a);
00353     Stg::Velocity v = this->positionmodels[r]->GetVelocity();
00354     this->odomMsgs[r].twist.twist.linear.x = v.x;
00355     this->odomMsgs[r].twist.twist.linear.y = v.y;
00356     this->odomMsgs[r].twist.twist.angular.z = v.a;
00357 
00358     //@todo Publish stall on a separate topic when one becomes available
00359     //this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
00360     //
00361     this->odomMsgs[r].header.frame_id = mapName("odom", r);
00362     this->odomMsgs[r].header.stamp = sim_time;
00363 
00364     this->odom_pubs_[r].publish(this->odomMsgs[r]);
00365 
00366     // broadcast odometry transform
00367     tf::Quaternion odomQ;
00368     tf::quaternionMsgToTF(odomMsgs[r].pose.pose.orientation, odomQ);
00369     tf::Transform txOdom(odomQ, 
00370                          tf::Point(odomMsgs[r].pose.pose.position.x,
00371                                    odomMsgs[r].pose.pose.position.y, 0.0));
00372     tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
00373                                           mapName("odom", r),
00374                                           mapName("base_footprint", r)));
00375 
00376     // Also publish the ground truth pose and velocity
00377     Stg::Pose gpose = this->positionmodels[r]->GetGlobalPose();
00378     Stg::Velocity gvel = this->positionmodels[r]->GetGlobalVelocity();
00379     // Note that we correct for Stage's screwed-up coord system.
00380     tf::Quaternion q_gpose;
00381     q_gpose.setRPY(0.0, 0.0, gpose.a-M_PI/2.0);
00382     tf::Transform gt(q_gpose, tf::Point(gpose.y, -gpose.x, 0.0));
00383     tf::Quaternion q_gvel;
00384     q_gvel.setRPY(0.0, 0.0, gvel.a-M_PI/2.0);
00385     tf::Transform gv(q_gvel, tf::Point(gvel.y, -gvel.x, 0.0));
00386 
00387     this->groundTruthMsgs[r].pose.pose.position.x     = gt.getOrigin().x();
00388     this->groundTruthMsgs[r].pose.pose.position.y     = gt.getOrigin().y();
00389     this->groundTruthMsgs[r].pose.pose.position.z     = gt.getOrigin().z();
00390     this->groundTruthMsgs[r].pose.pose.orientation.x  = gt.getRotation().x();
00391     this->groundTruthMsgs[r].pose.pose.orientation.y  = gt.getRotation().y();
00392     this->groundTruthMsgs[r].pose.pose.orientation.z  = gt.getRotation().z();
00393     this->groundTruthMsgs[r].pose.pose.orientation.w  = gt.getRotation().w();
00394     this->groundTruthMsgs[r].twist.twist.linear.x = gv.getOrigin().x();
00395     this->groundTruthMsgs[r].twist.twist.linear.y = gv.getOrigin().y();
00396     //this->groundTruthMsgs[r].twist.twist.angular.z = tf::getYaw(gv.getRotation());
00397     //this->groundTruthMsgs[r].twist.twist.linear.x = gvel.x;
00398     //this->groundTruthMsgs[r].twist.twist.linear.y = gvel.y;
00399     this->groundTruthMsgs[r].twist.twist.angular.z = gvel.a;
00400 
00401     this->groundTruthMsgs[r].header.frame_id = mapName("odom", r);
00402     this->groundTruthMsgs[r].header.stamp = sim_time;
00403 
00404     this->ground_truth_pubs_[r].publish(this->groundTruthMsgs[r]);
00405   }
00406 
00407   this->clockMsg.clock = sim_time;
00408   this->clock_pub_.publish(this->clockMsg);
00409 }
00410 
00411 int 
00412 main(int argc, char** argv)
00413 { 
00414   if( argc < 2 )
00415   {
00416     puts(USAGE);
00417     exit(-1);
00418   }
00419 
00420   ros::init(argc, argv, "stageros");
00421 
00422   bool gui = true;
00423   for(int i=0;i<(argc-1);i++)
00424   {
00425     if(!strcmp(argv[i], "-g"))
00426       gui = false;
00427   }
00428 
00429   StageNode sn(argc-1,argv,gui,argv[argc-1]);
00430 
00431   if(sn.SubscribeModels() != 0)
00432     exit(-1);
00433 
00434   boost::thread t = boost::thread(boost::bind(&ros::spin));
00435 
00436   // New in Stage 4.1.1: must Start() the world.
00437   sn.world->Start();
00438 
00439   // TODO: get rid of this fixed-duration sleep, using some Stage builtin
00440   // PauseUntilNextUpdate() functionality.
00441   ros::WallRate r(10.0);
00442   while(ros::ok() && !sn.world->TestQuit())
00443   {
00444     if(gui)
00445       Fl::wait(r.expectedCycleTime().toSec());
00446     else
00447     {
00448       sn.UpdateWorld();
00449       r.sleep();
00450     }
00451   }
00452   t.join();
00453 
00454   exit(0);
00455 }
00456 


stage
Author(s): Richard Vaughan, with contributions from many others. See web page for a full credits list.
autogenerated on Tue Apr 22 2014 19:54:08