stageros.cpp
Go to the documentation of this file.
1 /*
2  * stageros
3  * Copyright (c) 2008, Willow Garage, Inc.
4  *
5  * This program is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
18  */
19 
27 #include <stdio.h>
28 #include <stdlib.h>
29 #include <string.h>
30 
31 #include <sys/types.h>
32 #include <sys/stat.h>
33 #include <unistd.h>
34 #include <signal.h>
35 
36 
37 // libstage
38 #include <stage.hh>
39 
40 // roscpp
41 #include <ros/ros.h>
42 #include <boost/thread/mutex.hpp>
43 #include <boost/thread/thread.hpp>
44 #include <sensor_msgs/LaserScan.h>
45 #include <sensor_msgs/Image.h>
47 #include <sensor_msgs/CameraInfo.h>
48 #include <nav_msgs/Odometry.h>
49 #include <geometry_msgs/Twist.h>
50 #include <rosgraph_msgs/Clock.h>
51 
52 #include <std_srvs/Empty.h>
53 
55 
56 #define USAGE "stageros <worldfile>"
57 #define IMAGE "image"
58 #define DEPTH "depth"
59 #define CAMERA_INFO "camera_info"
60 #define ODOM "odom"
61 #define BASE_SCAN "base_scan"
62 #define BASE_POSE_GROUND_TRUTH "base_pose_ground_truth"
63 #define CMD_VEL "cmd_vel"
64 
65 // Our node
66 class StageNode
67 {
68 private:
69 
70  // roscpp-related bookkeeping
72 
73  // A mutex to lock access to fields that are used in message callbacks
74  boost::mutex msg_lock;
75 
76  // The models that we're interested in
77  std::vector<Stg::ModelCamera *> cameramodels;
78  std::vector<Stg::ModelRanger *> lasermodels;
79  std::vector<Stg::ModelPosition *> positionmodels;
80 
81  //a structure representing a robot inthe simulator
82  struct StageRobot
83  {
84  //stage related models
86  std::vector<Stg::ModelCamera *> cameramodels; //multiple cameras per position
87  std::vector<Stg::ModelRanger *> lasermodels; //multiple rangers per position
88 
89  //ros publishers
91  ros::Publisher ground_truth_pub; //one ground truth
92 
93  std::vector<ros::Publisher> image_pubs; //multiple images
94  std::vector<ros::Publisher> depth_pubs; //multiple depths
95  std::vector<ros::Publisher> camera_pubs; //multiple cameras
96  std::vector<ros::Publisher> laser_pubs; //multiple lasers
97 
98  ros::Subscriber cmdvel_sub; //one cmd_vel subscriber
99  };
100 
101  std::vector<StageRobot const *> robotmodels_;
102 
103  // Used to remember initial poses for soft reset
104  std::vector<Stg::Pose> initial_poses;
106 
108 
111 
112  // A helper function that is executed for each stage model. We use it
113  // to search for models of interest.
114  static void ghfunc(Stg::Model* mod, StageNode* node);
115 
116  static bool s_update(Stg::World* world, StageNode* node)
117  {
118  node->WorldCallback();
119  // We return false to indicate that we want to be called again (an
120  // odd convention, but that's the way that Stage works).
121  return false;
122  }
123 
124  // Appends the given robot ID to the given message name. If omitRobotID
125  // is true, an unaltered copy of the name is returned.
126  const char *mapName(const char *name, size_t robotID, Stg::Model* mod) const;
127  const char *mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const;
128 
130 
131  // Last time that we received a velocity command
134 
135  // Current simulation time
137 
138  // Last time we saved global position (for velocity calculation).
140  // Last published global pose of each robot
141  std::vector<Stg::Pose> base_last_globalpos;
142 
143 public:
144  // Constructor; stage itself needs argc/argv. fname is the .world file
145  // that stage should load.
146  StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names);
147  ~StageNode();
148 
149  // Subscribe to models of interest. Currently, we find and subscribe
150  // to the first 'laser' model and the first 'position' model. Returns
151  // 0 on success (both models subscribed), -1 otherwise.
152  int SubscribeModels();
153 
154  // Our callback
155  void WorldCallback();
156 
157  // Do one update of the world. May pause if the next update time
158  // has not yet arrived.
159  bool UpdateWorld();
160 
161  // Message callback for a MsgBaseVel message, which set velocities.
163 
164  // Service callback for soft reset
165  bool cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response);
166 
167  // The main simulator object
169 };
170 
171 // since stageros is single-threaded, this is OK. revisit if that changes!
172 const char *
173 StageNode::mapName(const char *name, size_t robotID, Stg::Model* mod) const
174 {
175  //ROS_INFO("Robot %lu: Device %s", robotID, name);
176  bool umn = this->use_model_names;
177 
178  if ((positionmodels.size() > 1 ) || umn)
179  {
180  static char buf[100];
181  std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
182 
183  if ((found==std::string::npos) && umn)
184  {
185  snprintf(buf, sizeof(buf), "/%s/%s", ((Stg::Ancestor *) mod)->Token(), name);
186  }
187  else
188  {
189  snprintf(buf, sizeof(buf), "/robot_%u/%s", (unsigned int)robotID, name);
190  }
191 
192  return buf;
193  }
194  else
195  return name;
196 }
197 
198 const char *
199 StageNode::mapName(const char *name, size_t robotID, size_t deviceID, Stg::Model* mod) const
200 {
201  //ROS_INFO("Robot %lu: Device %s:%lu", robotID, name, deviceID);
202  bool umn = this->use_model_names;
203 
204  if ((positionmodels.size() > 1 ) || umn)
205  {
206  static char buf[100];
207  std::size_t found = std::string(((Stg::Ancestor *) mod)->Token()).find(":");
208 
209  if ((found==std::string::npos) && umn)
210  {
211  snprintf(buf, sizeof(buf), "/%s/%s_%u", ((Stg::Ancestor *) mod)->Token(), name, (unsigned int)deviceID);
212  }
213  else
214  {
215  snprintf(buf, sizeof(buf), "/robot_%u/%s_%u", (unsigned int)robotID, name, (unsigned int)deviceID);
216  }
217 
218  return buf;
219  }
220  else
221  {
222  static char buf[100];
223  snprintf(buf, sizeof(buf), "/%s_%u", name, (unsigned int)deviceID);
224  return buf;
225  }
226 }
227 
228 void
230 {
231  if (dynamic_cast<Stg::ModelRanger *>(mod))
232  node->lasermodels.push_back(dynamic_cast<Stg::ModelRanger *>(mod));
233  if (dynamic_cast<Stg::ModelPosition *>(mod)) {
234  Stg::ModelPosition * p = dynamic_cast<Stg::ModelPosition *>(mod);
235  // remember initial poses
236  node->positionmodels.push_back(p);
237  node->initial_poses.push_back(p->GetGlobalPose());
238  }
239  if (dynamic_cast<Stg::ModelCamera *>(mod))
240  node->cameramodels.push_back(dynamic_cast<Stg::ModelCamera *>(mod));
241 }
242 
243 
244 
245 
246 bool
247 StageNode::cb_reset_srv(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response)
248 {
249  ROS_INFO("Resetting stage!");
250  for (size_t r = 0; r < this->positionmodels.size(); r++) {
251  this->positionmodels[r]->SetPose(this->initial_poses[r]);
252  this->positionmodels[r]->SetStall(false);
253  }
254  return true;
255 }
256 
257 
258 
259 void
261 {
262  boost::mutex::scoped_lock lock(msg_lock);
263  this->positionmodels[idx]->SetSpeed(msg->linear.x,
264  msg->linear.y,
265  msg->angular.z);
266  this->base_last_cmd = this->sim_time;
267 }
268 
269 StageNode::StageNode(int argc, char** argv, bool gui, const char* fname, bool use_model_names)
270 {
271  this->use_model_names = use_model_names;
272  this->sim_time.fromSec(0.0);
273  this->base_last_cmd.fromSec(0.0);
274  double t;
275  ros::NodeHandle localn("~");
276  if(!localn.getParam("base_watchdog_timeout", t))
277  t = 0.2;
279 
280  if(!localn.getParam("is_depth_canonical", isDepthCanonical))
281  isDepthCanonical = true;
282 
283 
284  // We'll check the existence of the world file, because libstage doesn't
285  // expose its failure to open it. Could go further with checks (e.g., is
286  // it readable by this user).
287  struct stat s;
288  if(stat(fname, &s) != 0)
289  {
290  ROS_FATAL("The world file %s does not exist.", fname);
291  ROS_BREAK();
292  }
293 
294  // initialize libstage
295  Stg::Init( &argc, &argv );
296 
297  if(gui)
298  this->world = new Stg::WorldGui(600, 400, "Stage (ROS)");
299  else
300  this->world = new Stg::World();
301 
302  // Apparently an Update is needed before the Load to avoid crashes on
303  // startup on some systems.
304  // As of Stage 4.1.1, this update call causes a hang on start.
305  //this->UpdateWorld();
306  this->world->Load(fname);
307 
308  // We add our callback here, after the Update, so avoid our callback
309  // being invoked before we're ready.
311 
313 }
314 
315 
316 // Subscribe to models of interest. Currently, we find and subscribe
317 // to the first 'laser' model and the first 'position' model. Returns
318 // 0 on success (both models subscribed), -1 otherwise.
319 //
320 // Eventually, we should provide a general way to map stage models onto ROS
321 // topics, similar to Player .cfg files.
322 int
324 {
325  n_.setParam("/use_sim_time", true);
326 
327  for (size_t r = 0; r < this->positionmodels.size(); r++)
328  {
329  StageRobot* new_robot = new StageRobot;
330  new_robot->positionmodel = this->positionmodels[r];
331  new_robot->positionmodel->Subscribe();
332 
333 
334  for (size_t s = 0; s < this->lasermodels.size(); s++)
335  {
336  if (this->lasermodels[s] and this->lasermodels[s]->Parent() == new_robot->positionmodel)
337  {
338  new_robot->lasermodels.push_back(this->lasermodels[s]);
339  this->lasermodels[s]->Subscribe();
340  }
341  }
342 
343  for (size_t s = 0; s < this->cameramodels.size(); s++)
344  {
345  if (this->cameramodels[s] and this->cameramodels[s]->Parent() == new_robot->positionmodel)
346  {
347  new_robot->cameramodels.push_back(this->cameramodels[s]);
348  this->cameramodels[s]->Subscribe();
349  }
350  }
351 
352  ROS_INFO("Found %lu laser devices and %lu cameras in robot %lu", new_robot->lasermodels.size(), new_robot->cameramodels.size(), r);
353 
354  new_robot->odom_pub = n_.advertise<nav_msgs::Odometry>(mapName(ODOM, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
355  new_robot->ground_truth_pub = n_.advertise<nav_msgs::Odometry>(mapName(BASE_POSE_GROUND_TRUTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10);
356  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));
357 
358  for (size_t s = 0; s < new_robot->lasermodels.size(); ++s)
359  {
360  if (new_robot->lasermodels.size() == 1)
361  new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
362  else
363  new_robot->laser_pubs.push_back(n_.advertise<sensor_msgs::LaserScan>(mapName(BASE_SCAN, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
364 
365  }
366 
367  for (size_t s = 0; s < new_robot->cameramodels.size(); ++s)
368  {
369  if (new_robot->cameramodels.size() == 1)
370  {
371  new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
372  new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
373  new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
374  }
375  else
376  {
377  new_robot->image_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(IMAGE, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
378  new_robot->depth_pubs.push_back(n_.advertise<sensor_msgs::Image>(mapName(DEPTH, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
379  new_robot->camera_pubs.push_back(n_.advertise<sensor_msgs::CameraInfo>(mapName(CAMERA_INFO, r, s, static_cast<Stg::Model*>(new_robot->positionmodel)), 10));
380  }
381  }
382 
383  this->robotmodels_.push_back(new_robot);
384  }
385  clock_pub_ = n_.advertise<rosgraph_msgs::Clock>("/clock", 10);
386 
387  // advertising reset service
388  reset_srv_ = n_.advertiseService("reset_positions", &StageNode::cb_reset_srv, this);
389 
390  return(0);
391 }
392 
394 {
395  for (std::vector<StageRobot const*>::iterator r = this->robotmodels_.begin(); r != this->robotmodels_.end(); ++r)
396  delete *r;
397 }
398 
399 bool
401 {
402  return this->world->UpdateAll();
403 }
404 
405 void
407 {
408  boost::mutex::scoped_lock lock(msg_lock);
409 
410  this->sim_time.fromSec(world->SimTimeNow() / 1e6);
411  // We're not allowed to publish clock==0, because it used as a special
412  // value in parts of ROS, #4027.
413  if(this->sim_time.sec == 0 && this->sim_time.nsec == 0)
414  {
415  ROS_DEBUG("Skipping initial simulation step, to avoid publishing clock==0");
416  return;
417  }
418 
419  // TODO make this only affect one robot if necessary
420  if((this->base_watchdog_timeout.toSec() > 0.0) &&
421  ((this->sim_time - this->base_last_cmd) >= this->base_watchdog_timeout))
422  {
423  for (size_t r = 0; r < this->positionmodels.size(); r++)
424  this->positionmodels[r]->SetSpeed(0.0, 0.0, 0.0);
425  }
426 
427  //loop on the robot models
428  for (size_t r = 0; r < this->robotmodels_.size(); ++r)
429  {
430  StageRobot const * robotmodel = this->robotmodels_[r];
431 
432  //loop on the laser devices for the current robot
433  for (size_t s = 0; s < robotmodel->lasermodels.size(); ++s)
434  {
435  Stg::ModelRanger const* lasermodel = robotmodel->lasermodels[s];
436  const std::vector<Stg::ModelRanger::Sensor>& sensors = lasermodel->GetSensors();
437 
438  if( sensors.size() > 1 )
439  ROS_WARN( "ROS Stage currently supports rangers with 1 sensor only." );
440 
441  // for now we access only the zeroth sensor of the ranger - good
442  // enough for most laser models that have a single beam origin
443  const Stg::ModelRanger::Sensor& sensor = sensors[0];
444 
445  if( sensor.ranges.size() )
446  {
447  // Translate into ROS message format and publish
448  sensor_msgs::LaserScan msg;
449  msg.angle_min = -sensor.fov/2.0;
450  msg.angle_max = +sensor.fov/2.0;
451  msg.angle_increment = sensor.fov/(double)(sensor.sample_count-1);
452  msg.range_min = sensor.range.min;
453  msg.range_max = sensor.range.max;
454  msg.ranges.resize(sensor.ranges.size());
455  msg.intensities.resize(sensor.intensities.size());
456 
457  for(unsigned int i = 0; i < sensor.ranges.size(); i++)
458  {
459  msg.ranges[i] = sensor.ranges[i];
460  msg.intensities[i] = sensor.intensities[i];
461  }
462 
463  if (robotmodel->lasermodels.size() > 1)
464  msg.header.frame_id = mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
465  else
466  msg.header.frame_id = mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
467 
468  msg.header.stamp = sim_time;
469  robotmodel->laser_pubs[s].publish(msg);
470  }
471 
472  // Also publish the base->base_laser_link Tx. This could eventually move
473  // into being retrieved from the param server as a static Tx.
474  Stg::Pose lp = lasermodel->GetPose();
475  tf::Quaternion laserQ;
476  laserQ.setRPY(0.0, 0.0, lp.a);
477  tf::Transform txLaser = tf::Transform(laserQ, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z + lp.z));
478 
479  if (robotmodel->lasermodels.size() > 1)
480  tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
481  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
482  mapName("base_laser_link", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
483  else
484  tf.sendTransform(tf::StampedTransform(txLaser, sim_time,
485  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
486  mapName("base_laser_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
487  }
488 
489  //the position of the robot
491  sim_time,
492  mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
493  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
494 
495  // Get latest odometry data
496  // Translate into ROS message format and publish
497  nav_msgs::Odometry odom_msg;
498  odom_msg.pose.pose.position.x = robotmodel->positionmodel->est_pose.x;
499  odom_msg.pose.pose.position.y = robotmodel->positionmodel->est_pose.y;
500  odom_msg.pose.pose.orientation = tf::createQuaternionMsgFromYaw(robotmodel->positionmodel->est_pose.a);
501  Stg::Velocity v = robotmodel->positionmodel->GetVelocity();
502  odom_msg.twist.twist.linear.x = v.x;
503  odom_msg.twist.twist.linear.y = v.y;
504  odom_msg.twist.twist.angular.z = v.a;
505 
506  //@todo Publish stall on a separate topic when one becomes available
507  //this->odomMsgs[r].stall = this->positionmodels[r]->Stall();
508  //
509  odom_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
510  odom_msg.header.stamp = sim_time;
511 
512  robotmodel->odom_pub.publish(odom_msg);
513 
514  // broadcast odometry transform
515  tf::Quaternion odomQ;
516  tf::quaternionMsgToTF(odom_msg.pose.pose.orientation, odomQ);
517  tf::Transform txOdom(odomQ, tf::Point(odom_msg.pose.pose.position.x, odom_msg.pose.pose.position.y, 0.0));
518  tf.sendTransform(tf::StampedTransform(txOdom, sim_time,
519  mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
520  mapName("base_footprint", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
521 
522  // Also publish the ground truth pose and velocity
523  Stg::Pose gpose = robotmodel->positionmodel->GetGlobalPose();
524  tf::Quaternion q_gpose;
525  q_gpose.setRPY(0.0, 0.0, gpose.a);
526  tf::Transform gt(q_gpose, tf::Point(gpose.x, gpose.y, 0.0));
527  // Velocity is 0 by default and will be set only if there is previous pose and time delta>0
528  Stg::Velocity gvel(0,0,0,0);
529  if (this->base_last_globalpos.size()>r){
530  Stg::Pose prevpose = this->base_last_globalpos.at(r);
531  double dT = (this->sim_time-this->base_last_globalpos_time).toSec();
532  if (dT>0)
533  gvel = Stg::Velocity(
534  (gpose.x - prevpose.x)/dT,
535  (gpose.y - prevpose.y)/dT,
536  (gpose.z - prevpose.z)/dT,
537  Stg::normalize(gpose.a - prevpose.a)/dT
538  );
539  this->base_last_globalpos.at(r) = gpose;
540  }else //There are no previous readings, adding current pose...
541  this->base_last_globalpos.push_back(gpose);
542 
543  nav_msgs::Odometry ground_truth_msg;
544  ground_truth_msg.pose.pose.position.x = gt.getOrigin().x();
545  ground_truth_msg.pose.pose.position.y = gt.getOrigin().y();
546  ground_truth_msg.pose.pose.position.z = gt.getOrigin().z();
547  ground_truth_msg.pose.pose.orientation.x = gt.getRotation().x();
548  ground_truth_msg.pose.pose.orientation.y = gt.getRotation().y();
549  ground_truth_msg.pose.pose.orientation.z = gt.getRotation().z();
550  ground_truth_msg.pose.pose.orientation.w = gt.getRotation().w();
551  ground_truth_msg.twist.twist.linear.x = gvel.x;
552  ground_truth_msg.twist.twist.linear.y = gvel.y;
553  ground_truth_msg.twist.twist.linear.z = gvel.z;
554  ground_truth_msg.twist.twist.angular.z = gvel.a;
555 
556  ground_truth_msg.header.frame_id = mapName("odom", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
557  ground_truth_msg.header.stamp = sim_time;
558 
559  robotmodel->ground_truth_pub.publish(ground_truth_msg);
560 
561  //cameras
562  for (size_t s = 0; s < robotmodel->cameramodels.size(); ++s)
563  {
564  Stg::ModelCamera* cameramodel = robotmodel->cameramodels[s];
565  // Get latest image data
566  // Translate into ROS message format and publish
567  if (robotmodel->image_pubs[s].getNumSubscribers() > 0 && cameramodel->FrameColor())
568  {
569  sensor_msgs::Image image_msg;
570 
571  image_msg.height = cameramodel->getHeight();
572  image_msg.width = cameramodel->getWidth();
573  image_msg.encoding = "rgba8";
574  //this->imageMsgs[r].is_bigendian="";
575  image_msg.step = image_msg.width*4;
576  image_msg.data.resize(image_msg.width * image_msg.height * 4);
577 
578  memcpy(&(image_msg.data[0]), cameramodel->FrameColor(), image_msg.width * image_msg.height * 4);
579 
580  //invert the opengl weirdness
581  int height = image_msg.height - 1;
582  int linewidth = image_msg.width*4;
583 
584  char* temp = new char[linewidth];
585  for (int y = 0; y < (height+1)/2; y++)
586  {
587  memcpy(temp,&image_msg.data[y*linewidth],linewidth);
588  memcpy(&(image_msg.data[y*linewidth]),&(image_msg.data[(height-y)*linewidth]),linewidth);
589  memcpy(&(image_msg.data[(height-y)*linewidth]),temp,linewidth);
590  }
591 
592  if (robotmodel->cameramodels.size() > 1)
593  image_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
594  else
595  image_msg.header.frame_id = mapName("camera", r,static_cast<Stg::Model*>(robotmodel->positionmodel));
596  image_msg.header.stamp = sim_time;
597 
598  robotmodel->image_pubs[s].publish(image_msg);
599  }
600 
601  //Get latest depth data
602  //Translate into ROS message format and publish
603  //Skip if there are no subscribers
604  if (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth())
605  {
606  sensor_msgs::Image depth_msg;
607  depth_msg.height = cameramodel->getHeight();
608  depth_msg.width = cameramodel->getWidth();
610  //this->depthMsgs[r].is_bigendian="";
611  int sz = this->isDepthCanonical?sizeof(float):sizeof(uint16_t);
612  size_t len = depth_msg.width * depth_msg.height;
613  depth_msg.step = depth_msg.width * sz;
614  depth_msg.data.resize(len*sz);
615 
616  //processing data according to REP118
617  if (this->isDepthCanonical){
618  double nearClip = cameramodel->getCamera().nearClip();
619  double farClip = cameramodel->getCamera().farClip();
620  memcpy(&(depth_msg.data[0]),cameramodel->FrameDepth(),len*sz);
621  float * data = (float*)&(depth_msg.data[0]);
622  for (size_t i=0;i<len;++i)
623  if(data[i]<=nearClip)
624  data[i] = -INFINITY;
625  else if(data[i]>=farClip)
626  data[i] = INFINITY;
627  }
628  else{
629  int nearClip = (int)(cameramodel->getCamera().nearClip() * 1000);
630  int farClip = (int)(cameramodel->getCamera().farClip() * 1000);
631  for (size_t i=0;i<len;++i){
632  int v = (int)(cameramodel->FrameDepth()[i]*1000);
633  if (v<=nearClip || v>=farClip) v = 0;
634  ((uint16_t*)&(depth_msg.data[0]))[i] = (uint16_t) ((v<=nearClip || v>=farClip) ? 0 : v );
635  }
636  }
637 
638  //invert the opengl weirdness
639  int height = depth_msg.height - 1;
640  int linewidth = depth_msg.width*sz;
641 
642  char* temp = new char[linewidth];
643  for (int y = 0; y < (height+1)/2; y++)
644  {
645  memcpy(temp,&depth_msg.data[y*linewidth],linewidth);
646  memcpy(&(depth_msg.data[y*linewidth]),&(depth_msg.data[(height-y)*linewidth]),linewidth);
647  memcpy(&(depth_msg.data[(height-y)*linewidth]),temp,linewidth);
648  }
649 
650  if (robotmodel->cameramodels.size() > 1)
651  depth_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
652  else
653  depth_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
654  depth_msg.header.stamp = sim_time;
655  robotmodel->depth_pubs[s].publish(depth_msg);
656  }
657 
658  //sending camera's tf and info only if image or depth topics are subscribed to
659  if ((robotmodel->image_pubs[s].getNumSubscribers()>0 && cameramodel->FrameColor())
660  || (robotmodel->depth_pubs[s].getNumSubscribers()>0 && cameramodel->FrameDepth()))
661  {
662 
663  Stg::Pose lp = cameramodel->GetPose();
664  tf::Quaternion Q; Q.setRPY(
665  (cameramodel->getCamera().pitch()*M_PI/180.0)-M_PI,
666  0.0,
667  lp.a+(cameramodel->getCamera().yaw()*M_PI/180.0) - robotmodel->positionmodel->GetPose().a
668  );
669 
670  tf::Transform tr = tf::Transform(Q, tf::Point(lp.x, lp.y, robotmodel->positionmodel->GetGeom().size.z+lp.z));
671 
672  if (robotmodel->cameramodels.size() > 1)
673  tf.sendTransform(tf::StampedTransform(tr, sim_time,
674  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
675  mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel))));
676  else
677  tf.sendTransform(tf::StampedTransform(tr, sim_time,
678  mapName("base_link", r, static_cast<Stg::Model*>(robotmodel->positionmodel)),
679  mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel))));
680 
681  sensor_msgs::CameraInfo camera_msg;
682  if (robotmodel->cameramodels.size() > 1)
683  camera_msg.header.frame_id = mapName("camera", r, s, static_cast<Stg::Model*>(robotmodel->positionmodel));
684  else
685  camera_msg.header.frame_id = mapName("camera", r, static_cast<Stg::Model*>(robotmodel->positionmodel));
686  camera_msg.header.stamp = sim_time;
687  camera_msg.height = cameramodel->getHeight();
688  camera_msg.width = cameramodel->getWidth();
689 
690  double fx,fy,cx,cy;
691  cx = camera_msg.width / 2.0;
692  cy = camera_msg.height / 2.0;
693  double fovh = cameramodel->getCamera().horizFov()*M_PI/180.0;
694  double fovv = cameramodel->getCamera().vertFov()*M_PI/180.0;
695  //double fx_ = 1.43266615300557*this->cameramodels[r]->getWidth()/tan(fovh);
696  //double fy_ = 1.43266615300557*this->cameramodels[r]->getHeight()/tan(fovv);
697  fx = cameramodel->getWidth()/(2*tan(fovh/2));
698  fy = cameramodel->getHeight()/(2*tan(fovv/2));
699 
700  //ROS_INFO("fx=%.4f,%.4f; fy=%.4f,%.4f", fx, fx_, fy, fy_);
701 
702 
703  camera_msg.D.resize(4, 0.0);
704 
705  camera_msg.K[0] = fx;
706  camera_msg.K[2] = cx;
707  camera_msg.K[4] = fy;
708  camera_msg.K[5] = cy;
709  camera_msg.K[8] = 1.0;
710 
711  camera_msg.R[0] = 1.0;
712  camera_msg.R[4] = 1.0;
713  camera_msg.R[8] = 1.0;
714 
715  camera_msg.P[0] = fx;
716  camera_msg.P[2] = cx;
717  camera_msg.P[5] = fy;
718  camera_msg.P[6] = cy;
719  camera_msg.P[10] = 1.0;
720 
721  robotmodel->camera_pubs[s].publish(camera_msg);
722 
723  }
724 
725  }
726  }
727 
728  this->base_last_globalpos_time = this->sim_time;
729  rosgraph_msgs::Clock clock_msg;
730  clock_msg.clock = sim_time;
731  this->clock_pub_.publish(clock_msg);
732 }
733 
734 int
735 main(int argc, char** argv)
736 {
737  if( argc < 2 )
738  {
739  puts(USAGE);
740  exit(-1);
741  }
742 
743  ros::init(argc, argv, "stageros");
744 
745  bool gui = true;
746  bool use_model_names = false;
747  for(int i=0;i<(argc-1);i++)
748  {
749  if(!strcmp(argv[i], "-g"))
750  gui = false;
751  if(!strcmp(argv[i], "-u"))
752  use_model_names = true;
753  }
754 
755  StageNode sn(argc-1,argv,gui,argv[argc-1], use_model_names);
756 
757  if(sn.SubscribeModels() != 0)
758  exit(-1);
759 
760  boost::thread t = boost::thread(boost::bind(&ros::spin));
761 
762  // New in Stage 4.1.1: must Start() the world.
763  sn.world->Start();
764 
765  // TODO: get rid of this fixed-duration sleep, using some Stage builtin
766  // PauseUntilNextUpdate() functionality.
767  ros::WallRate r(10.0);
768  while(ros::ok() && !sn.world->TestQuit())
769  {
770  if(gui)
771  Fl::wait(r.expectedCycleTime().toSec());
772  else
773  {
774  sn.UpdateWorld();
775  r.sleep();
776  }
777  }
778  t.join();
779 
780  exit(0);
781 }
782 
#define BASE_POSE_GROUND_TRUTH
Definition: stageros.cpp:62
virtual void Start()
#define DEPTH
Definition: stageros.cpp:58
static bool UpdateAll()
#define ROS_FATAL(...)
#define BASE_SCAN
Definition: stageros.cpp:61
bool cb_reset_srv(std_srvs::Empty::Request &request, std_srvs::Empty::Response &response)
Definition: stageros.cpp:247
const char * mapName(const char *name, size_t robotID, Stg::Model *mod) const
Definition: stageros.cpp:173
ros::Time sim_time
Definition: stageros.cpp:136
#define USAGE
Definition: stageros.cpp:56
usec_t SimTimeNow(void) const
bool isDepthCanonical
Definition: stageros.cpp:109
void publish(const boost::shared_ptr< M > &message) const
Stg::ModelPosition * positionmodel
Definition: stageros.cpp:85
bool use_model_names
Definition: stageros.cpp:110
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
int getHeight(void) const
std::vector< ros::Publisher > camera_pubs
Definition: stageros.cpp:95
void Init(int *argc, char **argv[])
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
int(* model_callback_t)(Model *mod, void *user)
double nearClip(void) const
boost::mutex msg_lock
Definition: stageros.cpp:74
void cmdvelReceived(int idx, const boost::shared_ptr< geometry_msgs::Twist const > &msg)
Definition: stageros.cpp:260
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
double yaw(void) const
TFSIMD_FORCE_INLINE const tfScalar & y() const
#define ROS_WARN(...)
Stg::World * world
Definition: stageros.cpp:168
ros::Time base_last_cmd
Definition: stageros.cpp:132
std::vector< StageRobot const * > robotmodels_
Definition: stageros.cpp:101
Time & fromSec(double t)
ROSCPP_DECL void spin(Spinner &spinner)
void ForEachDescendant(model_callback_t func, void *arg)
float s
std::vector< ros::Publisher > laser_pubs
Definition: stageros.cpp:96
void AddUpdateCallback(world_callback_t cb, void *user)
meters_t z
meters_t y
TFSIMD_FORCE_INLINE const tfScalar & x() const
bool UpdateWorld()
Definition: stageros.cpp:400
static bool s_update(Stg::World *world, StageNode *node)
Definition: stageros.cpp:116
double normalize(double a)
std::vector< Stg::ModelRanger * > lasermodels
Definition: stageros.cpp:87
ros::Publisher odom_pub
Definition: stageros.cpp:90
TFSIMD_FORCE_INLINE const tfScalar & z() const
#define CAMERA_INFO
Definition: stageros.cpp:59
Pose GetGlobalPose() const
void setRPY(const tfScalar &roll, const tfScalar &pitch, const tfScalar &yaw)
Duration & fromSec(double t)
static void quaternionMsgToTF(const geometry_msgs::Quaternion &msg, Quaternion &bt)
int main(int argc, char **argv)
Definition: stageros.cpp:735
#define ROS_INFO(...)
std::vector< Stg::ModelPosition * > positionmodels
Definition: stageros.cpp:79
const std::string TYPE_32FC1
int SubscribeModels()
Definition: stageros.cpp:323
int getWidth(void) const
#define CMD_VEL
Definition: stageros.cpp:63
ROSCPP_DECL bool ok()
TFSIMD_FORCE_INLINE const tfScalar & y() const
const std::string TYPE_16UC1
void WorldCallback()
Definition: stageros.cpp:406
const std::vector< Sensor > & GetSensors() const
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
meters_t z
ros::Publisher clock_pub_
Definition: stageros.cpp:107
void Subscribe()
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
ros::Subscriber cmdvel_sub
Definition: stageros.cpp:98
double horizFov(void) const
ros::NodeHandle n_
Definition: stageros.cpp:71
const GLfloat * FrameDepth() const
std::vector< Stg::Pose > initial_poses
Definition: stageros.cpp:104
const PerspectiveCamera & getCamera(void) const
double pitch(void) const
Velocity GetVelocity() const
#define ODOM
Definition: stageros.cpp:60
ros::Publisher ground_truth_pub
Definition: stageros.cpp:91
static geometry_msgs::Quaternion createQuaternionMsgFromYaw(double yaw)
ros::Time base_last_globalpos_time
Definition: stageros.cpp:139
Quaternion getRotation() const
static void ghfunc(Stg::Model *mod, StageNode *node)
Definition: stageros.cpp:229
ros::ServiceServer reset_srv_
Definition: stageros.cpp:105
bool getParam(const std::string &key, std::string &s) const
std::vector< Stg::ModelCamera * > cameramodels
Definition: stageros.cpp:77
WallDuration expectedCycleTime() const
ros::Duration base_watchdog_timeout
Definition: stageros.cpp:133
StageNode(int argc, char **argv, bool gui, const char *fname, bool use_model_names)
Definition: stageros.cpp:269
std::vector< ros::Publisher > depth_pubs
Definition: stageros.cpp:94
static const Transform & getIdentity()
const GLubyte * FrameColor() const
std::vector< ros::Publisher > image_pubs
Definition: stageros.cpp:93
std::vector< Stg::ModelCamera * > cameramodels
Definition: stageros.cpp:86
Pose GetPose() const
radians_t a
std::vector< meters_t > ranges
int(* world_callback_t)(World *world, void *user)
#define IMAGE
Definition: stageros.cpp:57
std::vector< double > intensities
double vertFov(void) const
#define ROS_BREAK()
tf::TransformBroadcaster tf
Definition: stageros.cpp:129
std::vector< Stg::ModelRanger * > lasermodels
Definition: stageros.cpp:78
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
bool TestQuit() const
meters_t x
std::vector< Stg::Pose > base_last_globalpos
Definition: stageros.cpp:141
double farClip(void) const
virtual void Load(const std::string &worldfile_path)
#define ROS_DEBUG(...)
Geom GetGeom() const


stage_ros
Author(s): Brian Gerky
autogenerated on Sat Apr 24 2021 02:23:34