00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
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
00053 #include <stage.hh>
00054
00055
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 #if ROS_VERSION_MINIMUM(1, 4, 0) // Diamondback or later?
00063 #include <rosgraph_msgs/Clock.h>
00064 typedef rosgraph_msgs::Clock Clock;
00065 #else // Cturtle or earlier?
00066 #include <roslib/Clock.h>
00067 typedef roslib::Clock Clock;
00068 #endif
00069
00070 #include <art/frames.h>
00071
00072 #include "vehicle_model.h"
00073
00074 #define USAGE "artstage [-g] [ <worldfile> ]"
00075
00076
00077 #define FRONT_LASER "front_sick"
00078
00079
00080 class StageNode
00081 {
00082 private:
00083
00084 sensor_msgs::LaserScan *laserMsgs;
00085 Clock clockMsg;
00086
00087
00088 ros::NodeHandle n_;
00089
00090
00091 boost::mutex msg_lock;
00092
00093
00094 std::vector<Stg::ModelLaser *> lasermodels;
00095 std::vector<Stg::ModelPosition *> positionmodels;
00096 std::vector<ros::Publisher> laser_pubs_;
00097 ros::Publisher clock_pub_;
00098
00099
00100 std::vector<ArtVehicleModel *> vehicleModels_;
00101
00102
00103
00104 static void ghfunc(Stg::Model* mod, StageNode* node);
00105
00106 static bool s_update(Stg::World* world, StageNode* node)
00107 {
00108 node->WorldCallback();
00109
00110
00111 return false;
00112 }
00113
00114
00115
00116 const char *mapName(const char *name, size_t robotID);
00117 const inline char *mapName(const std::string name, size_t robotID)
00118 {return mapName(name.c_str(), robotID);}
00119
00120 tf::TransformBroadcaster tf;
00121
00122
00123 ros::Time sim_time;
00124
00125 public:
00126
00127
00128 StageNode(int argc, char** argv, bool gui, const char* fname);
00129 ~StageNode();
00130
00131
00132
00133
00134 int SubscribeModels();
00135
00136
00137 void WorldCallback();
00138
00139
00140
00141 bool UpdateWorld();
00142
00143
00144 Stg::World* world;
00145 };
00146
00147
00148 const char *
00149 StageNode::mapName(const char *name, size_t robotID)
00150 {
00151 if (positionmodels.size() > 1)
00152 {
00153 static char buf[100];
00154 snprintf(buf, sizeof(buf), "/robot_%lu/%s", (long unsigned) robotID, name);
00155 return buf;
00156 }
00157 else
00158 return name;
00159 }
00160
00161 void
00162 StageNode::ghfunc(Stg::Model* mod, StageNode* node)
00163 {
00164 if (dynamic_cast<Stg::ModelLaser *>(mod))
00165 node->lasermodels.push_back(dynamic_cast<Stg::ModelLaser *>(mod));
00166
00167 #if 0 // newer version
00168 if (dynamic_cast<Stg::ModelPosition *>(mod))
00169 node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
00170 #else // earlier version
00171
00172 if (dynamic_cast<Stg::ModelPosition *>(mod)
00173 && node->positionmodels.size() == 0)
00174 node->positionmodels.push_back(dynamic_cast<Stg::ModelPosition *>(mod));
00175 #endif
00176 }
00177
00178 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname)
00179 {
00180 ROS_INFO_STREAM("libstage version " << Stg::Version());
00181
00182 this->sim_time.fromSec(0.0);
00183
00184
00185
00186
00187 struct stat s;
00188 if(stat(fname, &s) != 0)
00189 {
00190 ROS_FATAL("The world file %s does not exist.", fname);
00191 ROS_BREAK();
00192 }
00193
00194
00195 Stg::Init( &argc, &argv );
00196
00197 if(gui)
00198 this->world = new Stg::WorldGui(800, 700, "Stage (ART)");
00199 else
00200 this->world = new Stg::World();
00201
00202
00203
00204
00205 this->UpdateWorld();
00206 this->world->Load(fname);
00207
00208
00209
00210 this->world->AddUpdateCallback((Stg::stg_world_callback_t)s_update, this);
00211
00212 this->world->ForEachDescendant((Stg::stg_model_callback_t)ghfunc, this);
00213
00214 size_t numRobots = positionmodels.size();
00215 ROS_INFO_STREAM("found " << numRobots << " position model(s) in the file");
00216
00218 if (numRobots != 1)
00219 {
00220 #if 0
00221 ROS_FATAL("artstage currently only simulates one position model");
00222 ROS_BREAK();
00223 #endif
00224 size_t numRobots = positionmodels.size();
00225 ROS_INFO("found %u position/laser pair%s in the file",
00226 (unsigned int)numRobots, (numRobots==1) ? "" : "s");
00227 }
00228
00229 this->laserMsgs = new sensor_msgs::LaserScan[numRobots];
00230
00231 for (size_t r = 0; r < numRobots; r++)
00232 {
00233
00234 vehicleModels_.push_back(new ArtVehicleModel(positionmodels[r], &tf, ""));
00235 }
00236 }
00237
00238
00239
00240
00241
00242
00243
00244
00245 int
00246 StageNode::SubscribeModels()
00247 {
00248 n_.setParam("/use_sim_time", true);
00249
00250 for (size_t r = 0; r < this->positionmodels.size(); r++)
00251 {
00252 if(this->lasermodels[r])
00253 {
00254 this->lasermodels[r]->Subscribe();
00255 }
00256 else
00257 {
00258 ROS_ERROR("no laser");
00259 return(-1);
00260 }
00261 if(this->positionmodels[r])
00262 {
00263 this->positionmodels[r]->Subscribe();
00264 }
00265 else
00266 {
00267 ROS_ERROR("no position");
00268 return(-1);
00269 }
00270
00271
00272 laser_pubs_.push_back(n_.advertise<sensor_msgs::LaserScan>
00273 (mapName(FRONT_LASER,r), 10));
00274
00275
00276 vehicleModels_[r]->setup();
00277 }
00278 clock_pub_ = n_.advertise<Clock>("/clock",10);
00279 return(0);
00280 }
00281
00282 StageNode::~StageNode()
00283 {
00284 delete[] laserMsgs;
00285 for (size_t r = 0; r < vehicleModels_.size(); r++)
00286 delete vehicleModels_[r];
00287 }
00288
00289 bool
00290 StageNode::UpdateWorld()
00291 {
00292 return this->world->UpdateAll();
00293 }
00294
00295 void
00296 StageNode::WorldCallback()
00297 {
00298 boost::mutex::scoped_lock lock(msg_lock);
00299
00300 this->sim_time.fromSec(world->SimTimeNow() / 1e6);
00301
00302
00303 if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
00304 {
00305 ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
00306 return;
00307 }
00308
00309
00310 for (size_t r = 0; r < this->lasermodels.size(); r++)
00311 {
00312 std::vector<Stg::ModelLaser::Sample> samples = this->lasermodels[r]->GetSamples();
00313 if(samples.size())
00314 {
00315
00316 Stg::ModelLaser::Config cfg = this->lasermodels[r]->GetConfig();
00317 this->laserMsgs[r].angle_min = -cfg.fov/2.0;
00318 this->laserMsgs[r].angle_max = +cfg.fov/2.0;
00319 this->laserMsgs[r].angle_increment = cfg.fov/(double)(cfg.sample_count-1);
00320 this->laserMsgs[r].range_min = 0.0;
00321 this->laserMsgs[r].range_max = cfg.range_bounds.max;
00322 this->laserMsgs[r].ranges.resize(cfg.sample_count);
00323 this->laserMsgs[r].intensities.resize(cfg.sample_count);
00324 for(unsigned int i=0;i<cfg.sample_count;i++)
00325 {
00326 this->laserMsgs[r].ranges[i] = samples[i].range;
00327 this->laserMsgs[r].intensities[i] = (uint8_t)samples[i].reflectance;
00328 }
00329
00330
00331 this->laserMsgs[r].header.frame_id = "/" + ArtFrames::front_sick;
00332 this->laserMsgs[r].header.stamp = sim_time;
00333 this->laser_pubs_[r].publish(this->laserMsgs[r]);
00334 }
00335 }
00336
00337
00338 for (size_t r = 0; r < this->positionmodels.size(); r++)
00339 {
00340 vehicleModels_[r]->update(this->sim_time);
00341 }
00342
00343 this->clockMsg.clock = sim_time;
00344 this->clock_pub_.publish(this->clockMsg);
00345 }
00346
00347 static bool quit = false;
00348 void
00349 sigint_handler(int num)
00350 {
00351 quit = true;
00352 }
00353
00354 int
00355 main(int argc, char** argv)
00356 {
00357 ros::init(argc, argv, "artstage");
00358
00359 ros::NodeHandle private_nh("~");
00360 bool gui = true;
00361 std::string world_file;
00362 if (private_nh.getParam("world_file", world_file))
00363 {
00364
00365 for (int i=0; i<argc; i++)
00366 {
00367 if(!strcmp(argv[i], "-g"))
00368 gui = false;
00369 }
00370 }
00371 else
00372 {
00373
00374
00375 if (argc < 2)
00376 {
00377 puts(USAGE);
00378 exit(-1);
00379 }
00380 for (int i=0; i<(argc-1); i++)
00381 {
00382 if(!strcmp(argv[i], "-g"))
00383 gui = false;
00384 }
00385 world_file = argv[argc-1];
00386 }
00387
00388 StageNode sn(argc-1, argv, gui, world_file.c_str());
00389
00390 if(sn.SubscribeModels() != 0)
00391 exit(-1);
00392
00393
00394 boost::thread t(boost::thread(boost::bind(&ros::spin)));
00395
00396
00397
00398 ros::WallRate r(10.0);
00399
00400
00401 while(ros::ok() && !sn.world->TestQuit())
00402 {
00403 if(gui)
00404 Fl::wait(r.expectedCycleTime().toSec());
00405 else
00406 {
00407 sn.UpdateWorld();
00408 r.sleep();
00409 }
00410 }
00411 t.join();
00412
00413 exit(0);
00414 }