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 warning after 10 seconds if any callback has not been called since the start (rtabmap, rtabmapviz and odometry nodes).

Browse Source
This commit is contained in:
matlabbe
2016-09-28 12:08:11 -04:00
parent 24afa89477
commit 5119b3ab90
12 changed files with 122 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/nodelets/rgbd_odometry.cpp
+6 -2
View File
@@ -114,6 +114,7 @@ private:
NODELET_FATAL("Only 2 cameras maximum supported yet.");
}
std::string subscribedTopicsMsg;
if(rgbdCameras == 2)
{
rgbd_image1_sub_.subscribe(nh, "rgbd_image0", 1);
@@ -135,7 +136,7 @@ private:
rgbd_image2_sub_);
exactSync2_->registerCallback(boost::bind(&RGBDOdometry::callback2, this, _1, _2));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
rgbd_image1_sub_.getTopic().c_str(),
@@ -167,13 +168,14 @@ private:
exactSync_->registerCallback(boost::bind(&RGBDOdometry::callback, this, _1, _2, _3));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
image_mono_sub_.getTopic().c_str(),
image_depth_sub_.getTopic().c_str(),
info_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -303,6 +305,7 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(1);
@@ -320,6 +323,7 @@ private:
const rtabmap_ros::RGBDImageConstPtr& image,
const rtabmap_ros::RGBDImageConstPtr& image2)
{
callbackCalled();
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2);
src/nodelets/rgbdicp_odometry.cpp
+5 -2
View File
@@ -125,6 +125,7 @@ private:
image_depth_sub_.subscribe(depth_it, depth_nh.resolveName("image"), 1, hintsDepth);
info_sub_.subscribe(rgb_nh, "camera_info", 1);
std::string subscribedTopicsMsg;
if(subscribeScanCloud)
{
cloud_sub_.subscribe(nh, "scan_cloud", 1);
@@ -139,7 +140,7 @@ private:
exactCloudSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackCloud, this, _1, _2, _3, _4));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
image_mono_sub_.getTopic().c_str(),
@@ -161,7 +162,7 @@ private:
exactScanSync_->registerCallback(boost::bind(&RGBDICPOdometry::callbackScan, this, _1, _2, _3, _4));
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s, \n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
image_mono_sub_.getTopic().c_str(),
@@ -169,6 +170,7 @@ private:
info_sub_.getTopic().c_str(),
scan_sub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -219,6 +221,7 @@ 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 ||
src/nodelets/stereo_odometry.cpp
+3 -1
View File
@@ -113,13 +113,14 @@ private:
}
NODELET_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s",
ros::this_node::getName().c_str(),
approxSync?"approx":"exact",
imageRectLeft_.getTopic().c_str(),
imageRectRight_.getTopic().c_str(),
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
virtual void updateParameters(ParametersMap & parameters)
@@ -139,6 +140,7 @@ private:
const sensor_msgs::CameraInfoConstPtr& cameraInfoLeft,
const sensor_msgs::CameraInfoConstPtr& cameraInfoRight)
{
callbackCalled();
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||