i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 16:57:46 +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/CommonDataSubscriber.cpp
+41 -9
View File
@@ -32,6 +32,9 @@ namespace rtabmap_ros {
CommonDataSubscriber::CommonDataSubscriber() :
queueSize_(10),
approxSync_(true),
warningThread_(0),
callbackCalled_(false),
subscribedToDepth_(true),
subscribedToStereo_(false),
subscribedToRGBD_(false),
@@ -126,7 +129,6 @@ CommonDataSubscriber::CommonDataSubscriber() :
bool subscribeOdomInfo = false;
bool subscribeUserData = false;
int rgbdCameras = 1;
bool approxSync = true;
// ROS related parameters (private)
pnh.param("subscribe_depth", subscribedToDepth_, subscribedToDepth_);
@@ -170,7 +172,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
}
if(subscribedToStereo_)
{
approxSync = false; // default for stereo: exact sync
approxSync_ = false; // default for stereo: exact sync
}
std::string odomFrameId;
@@ -186,11 +188,11 @@ CommonDataSubscriber::CommonDataSubscriber() :
ROS_WARN("Parameter \"stereo_approx_sync\" has been renamed "
"to \"approx_sync\"! Your value is still copied to "
"corresponding parameter.");
pnh.param("stereo_approx_sync", approxSync, approxSync);
pnh.param("stereo_approx_sync", approxSync_, approxSync_);
}
else
{
pnh.param("approx_sync", approxSync, approxSync);
pnh.param("approx_sync", approxSync_, approxSync_);
}
if(rgbdCameras <= 0 && subscribedToRGBD_)
@@ -200,7 +202,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
ROS_INFO("%s: queue_size = %d", ros::this_node::getName().c_str(), queueSize_);
ROS_INFO("%s: rgbd_cameras = %d", ros::this_node::getName().c_str(), rgbdCameras);
ROS_INFO("%s: approx_sync = %s", ros::this_node::getName().c_str(), approxSync?"true":"false");
ROS_INFO("%s: approx_sync = %s", ros::this_node::getName().c_str(), approxSync_?"true":"false");
bool subscribeOdom = odomFrameId.empty();
if(subscribedToDepth_)
@@ -212,7 +214,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
approxSync_);
}
else if(subscribedToStereo_)
{
@@ -220,7 +222,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
subscribeOdom,
subscribeOdomInfo,
queueSize_,
approxSync);
approxSync_);
}
else if(subscribedToRGBD_)
{
@@ -233,7 +235,7 @@ CommonDataSubscriber::CommonDataSubscriber() :
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
approxSync_);
}
else
{
@@ -244,13 +246,23 @@ CommonDataSubscriber::CommonDataSubscriber() :
subscribeScan3d,
subscribeOdomInfo,
queueSize_,
approxSync);
approxSync_);
}
}
if(subscribedToDepth_ || subscribedToStereo_ || subscribedToRGBD_)
{
warningThread_ = new boost::thread(boost::bind(&CommonDataSubscriber::warningLoop, this));
ROS_INFO(subscribedTopicsMsg_.c_str());
}
}
CommonDataSubscriber::~CommonDataSubscriber()
{
if(warningThread_)
{
delete warningThread_;
}
// RGB + Depth
SYNC_DEL(depth);
SYNC_DEL(depthScan2d);
@@ -337,6 +349,25 @@ CommonDataSubscriber::~CommonDataSubscriber()
rgbdSubs_.clear();
}
void CommonDataSubscriber::warningLoop()
{
ros::Duration r(10.0);
while(ros::ok() && !callbackCalled_)
{
r.sleep();
if(ros::ok() && !callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 10 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
ros::this_node::getName().c_str(),
approxSync_?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg_.c_str());
}
}
}
void CommonDataSubscriber::commonSingleDepthCallback(
const nav_msgs::OdometryConstPtr & odomMsg,
const rtabmap_ros::UserDataConstPtr & userDataMsg,
@@ -347,6 +378,7 @@ void CommonDataSubscriber::commonSingleDepthCallback(
const sensor_msgs::PointCloud2ConstPtr& scan3dMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
callbackCalled();
std::vector<cv_bridge::CvImageConstPtr> imageMsgs;
std::vector<cv_bridge::CvImageConstPtr> depthMsgs;
std::vector<sensor_msgs::CameraInfo> cameraInfoMsgs;
src/OdometryROS.cpp
+31
View File
@@ -58,6 +58,8 @@ namespace rtabmap_ros {
OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
odometry_(0),
warningThread_(0),
callbackCalled_(false),
frameId_("base_link"),
odomFrameId_("odom"),
groundTruthFrameId_(""),
@@ -79,6 +81,10 @@ OdometryROS::OdometryROS(bool stereoParams, bool visParams, bool icpParams) :
OdometryROS::~OdometryROS()
{
if(warningThread_)
{
delete warningThread_;
}
ros::NodeHandle & pnh = getPrivateNodeHandle();
if(pnh.ok())
{
@@ -297,6 +303,31 @@ void OdometryROS::onInit()
onOdomInit();
}
void OdometryROS::startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync)
{
warningThread_ = new boost::thread(boost::bind(&OdometryROS::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO(subscribedTopicsMsg.c_str());
}
void OdometryROS::warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(10.0);
while(ros::ok() && !callbackCalled_)
{
r.sleep();
if(ros::ok() && !callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 10 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
ros::this_node::getName().c_str(),
approxSync?"":"Parameter \"approx_sync\" is false, which means that input "
"topics should have all the exact timestamp for the callback to be called.",
subscribedTopicsMsg.c_str());
}
}
}
Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const
{
// TF ready?
src/impl/CommonDataSubscriberRGBD.cpp
+2 -1
View File
@@ -377,7 +377,8 @@ void CommonDataSubscriber::setupRGBDCallbacks(
{
rgbdSub_ = nh.subscribe("rgbd_image", 1, &CommonDataSubscriber::rgbdCallback, this);
ROS_INFO("\n%s subscribed to:\n %s",
subscribedTopicsMsg_ =
uFormat("\n%s subscribed to:\n %s",
ros::this_node::getName().c_str(),
rgbdSub_.getTopic().c_str());
}
src/impl/CommonDataSubscriberRGBD2.cpp
+1
View File
@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_ros {
#define IMAGE_CONVERSION() \
callbackCalled(); \
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(2); \
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(2); \
rtabmap_ros::toCvShare(image1Msg, imageMsgs[0], depthMsgs[0]); \
src/impl/CommonDataSubscriberStereo.cpp
+4
View File
@@ -36,6 +36,7 @@ void CommonDataSubscriber::stereoCallback(
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
{
callbackCalled();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scanMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
@@ -49,6 +50,7 @@ void CommonDataSubscriber::stereoInfoCallback(
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg)
{
callbackCalled();
nav_msgs::OdometryConstPtr odomMsg; // Null
sensor_msgs::LaserScanConstPtr scan2dMsg; // null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
@@ -63,6 +65,7 @@ void CommonDataSubscriber::stereoOdomCallback(
const sensor_msgs::CameraInfoConstPtr& leftCamInfoMsg,
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg)
{
callbackCalled();
sensor_msgs::LaserScanConstPtr scanMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // null
rtabmap_ros::OdomInfoConstPtr odomInfoMsg; // null
@@ -76,6 +79,7 @@ void CommonDataSubscriber::stereoOdomInfoCallback(
const sensor_msgs::CameraInfoConstPtr& rightCamInfoMsg,
const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
{
callbackCalled();
sensor_msgs::LaserScanConstPtr scan2dMsg; // Null
sensor_msgs::PointCloud2ConstPtr scan3dMsg; // Null
commonStereoCallback(odomMsg, leftImageMsg, rightImageMsg, leftCamInfoMsg, rightCamInfoMsg, scan2dMsg, scan3dMsg, odomInfoMsg);
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 ||