i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 02:07: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_throttle.cpp
+54 -32
View File
@@ -138,38 +138,11 @@ private:
last_update_ = ros::Time::now();
if(imageLeftPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageLeftPub_.publish(out.toImageMsg());
}
else
{
imageLeftPub_.publish(imageLeft);
}
}
if(imageRightPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageRightPub_.publish(out.toImageMsg());
}
else
{
imageRightPub_.publish(imageRight);
}
}
double leftStamp = imageLeft->header.stamp.toSec();
double rightStamp = imageRight->header.stamp.toSec();
double leftInfoStamp = camInfoLeft->header.stamp.toSec();
double rightInfoStamp = camInfoRight->header.stamp.toSec();
if(infoLeftPub_.getNumSubscribers())
{
if(decimation_ > 1)
@@ -220,6 +193,55 @@ private:
infoRightPub_.publish(camInfoRight);
}
}
if(imageLeftPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageLeft);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageLeftPub_.publish(out.toImageMsg());
}
else
{
imageLeftPub_.publish(imageLeft);
}
}
if(imageRightPub_.getNumSubscribers())
{
if(decimation_ > 1)
{
cv_bridge::CvImageConstPtr imagePtr = cv_bridge::toCvShare(imageRight);
cv_bridge::CvImage out;
out.header = imagePtr->header;
out.encoding = imagePtr->encoding;
out.image = rtabmap::util2d::decimate(imagePtr->image, decimation_);
imageRightPub_.publish(out.toImageMsg());
}
else
{
imageRightPub_.publish(imageRight);
}
}
if( leftStamp != imageLeft->header.stamp.toSec() ||
rightStamp != imageRight->header.stamp.toSec() ||
leftInfoStamp != camInfoLeft->header.stamp.toSec() ||
rightInfoStamp != camInfoRight->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, camInfoLeft->header.stamp.toSec(),
rightInfoStamp, camInfoRight->header.stamp.toSec());
}
}
image_transport::Publisher imageLeftPub_;