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/data_throttle.cpp
+50 -32
View File
@@ -140,38 +140,10 @@ private:
last_update_ = ros::Time::now();
if(imagePub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg());
}
else
{
imagePub_.publish(image);
}
}
if(imageDepthPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg());
}
else
{
imageDepthPub_.publish(imageDepth);
}
}
double rgbStamp = image->header.stamp.toSec();
double depthStamp = imageDepth->header.stamp.toSec();
double infoStamp = camInfo->header.stamp.toSec();
if(infoPub_.getNumSubscribers())
{
if(decimation_ > 1)
@@ -196,6 +168,52 @@ private:
infoPub_.publish(camInfo);
}
}
if(imagePub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(image);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imagePub_.publish(out.toImageMsg());
}
else
{
imagePub_.publish(image);
}
}
if(imageDepthPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageDepth);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageDepthPub_.publish(out.toImageMsg());
}
else
{
imageDepthPub_.publish(imageDepth);
}
}
if( rgbStamp != image->header.stamp.toSec() ||
depthStamp != imageDepth->header.stamp.toSec() ||
infoStamp != camInfo->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, imageDepth->header.stamp.toSec(),
infoStamp, camInfo->header.stamp.toSec());
}
}
image_transport::Publisher imagePub_;