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

merged master->ros2 (diagnostics, #1046)

Browse Source
This commit is contained in:
matlabbe
2023-10-14 15:22:56 -07:00
parent 0473206ade 61bd84bcd6
commit a3b6f0a88f
34 changed files with 393 additions and 288 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+2 -4
View File
@@ -52,10 +52,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
namespace rtabmap_sync {
class CommonDataSubscriber {
class CommonDataSubscriber : public SyncDiagnostic {
public:
RTABMAP_SYNC_PUBLIC
CommonDataSubscriber(rclcpp::Node & node, bool gui);
@@ -119,7 +120,6 @@ protected:
const cv::Mat & localDescriptors = cv::Mat());
private:
void callbackCalled() {callbackCalled_ = true;}
void setupDepthCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
@@ -245,8 +245,6 @@ protected:
private:
bool approxSync_;
std::thread* warningThread_;
bool callbackCalled_;
bool subscribedToDepth_;
bool subscribedToStereo_;
bool subscribedToRGB_;
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+108
View File
@@ -0,0 +1,108 @@
#ifndef INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
#define INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_
#include "rtabmap/utilite/UStl.h"
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <diagnostic_updater/publisher.hpp>
#include "rtabmap_conversions/MsgConversion.h"
#include "rtabmap/utilite/ULogger.h"
using namespace std::chrono_literals;
namespace rtabmap_sync {
class SyncDiagnostic {
public:
SyncDiagnostic(rclcpp::Node * node, double tolerance = 0.1, int windowSize = 5) :
node_(node),
diagnosticUpdater_(node),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
targetFrequency_(0.0),
windowSize_(windowSize)
{
UASSERT(windowSize_ >= 1);
}
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_ = node_->create_wall_timer(1s, std::bind(&SyncDiagnostic::diagnosticTimerCallback, this), nullptr);
}
void tick(const rclcpp::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_;
window_.push_back(singlePeriod);
if(window_.size() > windowSize_)
{
window_.pop_front();
}
double period = 0.0;
if(window_.size() == windowSize_)
{
for(size_t i=0; i<window_.size(); ++i)
{
period += window_[i];
}
period /= windowSize_;
}
if(period>0.0 && targetFrequency == 0 && (targetFrequency_ == 0.0 || period < 1.0/targetFrequency_))
{
targetFrequency_ = 1.0/period;
}
else if(targetFrequency>0)
{
targetFrequency_ = targetFrequency;
}
lastCallbackCalledStamp_ = rtabmap_conversions::timestampFromROS(stamp);
}
private:
void diagnosticTimerCallback()
{
if(rtabmap_conversions::timestampFromROS(node_->now())-lastCallbackCalledStamp_ >= 5 && !topicsNotReceivedWarningMsg_.empty())
{
RCLCPP_WARN_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, topicsNotReceivedWarningMsg_.c_str());
}
}
private:
rclcpp::Node * node_;
std::string topicsNotReceivedWarningMsg_;
diagnostic_updater::Updater diagnosticUpdater_;
diagnostic_updater::FrequencyStatus frequencyStatus_;
rclcpp::TimerBase::SharedPtr diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;
int windowSize_;
std::deque<double> window_;
};
}
#endif /* INCLUDE_RTABMAP_SYNC_SYNCDIAGNOSTIC_H_ */
rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp
+2 -5
View File
@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h>
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include "rtabmap_sync/SyncDiagnostic.h"
namespace rtabmap_sync
{
class RGBSync : public rclcpp::Node
class RGBSync : public rclcpp::Node, public SyncDiagnostic
{
public:
RTABMAP_SYNC_PUBLIC
@@ -57,13 +58,9 @@ public:
private:
double compressedRate_;
std::thread * warningThread_;
bool callbackCalled_;
rclcpp::Time lastCompressedPublished_;
std::string subscribedTopicsMsg_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp
+2 -5
View File
@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h>
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include "rtabmap_sync/SyncDiagnostic.h"
namespace rtabmap_sync
{
class RGBDSync : public rclcpp::Node
class RGBDSync : public rclcpp::Node, public SyncDiagnostic
{
public:
RTABMAP_SYNC_PUBLIC
@@ -60,13 +61,9 @@ private:
double depthScale_;
int decimation_;
double compressedRate_;
std::thread * warningThread_;
bool callbackCalled_;
rclcpp::Time lastCompressedPublished_;
std::string subscribedTopicsMsg_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageCompressedPub_;
rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp
+2 -6
View File
@@ -41,11 +41,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include "rtabmap_msgs/msg/rgbd_images.hpp"
#include "rtabmap_sync/CommonDataSubscriber.h"
#include "rtabmap_sync/SyncDiagnostic.h"
namespace rtabmap_sync
{
class RGBDXSync : public rclcpp::Node
class RGBDXSync : public rclcpp::Node, public SyncDiagnostic
{
public:
RTABMAP_SYNC_PUBLIC
@@ -68,11 +69,6 @@ private:
DATA_SYNCS8(rgbd8, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
private:
std::thread * warningThread_;
bool callbackCalled_;
std::string subscribedTopicsMsg_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdImagesPub_;
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp
+2 -4
View File
@@ -39,11 +39,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <message_filters/subscriber.h>
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include "rtabmap_sync/SyncDiagnostic.h"
namespace rtabmap_sync
{
class StereoSync : public rclcpp::Node
class StereoSync : public rclcpp::Node, public SyncDiagnostic
{
public:
RTABMAP_SYNC_PUBLIC
@@ -58,9 +59,6 @@ public:
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight);
private:
double compressedRate_;
std::thread * warningThread_;
std::string subscribedTopicsMsg_;
bool callbackCalled_;
rclcpp::Time lastCompressedPublished_;
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImagePub_;