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 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_sync/src/nodelets/rgbd_sync.cpp
+16 -38
View File
@@ -53,35 +53,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/util2d.h"
#include "rtabmap/utilite/UConversion.h"
#include "rtabmap_sync/SyncDiagnostic.h"
namespace rtabmap_sync
{
class RGBDSync : public nodelet::Nodelet
class RGBDSync : public nodelet::Nodelet, public SyncDiagnostic
{
public:
RGBDSync() :
depthScale_(1.0),
decimation_(1),
compressedRate_(0),
warningThread_(0),
callbackCalled_(false),
approxSyncDepth_(0),
exactSyncDepth_(0)
{}
virtual ~RGBDSync()
{
if(approxSyncDepth_)
delete approxSyncDepth_;
if(exactSyncDepth_)
delete exactSyncDepth_;
if(warningThread_)
{
callbackCalled_=true;
warningThread_->join();
delete warningThread_;
}
delete approxSyncDepth_;
delete exactSyncDepth_;
}
private:
@@ -149,28 +140,16 @@ private:
imageSub_.getTopic().c_str(),
imageDepthSub_.getTopic().c_str(),
cameraInfoSub_.getTopic().c_str());
NODELET_INFO(subscribedTopicsMsg.c_str());
warningThread_ = new boost::thread(boost::bind(&RGBDSync::warningLoop, this, subscribedTopicsMsg, approxSync));
NODELET_INFO("%s", subscribedTopicsMsg.c_str());
}
void warningLoop(const std::string & subscribedTopicsMsg, bool approxSync)
{
ros::Duration r(5.0);
while(!callbackCalled_)
{
r.sleep();
if(!callbackCalled_)
{
ROS_WARN("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
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());
}
}
initDiagnostic(rgb_nh.resolveName("image"),
uFormat("%s: Did not receive data since 5 seconds! Make sure the input topics are "
"published (\"$ rostopic hz my_topic\") and the timestamps in their "
"header are set. %s%s",
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 callback(
@@ -178,7 +157,8 @@ private:
const sensor_msgs::ImageConstPtr& depth,
const sensor_msgs::CameraInfoConstPtr& cameraInfo)
{
callbackCalled_ = true;
tick(image->header.stamp);
if(rgbdImagePub_.getNumSubscribers() || rgbdImageCompressedPub_.getNumSubscribers())
{
double rgbStamp = image->header.stamp.toSec();
@@ -309,8 +289,6 @@ private:
double depthScale_;
int decimation_;
double compressedRate_;
boost::thread * warningThread_;
bool callbackCalled_;
ros::Time lastCompressedPublished_;