i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 17:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added input stamps double-verification in some nodelets (data_throttle, stereo_throttle, rgbd_sync, stereo_sync, pointcloud_to_depthimage)

Browse Source 
rtabmapviz: added "max_odom_update_rate" parameters
Odom: moved IMU callback from stereo_odometry nodelet to OdometryROS, added "wait_imu_to_init" parameter to initialize odom with IMU first
Fixed fake camera local transform on scan-only callbacks (rtabmap, rtabmapviz)
This commit is contained in:
matlabbe
2019-01-15 15:50:10 -05:00
parent 5b9ed73756
commit 19433f809f
11 changed files with 299 additions and 125 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/stereo_sync.cpp
+20
View File
@@ -161,6 +161,11 @@ private:
callbackCalled_ = true;
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
double leftStamp = imageLeft->header.stamp.toSec();
double rightStamp = imageRight->header.stamp.toSec();
double leftInfoStamp = cameraInfoLeft->header.stamp.toSec();
double rightInfoStamp = cameraInfoRight->header.stamp.toSec();
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfoLeft->header.frame_id;
msg.header.stamp = imageLeft->header.stamp>imageRight->header.stamp?imageLeft->header.stamp:imageRight->header.stamp;
@@ -186,6 +191,21 @@ private:
msg.depth = *imageRight;
rgbdImagePub_.publish(msg);
}
if( leftStamp != imageLeft->header.stamp.toSec() ||
rightStamp != imageRight->header.stamp.toSec() ||
leftInfoStamp != cameraInfoLeft->header.stamp.toSec() ||
rightInfoStamp != cameraInfoRight->header.stamp.toSec())
{
NODELET_ERROR("Input stamps changed between the beginning and the end of the callback! Make "
"sure the node publishing the topics doesn't override the same data after publishing them. A "
"solution is to use this node within another nodelet manager. Stamps: "
"left%f->%f right=%f->%f info_left=%f->%f info_right=%f->%f",
leftStamp, imageLeft->header.stamp.toSec(),
rightStamp, imageRight->header.stamp.toSec(),
leftInfoStamp, cameraInfoLeft->header.stamp.toSec(),
rightInfoStamp, cameraInfoRight->header.stamp.toSec());
}
}
}