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 <pcl_conversions/pcl_conversions.h>
35 #include <nav_msgs/Odometry.h>
36 #include <cv_bridge/cv_bridge.h>
37 #include <image_transport/image_transport.h>
38 #include <tf2_ros/transform_broadcaster.h>
39 #include <std_srvs/Empty.h>
40 #include <rtabmap_ros/MsgConversion.h>
41 #include <rtabmap_ros/SetGoal.h>
42 #include <rtabmap/utilite/ULogger.h>
43 #include <rtabmap/utilite/UStl.h>
44 #include <rtabmap/utilite/UThread.h>
45 #include <rtabmap/utilite/UDirectory.h>
46 #include <rtabmap/utilite/UConversion.h>
47 #include <rtabmap/core/util3d.h>
48 #include <rtabmap/core/DBReader.h>
49 #include <rtabmap/core/OdometryEvent.h>
50 #include <cmath>
51 
52 bool paused = false;
53 bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
54 {
55  if(paused)
56  {
57  ROS_WARN("Already paused!");
58  }
59  else
60  {
61  paused = true;
62  ROS_INFO("paused!");
63  }
64  return true;
65 }
66 
67 bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
68 {
69  if(!paused)
70  {
71  ROS_WARN("Already running!");
72  }
73  else
74  {
75  paused = false;
76  ROS_INFO("resumed!");
77  }
78  return true;
79 }
80 
81 int main(int argc, char** argv)
82 {
83  ros::init(argc, argv, "data_player");
84 
85  //ULogger::setType(ULogger::kTypeConsole);
86  //ULogger::setLevel(ULogger::kDebug);
87  //ULogger::setEventLevel(ULogger::kWarning);
88 
89 
90  ros::NodeHandle nh;
91  ros::NodeHandle pnh("~");
92 
93  std::string frameId = "base_link";
94  std::string odomFrameId = "odom";
95  std::string cameraFrameId = "camera_optical_link";
96  std::string scanFrameId = "base_laser_link";
97  double rate = -1.0f;
98  std::string databasePath = "";
99  bool publishTf = true;
100  int startId = 0;
101 
102  pnh.param("frame_id", frameId, frameId);
103  pnh.param("odom_frame_id", odomFrameId, odomFrameId);
104  pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
105  pnh.param("scan_frame_id", scanFrameId, scanFrameId);
106  pnh.param("rate", rate, rate); // Set -1 to use database stamps
107  pnh.param("database", databasePath, databasePath);
108  pnh.param("publish_tf", publishTf, publishTf);
109  pnh.param("start_id", startId, startId);
110 
111  // based on URG-04LX
112  double scanAngleMin, scanAngleMax, scanAngleIncrement, scanTime, scanRangeMin, scanRangeMax;
113  pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
114  pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
115  pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
116  pnh.param<double>("scan_time", scanTime, 0);
117  pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
118  pnh.param<double>("scan_range_max", scanRangeMax, 60);
119 
120  ROS_INFO("frame_id = %s", frameId.c_str());
121  ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
122  ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
123  ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
124  ROS_INFO("rate = %f", rate);
125  ROS_INFO("publish_tf = %s", publishTf?"true":"false");
126  ROS_INFO("start_id = %d", startId);
127 
128  if(databasePath.empty())
129  {
130  ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
131  return -1;
132  }
133  databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
134  if(databasePath.size() && databasePath.at(0) != '/')
135  {
136  databasePath = UDirectory::currentDir(true) + databasePath;
137  }
138  ROS_INFO("database = %s", databasePath.c_str());
139 
140  rtabmap::DBReader reader(databasePath, rate, false, false, false, startId);
141  if(!reader.init())
142  {
143  ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
144  return -1;
145  }
146 
147  ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
148  ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
149 
150  image_transport::ImageTransport it(nh);
151  image_transport::Publisher imagePub;
152  image_transport::Publisher rgbPub;
153  image_transport::Publisher depthPub;
154  image_transport::Publisher leftPub;
155  image_transport::Publisher rightPub;
156  ros::Publisher rgbCamInfoPub;
157  ros::Publisher depthCamInfoPub;
158  ros::Publisher leftCamInfoPub;
159  ros::Publisher rightCamInfoPub;
160  ros::Publisher odometryPub;
161  ros::Publisher scanPub;
162  tf2_ros::TransformBroadcaster tfBroadcaster;
163 
164  UTimer timer;
165  rtabmap::CameraInfo cameraInfo;
166  rtabmap::SensorData data = reader.takeImage(&cameraInfo);
167  rtabmap::OdometryInfo odomInfo;
168  odomInfo.reg.covariance = cameraInfo.odomCovariance;
169  rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
170  double acquisitionTime = timer.ticks();
171  while(ros::ok() && odom.data().id())
172  {
173  ROS_INFO("Reading sensor data %d...", odom.data().id());
174 
175  ros::Time time = ros::Time::now();
176 
177  sensor_msgs::CameraInfo camInfoA; //rgb or left
178  sensor_msgs::CameraInfo camInfoB; //depth or right
179 
180  camInfoA.K.assign(0);
181  camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
182  camInfoA.R.assign(0);
183  camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
184  camInfoA.P.assign(0);
185  camInfoA.P[10] = 1;
186 
187  camInfoA.header.frame_id = cameraFrameId;
188  camInfoA.header.stamp = time;
189 
190  camInfoB = camInfoA;
191 
192  int type = -1;
193  if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
194  {
195  if(odom.data().cameraModels().size() > 1)
196  {
197  ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
198  }
199  else
200  {
201  //depth
202  if(odom.data().cameraModels().size())
203  {
204  camInfoA.D.resize(5,0);
205 
206  camInfoA.P[0] = odom.data().cameraModels()[0].fx();
207  camInfoA.K[0] = odom.data().cameraModels()[0].fx();
208  camInfoA.P[5] = odom.data().cameraModels()[0].fy();
209  camInfoA.K[4] = odom.data().cameraModels()[0].fy();
210  camInfoA.P[2] = odom.data().cameraModels()[0].cx();
211  camInfoA.K[2] = odom.data().cameraModels()[0].cx();
212  camInfoA.P[6] = odom.data().cameraModels()[0].cy();
213  camInfoA.K[5] = odom.data().cameraModels()[0].cy();
214 
215  camInfoB = camInfoA;
216  }
217 
218  type=0;
219 
220  if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
221  if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
222  if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
223  if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
224  }
225  }
226  else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
227  {
228  //stereo
229  if(odom.data().stereoCameraModel().isValidForProjection())
230  {
231  camInfoA.D.resize(8,0);
232 
233  camInfoA.P[0] = odom.data().stereoCameraModel().left().fx();
234  camInfoA.K[0] = odom.data().stereoCameraModel().left().fx();
235  camInfoA.P[5] = odom.data().stereoCameraModel().left().fy();
236  camInfoA.K[4] = odom.data().stereoCameraModel().left().fy();
237  camInfoA.P[2] = odom.data().stereoCameraModel().left().cx();
238  camInfoA.K[2] = odom.data().stereoCameraModel().left().cx();
239  camInfoA.P[6] = odom.data().stereoCameraModel().left().cy();
240  camInfoA.K[5] = odom.data().stereoCameraModel().left().cy();
241 
242  camInfoB = camInfoA;
243  camInfoB.P[3] = odom.data().stereoCameraModel().right().Tx(); // Right_Tx = -baseline*fx
244  }
245 
246  type=1;
247 
248  if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
249  if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
250  if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
251  if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
252 
253  }
254  else
255  {
256  if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
257  }
258 
259  camInfoA.height = odom.data().imageRaw().rows;
260  camInfoA.width = odom.data().imageRaw().cols;
261  camInfoB.height = odom.data().depthOrRightRaw().rows;
262  camInfoB.width = odom.data().depthOrRightRaw().cols;
263 
264  if(!odom.data().laserScanRaw().isEmpty())
265  {
266  if(scanPub.getTopic().empty()) scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
267  }
268 
269  // publish transforms first
270  if(publishTf)
271  {
272  rtabmap::Transform localTransform;
273  if(odom.data().cameraModels().size() == 1)
274  {
275  localTransform = odom.data().cameraModels()[0].localTransform();
276  }
277  else if(odom.data().stereoCameraModel().isValidForProjection())
278  {
279  localTransform = odom.data().stereoCameraModel().left().localTransform();
280  }
281  if(!localTransform.isNull())
282  {
283  geometry_msgs::TransformStamped baseToCamera;
284  baseToCamera.child_frame_id = cameraFrameId;
285  baseToCamera.header.frame_id = frameId;
286  baseToCamera.header.stamp = time;
287  rtabmap_ros::transformToGeometryMsg(localTransform, baseToCamera.transform);
288  tfBroadcaster.sendTransform(baseToCamera);
289  }
290 
291  if(!odom.pose().isNull())
292  {
293  geometry_msgs::TransformStamped odomToBase;
294  odomToBase.child_frame_id = frameId;
295  odomToBase.header.frame_id = odomFrameId;
296  odomToBase.header.stamp = time;
297  rtabmap_ros::transformToGeometryMsg(odom.pose(), odomToBase.transform);
298  tfBroadcaster.sendTransform(odomToBase);
299  }
300 
301  if(!scanPub.getTopic().empty())
302  {
303  geometry_msgs::TransformStamped baseToLaserScan;
304  baseToLaserScan.child_frame_id = scanFrameId;
305  baseToLaserScan.header.frame_id = frameId;
306  baseToLaserScan.header.stamp = time;
307  rtabmap_ros::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
308  tfBroadcaster.sendTransform(baseToLaserScan);
309  }
310  }
311  if(!odom.pose().isNull())
312  {
313  if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
314 
315  if(odometryPub.getNumSubscribers())
316  {
317  nav_msgs::Odometry odomMsg;
318  odomMsg.child_frame_id = frameId;
319  odomMsg.header.frame_id = odomFrameId;
320  odomMsg.header.stamp = time;
321  rtabmap_ros::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
322  UASSERT(odomMsg.pose.covariance.size() == 36 &&
323  odom.covariance().total() == 36 &&
324  odom.covariance().type() == CV_64FC1);
325  memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
326  odometryPub.publish(odomMsg);
327  }
328  }
329 
330  if(type >= 0)
331  {
332  if(rgbCamInfoPub.getNumSubscribers() && type == 0)
333  {
334  rgbCamInfoPub.publish(camInfoA);
335  }
336  if(leftCamInfoPub.getNumSubscribers() && type == 1)
337  {
338  leftCamInfoPub.publish(camInfoA);
339  }
340  if(depthCamInfoPub.getNumSubscribers() && type == 0)
341  {
342  depthCamInfoPub.publish(camInfoB);
343  }
344  if(rightCamInfoPub.getNumSubscribers() && type == 1)
345  {
346  rightCamInfoPub.publish(camInfoB);
347  }
348  }
349 
350  if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
351  {
352  cv_bridge::CvImage img;
353  if(odom.data().imageRaw().channels() == 1)
354  {
355  img.encoding = sensor_msgs::image_encodings::MONO8;
356  }
357  else
358  {
359  img.encoding = sensor_msgs::image_encodings::BGR8;
360  }
361  img.image = odom.data().imageRaw();
362  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
363  imageRosMsg->header.frame_id = cameraFrameId;
364  imageRosMsg->header.stamp = time;
365 
366  if(imagePub.getNumSubscribers())
367  {
368  imagePub.publish(imageRosMsg);
369  }
370  if(rgbPub.getNumSubscribers() && type == 0)
371  {
372  rgbPub.publish(imageRosMsg);
373  }
374  if(leftPub.getNumSubscribers() && type == 1)
375  {
376  leftPub.publish(imageRosMsg);
377  leftCamInfoPub.publish(camInfoA);
378  }
379  }
380 
381  if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
382  {
383  cv_bridge::CvImage img;
384  if(odom.data().depthRaw().type() == CV_32FC1)
385  {
386  img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
387  }
388  else
389  {
390  img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
391  }
392  img.image = odom.data().depthRaw();
393  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
394  imageRosMsg->header.frame_id = cameraFrameId;
395  imageRosMsg->header.stamp = time;
396 
397  depthPub.publish(imageRosMsg);
398  depthCamInfoPub.publish(camInfoB);
399  }
400 
401  if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
402  {
403  cv_bridge::CvImage img;
404  img.encoding = sensor_msgs::image_encodings::MONO8;
405  img.image = odom.data().rightRaw();
406  sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
407  imageRosMsg->header.frame_id = cameraFrameId;
408  imageRosMsg->header.stamp = time;
409 
410  rightPub.publish(imageRosMsg);
411  rightCamInfoPub.publish(camInfoB);
412  }
413 
414  if(scanPub.getNumSubscribers() && !odom.data().laserScanRaw().isEmpty())
415  {
416  //inspired from pointcloud_to_laserscan package
417  sensor_msgs::LaserScan msg;
418  msg.header.frame_id = scanFrameId;
419  msg.header.stamp = time;
420 
421  msg.angle_min = scanAngleMin;
422  msg.angle_max = scanAngleMax;
423  msg.angle_increment = scanAngleIncrement;
424  msg.time_increment = 0.0;
425  msg.scan_time = scanTime;
426  msg.range_min = scanRangeMin;
427  msg.range_max = scanRangeMax;
428 
429  uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
430  msg.ranges.assign(rangesSize, 0.0);
431 
432  const cv::Mat & scan = odom.data().laserScanRaw().data();
433  for (int i=0; i<scan.cols; ++i)
434  {
435  const float * ptr = scan.ptr<float>(0,i);
436  double range = hypot(ptr[0], ptr[1]);
437  if (range >= scanRangeMin && range <=scanRangeMax)
438  {
439  double angle = atan2(ptr[1], ptr[0]);
440  if (angle >= msg.angle_min && angle <= msg.angle_max)
441  {
442  int index = (angle - msg.angle_min) / msg.angle_increment;
443  if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
444  {
445  msg.ranges[index] = range;
446  }
447  }
448  }
449  }
450 
451  scanPub.publish(msg);
452  }
453 
454  if(odom.data().userDataRaw().type() == CV_8SC1 &&
455  odom.data().userDataRaw().cols >= 7 && // including null str ending
456  odom.data().userDataRaw().rows == 1 &&
457  memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
458  {
459  //GOAL format detected, remove it from the user data and send it as goal event
460  std::string goalStr = (const char *)odom.data().userDataRaw().data;
461  if(!goalStr.empty())
462  {
463  std::list<std::string> strs = uSplit(goalStr, ':');
464  if(strs.size() == 2)
465  {
466  int goalId = atoi(strs.rbegin()->c_str());
467 
468  if(goalId > 0)
469  {
470  ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
471  rtabmap_ros::SetGoal setGoalSrv;
472  setGoalSrv.request.node_id = goalId;
473  setGoalSrv.request.node_label = "";
474  if(!ros::service::call("set_goal", setGoalSrv))
475  {
476  ROS_ERROR("Can't call \"set_goal\" service");
477  }
478  }
479  }
480  }
481  }
482 
483  ros::spinOnce();
484 
485  while(ros::ok() && paused)
486  {
487  uSleep(100);
488  ros::spinOnce();
489  }
490 
491  timer.restart();
492  cameraInfo = rtabmap::CameraInfo();
493  data = reader.takeImage(&cameraInfo);
494  odomInfo.reg.covariance = cameraInfo.odomCovariance;
495  odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
496  acquisitionTime = timer.ticks();
497  }
498 
499 
500  return 0;
501 }
UDirectory::homeDir
static std::string homeDir()
rtabmap::OdometryEvent::data
SensorData & data()
UTimer::restart
double restart()
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:81
ros::service::call
bool call(const std::string &service_name, MReq &req, MRes &res)
image_transport.h
rtabmap_ros::transformToGeometryMsg
void transformToGeometryMsg(const rtabmap::Transform &transform, geometry_msgs::Transform &msg)
Definition: MsgConversion.cpp:69
rtabmap::LaserScan::isEmpty
bool isEmpty() const
resumeCallback
bool resumeCallback(std_srvs::Empty::Request &, std_srvs::Empty::Response &)
Definition: DbPlayerNode.cpp:67
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
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(...)
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:53
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:52
rtabmap::Transform::isNull
bool isNull() const
cv_bridge.h
imagePub
image_transport::Publisher imagePub
rtabmap::CameraModel::fy
double fy() const
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) 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::localTransform
Transform localTransform() 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)
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
uint32_t
detail::uint32 uint32_t
uSleep
void uSleep(unsigned int ms)
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
transform_to_tf.TransformStamped
TransformStamped
Definition: transform_to_tf.py:31
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
ros::Time::now
static Time now()
UTimer::ticks
double ticks()
image_transport::ImageTransport
image_transport::Publisher
rtabmap::OdometryInfo::reg
RegistrationInfo reg
ros::Publisher::getTopic
std::string getTopic() const
image_encodings.h
ros::spinOnce
ROSCPP_DECL void spinOnce()
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
rtabmap::CameraModel::localTransform
const Transform & localTransform() const

rtabmap_ros
Author(s): Mathieu Labbe
autogenerated on Fri Jun 7 2019 21:55:03