i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17: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/rgbd_sync.cpp
+17
View File
@@ -161,6 +161,10 @@ private:
callbackCalled_ = true;
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
double rgbStamp = image->header.stamp.toSec();
double depthStamp = depth->header.stamp.toSec();
double infoStamp = cameraInfo->header.stamp.toSec();
rtabmap_ros::RGBDImage msg;
msg.header.frame_id = cameraInfo->header.frame_id;
msg.header.stamp = image->header.stamp>depth->header.stamp?image->header.stamp:depth->header.stamp;
@@ -204,6 +208,19 @@ private:
}
rgbdImagePub_.publish(msg);
}
if( rgbStamp != image->header.stamp.toSec() ||
depthStamp != depth->header.stamp.toSec() ||
infoStamp != cameraInfo->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: "
"rgb=%f->%f depth=%f->%f info=%f->%f",
rgbStamp, image->header.stamp.toSec(),
depthStamp, depth->header.stamp.toSec(),
infoStamp, cameraInfo->header.stamp.toSec());
}
}
}