$search
00001 /* 00002 * ROS stage simulation for ART autonomous vehicle 00003 * 00004 * (based on stage/src/stageros.cpp Willow Garage ROS node) 00005 * 00006 * Copyright (c) 2008, Willow Garage, Inc. 00007 * Copyright (c) 2009, Austin Robot Technology 00008 * 00009 * This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU General Public License for more details. 00018 * 00019 * You should have received a copy of the GNU General Public License 00020 * along with this program; if not, write to the Free Software 00021 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00022 * 00023 * $Id: artstage.cc 2252 2012-04-07 13:52:25Z jack.oquin $ 00024 */ 00025 00026 00042 #include <stdio.h> 00043 #include <stdlib.h> 00044 #include <string.h> 00045 00046 #include <sys/types.h> 00047 #include <sys/stat.h> 00048 #include <unistd.h> 00049 #include <signal.h> 00050 00051 00052 // libstage 00053 #include <stage.hh> 00054 00055 // roscpp 00056 #include <ros/ros.h> 00057 #include <boost/thread/mutex.hpp> 00058 #include <boost/thread/thread.hpp> 00059 #include <sensor_msgs/LaserScan.h> 00060 #include <tf/transform_broadcaster.h> 00061 00062 #include <rosgraph_msgs/Clock.h> 00063 typedef rosgraph_msgs::Clock Clock; 00064 00065 #if ROS_VERSION_MINIMUM(1, 8, 0) // Fuerte or later? 00066 00067 #define STAGE_VERSION 4 // Fuerte: stage 4.1.1 00068 typedef Stg::ModelRanger StgLaser; // use new ranger interface 00069 typedef Stg::model_callback_t model_callback_t; 00070 typedef Stg::world_callback_t world_callback_t; 00071 00072 #else 00073 00074 #define STAGE_VERSION 3 // Electric: stage 3.2.2 00075 typedef Stg::ModelLaser StgLaser; // use old laser interface 00076 typedef Stg::stg_model_callback_t model_callback_t; 00077 typedef Stg::stg_world_callback_t world_callback_t; 00078 00079 #endif 00080 00081 #include <art/frames.h> // ART vehicle frames of reference 00082 00083 #include "vehicle_model.h" 00084 00085 #define USAGE "artstage [-g] [ <worldfile> ]" 00086 00087 // ROS relative topic names 00088 #define FRONT_LASER "front_sick" 00089 00090 // Our node 00091 class StageNode 00092 { 00093 private: 00094 // Messages that we'll send or receive 00095 sensor_msgs::LaserScan *laserMsgs; 00096 Clock clockMsg; 00097 00098 // roscpp-related bookkeeping 00099 ros::NodeHandle n_; 00100 00101 // A mutex to lock access to fields that are used in message callbacks 00102 boost::mutex msg_lock; 00103 00104 // The models that we're interested in 00105 std::vector<StgLaser *> lasermodels; 00106 std::vector<Stg::ModelPosition *> positionmodels; 00107 std::vector<ros::Publisher> laser_pubs_; 00108 ros::Publisher clock_pub_; 00109 00110 // ART vehicle dynamics simulation 00111 std::vector<ArtVehicleModel *> vehicleModels_; 00112 00113 // A helper function that is executed for each stage model. We use it 00114 // to search for models of interest. 00115 static void ghfunc(Stg::Model* mod, StageNode* node); 00116 00117 static bool s_update(Stg::World* world, StageNode* node) 00118 { 00119 node->WorldCallback(); 00120 // We return false to indicate that we want to be called again (an 00121 // odd convention, but that's the way that Stage works). 00122 return false; 00123 } 00124 00125 // Appends the given robot ID to the given message name. If omitRobotID 00126 // is true, an unaltered copy of the name is returned. 00127 const char *mapName(const char *name, size_t robotID); 00128 const inline char *mapName(const std::string name, size_t robotID) 00129 {return mapName(name.c_str(), robotID);} 00130 00131 tf::TransformBroadcaster tf; 00132 00133 // Current simulation time 00134 ros::Time sim_time; 00135 00136 public: 00137 // Constructor; stage itself needs argc/argv. fname is the .world file 00138 // that stage should load. 00139 StageNode(int argc, char** argv, bool gui, const char* fname); 00140 ~StageNode(); 00141 00142 // Subscribe to models of interest. Currently, we find and subscribe 00143 // to the first 'laser' model and the first 'position' model. Returns 00144 // 0 on success (both models subscribed), -1 otherwise. 00145 int SubscribeModels(); 00146 00147 // Our callback 00148 void WorldCallback(); 00149 00150 // Do one update of the world. May pause if the next update time 00151 // has not yet arrived. 00152 bool UpdateWorld(); 00153 00154 // The main simulator object 00155 Stg::World* world; 00156 }; 00157 00158 // since stage_art is single-threaded, this is OK. revisit if that changes! 00159 const char * 00160 StageNode::mapName(const char *name, size_t robotID) 00161 { 00162 if (positionmodels.size() > 1) 00163 { 00164 static char buf[100]; 00165 snprintf(buf, sizeof(buf), "/robot_%lu/%s", (long unsigned) robotID, name); 00166 return buf; 00167 } 00168 else 00169 return name; 00170 } 00171 00172 void 00173 StageNode::ghfunc(Stg::Model* mod, StageNode* node) 00174 { 00175 if (dynamic_cast<StgLaser *>(mod)) 00176 node->lasermodels.push_back(dynamic_cast<StgLaser *>(mod)); 00177 00178 #if 0 // newer version 00179 if (dynamic_cast<Stg::ModelPosition *>(mod)) 00180 node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod)); 00181 #else // earlier version 00182 // only use the first position model 00183 if (dynamic_cast<Stg::ModelPosition *>(mod) 00184 && node->positionmodels.size() == 0) 00185 node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod)); 00186 #endif 00187 } 00188 00189 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname) 00190 { 00191 ROS_INFO_STREAM("libstage version " << Stg::Version()); 00192 00193 this->sim_time.fromSec(0.0); 00194 00195 // We'll check the existence of the world file, because libstage doesn't 00196 // expose its failure to open it. Could go further with checks (e.g., is 00197 // it readable by this user). 00198 struct stat s; 00199 if(stat(fname, &s) != 0) 00200 { 00201 ROS_FATAL("The world file %s does not exist.", fname); 00202 ROS_BREAK(); 00203 } 00204 00205 // initialize libstage 00206 Stg::Init( &argc, &argv ); 00207 00208 if(gui) 00209 this->world = new Stg::WorldGui(800, 700, "Stage (ART)"); 00210 else 00211 this->world = new Stg::World(); 00212 00213 00214 // Apparently an Update is needed before the Load to avoid crashes on 00215 // startup on some systems. 00216 #if STAGE_VERSION == 3 00217 // As of Stage 4.1.1, this update call causes a hang on start. 00218 this->UpdateWorld(); 00219 #endif 00220 this->world->Load(fname); 00221 00222 // We add our callback here, after the Update, so avoid our callback 00223 // being invoked before we're ready. 00224 this->world->AddUpdateCallback((world_callback_t)s_update, this); 00225 00226 this->world->ForEachDescendant((model_callback_t)ghfunc, this); 00227 00228 size_t numRobots = positionmodels.size(); 00229 ROS_INFO_STREAM("found " << numRobots << " position model(s) in the file"); 00230 00232 if (numRobots != 1) 00233 { 00234 #if 0 00235 ROS_FATAL("artstage currently only simulates one position model"); 00236 ROS_BREAK(); 00237 #endif 00238 size_t numRobots = positionmodels.size(); 00239 ROS_INFO("found %u position/laser pair%s in the file", 00240 (unsigned int)numRobots, (numRobots==1) ? "" : "s"); 00241 } 00242 00243 this->laserMsgs = new sensor_msgs::LaserScan[numRobots]; 00244 00245 for (size_t r = 0; r < numRobots; r++) 00246 { 00247 // TODO assign a namespace to each robot (from stage??) 00248 vehicleModels_.push_back(new ArtVehicleModel(positionmodels[r], &tf, "")); 00249 } 00250 } 00251 00252 00253 // Subscribe to models of interest. Currently, we find and subscribe 00254 // to the first 'laser' model and the first 'position' model. Returns 00255 // 0 on success (both models subscribed), -1 otherwise. 00256 // 00257 // Eventually, we should provide a general way to map stage models onto ROS 00258 // topics, similar to Player .cfg files. 00259 int 00260 StageNode::SubscribeModels() 00261 { 00262 n_.setParam("/use_sim_time", true); 00263 00264 for (size_t r = 0; r < this->positionmodels.size(); r++) 00265 { 00266 if(this->lasermodels[r]) 00267 { 00268 this->lasermodels[r]->Subscribe(); 00269 } 00270 else 00271 { 00272 ROS_ERROR("no laser"); 00273 return(-1); 00274 } 00275 if(this->positionmodels[r]) 00276 { 00277 this->positionmodels[r]->Subscribe(); 00278 } 00279 else 00280 { 00281 ROS_ERROR("no position"); 00282 return(-1); 00283 } 00284 00285 // subscribe to ROS topics 00286 laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan> 00287 (mapName(FRONT_LASER,r), 10)); 00288 00289 // set up vehicle model ROS topics 00290 vehicleModels_[r]->setup(); 00291 } 00292 clock_pub_ = n_.advertise<Clock>("/clock",10); 00293 return(0); 00294 } 00295 00296 StageNode::~StageNode() 00297 { 00298 delete[] laserMsgs; 00299 for (size_t r = 0; r < vehicleModels_.size(); r++) 00300 delete vehicleModels_[r]; 00301 } 00302 00303 bool 00304 StageNode::UpdateWorld() 00305 { 00306 return this->world->UpdateAll(); 00307 } 00308 00309 void 00310 StageNode::WorldCallback() 00311 { 00312 boost::mutex::scoped_lock lock(msg_lock); 00313 00314 this->sim_time.fromSec(world->SimTimeNow() / 1e6); 00315 // We're not allowed to publish clock==0, because it used as a special 00316 // value in parts of ROS, #4027. 00317 if(this->sim_time.sec == 0 && this->sim_time.nsec == 0) 00318 { 00319 ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0"); 00320 return; 00321 } 00322 00323 // Get latest laser data 00324 for (size_t r = 0; r < this->lasermodels.size(); r++) 00325 { 00326 00327 #if STAGE_VERSION == 4 00328 00329 const std::vector<Stg::ModelRanger::Sensor>& sensors = 00330 this->lasermodels[r]->GetSensors(); 00331 00332 if( sensors.size() > 1 ) 00333 ROS_WARN( "ART Stage currently supports rangers with 1 sensor only." ); 00334 00335 // for now we access only the zeroth sensor of the ranger - good 00336 // enough for most laser models that have a single beam origin 00337 const Stg::ModelRanger::Sensor& s = sensors[0]; 00338 00339 if( s.ranges.size() ) 00340 { 00341 // Translate into ROS message format and publish 00342 this->laserMsgs[r].angle_min = -s.fov/2.0; 00343 this->laserMsgs[r].angle_max = +s.fov/2.0; 00344 this->laserMsgs[r].angle_increment = s.fov/(double)(s.sample_count-1); 00345 this->laserMsgs[r].range_min = s.range.min; 00346 this->laserMsgs[r].range_max = s.range.max; 00347 this->laserMsgs[r].ranges.resize(s.ranges.size()); 00348 this->laserMsgs[r].intensities.resize(s.intensities.size()); 00349 00350 for(unsigned int i=0; i<s.ranges.size(); i++) 00351 { 00352 this->laserMsgs[r].ranges[i] = s.ranges[i]; 00353 this->laserMsgs[r].intensities[i] = (uint8_t)s.intensities[i]; 00354 } 00355 00356 // TODO map each laser to separate frame and topic names 00357 this->laserMsgs[r].header.frame_id = "/" + ArtFrames::front_sick; 00358 this->laserMsgs[r].header.stamp = sim_time; 00359 this->laser_pubs_[r].publish(this->laserMsgs[r]); 00360 } 00361 00362 #else // STAGE_VERSION 3 00363 00364 std::vector<StgLaser::Sample> samples = this->lasermodels[r]->GetSamples(); 00365 if(samples.size()) 00366 { 00367 // Translate into ROS message format and publish 00368 StgLaser::Config cfg = this->lasermodels[r]->GetConfig(); 00369 this->laserMsgs[r].angle_min = -cfg.fov/2.0; 00370 this->laserMsgs[r].angle_max = +cfg.fov/2.0; 00371 this->laserMsgs[r].angle_increment = cfg.fov/(double)(cfg.sample_count-1); 00372 this->laserMsgs[r].range_min = 0.0; 00373 this->laserMsgs[r].range_max = cfg.range_bounds.max; 00374 this->laserMsgs[r].ranges.resize(cfg.sample_count); 00375 this->laserMsgs[r].intensities.resize(cfg.sample_count); 00376 for(unsigned int i=0;i<cfg.sample_count;i++) 00377 { 00378 this->laserMsgs[r].ranges[i] = samples[i].range; 00379 this->laserMsgs[r].intensities[i] = (uint8_t)samples[i].reflectance; 00380 } 00381 00382 // TODO map each laser to separate frame and topic names 00383 this->laserMsgs[r].header.frame_id = "/" + ArtFrames::front_sick; 00384 this->laserMsgs[r].header.stamp = sim_time; 00385 this->laser_pubs_[r].publish(this->laserMsgs[r]); 00386 } 00387 00388 #endif // STAGE_VERSION 00389 } 00390 00391 // update latest position data 00392 for (size_t r = 0; r < this->positionmodels.size(); r++) 00393 { 00394 vehicleModels_[r]->update(this->sim_time); 00395 } 00396 00397 this->clockMsg.clock = sim_time; 00398 this->clock_pub_.publish(this->clockMsg); 00399 } 00400 00401 #if STAGE_VERSION == 3 00402 static bool quit = false; 00403 void 00404 sigint_handler(int num) 00405 { 00406 quit = true; 00407 } 00408 #endif // STAGE_VERSION 3 00409 00410 int 00411 main(int argc, char** argv) 00412 { 00413 ros::init(argc, argv, "artstage"); 00414 00415 ros::NodeHandle private_nh("~"); 00416 bool gui = true; 00417 std::string world_file; 00418 if (private_nh.getParam("world_file", world_file)) 00419 { 00420 // world_file parameter defined 00421 for (int i=0; i<argc; i++) 00422 { 00423 if(!strcmp(argv[i], "-g")) 00424 gui = false; 00425 } 00426 } 00427 else 00428 { 00429 // no world_file parameter 00430 // TODO: clean up repetitive code sequences 00431 if (argc < 2) 00432 { 00433 puts(USAGE); 00434 exit(-1); 00435 } 00436 for (int i=0; i<(argc-1); i++) 00437 { 00438 if(!strcmp(argv[i], "-g")) 00439 gui = false; 00440 } 00441 world_file = argv[argc-1]; 00442 } 00443 00444 StageNode sn(argc-1, argv, gui, world_file.c_str()); 00445 00446 if(sn.SubscribeModels() != 0) 00447 exit(-1); 00448 00449 // spawn a thread to read incoming ROS messages 00450 boost::thread t(boost::thread(boost::bind(&ros::spin))); 00451 00452 #if STAGE_VERSION == 4 00453 // New in Stage 4.1.1: must Start() the world. 00454 sn.world->Start(); 00455 #endif 00456 00457 // TODO: get rid of this fixed-duration sleep, using some Stage builtin 00458 // PauseUntilNextUpdate() functionality. 00459 ros::WallRate r(10.0); 00460 00461 // run stage update loop in the main thread 00462 while(ros::ok() && !sn.world->TestQuit()) 00463 { 00464 if(gui) 00465 Fl::wait(r.expectedCycleTime().toSec()); 00466 else 00467 { 00468 sn.UpdateWorld(); 00469 r.sleep(); 00470 } 00471 } 00472 t.join(); 00473 00474 exit(0); 00475 }