i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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
include/rtabmap_ros/CommonDataSubscriber.h
+10
View File
@@ -50,6 +50,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/OdomInfo.h>
#include <rtabmap_ros/CommonDataSubscriberDefines.h>
#include <boost/thread.hpp>
namespace rtabmap_ros {
class CommonDataSubscriber {
@@ -97,6 +99,8 @@ protected:
const rtabmap_ros::OdomInfoConstPtr& odomInfoMsg);
private:
void warningLoop();
void callbackCalled() {callbackCalled_ = true;}
void setupDepthCallbacks(
bool subscribeOdom,
bool subscribeUserData,
@@ -127,8 +131,14 @@ private:
int queueSize,
bool approxSync);
protected:
std::string subscribedTopicsMsg_;
private:
int queueSize_;
bool approxSync_;
boost::thread* warningThread_;
bool callbackCalled_;
bool subscribedToDepth_;
bool subscribedToStereo_;
bool subscribedToRGBD_;
include/rtabmap_ros/CommonDataSubscriberDefines.h
+6 -5
View File
@@ -28,6 +28,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#ifndef INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
#define INCLUDE_RTABMAP_ROS_COMMONDATASUBSCRIBERIMPL_H_
#include <rtabmap/utilite/UConversion.h>
#define DATA_SYNC2(PREFIX, SYNC_NAME, MSG0, MSG1) \
typedef message_filters::sync_policies::SYNC_NAME##Time<MSG0, MSG1> PREFIX##SYNC_NAME##SyncPolicy; \
@@ -98,7 +99,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2)); \
} \
ROS_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(), \
APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \
@@ -117,7 +118,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3)); \
} \
ROS_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(), \
APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \
@@ -137,7 +138,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4)); \
} \
ROS_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(), \
APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \
@@ -158,7 +159,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5)); \
} \
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s", \
ros::this_node::getName().c_str(), \
approxSync?"approx":"exact", \
SUB0.getTopic().c_str(), \
@@ -180,7 +181,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PREFIX##ExactSyncPolicy(QUEUE_SIZE), SUB0, SUB1, SUB2, SUB3, SUB4, SUB5); \
PREFIX##ExactSync_->registerCallback(boost::bind(&CommonDataSubscriber::PREFIX##Callback, this, _1, _2, _3, _4, _5, _6)); \
} \
ROS_INFO("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", \
subscribedTopicsMsg_ = uFormat("\n%s subscribed to (%s sync):\n %s,\n %s,\n %s,\n %s,\n %s,\n %s", \
ros::this_node::getName().c_str(), \
APPROX?"approx":"exact", \
SUB0.getTopic().c_str(), \
include/rtabmap_ros/OdometryROS.h
+8
View File
@@ -41,6 +41,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/SensorData.h>
#include <rtabmap/core/Parameters.h>
#include <boost/thread.hpp>
namespace rtabmap {
class Odometry;
}
@@ -72,16 +74,22 @@ public:
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
protected:
void startWarningThread(const std::string & subscribedTopicsMsg, bool approxSync);
void callbackCalled() {callbackCalled_ = true;}
virtual void flushCallbacks() = 0;
tf::TransformListener & tfListener() {return tfListener_;}
private:
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync);
virtual void onInit();
virtual void onOdomInit() = 0;
virtual void updateParameters(rtabmap::ParametersMap & parameters) {}
private:
rtabmap::Odometry * odometry_;
boost::thread * warningThread_;
bool callbackCalled_;
// parameters
std::string frameId_;