DbPlayerNode.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <ros/ros.h>
29 #include <sensor_msgs/Image.h>
30 #include <sensor_msgs/image_encodings.h>
31 #include <sensor_msgs/PointCloud2.h>
32 #include <sensor_msgs/LaserScan.h>
33 #include <sensor_msgs/CameraInfo.h>
34 #include <sensor_msgs/NavSatFix.h>
35 #include <geometry_msgs/PoseWithCovarianceStamped.h>
36 #include <rosgraph_msgs/Clock.h>
37 #include <pcl_conversions/pcl_conversions.h>
38 #include <nav_msgs/Odometry.h>
39 #include <cv_bridge/cv_bridge.h>
40 #include <image_transport/image_transport.h>
41 #include <tf2_ros/transform_broadcaster.h>
42 #include <std_srvs/Empty.h>
43 #include <rtabmap_conversions/MsgConversion.h>
44 #include <rtabmap_msgs/SetGoal.h>
45 #include <rtabmap/utilite/ULogger.h>
46 #include <rtabmap/utilite/UStl.h>
47 #include <rtabmap/utilite/UThread.h>
48 #include <rtabmap/utilite/UDirectory.h>
49 #include <rtabmap/utilite/UConversion.h>
50 #include <rtabmap/core/util3d.h>
51 #include <rtabmap/core/DBReader.h>
52 #include <rtabmap/core/OdometryEvent.h>
53 #include <cmath>
54 
55 #ifndef _WIN32
56 #include <sys/ioctl.h>
57 #include <termios.h>
58 bool spacehit()
59 {
60  bool charAvailable = true;
61  bool hit = false;
62  while(charAvailable)
63  {
64  termios term;
65  tcgetattr(0, &term);
66 
67  termios term2 = term;
68  term2.c_lflag &= ~ICANON;
69  term2.c_lflag &= ~ECHO;
70  term2.c_lflag &= ~ISIG;
71  term2.c_cc[VMIN] = 0;
72  term2.c_cc[VTIME] = 0;
73  tcsetattr(0, TCSANOW, &term2);
74 
75  int c = getchar();
76  if(c != EOF)
77  {
78  if(c == ' ')
79  {
80  hit = true;
81  }
82  }
83  else
84  {
85  charAvailable = false;
86  }
87 
88  tcsetattr(0, TCSANOW, &term);
89  }
90 
91  return hit;
92 }
93 #endif
94 
95 bool paused = false;
96 bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
97 {
98  if(paused)
99  {
100  ROS_WARN("Already paused!");
101  }
102  else
103  {
104  paused = true;
105  ROS_INFO("paused!");
106  }
107  return true;
108 }
109 
110 bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
111 {
112  if(!paused)
113  {
114  ROS_WARN("Already running!");
115  }
116  else
117  {
118  paused = false;
119  ROS_INFO("resumed!");
120  }
121  return true;
122 }
123 
124 int main(int argc, char** argv)
125 {
126  ros::init(argc, argv, "data_player");
127 
128  //ULogger::setType(ULogger::kTypeConsole);
129  //ULogger::setLevel(ULogger::kDebug);
130  //ULogger::setEventLevel(ULogger::kWarning);
131 
132  bool publishClock = false;
133  for(int i=1;i<argc;++i)
134  {
135  if(strcmp(argv[i], "--clock") == 0)
136  {
137  publishClock = true;
138  }
139  }
140 
141  ros::NodeHandle nh;
142  ros::NodeHandle pnh("~");
143 
144  std::string frameId = "base_link";
145  std::string odomFrameId = "odom";
146  std::string cameraFrameId = "camera_optical_link";
147  std::string scanFrameId = "base_laser_link";
148  double rate = 1.0f;
149  std::string databasePath = "";
150  bool publishTf = true;
151  int startId = 0;
152  bool useDbStamps = true;
153 
154  pnh.param("frame_id", frameId, frameId);
155  pnh.param("odom_frame_id", odomFrameId, odomFrameId);
156  pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
157  pnh.param("scan_frame_id", scanFrameId, scanFrameId);
158  pnh.param("rate", rate, rate); // Ratio of the database stamps
159  pnh.param("database", databasePath, databasePath);
160  pnh.param("publish_tf", publishTf, publishTf);
161  pnh.param("start_id", startId, startId);
162 
163  // A general 360 lidar with 0.5 deg increment
164  double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
165  pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
166  pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
167  pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
168  pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
169  pnh.param<double>("scan_range_max", scanRangeMax, 60);
170 
171  ROS_INFO("frame_id = %s", frameId.c_str());
172  ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
173  ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
174  ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
175  ROS_INFO("rate = %f", rate);
176  ROS_INFO("publish_tf = %s", publishTf?"true":"false");
177  ROS_INFO("start_id = %d", startId);
178  ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
179 
180  if(databasePath.empty())
181  {
182  ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
183  return -1;
184  }
185  databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
186  if(databasePath.size() && databasePath.at(0) != '/')
187  {
188  databasePath = UDirectory::currentDir(true) + databasePath;
189  }
190  ROS_INFO("database = %s", databasePath.c_str());
191 
192  rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
193  if(!reader.init())
194  {
195  ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
196  return -1;
197  }
198 
199  ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
200  ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
201 
202  image_transport::ImageTransport it(nh);
203  image_transport::Publisher imagePub;
204  image_transport::Publisher rgbPub;
205  image_transport::Publisher depthPub;
206  image_transport::Publisher leftPub;
207  image_transport::Publisher rightPub;
208  ros::Publisher rgbCamInfoPub;
209  ros::Publisher depthCamInfoPub;
210  ros::Publisher leftCamInfoPub;
211  ros::Publisher rightCamInfoPub;
212  ros::Publisher odometryPub;
213  ros::Publisher scanPub;
214  ros::Publisher scanCloudPub;
215  ros::Publisher globalPosePub;
216  ros::Publisher gpsFixPub;
217  ros::Publisher clockPub;
218  tf2_ros::TransformBroadcaster tfBroadcaster;
219 
220  if(publishClock)
221  {
222  clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
223  }
224 
225  UTimer timer;
226  rtabmap::CameraInfo cameraInfo;
227  rtabmap::SensorData data = reader.takeImage(&cameraInfo);
228  rtabmap::OdometryInfo odomInfo;
229  odomInfo.reg.covariance = cameraInfo.odomCovariance;
230  rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
231  double acquisitionTime = timer.ticks();
232  while(ros::ok() && odom.data().id())
233  {
234  ROS_INFO("Reading sensor data %d...", odom.data().id());
235 
236  ros::Time time(odom.data().stamp());
237 
238  if(publishClock)
239  {
240  rosgraph_msgs::Clock msg;
241  msg.clock = time;
242  clockPub.publish(msg);
243  }
244 
245  sensor_msgs::CameraInfo camInfoA; //rgb or left
246  sensor_msgs::CameraInfo camInfoB; //depth or right
247 
248  camInfoA.K.assign(0);
249  camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
250  camInfoA.R.assign(0);
251  camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
252  camInfoA.P.assign(0);
253  camInfoA.P[10] = 1;
254 
255  camInfoA.header.frame_id = cameraFrameId;
256  camInfoA.header.stamp = time;
257 
258  camInfoB = camInfoA;
259 
260  int type = -1;
261  if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
262  {
263  if(odom.data().cameraModels().size() > 1)
264  {
265  ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
266  }
267  else
268  {
269  //depth
270  if(odom.data().cameraModels().size())
271  {
272  camInfoA.D.resize(5,0);
273 
274  camInfoA.P[0] = odom.data().cameraModels()[0].fx();
275  camInfoA.K[0] = odom.data().cameraModels()[0].fx();
276  camInfoA.P[5] = odom.data().cameraModels()[0].fy();
277  camInfoA.K[4] = odom.data().cameraModels()[0].fy();
278  camInfoA.P[2] = odom.data().cameraModels()[0].cx();
279  camInfoA.K[2] = odom.data().cameraModels()[0].cx();
280  camInfoA.P[6] = odom.data().cameraModels()[0].cy();
281  camInfoA.K[5] = odom.data().cameraModels()[0].cy();
282 
283  camInfoB = camInfoA;
284  }
285 
286  type=0;
287 
288  if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
289  if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
290  if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
291  if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
292  }
293  }
294  else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
295  {
296  if(odom.data().stereoCameraModels().size() > 1)
297  {
298  ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
299  }
300  else
301  {
302  //stereo
303  if(odom.data().stereoCameraModels()[0].isValidForProjection())
304  {
305  camInfoA.D.resize(8,0);
306 
307  camInfoA.P[0] = odom.data().stereoCameraModels()[0].left().fx();
308  camInfoA.K[0] = odom.data().stereoCameraModels()[0].left().fx();
309  camInfoA.P[5] = odom.data().stereoCameraModels()[0].left().fy();
310  camInfoA.K[4] = odom.data().stereoCameraModels()[0].left().fy();
311  camInfoA.P[2] = odom.data().stereoCameraModels()[0].left().cx();
312  camInfoA.K[2] = odom.data().stereoCameraModels()[0].left().cx();
313  camInfoA.P[6] = odom.data().stereoCameraModels()[0].left().cy();
314  camInfoA.K[5] = odom.data().stereoCameraModels()[0].left().cy();
315 
316  camInfoB = camInfoA;
317  camInfoB.P[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
318  }
319 
320  type=1;
321 
322  if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
323  if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
324  if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
325  if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
326  }
327 
328  }
329  else
330  {
331  if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
332  }
333 
334  camInfoA.height = odom.data().imageRaw().rows;
335  camInfoA.width = odom.data().imageRaw().cols;
336  camInfoB.height = odom.data().depthOrRightRaw().rows;
337  camInfoB.width = odom.data().depthOrRightRaw().cols;
338 
339  if(!odom.data().laserScanRaw().isEmpty())
340  {
341  if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
342  {
343  scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
344  if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
345  {
346  ROS_INFO("Scan will be published.");
347  }
348  else
349  {
350  ROS_INFO("Scan will be published with those parameters:");
351  ROS_INFO(" scan_angle_min=%f", scanAngleMin);
352  ROS_INFO(" scan_angle_max=%f", scanAngleMax);
353  ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
354  ROS_INFO(" scan_range_min=%f", scanRangeMin);
355  ROS_INFO(" scan_range_max=%f", scanRangeMax);
356  }
357  }
358  else if(scanCloudPub.getTopic().empty())
359  {
360  scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
361  ROS_INFO("Scan cloud will be published.");
362  }
363  }
364 
365  if(!odom.data().globalPose().isNull() &&
366  odom.data().globalPoseCovariance().cols==6 &&
367  odom.data().globalPoseCovariance().rows==6)
368  {
369  if(globalPosePub.getTopic().empty())
370  {
371  globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
372  ROS_INFO("Global pose will be published.");
373  }
374  }
375 
376  if(odom.data().gps().stamp() > 0.0)
377  {
378  if(gpsFixPub.getTopic().empty())
379  {
380  gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
381  ROS_INFO("GPS will be published.");
382  }
383  }
384 
385  // publish transforms first
386  if(publishTf)
387  {
388  rtabmap::Transform localTransform;
389  if(odom.data().cameraModels().size() == 1)
390  {
391  localTransform = odom.data().cameraModels()[0].localTransform();
392  }
393  else if(odom.data().stereoCameraModels().size() == 1)
394  {
395  localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
396  }
397  if(!localTransform.isNull())
398  {
399  geometry_msgs::TransformStamped baseToCamera;
400  baseToCamera.child_frame_id = cameraFrameId;
401  baseToCamera.header.frame_id = frameId;
402  baseToCamera.header.stamp = time;
403  rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
404  tfBroadcaster.sendTransform(baseToCamera);
405  }
406 
407  if(!odom.pose().isNull())
408  {
409  geometry_msgs::TransformStamped odomToBase;
410  odomToBase.child_frame_id = frameId;
411  odomToBase.header.frame_id = odomFrameId;
412  odomToBase.header.stamp = time;
413  rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
414  tfBroadcaster.sendTransform(odomToBase);
415  }
416 
417  if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
418  {
419  geometry_msgs::TransformStamped baseToLaserScan;
420  baseToLaserScan.child_frame_id = scanFrameId;
421  baseToLaserScan.header.frame_id = frameId;
422  baseToLaserScan.header.stamp = time;
423  rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
424  tfBroadcaster.sendTransform(baseToLaserScan);
425  }
426  }
427  if(!odom.pose().isNull())
428  {
429  if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
430 
431  if(odometryPub.getNumSubscribers())
432  {
433  nav_msgs::Odometry odomMsg;
434  odomMsg.child_frame_id = frameId;
435  odomMsg.header.frame_id = odomFrameId;
436  odomMsg.header.stamp = time;
437  rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
438  UASSERT(odomMsg.pose.covariance.size() == 36 &&
439  odom.covariance().total() == 36 &&
440  odom.covariance().type() == CV_64FC1);
441  memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
442  odometryPub.publish(odomMsg);
443  }
444  }
445 
446  // Publish async topics first (so that they can catched by rtabmap before the image topics)
447  if(globalPosePub.getNumSubscribers() > 0 &&
448  !odom.data().globalPose().isNull() &&
449  odom.data().globalPoseCovariance().cols==6 &&
450  odom.data().globalPoseCovariance().rows==6)
451  {
452  geometry_msgs::PoseWithCovarianceStamped msg;
453  rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
454  memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
455  msg.header.frame_id = frameId;
456  msg.header.stamp = time;
457  globalPosePub.publish(msg);
458  }
459 
460  if(odom.data().gps().stamp() > 0.0)
461  {
462  sensor_msgs::NavSatFix msg;
463  msg.longitude = odom.data().gps().longitude();
464  msg.latitude = odom.data().gps().latitude();
465  msg.altitude = odom.data().gps().altitude();
466  msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
467  msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
468  msg.header.frame_id = frameId;
469  msg.header.stamp.fromSec(odom.data().gps().stamp());
470  gpsFixPub.publish(msg);
471  }
472 
473  if(type >= 0)
474  {
475  if(rgbCamInfoPub.getNumSubscribers() && type == 0)
476  {
477  rgbCamInfoPub.publish(camInfoA);
478  }
479  if(leftCamInfoPub.getNumSubscribers() && type == 1)
480  {
481  leftCamInfoPub.publish(camInfoA);
482  }
483  if(depthCamInfoPub.getNumSubscribers() && type == 0)
484  {
485  depthCamInfoPub.publish(camInfoB);
486  }
487  if(rightCamInfoPub.getNumSubscribers() && type == 1)
488  {
489  rightCamInfoPub.publish(camInfoB);
490  }
491  }
492 
493  if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
494  {
495  cv_bridge::CvImage img;
496  if(odom.data().imageRaw().channels() == 1)
497  {
498  img.encoding = sensor_msgs::image_encodings::MONO8;
499  }
500  else
501  {
502  img.encoding = sensor_msgs::image_encodings::BGR8;
503  }
504  img.image = odom.data().imageRaw();
505  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
506  imageRosMsg->header.frame_id = cameraFrameId;
507  imageRosMsg->header.stamp = time;
508 
509  if(imagePub.getNumSubscribers())
510  {
511  imagePub.publish(imageRosMsg);
512  }
513  if(rgbPub.getNumSubscribers() && type == 0)
514  {
515  rgbPub.publish(imageRosMsg);
516  }
517  if(leftPub.getNumSubscribers() && type == 1)
518  {
519  leftPub.publish(imageRosMsg);
520  leftCamInfoPub.publish(camInfoA);
521  }
522  }
523 
524  if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
525  {
526  cv_bridge::CvImage img;
527  if(odom.data().depthRaw().type() == CV_32FC1)
528  {
529  img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
530  }
531  else
532  {
533  img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
534  }
535  img.image = odom.data().depthRaw();
536  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
537  imageRosMsg->header.frame_id = cameraFrameId;
538  imageRosMsg->header.stamp = time;
539 
540  depthPub.publish(imageRosMsg);
541  depthCamInfoPub.publish(camInfoB);
542  }
543 
544  if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
545  {
546  cv_bridge::CvImage img;
547  img.encoding = sensor_msgs::image_encodings::MONO8;
548  img.image = odom.data().rightRaw();
549  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
550  imageRosMsg->header.frame_id = cameraFrameId;
551  imageRosMsg->header.stamp = time;
552 
553  rightPub.publish(imageRosMsg);
554  rightCamInfoPub.publish(camInfoB);
555  }
556 
557  if(!odom.data().laserScanRaw().isEmpty())
558  {
559  if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
560  {
561  //inspired from pointcloud_to_laserscan package
562  sensor_msgs::LaserScan msg;
563  msg.header.frame_id = scanFrameId;
564  msg.header.stamp = time;
565 
566  msg.angle_min = scanAngleMin;
567  msg.angle_max = scanAngleMax;
568  msg.angle_increment = scanAngleIncrement;
569  msg.time_increment = 0.0;
570  msg.scan_time = 0;
571  msg.range_min = scanRangeMin;
572  msg.range_max = scanRangeMax;
573  if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
574  {
575  msg.angle_min = odom.data().laserScanRaw().angleMin();
576  msg.angle_max = odom.data().laserScanRaw().angleMax();
577  msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
578  msg.range_min = odom.data().laserScanRaw().rangeMin();
579  msg.range_max = odom.data().laserScanRaw().rangeMax();
580  }
581 
582  uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
583  msg.ranges.assign(rangesSize, 0.0);
584 
585  const cv::Mat & scan = odom.data().laserScanRaw().data();
586  for (int i=0; i<scan.cols; ++i)
587  {
588  const float * ptr = scan.ptr<float>(0,i);
589  double range = hypot(ptr[0], ptr[1]);
590  if (range >= msg.range_min && range <=msg.range_max)
591  {
592  double angle = atan2(ptr[1], ptr[0]);
593  if (angle >= msg.angle_min && angle <= msg.angle_max)
594  {
595  int index = (angle - msg.angle_min) / msg.angle_increment;
596  if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
597  {
598  msg.ranges[index] = range;
599  }
600  }
601  }
602  }
603 
604  scanPub.publish(msg);
605  }
606  else if(scanCloudPub.getNumSubscribers())
607  {
608  sensor_msgs::PointCloud2 msg;
609  pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
610  msg.header.frame_id = scanFrameId;
611  msg.header.stamp = time;
612  scanCloudPub.publish(msg);
613  }
614  }
615 
616  if(odom.data().userDataRaw().type() == CV_8SC1 &&
617  odom.data().userDataRaw().cols >= 7 && // including null str ending
618  odom.data().userDataRaw().rows == 1 &&
619  memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
620  {
621  //GOAL format detected, remove it from the user data and send it as goal event
622  std::string goalStr = (const char *)odom.data().userDataRaw().data;
623  if(!goalStr.empty())
624  {
625  std::list<std::string> strs = uSplit(goalStr, ':');
626  if(strs.size() == 2)
627  {
628  int goalId = atoi(strs.rbegin()->c_str());
629 
630  if(goalId > 0)
631  {
632  ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
633  rtabmap_msgs::SetGoal setGoalSrv;
634  setGoalSrv.request.node_id = goalId;
635  setGoalSrv.request.node_label = "";
636  if(!ros::service::call("set_goal", setGoalSrv))
637  {
638  ROS_ERROR("Can't call \"set_goal\" service");
639  }
640  }
641  }
642  }
643  }
644 
645  ros::spinOnce();
646 
647  while(ros::ok())
648  {
649 #ifndef _WIN32
650  if (spacehit()) {
651  paused = !paused;
652  if(paused)
653  {
654  ROS_INFO("paused!");
655  }
656  else
657  {
658  ROS_INFO("resumed!");
659  }
660  }
661 #endif
662 
663  if(!paused)
664  {
665  break;
666  }
667 
668  uSleep(100);
669  ros::spinOnce();
670  }
671 
672  timer.restart();
673  cameraInfo = rtabmap::CameraInfo();
674  data = reader.takeImage(&cameraInfo);
675  odomInfo.reg.covariance = cameraInfo.odomCovariance;
676  odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
677  acquisitionTime = timer.ticks();
678  }
679 
680 
681  return 0;
682 }
rtabmap::SensorData
reader
reader
rtabmap::LaserScan::localTransform
Transform localTransform() const
rtabmap::OdometryEvent
image_transport::Publisher::getTopic
std::string getTopic() const
DBReader.h
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
ros::Publisher
image_encodings.h
rtabmap::SensorData::globalPoseCovariance
const cv::Mat & globalPoseCovariance() const
image_transport::ImageTransport
rtabmap::OdometryEvent::pose
const Transform & pose() const
rtabmap::LaserScan::rangeMax
float rangeMax() const
timer
resumeCallback
bool resumeCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: DbPlayerNode.cpp:110
rtabmap_conversions::transformToPoseMsg
void transformToPoseMsg(const rtabmap::Transform &transform, geometry_msgs::Pose &msg)
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
c
Scalar Scalar * c
main
int main(int argc, char **argv)
Definition: DbPlayerNode.cpp:124
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
uint32_t
::uint32_t uint32_t
ros.h
rtabmap::LaserScan::data
const cv::Mat & data() const
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::GPS::altitude
const double & altitude() const
img
img
patrol.frameId
string frameId
Definition: patrol.py:11
ros::spinOnce
ROSCPP_DECL void spinOnce()
if
if(ret >=0) *info
type
UStl.h
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
rtabmap::GPS::error
const double & error() const
transform_to_tf.TransformStamped
TransformStamped
Definition: transform_to_tf.py:31
rtabmap::SensorData::stamp
double stamp() const
timer::restart
void restart()
UThread.h
rtabmap::Transform::isNull
bool isNull() const
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
rtabmap::LaserScan::isEmpty
bool isEmpty() const
ros::ok
ROSCPP_DECL bool ok()
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
transform_broadcaster.h
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
rtabmap::SensorData::globalPose
const Transform & globalPose() const
ros::ServiceServer
range
Double_ range(const Point2_ &p, const Point2_ &q)
UDirectory::homeDir
static std::string homeDir()
uReplaceChar
std::string uReplaceChar(const std::string &str, char before, char after)
UDirectory::currentDir
static std::string currentDir(bool trailingSeparator=false)
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
data
int data[]
pauseCallback
bool pauseCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: DbPlayerNode.cpp:96
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
rtabmap::SensorData::id
int id() const
rtabmap::LaserScan::is2d
bool is2d() const
rtabmap::GPS::latitude
const double & latitude() const
rtabmap::GPS::longitude
const double & longitude() const
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
ceil
GLM_FUNC_DECL genType ceil(genType const &x)
rtabmap::RegistrationInfo::covariance
cv::Mat covariance
UASSERT
#define UASSERT(condition)
argv
argv
UDirectory.h
ROS_WARN
#define ROS_WARN(...)
rtabmap::GPS::stamp
const double & stamp() const
angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
time
#define time
rtabmap::LaserScan::angleMax
float angleMax() const
spacehit
bool spacehit()
Definition: DbPlayerNode.cpp:58
rtabmap::SensorData::userDataRaw
const cv::Mat & userDataRaw() const
rtabmap::OdometryEvent::covariance
const cv::Mat & covariance() const
atan2
AnnoyingScalar atan2(const AnnoyingScalar &y, const AnnoyingScalar &x)
image_transport.h
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
rtabmap::Transform
ros::Publisher::getTopic
std::string getTopic() const
uSplit
std::list< std::string > uSplit(const std::string &str, char separator=' ')
ros::Time
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
image_transport::Publisher
sensor_msgs::image_encodings::MONO8
const std::string MONO8
republish_tf_static.msg
msg
Definition: republish_tf_static.py:5
cv_bridge.h
ROS_ERROR
#define ROS_ERROR(...)
rtabmap_conversions::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
cv_bridge::CvImage
tf2_ros::TransformBroadcaster
uSleep
void uSleep(unsigned int ms)
rtabmap::OdometryInfo::reg
RegistrationInfo reg
OdometryEvent.h
UTimer
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
rtabmap::OdometryInfo
M_PI
#define M_PI
rtabmap::DBReader
ULogger.h
sensor_msgs::image_encodings::BGR8
const std::string BGR8
rtabmap::OdometryEvent::data
SensorData & data()
rtabmap::SensorData::gps
const GPS & gps() const
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
ROS_INFO
#define ROS_INFO(...)
rtabmap::CameraInfo
RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo
UConversion.h
MsgConversion.h
rtabmap::LaserScan::angleMin
float angleMin() const
i
int i
rtabmap::LaserScan::rangeMin
float rangeMin() const
util3d.h
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::ModelCoefficients &pcl_mc, pcl_msgs::ModelCoefficients &mc)
ros::NodeHandle
paused
bool paused
Definition: DbPlayerNode.cpp:95
pcl_conversions.h

rtabmap_util
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:40:50