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 sync diagnostic (#1026)

Browse Source 
* Added sync diagnotic

* Removed composite task to avoid empty msg, added odom status task
This commit is contained in:
matlabbe
2023-08-27 12:30:53 -07:00
committed by GitHub
parent cd52f7664c
commit 4a3863cdc7
28 changed files with 264 additions and 285 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_odom/src/nodelets/icp_odometry.cpp
+7
View File
@@ -157,6 +157,11 @@ private:
cloud_sub_ = nh.subscribe("scan_cloud", queueSize, &ICPOdometry::callbackCloud, this);
filtered_scan_pub_ = nh.advertise<sensor_msgs::PointCloud2>("odom_filtered_input_scan", 1);
initDiagnosticMsg(uFormat("\n%s subscribed to %s and %s (make sure only one of this topic is published, otherwise remap one to a dummy topic name).",
getName().c_str(),
scan_sub_.getTopic().c_str(),
cloud_sub_.getTopic().c_str()), true);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -329,6 +334,7 @@ private:
scan_sub_.shutdown();
return;
}
scanReceived_ = true;
if(this->isPaused())
{
@@ -570,6 +576,7 @@ private:
cloud_sub_.shutdown();
return;
}
cloudReceived_ = true;
if(this->isPaused())
{
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+3 -8
View File
@@ -161,6 +161,7 @@ private:
NODELET_INFO("RGBDOdometry: rgbd_cameras = %d", rgbdCameras);
NODELET_INFO("RGBDOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -356,6 +357,7 @@ private:
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3));
}
subscribedTopic = rgb_nh.resolveName("image");
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
@@ -364,7 +366,7 @@ private:
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -546,7 +548,6 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -575,7 +576,6 @@ private:
void callbackRGBD(
const rtabmap_msgs::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -591,7 +591,6 @@ private:
void callbackRGBDX(
const rtabmap_msgs::RGBDImagesConstPtr& images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -616,7 +615,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
@@ -636,7 +634,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(3);
@@ -659,7 +656,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(4);
@@ -685,7 +681,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image4,
const rtabmap_msgs::RGBDImageConstPtr& image5)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(5);
rtabmap_odom/src/nodelets/rgbdicp_odometry.cpp
+1 -2
View File
@@ -202,7 +202,7 @@ private:
info_sub_.getTopic().c_str(),
scan_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -243,7 +243,6 @@ private:
const sensor_msgs::LaserScanConstPtr& scanMsg,
const sensor_msgs::PointCloud2ConstPtr& cloudMsg)
{
callbackCalled();
if(!this->isPaused())
{
if(!(image->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+3 -8
View File
@@ -111,6 +111,7 @@ private:
NODELET_INFO("StereoOdometry: subscribe_rgbd = %s", subscribeRGBD?"true":"false");
NODELET_INFO("StereoOdometry: keep_color = %s", keepColor_?"true":"false");
std::string subscribedTopic;
std::string subscribedTopicsMsg;
if(subscribeRGBD)
{
@@ -271,7 +272,7 @@ private:
exactSync_->registerCallback(boost::bind(&StereoOdometry::callback, this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
}
subscribedTopic = left_nh.resolveName("image_rect");
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s",
getName().c_str(),
approxSync?"approx":"exact",
@@ -282,7 +283,7 @@ private:
cameraInfoRight_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
initDiagnosticMsg(subscribedTopicsMsg, approxSync, subscribedTopic);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -578,7 +579,6 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -609,7 +609,6 @@ private:
void callbackRGBD(
const rtabmap_msgs::RGBDImageConstPtr& image)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(1);
@@ -627,7 +626,6 @@ private:
void callbackRGBDX(
const rtabmap_msgs::RGBDImagesConstPtr& images)
{
callbackCalled();
if(!this->isPaused())
{
if(images->rgbd_images.empty())
@@ -654,7 +652,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image,
const rtabmap_msgs::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(2);
@@ -677,7 +674,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image2,
const rtabmap_msgs::RGBDImageConstPtr& image3)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(3);
@@ -704,7 +700,6 @@ private:
const rtabmap_msgs::RGBDImageConstPtr& image3,
const rtabmap_msgs::RGBDImageConstPtr& image4)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(4);