00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
00038 #include <stage.hh>
00039
00040
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 <sensor_msgs/Image.h>
00046 #include <sensor_msgs/image_encodings.h>
00047 #include <sensor_msgs/CameraInfo.h>
00048 #include <nav_msgs/Odometry.h>
00049 #include <geometry_msgs/Twist.h>
00050 #include <rosgraph_msgs/Clock.h>
00051
00052 #include <std_srvs/Empty.h>
00053
00054 #include "tf/transform_broadcaster.h"
00055
00056 #define USAGE "stageros <worldfile>"
00057 #define IMAGE "image"
00058 #define DEPTH "depth"
00059 #define CAMERA_INFO "camera_info"
00060 #define ODOM "odom"
00061 #define BASE_SCAN "base_scan"
00062 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
00063 #define CMD_VEL "cmd_vel"
00064
00065
00066 class StageNode
00067 {
00068 private:
00069
00070
00071 ros::NodeHandle n_;
00072
00073
00074 boost::mutex msg_lock;
00075
00076
00077 std::vector<Stg::ModelCamera *> cameramodels;
00078 std::vector<Stg::ModelRanger *> lasermodels;
00079 std::vector<Stg::ModelPosition *> positionmodels;
00080
00081
00082 struct StageRobot
00083 {
00084
00085 Stg::ModelPosition* positionmodel;
00086 std::vector<Stg::ModelCamera *> cameramodels;
00087 std::vector<Stg::ModelRanger *> lasermodels;
00088
00089
00090 ros::Publisher odom_pub;
00091 ros::Publisher ground_truth_pub;
00092
00093 std::vector<ros::Publisher> image_pubs;
00094 std::vector<ros::Publisher> depth_pubs;
00095 std::vector<ros::Publisher> camera_pubs;
00096 std::vector<ros::Publisher> laser_pubs;
00097
00098 ros::Subscriber cmdvel_sub;
00099 };
00100
00101 std::vector<StageRobot const *> robotmodels_;
00102
00103
00104 std::vector<Stg::Pose> initial_poses;
00105 ros::ServiceServer reset_srv_;
00106
00107 ros::Publisher clock_pub_;
00108
00109 bool isDepthCanonical;
00110 bool use_model_names;
00111
00112
00113
00114 static void ghfunc(Stg::Model* mod, StageNode* node);
00115
00116 static bool s_update(Stg::World* world, StageNode* node)
00117 {
00118 node->WorldCallback();
00119
00120
00121 return false;
00122 }
00123
00124
00125
00126 const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
00127 const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
00128
00129 tf::TransformBroadcaster tf;
00130
00131
00132 ros::Time base_last_cmd;
00133 ros::Duration base_watchdog_timeout;
00134
00135
00136 ros::Time sim_time;
00137
00138
00139 ros::Time base_last_globalpos_time;
00140
00141 std::vector<Stg::Pose> base_last_globalpos;
00142
00143 public:
00144
00145
00146 StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names);
00147 ~StageNode();
00148
00149
00150
00151
00152 int SubscribeModels();
00153
00154
00155 void WorldCallback();
00156
00157
00158
00159 bool UpdateWorld();
00160
00161
00162 void cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg);
00163
00164
00165 bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
00166
00167
00168 Stg::World* world;
00169 };
00170
00171
00172 const char *
00173 StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
00174 {
00175
00176 bool umn = this->use_model_names;
00177
00178 if ((positionmodels.size() > 1 ) || umn)
00179 {
00180 static char buf[100];
00181 std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
00182
00183 if ((found==std::string::npos) && umn)
00184 {
00185 snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
00186 }
00187 else
00188 {
00189 snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
00190 }
00191
00192 return buf;
00193 }
00194 else
00195 return name;
00196 }
00197
00198 const char *
00199 StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
00200 {
00201
00202 bool umn = this->use_model_names;
00203
00204 if ((positionmodels.size() > 1 ) || umn)
00205 {
00206 static char buf[100];
00207 std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
00208
00209 if ((found==std::string::npos) && umn)
00210 {
00211 snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
00212 }
00213 else
00214 {
00215 snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
00216 }
00217
00218 return buf;
00219 }
00220 else
00221 {
00222 static char buf[100];
00223 snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
00224 return buf;
00225 }
00226 }
00227
00228 void
00229 StageNode::ghfunc(Stg::Model* mod, StageNode* node)
00230 {
00231 if (dynamic_cast<Stg::ModelRanger *>(mod))
00232 node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
00233 if (dynamic_cast<Stg::ModelPosition *>(mod)) {
00234 Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
00235
00236 node->positionmodels.push_back(p);
00237 node->initial_poses.push_back(p->GetGlobalPose());
00238 }
00239 if (dynamic_cast<Stg::ModelCamera *>(mod))
00240 node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
00241 }
00242
00243
00244
00245
00246 bool
00247 StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
00248 {
00249 ROS_INFO("Resetting stage!");
00250 for (size_t r = 0; r < this->positionmodels.size(); r++) {
00251 this->positionmodels[r]->SetPose(this->initial_poses[r]);
00252 this->positionmodels[r]->SetStall(false);
00253 }
00254 return true;
00255 }
00256
00257
00258
00259 void
00260 StageNode::cmdvelReceived(int idx, const boost::shared_ptr<geometry_msgs::Twist const>& msg)
00261 {
00262 boost::mutex::scoped_lock lock(msg_lock);
00263 this->positionmodels[idx]->SetSpeed(msg->linear.x,
00264 msg->linear.y,
00265 msg->angular.z);
00266 this->base_last_cmd = this->sim_time;
00267 }
00268
00269 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
00270 {
00271 this->use_model_names = use_model_names;
00272 this->sim_time.fromSec(0.0);
00273 this->base_last_cmd.fromSec(0.0);
00274 double t;
00275 ros::NodeHandle localn("~");
00276 if(!localn.getParam("base_watchdog_timeout", t))
00277 t = 0.2;
00278 this->base_watchdog_timeout.fromSec(t);
00279
00280 if(!localn.getParam("is_depth_canonical", isDepthCanonical))
00281 isDepthCanonical = true;
00282
00283
00284
00285
00286
00287 struct stat s;
00288 if(stat(fname, &s) != 0)
00289 {
00290 ROS_FATAL("The world file %s does not exist.", fname);
00291 ROS_BREAK();
00292 }
00293
00294
00295 Stg::Init( &argc, &argv );
00296
00297 if(gui)
00298 this->world = new Stg::WorldGui(600, 400, "Stage (ROS)");
00299 else
00300 this->world = new Stg::World();
00301
00302
00303
00304
00305
00306 this->world->Load(fname);
00307
00308
00309
00310 this->world->AddUpdateCallback((Stg::world_callback_t)s_update, this);
00311
00312 this->world->ForEachDescendant((Stg::model_callback_t)ghfunc, this);
00313 }
00314
00315
00316
00317
00318
00319
00320
00321
00322 int
00323 StageNode::SubscribeModels()
00324 {
00325 n_.setParam("/use_sim_time", true);
00326
00327 for (size_t r = 0; r < this->positionmodels.size(); r++)
00328 {
00329 StageRobot* new_robot = new StageRobot;
00330 new_robot->positionmodel = this->positionmodels[r];
00331 new_robot->positionmodel->Subscribe();
00332
00333
00334 for (size_t s = 0; s < this->lasermodels.size(); s++)
00335 {
00336 if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel)
00337 {
00338 new_robot->lasermodels.push_back(this->lasermodels[s]);
00339 this->lasermodels[s]->Subscribe();
00340 }
00341 }
00342
00343 for (size_t s = 0; s < this->cameramodels.size(); s++)
00344 {
00345 if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel)
00346 {
00347 new_robot->cameramodels.push_back(this->cameramodels[s]);
00348 this->cameramodels[s]->Subscribe();
00349 }
00350 }
00351
00352 ROS_INFO("Found %lu laser devices and %lu cameras in robot %lu", new_robot->lasermodels.size(), new_robot->cameramodels.size(), r);
00353
00354 new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
00355 new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
00356 new_robot->cmdvel_sub = n_.subscribe<geometry_msgs::Twist>(mapName(CMD_VEL, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10, boost::bind(&StageNode::cmdvelReceived, this, r, _1));
00357
00358 for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
00359 {
00360 if (new_robot->lasermodels.size() == 1)
00361 new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00362 else
00363 new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00364
00365 }
00366
00367 for (size_t s = 0; s < new_robot->cameramodels.size(); ++s)
00368 {
00369 if (new_robot->cameramodels.size() == 1)
00370 {
00371 new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00372 new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00373 new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00374 }
00375 else
00376 {
00377 new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00378 new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00379 new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
00380 }
00381 }
00382
00383 this->robotmodels_.push_back(new_robot);
00384 }
00385 clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);
00386
00387
00388 reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this);
00389
00390 return(0);
00391 }
00392
00393 StageNode::~StageNode()
00394 {
00395 for (std::vector<StageRobot const*>::iterator r = this->robotmodels_.begin(); r != this->robotmodels_.end(); ++r)
00396 delete *r;
00397 }
00398
00399 bool
00400 StageNode::UpdateWorld()
00401 {
00402 return this->world->UpdateAll();
00403 }
00404
00405 void
00406 StageNode::WorldCallback()
00407 {
00408 boost::mutex::scoped_lock lock(msg_lock);
00409
00410 this->sim_time.fromSec(world->SimTimeNow() / 1e6);
00411
00412
00413 if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
00414 {
00415 ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
00416 return;
00417 }
00418
00419
00420 if((this->base_watchdog_timeout.toSec() > 0.0) &&
00421 ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
00422 {
00423 for (size_t r = 0; r < this->positionmodels.size(); r++)
00424 this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
00425 }
00426
00427
00428 for (size_t r = 0; r < this->robotmodels_.size(); ++r)
00429 {
00430 StageRobot const * robotmodel = this->robotmodels_[r];
00431
00432
00433 for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
00434 {
00435 Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s];
00436 const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->GetSensors();
00437
00438 if( sensors.size() > 1 )
00439 ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
00440
00441
00442
00443 const Stg::ModelRanger::Sensor& sensor = sensors[0];
00444
00445 if( sensor.ranges.size() )
00446 {
00447
00448 sensor_msgs::LaserScan msg;
00449 msg.angle_min = -sensor.fov/2.0;
00450 msg.angle_max = +sensor.fov/2.0;
00451 msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1);
00452 msg.range_min = sensor.range.min;
00453 msg.range_max = sensor.range.max;
00454 msg.ranges.resize(sensor.ranges.size());
00455 msg.intensities.resize(sensor.intensities.size());
00456
00457 for(unsigned int i = 0; i < sensor.ranges.size(); i++)
00458 {
00459 msg.ranges[i] = sensor.ranges[i];
00460 msg.intensities[i] = sensor.intensities[i];
00461 }
00462
00463 if (robotmodel->lasermodels.size() > 1)
00464 msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
00465 else
00466 msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
00467
00468 msg.header.stamp = sim_time;
00469 robotmodel->laser_pubs[s].publish(msg);
00470 }
00471
00472
00473
00474 Stg::Pose lp = lasermodel->GetPose();
00475 tf::Quaternion laserQ;
00476 laserQ.setRPY(0.0, 0.0, lp.a);
00477 tf::Transform txLaser = tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));
00478
00479 if (robotmodel->lasermodels.size() > 1)
00480 tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
00481 mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00482 mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00483 else
00484 tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
00485 mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00486 mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00487 }
00488
00489
00490 tf.sendTransform(tf::StampedTransform(tf::Transform::getIdentity(),
00491 sim_time,
00492 mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00493 mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00494
00495
00496
00497 nav_msgs::Odometry odom_msg;
00498 odom_msg.pose.pose.position.x = robotmodel->positionmodel->est_pose.x;
00499 odom_msg.pose.pose.position.y = robotmodel->positionmodel->est_pose.y;
00500 odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotmodel->positionmodel->est_pose.a);
00501 Stg::Velocity v = robotmodel->positionmodel->GetVelocity();
00502 odom_msg.twist.twist.linear.x = v.x;
00503 odom_msg.twist.twist.linear.y = v.y;
00504 odom_msg.twist.twist.angular.z = v.a;
00505
00506
00507
00508
00509 odom_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
00510 odom_msg.header.stamp = sim_time;
00511
00512 robotmodel->odom_pub.publish(odom_msg);
00513
00514
00515 tf::Quaternion odomQ;
00516 tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
00517 tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
00518 tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
00519 mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00520 mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00521
00522
00523 Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
00524 tf::Quaternion q_gpose;
00525 q_gpose.setRPY(0.0, 0.0, gpose.a);
00526 tf::Transform gt(q_gpose, tf::Point(gpose.x, gpose.y, 0.0));
00527
00528 Stg::Velocity gvel(0,0,0,0);
00529 if (this->base_last_globalpos.size()>r){
00530 Stg::Pose prevpose = this->base_last_globalpos.at(r);
00531 double dT = (this->sim_time-this->base_last_globalpos_time).toSec();
00532 if (dT>0)
00533 gvel = Stg::Velocity(
00534 (gpose.x - prevpose.x)/dT,
00535 (gpose.y - prevpose.y)/dT,
00536 (gpose.z - prevpose.z)/dT,
00537 Stg::normalize(gpose.a - prevpose.a)/dT
00538 );
00539 this->base_last_globalpos.at(r) = gpose;
00540 }else
00541 this->base_last_globalpos.push_back(gpose);
00542
00543 nav_msgs::Odometry ground_truth_msg;
00544 ground_truth_msg.pose.pose.position.x = gt.getOrigin().x();
00545 ground_truth_msg.pose.pose.position.y = gt.getOrigin().y();
00546 ground_truth_msg.pose.pose.position.z = gt.getOrigin().z();
00547 ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x();
00548 ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y();
00549 ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z();
00550 ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w();
00551 ground_truth_msg.twist.twist.linear.x = gvel.x;
00552 ground_truth_msg.twist.twist.linear.y = gvel.y;
00553 ground_truth_msg.twist.twist.linear.z = gvel.z;
00554 ground_truth_msg.twist.twist.angular.z = gvel.a;
00555
00556 ground_truth_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
00557 ground_truth_msg.header.stamp = sim_time;
00558
00559 robotmodel->ground_truth_pub.publish(ground_truth_msg);
00560
00561
00562 for (size_t s = 0; s < robotmodel->cameramodels.size(); ++s)
00563 {
00564 Stg::ModelCamera* cameramodel = robotmodel->cameramodels[s];
00565
00566
00567 if (robotmodel->image_pubs[s].getNumSubscribers() > 0 && cameramodel->FrameColor())
00568 {
00569 sensor_msgs::Image image_msg;
00570
00571 image_msg.height = cameramodel->getHeight();
00572 image_msg.width = cameramodel->getWidth();
00573 image_msg.encoding = "rgba8";
00574
00575 image_msg.step = image_msg.width*4;
00576 image_msg.data.resize(image_msg.width * image_msg.height * 4);
00577
00578 memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4);
00579
00580
00581 int height = image_msg.height - 1;
00582 int linewidth = image_msg.width*4;
00583
00584 char* temp = new char[linewidth];
00585 for (int y = 0; y < (height+1)/2; y++)
00586 {
00587 memcpy(temp,&image_msg.data[y*linewidth],linewidth);
00588 memcpy(&(image_msg.data[y*linewidth]),&(image_msg.data[(height-y)*linewidth]),linewidth);
00589 memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth);
00590 }
00591
00592 if (robotmodel->cameramodels.size() > 1)
00593 image_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
00594 else
00595 image_msg.header.frame_id = mapName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
00596 image_msg.header.stamp = sim_time;
00597
00598 robotmodel->image_pubs[s].publish(image_msg);
00599 }
00600
00601
00602
00603
00604 if (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth())
00605 {
00606 sensor_msgs::Image depth_msg;
00607 depth_msg.height = cameramodel->getHeight();
00608 depth_msg.width = cameramodel->getWidth();
00609 depth_msg.encoding = this->isDepthCanonical?sensor_msgs::image_encodings::TYPE_32FC1:sensor_msgs::image_encodings::TYPE_16UC1;
00610
00611 int sz = this->isDepthCanonical?sizeof(float):sizeof(uint16_t);
00612 size_t len = depth_msg.width * depth_msg.height;
00613 depth_msg.step = depth_msg.width * sz;
00614 depth_msg.data.resize(len*sz);
00615
00616
00617 if (this->isDepthCanonical){
00618 double nearClip = cameramodel->getCamera().nearClip();
00619 double farClip = cameramodel->getCamera().farClip();
00620 memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz);
00621 float * data = (float*)&(depth_msg.data[0]);
00622 for (size_t i=0;i<len;++i)
00623 if(data[i]<=nearClip)
00624 data[i] = -INFINITY;
00625 else if(data[i]>=farClip)
00626 data[i] = INFINITY;
00627 }
00628 else{
00629 int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000);
00630 int farClip = (int)(cameramodel->getCamera().farClip() * 1000);
00631 for (size_t i=0;i<len;++i){
00632 int v = (int)(cameramodel->FrameDepth()[i]*1000);
00633 if (v<=nearClip || v>=farClip) v = 0;
00634 ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
00635 }
00636 }
00637
00638
00639 int height = depth_msg.height - 1;
00640 int linewidth = depth_msg.width*sz;
00641
00642 char* temp = new char[linewidth];
00643 for (int y = 0; y < (height+1)/2; y++)
00644 {
00645 memcpy(temp,&depth_msg.data[y*linewidth],linewidth);
00646 memcpy(&(depth_msg.data[y*linewidth]),&(depth_msg.data[(height-y)*linewidth]),linewidth);
00647 memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth);
00648 }
00649
00650 if (robotmodel->cameramodels.size() > 1)
00651 depth_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
00652 else
00653 depth_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
00654 depth_msg.header.stamp = sim_time;
00655 robotmodel->depth_pubs[s].publish(depth_msg);
00656 }
00657
00658
00659 if ((robotmodel->image_pubs[s].getNumSubscribers()>0 && cameramodel->FrameColor())
00660 || (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth()))
00661 {
00662
00663 Stg::Pose lp = cameramodel->GetPose();
00664 tf::Quaternion Q; Q.setRPY(
00665 (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI,
00666 0.0,
00667 lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->positionmodel->GetPose().a
00668 );
00669
00670 tf::Transform tr = tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z));
00671
00672 if (robotmodel->cameramodels.size() > 1)
00673 tf.sendTransform(tf::StampedTransform(tr, sim_time,
00674 mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00675 mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00676 else
00677 tf.sendTransform(tf::StampedTransform(tr, sim_time,
00678 mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
00679 mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
00680
00681 sensor_msgs::CameraInfo camera_msg;
00682 if (robotmodel->cameramodels.size() > 1)
00683 camera_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
00684 else
00685 camera_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
00686 camera_msg.header.stamp = sim_time;
00687 camera_msg.height = cameramodel->getHeight();
00688 camera_msg.width = cameramodel->getWidth();
00689
00690 double fx,fy,cx,cy;
00691 cx = camera_msg.width / 2.0;
00692 cy = camera_msg.height / 2.0;
00693 double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0;
00694 double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0;
00695
00696
00697 fx = cameramodel->getWidth()/(2*tan(fovh/2));
00698 fy = cameramodel->getHeight()/(2*tan(fovv/2));
00699
00700
00701
00702
00703 camera_msg.D.resize(4, 0.0);
00704
00705 camera_msg.K[0] = fx;
00706 camera_msg.K[2] = cx;
00707 camera_msg.K[4] = fy;
00708 camera_msg.K[5] = cy;
00709 camera_msg.K[8] = 1.0;
00710
00711 camera_msg.R[0] = 1.0;
00712 camera_msg.R[4] = 1.0;
00713 camera_msg.R[8] = 1.0;
00714
00715 camera_msg.P[0] = fx;
00716 camera_msg.P[2] = cx;
00717 camera_msg.P[5] = fy;
00718 camera_msg.P[6] = cy;
00719 camera_msg.P[10] = 1.0;
00720
00721 robotmodel->camera_pubs[s].publish(camera_msg);
00722
00723 }
00724
00725 }
00726 }
00727
00728 this->base_last_globalpos_time = this->sim_time;
00729 rosgraph_msgs::Clock clock_msg;
00730 clock_msg.clock = sim_time;
00731 this->clock_pub_.publish(clock_msg);
00732 }
00733
00734 int
00735 main(int argc, char** argv)
00736 {
00737 if( argc < 2 )
00738 {
00739 puts(USAGE);
00740 exit(-1);
00741 }
00742
00743 ros::init(argc, argv, "stageros");
00744
00745 bool gui = true;
00746 bool use_model_names = false;
00747 for(int i=0;i<(argc-1);i++)
00748 {
00749 if(!strcmp(argv[i], "-g"))
00750 gui = false;
00751 if(!strcmp(argv[i], "-u"))
00752 use_model_names = true;
00753 }
00754
00755 StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
00756
00757 if(sn.SubscribeModels() != 0)
00758 exit(-1);
00759
00760 boost::thread t = boost::thread(boost::bind(&ros::spin));
00761
00762
00763 sn.world->Start();
00764
00765
00766
00767 ros::WallRate r(10.0);
00768 while(ros::ok() && !sn.world->TestQuit())
00769 {
00770 if(gui)
00771 Fl::wait(r.expectedCycleTime().toSec());
00772 else
00773 {
00774 sn.UpdateWorld();
00775 r.sleep();
00776 }
00777 }
00778 t.join();
00779
00780 exit(0);
00781 }
00782