i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-06 01:37:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2023-11-19 17:14:24 -08:00
parent 76e83081a7 67de27b1ee
commit 399fb33310
31 changed files with 1148 additions and 295 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+35 -3
View File
@@ -51,12 +51,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_msgs/msg/user_data.hpp>
#include <rtabmap_msgs/msg/odom_info.hpp>
#include <rtabmap_msgs/msg/scan_descriptor.hpp>
#include <rtabmap_msgs/msg/sensor_data.hpp>
#include <rtabmap_sync/CommonDataSubscriberDefines.h>
#include <rtabmap_sync/SyncDiagnostic.h>
namespace rtabmap_sync {
class CommonDataSubscriber : public SyncDiagnostic {
class CommonDataSubscriber {
public:
RTABMAP_SYNC_PUBLIC
CommonDataSubscriber(rclcpp::Node & node, bool gui);
@@ -69,15 +70,18 @@ public:
bool isSubscribedToRGBD() const {return subscribedToRGBD_;}
bool isSubscribedToScan2d() const {return subscribedToScan2d_;}
bool isSubscribedToScan3d() const {return subscribedToScan3d_;}
bool isSubscribedToSensorData() const {return subscribedToSensorData_;}
bool isSubscribedToOdomInfo() const {return subscribedToOdomInfo_;}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom();}
bool isDataSubscribed() const {return isSubscribedToDepth() || isSubscribedToStereo() || isSubscribedToRGBD() || isSubscribedToScan2d() || isSubscribedToScan3d() || isSubscribedToRGB() || isSubscribedToOdom() || isSubscribedToSensorData();}
int rgbdCameras() const {return isSubscribedToRGBD()?(int)rgbdSubs_.size():0;}
int getQueueSize() const {return queueSize_;}
bool isApproxSync() const {return approxSync_;}
const std::string & name() const {return name_;}
protected:
void setupCallbacks(rclcpp::Node & node);
void setupCallbacks(
rclcpp::Node & node,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>());
virtual void commonMultiCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
@@ -103,6 +107,10 @@ protected:
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
virtual void commonSensorDataCallback(
const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg,
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0;
void commonSingleCameraCallback(
const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg,
@@ -119,6 +127,8 @@ protected:
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
private:
void setupDepthCallbacks(
rclcpp::Node & node,
@@ -218,6 +228,12 @@ private:
int queueSize,
bool approxSync);
#endif
void setupSensorDataCallbacks(
rclcpp::Node & node,
bool subscribeOdom,
bool subscribeOdomInfo,
int queueSize,
bool approxSync);
void setupScanCallbacks(
rclcpp::Node & node,
bool subscribeScan2d,
@@ -242,6 +258,7 @@ protected:
rmw_qos_reliability_policy_t qosCameraInfo_;
rmw_qos_reliability_policy_t qosScan_;
rmw_qos_reliability_policy_t qosUserData_;
rmw_qos_reliability_policy_t qosSensorData_;
private:
bool approxSync_;
@@ -250,6 +267,7 @@ private:
bool subscribedToRGB_;
bool subscribedToOdom_;
bool subscribedToRGBD_;
bool subscribedToSensorData_;
bool subscribedToScan2d_;
bool subscribedToScan3d_;
bool subscribedToScanDescriptor_;
@@ -270,6 +288,10 @@ private:
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImages>::ConstSharedPtr rgbdXSubOnly_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImages> rgbdXSub_;
//for sensor data callback
rclcpp::Subscription<rtabmap_msgs::msg::SensorData>::ConstSharedPtr sensorDataSubOnly_;
message_filters::Subscriber<rtabmap_msgs::msg::SensorData> sensorDataSub_;
//stereo callback
image_transport::SubscriberFilter imageRectLeft_;
image_transport::SubscriberFilter imageRectRight_;
@@ -288,6 +310,8 @@ private:
rclcpp::Subscription<rtabmap_msgs::msg::ScanDescriptor>::ConstSharedPtr scanDescSubOnly_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::ConstSharedPtr odomSubOnly_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
// RGB + Depth
DATA_SYNCS3(depth, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo)
DATA_SYNCS4(depthScan2d, sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::LaserScan)
@@ -440,6 +464,14 @@ private:
DATA_SYNCS4(rgbdXOdomDataInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::UserData, rtabmap_msgs::msg::RGBDImages, rtabmap_msgs::msg::OdomInfo)
#endif
// SensorData
void sensorDataCallback(const rtabmap_msgs::msg::SensorData::ConstSharedPtr);
DATA_SYNCS2(sensorDataInfo, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
// SensorData + Odom
DATA_SYNCS2(sensorDataOdom, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData);
DATA_SYNCS3(sensorDataOdomInfo, nav_msgs::msg::Odometry, rtabmap_msgs::msg::SensorData, rtabmap_msgs::msg::OdomInfo);
#ifdef RTABMAP_SYNC_MULTI_RGBD
// 2 RGBD
DATA_SYNCS2(rgbd2, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage)
rtabmap_sync/include/rtabmap_sync/SyncDiagnostic.h
+9 -3
View File
@@ -19,6 +19,8 @@ class SyncDiagnostic {
node_(node),
diagnosticUpdater_(node),
frequencyStatus_(diagnostic_updater::FrequencyStatusParam(&targetFrequency_, &targetFrequency_, tolerance)),
timeStampStatus_(diagnostic_updater::TimeStampStatusParam()),
compositeTask_("Sync status"),
lastCallbackCalledStamp_(rtabmap_conversions::timestampFromROS(node_->now())-1),
targetFrequency_(0.0),
windowSize_(windowSize)
@@ -26,8 +28,7 @@ class SyncDiagnostic {
UASSERT(windowSize_ >= 1);
}
protected:
void initDiagnostic(
void init(
const std::string & topic,
const std::string & topicsNotReceivedWarningMsg,
std::vector<diagnostic_updater::DiagnosticTask*> otherTasks = std::vector<diagnostic_updater::DiagnosticTask*>())
@@ -40,7 +41,9 @@ protected:
// Assuming format is /back_camera/left/image, we want "back_camera"
strList.pop_back();
}
diagnosticUpdater_.add(frequencyStatus_);
compositeTask_.addTask(&frequencyStatus_);
compositeTask_.addTask(&timeStampStatus_);
diagnosticUpdater_.add(compositeTask_);
for(size_t i=0; i<otherTasks.size(); ++i)
{
diagnosticUpdater_.add(*otherTasks[i]);
@@ -53,6 +56,7 @@ protected:
void tick(const rclcpp::Time & stamp, double targetFrequency = 0)
{
frequencyStatus_.tick();
timeStampStatus_.tick(stamp);
double singlePeriod = rtabmap_conversions::timestampFromROS(stamp) - lastCallbackCalledStamp_;
window_.push_back(singlePeriod);
@@ -95,6 +99,8 @@ private:
std::string topicsNotReceivedWarningMsg_;
diagnostic_updater::Updater diagnosticUpdater_;
diagnostic_updater::FrequencyStatus frequencyStatus_;
diagnostic_updater::TimeStampStatus timeStampStatus_;
diagnostic_updater::CompositeDiagnosticTask compositeTask_;
rclcpp::TimerBase::SharedPtr diagnosticTimer_;
double lastCallbackCalledStamp_;
double targetFrequency_;
rtabmap_sync/include/rtabmap_sync/rgb_sync.hpp
+3 -1
View File
@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBSync : public rclcpp::Node, public SyncDiagnostic
class RGBSync : public rclcpp::Node
{
public:
RTABMAP_SYNC_PUBLIC
@@ -72,6 +72,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
}
rtabmap_sync/include/rtabmap_sync/rgbd_sync.hpp
+3 -1
View File
@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDSync : public rclcpp::Node, public SyncDiagnostic
class RGBDSync : public rclcpp::Node
{
public:
RTABMAP_SYNC_PUBLIC
@@ -76,6 +76,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyExactSyncDepthPolicy;
message_filters::Synchronizer<MyExactSyncDepthPolicy> * exactSyncDepth_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
}
rtabmap_sync/include/rtabmap_sync/rgbdx_sync.hpp
+3 -1
View File
@@ -46,7 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class RGBDXSync : public rclcpp::Node, public SyncDiagnostic
class RGBDXSync : public rclcpp::Node
{
public:
RTABMAP_SYNC_PUBLIC
@@ -72,6 +72,8 @@ private:
rclcpp::Publisher<rtabmap_msgs::msg::RGBDImages>::SharedPtr rgbdImagesPub_;
std::vector<message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage>*> rgbdSubs_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
}
rtabmap_sync/include/rtabmap_sync/stereo_sync.hpp
+3 -1
View File
@@ -44,7 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap_sync
{
class StereoSync : public rclcpp::Node, public SyncDiagnostic
class StereoSync : public rclcpp::Node
{
public:
RTABMAP_SYNC_PUBLIC
@@ -74,6 +74,8 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
std::unique_ptr<SyncDiagnostic> syncDiagnostic_;
};
}