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 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/include/rtabmap_sync/CommonDataSubscriber.h
+2 -5
View File
@@ -51,12 +51,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/OdomInfo.h>
#include <rtabmap_msgs/ScanDescriptor.h>
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
#include <boost/thread.hpp>
namespace rtabmap_sync {
class CommonDataSubscriber {
class CommonDataSubscriber : public SyncDiagnostic {
public:
CommonDataSubscriber(bool gui);
virtual ~CommonDataSubscriber();
@@ -122,8 +123,6 @@ protected:
const cv::Mat & localDescriptors = cv::Mat());
private:
void warningLoop();
void callbackCalled() {callbackCalled_ = true;}
void setupDepthCallbacks(
ros::NodeHandle & nh,
ros::NodeHandle & pnh,
@@ -256,8 +255,6 @@ protected:
private:
bool approxSync_;
boost::thread* warningThread_;
bool callbackCalled_;
bool subscribedToDepth_;
bool subscribedToStereo_;
bool subscribedToRGB_;
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+81
View File
@@ -0,0 +1,81 @@
#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
#include "rtabmap/utilite/UStl.h"
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(double tolerance = 0.1) :
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
lastCallbackCalledStamp_(ros::Time::now().toSec()-1),
targetFrequency_(0.0)
{}
protected:
void initDiagnostic(
const std::string & topic,
const std::string & topicsNotReceivedWarningMsg,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
{
topicsNotReceivedWarningMsg_ = topicsNotReceivedWarningMsg;
std::list<std::string> strList = uSplit(topic, '/');
for(int i=0; i<2 && strList.size()>1; ++i)
{
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
}
diagnosticUpdater_.setHardwareID(strList.empty()?"none":uJoin(strList, "/"));
diagnosticUpdater_.force_update();
diagnosticTimer_ = ros::NodeHandle().createTimer(ros::Duration(1), &SyncDiagnostic::diagnosticTimerCallback, this);
}
void tick(const ros::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
double period = stamp.toSec() - lastCallbackCalledStamp_;
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
{
targetFrequency_ = 1.0/period;
}
else if(targetFrequency>0)
{
targetFrequency_ = targetFrequency;
}
lastCallbackCalledStamp_ = stamp.toSec();
}
private:
void diagnosticTimerCallback(const ros::TimerEvent& event)
{
diagnosticUpdater_.update();
if(ros::Time::now().toSec()-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
{
ROS_WARN_THROTTLE(5, topicsNotReceivedWarningMsg_.c_str());
}
}
private:
std::string topicsNotReceivedWarningMsg_;
diagnostic_updater::Updater diagnosticUpdater_;
diagnostic_updater::FrequencyStatus frequencyStatus_;
ros::Timer diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;
};
}
#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */