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_ros/MsgConversion.h>
44 #include <rtabmap_ros/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  //stereo
297  if(odom.data().stereoCameraModel().isValidForProjection())
298  {
299  camInfoA.D.resize(8,0);
300 
301  camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
302  camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
303  camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
304  camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
305  camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
306  camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
307  camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
308  camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
309 
310  camInfoB = camInfoA;
311  camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
312  }
313 
314  type=1;
315 
316  if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
317  if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
318  if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
319  if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
320 
321  }
322  else
323  {
324  if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
325  }
326 
327  camInfoA.height = odom.data().imageRaw().rows;
328  camInfoA.width = odom.data().imageRaw().cols;
329  camInfoB.height = odom.data().depthOrRightRaw().rows;
330  camInfoB.width = odom.data().depthOrRightRaw().cols;
331 
332  if(!odom.data().laserScanRaw().isEmpty())
333  {
334  if(scanPub.getTopic().empty() && odom.data().laserScanRaw().is2d())
335  {
336  scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
337  if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
338  {
339  ROS_INFO("Scan will be published.");
340  }
341  else
342  {
343  ROS_INFO("Scan will be published with those parameters:");
344  ROS_INFO(" scan_angle_min=%f", scanAngleMin);
345  ROS_INFO(" scan_angle_max=%f", scanAngleMax);
346  ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
347  ROS_INFO(" scan_range_min=%f", scanRangeMin);
348  ROS_INFO(" scan_range_max=%f", scanRangeMax);
349  }
350  }
351  else if(scanCloudPub.getTopic().empty())
352  {
353  scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
354  ROS_INFO("Scan cloud will be published.");
355  }
356  }
357 
358  if(!odom.data().globalPose().isNull() &&
359  odom.data().globalPoseCovariance().cols==6 &&
360  odom.data().globalPoseCovariance().rows==6)
361  {
362  if(globalPosePub.getTopic().empty())
363  {
364  globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
365  ROS_INFO("Global pose will be published.");
366  }
367  }
368 
369  if(odom.data().gps().stamp() > 0.0)
370  {
371  if(gpsFixPub.getTopic().empty())
372  {
373  gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
374  ROS_INFO("GPS will be published.");
375  }
376  }
377 
378  // publish transforms first
379  if(publishTf)
380  {
381  rtabmap::Transform localTransform;
382  if(odom.data().cameraModels().size() == 1)
383  {
384  localTransform = odom.data().cameraModels()[0].localTransform();
385  }
386  else if(odom.data().stereoCameraModel().isValidForProjection())
387  {
388  localTransform = odom.data().stereoCameraModel().left().localTransform();
389  }
390  if(!localTransform.isNull())
391  {
392  geometry_msgs::TransformStamped baseToCamera;
393  baseToCamera.child_frame_id = cameraFrameId;
394  baseToCamera.header.frame_id = frameId;
395  baseToCamera.header.stamp = time;
396  rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
397  tfBroadcaster.sendTransform(baseToCamera);
398  }
399 
400  if(!odom.pose().isNull())
401  {
402  geometry_msgs::TransformStamped odomToBase;
403  odomToBase.child_frame_id = frameId;
404  odomToBase.header.frame_id = odomFrameId;
405  odomToBase.header.stamp = time;
406  rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
407  tfBroadcaster.sendTransform(odomToBase);
408  }
409 
410  if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
411  {
412  geometry_msgs::TransformStamped baseToLaserScan;
413  baseToLaserScan.child_frame_id = scanFrameId;
414  baseToLaserScan.header.frame_id = frameId;
415  baseToLaserScan.header.stamp = time;
416  rtabmap_ros::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
417  tfBroadcaster.sendTransform(baseToLaserScan);
418  }
419  }
420  if(!odom.pose().isNull())
421  {
422  if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
423 
424  if(odometryPub.getNumSubscribers())
425  {
426  nav_msgs::Odometry odomMsg;
427  odomMsg.child_frame_id = frameId;
428  odomMsg.header.frame_id = odomFrameId;
429  odomMsg.header.stamp = time;
430  rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
431  UASSERT(odomMsg.pose.covariance.size() == 36 &&
432  odom.covariance().total() == 36 &&
433  odom.covariance().type() == CV_64FC1);
434  memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
435  odometryPub.publish(odomMsg);
436  }
437  }
438 
439  // Publish async topics first (so that they can catched by rtabmap before the image topics)
440  if(globalPosePub.getNumSubscribers() > 0 &&
441  !odom.data().globalPose().isNull() &&
442  odom.data().globalPoseCovariance().cols==6 &&
443  odom.data().globalPoseCovariance().rows==6)
444  {
445  geometry_msgs::PoseWithCovarianceStamped msg;
446  rtabmap_ros::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
447  memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
448  msg.header.frame_id = frameId;
449  msg.header.stamp = time;
450  globalPosePub.publish(msg);
451  }
452 
453  if(odom.data().gps().stamp() > 0.0)
454  {
455  sensor_msgs::NavSatFix msg;
456  msg.longitude = odom.data().gps().longitude();
457  msg.latitude = odom.data().gps().latitude();
458  msg.altitude = odom.data().gps().altitude();
459  msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
460  msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
461  msg.header.frame_id = frameId;
462  msg.header.stamp.fromSec(odom.data().gps().stamp());
463  gpsFixPub.publish(msg);
464  }
465 
466  if(type >= 0)
467  {
468  if(rgbCamInfoPub.getNumSubscribers() && type == 0)
469  {
470  rgbCamInfoPub.publish(camInfoA);
471  }
472  if(leftCamInfoPub.getNumSubscribers() && type == 1)
473  {
474  leftCamInfoPub.publish(camInfoA);
475  }
476  if(depthCamInfoPub.getNumSubscribers() && type == 0)
477  {
478  depthCamInfoPub.publish(camInfoB);
479  }
480  if(rightCamInfoPub.getNumSubscribers() && type == 1)
481  {
482  rightCamInfoPub.publish(camInfoB);
483  }
484  }
485 
486  if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
487  {
488  cv_bridge::CvImage img;
489  if(odom.data().imageRaw().channels() == 1)
490  {
491  img.encoding = sensor_msgs::image_encodings::MONO8;
492  }
493  else
494  {
495  img.encoding = sensor_msgs::image_encodings::BGR8;
496  }
497  img.image = odom.data().imageRaw();
498  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
499  imageRosMsg->header.frame_id = cameraFrameId;
500  imageRosMsg->header.stamp = time;
501 
502  if(imagePub.getNumSubscribers())
503  {
504  imagePub.publish(imageRosMsg);
505  }
506  if(rgbPub.getNumSubscribers() && type == 0)
507  {
508  rgbPub.publish(imageRosMsg);
509  }
510  if(leftPub.getNumSubscribers() && type == 1)
511  {
512  leftPub.publish(imageRosMsg);
513  leftCamInfoPub.publish(camInfoA);
514  }
515  }
516 
517  if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
518  {
519  cv_bridge::CvImage img;
520  if(odom.data().depthRaw().type() == CV_32FC1)
521  {
522  img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
523  }
524  else
525  {
526  img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
527  }
528  img.image = odom.data().depthRaw();
529  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
530  imageRosMsg->header.frame_id = cameraFrameId;
531  imageRosMsg->header.stamp = time;
532 
533  depthPub.publish(imageRosMsg);
534  depthCamInfoPub.publish(camInfoB);
535  }
536 
537  if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
538  {
539  cv_bridge::CvImage img;
540  img.encoding = sensor_msgs::image_encodings::MONO8;
541  img.image = odom.data().rightRaw();
542  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
543  imageRosMsg->header.frame_id = cameraFrameId;
544  imageRosMsg->header.stamp = time;
545 
546  rightPub.publish(imageRosMsg);
547  rightCamInfoPub.publish(camInfoB);
548  }
549 
550  if(!odom.data().laserScanRaw().isEmpty())
551  {
552  if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
553  {
554  //inspired from pointcloud_to_laserscan package
555  sensor_msgs::LaserScan msg;
556  msg.header.frame_id = scanFrameId;
557  msg.header.stamp = time;
558 
559  msg.angle_min = scanAngleMin;
560  msg.angle_max = scanAngleMax;
561  msg.angle_increment = scanAngleIncrement;
562  msg.time_increment = 0.0;
563  msg.scan_time = 0;
564  msg.range_min = scanRangeMin;
565  msg.range_max = scanRangeMax;
566  if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
567  {
568  msg.angle_min = odom.data().laserScanRaw().angleMin();
569  msg.angle_max = odom.data().laserScanRaw().angleMax();
570  msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
571  msg.range_min = odom.data().laserScanRaw().rangeMin();
572  msg.range_max = odom.data().laserScanRaw().rangeMax();
573  }
574 
575  uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
576  msg.ranges.assign(rangesSize, 0.0);
577 
578  const cv::Mat & scan = odom.data().laserScanRaw().data();
579  for (int i=0; i<scan.cols; ++i)
580  {
581  const float * ptr = scan.ptr<float>(0,i);
582  double range = hypot(ptr[0], ptr[1]);
583  if (range >= msg.range_min && range <=msg.range_max)
584  {
585  double angle = atan2(ptr[1], ptr[0]);
586  if (angle >= msg.angle_min && angle <= msg.angle_max)
587  {
588  int index = (angle - msg.angle_min) / msg.angle_increment;
589  if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
590  {
591  msg.ranges[index] = range;
592  }
593  }
594  }
595  }
596 
597  scanPub.publish(msg);
598  }
599  else if(scanCloudPub.getNumSubscribers())
600  {
601  sensor_msgs::PointCloud2 msg;
602  pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
603  msg.header.frame_id = scanFrameId;
604  msg.header.stamp = time;
605  scanCloudPub.publish(msg);
606  }
607  }
608 
609  if(odom.data().userDataRaw().type() == CV_8SC1 &&
610  odom.data().userDataRaw().cols >= 7 && // including null str ending
611  odom.data().userDataRaw().rows == 1 &&
612  memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
613  {
614  //GOAL format detected, remove it from the user data and send it as goal event
615  std::string goalStr = (const char *)odom.data().userDataRaw().data;
616  if(!goalStr.empty())
617  {
618  std::list<std::string> strs = uSplit(goalStr, ':');
619  if(strs.size() == 2)
620  {
621  int goalId = atoi(strs.rbegin()->c_str());
622 
623  if(goalId > 0)
624  {
625  ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
626  rtabmap_ros::SetGoal setGoalSrv;
627  setGoalSrv.request.node_id = goalId;
628  setGoalSrv.request.node_label = "";
629  if(!ros::service::call("set_goal", setGoalSrv))
630  {
631  ROS_ERROR("Can't call \"set_goal\" service");
632  }
633  }
634  }
635  }
636  }
637 
638  ros::spinOnce();
639 
640  while(ros::ok())
641  {
642 #ifndef _WIN32
643  if (spacehit()) {
644  paused = !paused;
645  if(paused)
646  {
647  ROS_INFO("paused!");
648  }
649  else
650  {
651  ROS_INFO("resumed!");
652  }
653  }
654 #endif
655 
656  if(!paused)
657  {
658  break;
659  }
660 
661  uSleep(100);
662  ros::spinOnce();
663  }
664 
665  timer.restart();
666  cameraInfo = rtabmap::CameraInfo();
667  data = reader.takeImage(&cameraInfo);
668  odomInfo.reg.covariance = cameraInfo.odomCovariance;
669  odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
670  acquisitionTime = timer.ticks();
671  }
672 
673 
674  return 0;
675 }
UDirectory::homeDir
static std::string homeDir()
rtabmap::OdometryEvent::data
SensorData & data()
rtabmap::LaserScan::rangeMin
float rangeMin() const
sensor_msgs::image_encodings::BGR8
const std::string BGR8
ros::NodeHandle
test_prior_tf_to_pose.rate
rate
Definition: test_prior_tf_to_pose.py:19
rtabmap::LaserScan::data
const cv::Mat & data() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
rtabmap::SensorData::laserScanCompressed
const LaserScan & laserScanCompressed() const
uReplaceChar
std::string uReplaceChar(const std::string &str, char before, char after)
main
int main(int argc, char **argv)
Definition: DbPlayerNode.cpp:124
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
image_transport.h
rtabmap::LaserScan::rangeMax
float rangeMax() const
rtabmap_ros::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
Definition: MsgConversion.cpp:69
rtabmap::util3d::laserScanToPointCloud2
pcl::PCLPointCloud2::Ptr RTABMAP_EXP laserScanToPointCloud2(const LaserScan &laserScan, const Transform &transform=Transform())
rtabmap::LaserScan::isEmpty
bool isEmpty() const
resumeCallback
bool resumeCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: DbPlayerNode.cpp:110
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
cv_bridge::CvImage::encoding
std::string encoding
image_transport::ImageTransport::advertise
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
hypot
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
transform_broadcaster.h
rtabmap::LaserScan::is2d
bool is2d() const
angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
M_PI
#define M_PI
ros::NodeHandle::advertiseService
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
ROS_WARN
#define ROS_WARN(...)
rtabmap::SensorData::globalPoseCovariance
const cv::Mat & globalPoseCovariance() const
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::SensorData::rightRaw
cv::Mat rightRaw() const
uSplit
std::list< std::string > uSplit(const std::string &str, char separator= ' ')
UASSERT
#define UASSERT(condition)
pauseCallback
bool pauseCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: DbPlayerNode.cpp:96
rtabmap::OdometryEvent::pose
const Transform & pose() const
UDirectory::currentDir
static std::string currentDir(bool trailingSeparator=false)
rtabmap::StereoCameraModel::isValidForProjection
bool isValidForProjection() const
rtabmap::StereoCameraModel::left
const CameraModel & left() const
rtabmap::CameraModel::cx
double cx() const
paused
bool paused
Definition: DbPlayerNode.cpp:95
rtabmap::Transform::isNull
bool isNull() const
cv_bridge.h
rtabmap::SensorData::globalPose
const Transform & globalPose() const
imagePub
image_transport::Publisher imagePub
rtabmap::CameraModel::fy
double fy() const
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
rtabmap::LaserScan::angleMin
float angleMin() const
rtabmap::GPS::error
const double & error() const
ROS_INFO
#define ROS_INFO(...)
ros::NodeHandle::param
bool param(const std::string &param_name, T &param_val, const T &default_val) const
pcl_conversions.h
cv_bridge::CvImage
sensor_msgs::image_encodings::TYPE_32FC1
const std::string TYPE_32FC1
sensor_msgs::image_encodings::MONO8
const std::string MONO8
ros::ok
ROSCPP_DECL bool ok()
sensor_msgs::image_encodings::TYPE_16UC1
const std::string TYPE_16UC1
rtabmap::LaserScan::angleMax
float angleMax() const
rtabmap::LaserScan::localTransform
Transform localTransform() const
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
rtabmap::SensorData::gps
const GPS & gps() const
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
rtabmap::SensorData::userDataRaw
const cv::Mat & userDataRaw() const
atan2
GLM_FUNC_QUALIFIER T atan2(T x, T y)
tf2_ros::TransformBroadcaster::sendTransform
void sendTransform(const geometry_msgs::TransformStamped &transform)
spacehit
bool spacehit()
Definition: DbPlayerNode.cpp:58
rtabmap::SensorData::stamp
double stamp() const
rtabmap::CameraModel::cy
double cy() const
rtabmap::OdometryEvent::covariance
const cv::Mat & covariance() const
cv_bridge::CvImage::image
cv::Mat image
rtabmap_ros::transformToPoseMsg
void transformToPoseMsg(const rtabmap::Transform &transform, geometry_msgs::Pose &msg)
Definition: MsgConversion.cpp:104
patrol.frameId
string frameId
Definition: patrol.py:11
uint32_t
detail::uint32 uint32_t
uSleep
void uSleep(unsigned int ms)
rtabmap::GPS::longitude
const double & longitude() const
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
transform_to_tf.TransformStamped
TransformStamped
Definition: transform_to_tf.py:31
rtabmap::GPS::altitude
const double & altitude() const
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
rtabmap::CameraModel::fx
double fx() const
rtabmap::StereoCameraModel::right
const CameraModel & right() const
rtabmap::CameraModel::Tx
double Tx() const
rtabmap::SensorData::stereoCameraModel
const StereoCameraModel & stereoCameraModel() const
tf2_ros::TransformBroadcaster
image_transport::ImageTransport
image_transport::Publisher
rtabmap::OdometryInfo::reg
RegistrationInfo reg
ros::Publisher::getTopic
std::string getTopic() const
image_encodings.h
pcl_conversions::moveFromPCL
void moveFromPCL(pcl::PCLImage &pcl_image, sensor_msgs::Image &image)
ros::spinOnce
ROSCPP_DECL void spinOnce()
rtabmap::GPS::latitude
const double & latitude() const
ROS_ERROR
#define ROS_ERROR(...)
image_transport::Publisher::getTopic
std::string getTopic() const
ceil
GLM_FUNC_DECL genType ceil(genType const &x)
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
ethzasl_icp_mapper_sync_gt.Odometry
Odometry
Definition: ethzasl_icp_mapper_sync_gt.py:52
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
rtabmap::GPS::stamp
const double & stamp() const

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:42:19