From ad97f56d0b553a8da107628ca0826be51f56e79f Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 15 Oct 2024 21:00:35 -0700 Subject: [PATCH] Multithreaded rtabmap node (#1214) * Multi-threaded rtabmap node * Moved data sync logic inside CoreWrapper to process odometry topics outside processing thread * updated commend * re-enabled jazzy * disabled jazzy for this mr * Added async callbackgroup for imu and landmark topics * Making all shared variables thread-safe --- rtabmap_conversions/src/MsgConversion.cpp | 25 +- .../include/rtabmap_slam/CoreWrapper.h | 42 +- rtabmap_slam/src/CoreNode.cpp | 5 +- rtabmap_slam/src/CoreWrapper.cpp | 696 +++++++++++------- .../rtabmap_sync/CommonDataSubscriber.h | 45 +- rtabmap_sync/src/CommonDataSubscriber.cpp | 18 + .../src/impl/CommonDataSubscriberDepth.cpp | 71 +- .../src/impl/CommonDataSubscriberOdom.cpp | 9 +- .../src/impl/CommonDataSubscriberRGB.cpp | 69 +- .../src/impl/CommonDataSubscriberRGBD.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD2.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD3.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD4.cpp | 43 +- .../src/impl/CommonDataSubscriberRGBD5.cpp | 21 +- .../src/impl/CommonDataSubscriberRGBD6.cpp | 21 +- .../src/impl/CommonDataSubscriberRGBDX.cpp | 43 +- .../src/impl/CommonDataSubscriberScan.cpp | 35 +- .../impl/CommonDataSubscriberSensorData.cpp | 9 +- .../src/impl/CommonDataSubscriberStereo.cpp | 15 +- 19 files changed, 772 insertions(+), 524 deletions(-) diff --git a/rtabmap_conversions/src/MsgConversion.cpp b/rtabmap_conversions/src/MsgConversion.cpp index 0d820fb4a..4bfe58d36 100644 --- a/rtabmap_conversions/src/MsgConversion.cpp +++ b/rtabmap_conversions/src/MsgConversion.cpp @@ -1691,18 +1691,6 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b info.localBundleOutliers = msg.local_bundle_outliers; info.localBundleConstraints = msg.local_bundle_constraints; info.localBundleTime = msg.local_bundle_time; - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_ids.size()); - UASSERT(msg.local_bundle_models.size() == msg.local_bundle_poses.size()); - for(size_t i=0; i models; - for(size_t j=0; j models; + for(size_t j=0; j > & localKeyPoints = std::vector >(), const std::vector > & localPoints3d = std::vector >(), const std::vector & localDescriptors = std::vector()); + // Callback called from sync thread void commonMultiCameraCallbackImpl( const std::string & odomFrameId, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -148,6 +150,7 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib const std::vector > & localKeyPoints, const std::vector > & localPoints3d, const std::vector & localDescriptors); + // Callback called from sync thread virtual void commonLaserScanCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, @@ -155,11 +158,13 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib const sensor_msgs::msg::PointCloud2 & scan3dMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor = rtabmap_msgs::msg::GlobalDescriptor()); + // Callback called from sync thread virtual void commonOdomCallback( const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg); + // Callback called from sync thread virtual void commonSensorDataCallback( const rtabmap_msgs::msg::SensorData::ConstSharedPtr & sensorDataMsg, const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, @@ -195,6 +200,8 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib void goalNodeCallback(const rtabmap_msgs::msg::Goal::SharedPtr msg); void updateGoal(const rclcpp::Time & stamp); + void processAsync(); + void process( const rclcpp::Time & stamp, rtabmap::SensorData & data, @@ -266,11 +273,14 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib private: rtabmap::Rtabmap rtabmap_; bool paused_; + + UMutex lastPoseMutex_; rtabmap::Transform lastPose_; rclcpp::Time lastPoseStamp_; std::vector lastPoseVelocity_; + cv::Mat lastPoseCovariance_; bool lastPoseIntermediate_; - cv::Mat covariance_; + rtabmap::Transform currentMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_; bool latestNodeWasReached_; @@ -341,7 +351,7 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib std::shared_ptr tfBuffer_; std::shared_ptr tfListener_; - rclcpp::SyncParametersClient::SharedPtr parametersClient_; + rclcpp::AsyncParametersClient::SharedPtr parametersClient_; rclcpp::Subscription::SharedPtr parameterEventSub_; rclcpp::Service::SharedPtr updateSrv_; @@ -390,6 +400,7 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib // for loop closure detection only image_transport::Subscriber defaultSub_; + rclcpp::CallbackGroup::SharedPtr userDataAsyncCallbackGroup_; rclcpp::Subscription::SharedPtr userDataAsyncSub_; cv::Mat userData_; UMutex userDataMutex_; @@ -398,6 +409,8 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib geometry_msgs::msg::PoseWithCovarianceStamped globalPose_; rclcpp::Subscription::SharedPtr gpsFixAsyncSub_; rtabmap::GPS gps_; + + rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_; rclcpp::Subscription::SharedPtr landmarkDetectionSub_; rclcpp::Subscription::SharedPtr landmarkDetectionsSub_; #ifdef WITH_APRILTAG_MSGS @@ -407,10 +420,14 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib rclcpp::Subscription::SharedPtr fiducialTransfromsSub_; #endif std::map > landmarks_; // id, - rclcpp::Subscription::SharedPtr imuSub_; + UMutex landmarksMutex_; + rclcpp::CallbackGroup::SharedPtr imuCallbackGroup_; + rclcpp::Subscription::SharedPtr imuSub_; std::map imus_; std::string imuFrameId_; + UMutex imuMutex_; + rclcpp::Subscription::SharedPtr republishNodeDataSub_; rclcpp::Subscription::SharedPtr interOdomSub_; @@ -444,6 +461,23 @@ class CoreWrapper : public rclcpp::Node, public rtabmap_sync::CommonDataSubscrib double localizationError_; }; LocalizationStatusTask localizationDiagnostic_; + + rclcpp::CallbackGroup::SharedPtr processingCallbackGroup_; + struct SyncData { + bool valid; + rclcpp::Time stamp; + rtabmap::SensorData data; + rtabmap::Transform odom; + std::vector odomVelocity; + std::string odomFrameId; + cv::Mat odomCovariance; + rtabmap::OdometryInfo odomInfo; + double timeMsgConversion; + }; + rclcpp::TimerBase::SharedPtr syncTimer_; + SyncData syncData_; + UMutex syncDataMutex_; + bool triggerNewMapBeforeNextUpdate_; }; } diff --git a/rtabmap_slam/src/CoreNode.cpp b/rtabmap_slam/src/CoreNode.cpp index ec5d226ab..d16aa4f1b 100644 --- a/rtabmap_slam/src/CoreNode.cpp +++ b/rtabmap_slam/src/CoreNode.cpp @@ -84,8 +84,11 @@ int main(int argc, char** argv) rclcpp::init(argc, argv); rclcpp::NodeOptions options; options.arguments(arguments); + auto node = std::make_shared(options); + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); UINFO("rtabmap %s started...", RTABMAP_VERSION); - rclcpp::spin(std::make_shared(options)); + executor.spin(); rclcpp::shutdown(); return 0; } diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index a9da2ce59..508804a93 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -139,7 +139,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : alreadyRectifiedImages_(Parameters::defaultRtabmapImagesAlreadyRectified()), twoDMapping_(Parameters::defaultRegForce3DoF()), previousStamp_(0), - ulogToRosout_(this) + ulogToRosout_(this), + triggerNewMapBeforeNextUpdate_(false) { char * rosHomePath = getenv("ROS_HOME"); std::string workingDir = rosHomePath?rosHomePath:UDirectory::homeDir()+"/.ros"; @@ -148,6 +149,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : mapsManager_.init(*this, this->get_name(), true); + syncData_.valid = false; + tfBuffer_ = std::make_shared(this->get_clock()); //auto timer_interface = std::make_shared( // this->get_node_base_interface(), @@ -271,6 +274,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : RCLCPP_INFO(get_logger(), "rtabmap: scan_cloud_is_2d = %s", scanCloudIs2d_?"true":"false"); } + // Create the processing timer + processingCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + syncTimer_ = this->create_wall_timer(0s, std::bind(&CoreWrapper::processAsync, this), processingCallbackGroup_); + syncTimer_->cancel(); + + rclcpp::SubscriptionOptions subOptions; + subOptions.callback_group = processingCallbackGroup_; + infoPub_ = this->create_publisher("info", 1); mapDataPub_ = this->create_publisher("mapData", 1); mapGraphPub_ = this->create_publisher("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); @@ -282,11 +293,11 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : localGridEmpty_ = this->create_publisher("local_grid_empty", 1); localGridGround_ = this->create_publisher("local_grid_ground", 1); localizationPosePub_ = this->create_publisher("localization_pose", 1); - initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1)); + initialPoseSub_ = this->create_subscription("initialpose", 5, std::bind(&CoreWrapper::initialPoseCallback, this, std::placeholders::_1), subOptions); // planning topics - goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1)); - goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1)); + goalSub_ = this->create_subscription("goal", 5, std::bind(&CoreWrapper::goalCallback, this, std::placeholders::_1), subOptions); + goalNodeSub_ = this->create_subscription("goal_node", 5, std::bind(&CoreWrapper::goalNodeCallback, this, std::placeholders::_1), subOptions); nextMetricGoalPub_ = this->create_publisher("goal_out", 1); goalReachedPub_ = this->create_publisher("goal_reached", 1); globalPathPub_ = this->create_publisher("global_path", 1); @@ -560,7 +571,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : else { RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom messages"); - interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1)); + interOdomSub_ = this->create_subscription("inter_odom", 100, std::bind(&CoreWrapper::interOdomCallback, this, std::placeholders::_1), subOptions); } } @@ -649,45 +660,45 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : // setup services const std::string servicePrefix = get_name() + std::string("/"); - updateSrv_ = this->create_service(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - triggerNewMapSrv_ = this->create_service(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - backupDatabase_ = this->create_service(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - detectMoreLoopClosuresSrv_ = this->create_service(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - globalBundleAdjustmentSrv_ = this->create_service(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cleanupLocalGridsSrv_ = this->create_service(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeLocalizationSrv_ = this->create_service(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setModeMappingSrv_ = this->create_service(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodeDataSrv_ = this->create_service(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapDataSrv_ = this->create_service(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapData2Srv_ = this->create_service(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getMapSrv_ = this->create_service(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getProbMapSrv_ = this->create_service(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - publishMapDataSrv_ = this->create_service(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getPlanNodesSrv_ = this->create_service(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setGoalSrv_ = this->create_service(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - getNodesInRadiusSrv_ = this->create_service(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + updateSrv_ = this->create_service(servicePrefix + "update_parameters", std::bind(&CoreWrapper::updateRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + resetSrv_ = this->create_service(servicePrefix + "reset", std::bind(&CoreWrapper::resetRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + pauseSrv_ = this->create_service(servicePrefix + "pause", std::bind(&CoreWrapper::pauseRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + resumeSrv_ = this->create_service(servicePrefix + "resume", std::bind(&CoreWrapper::resumeRtabmapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + loadDatabaseSrv_ = this->create_service(servicePrefix + "load_database", std::bind(&CoreWrapper::loadDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + triggerNewMapSrv_ = this->create_service(servicePrefix + "trigger_new_map", std::bind(&CoreWrapper::triggerNewMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + backupDatabase_ = this->create_service(servicePrefix + "backup", std::bind(&CoreWrapper::backupDatabaseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + detectMoreLoopClosuresSrv_ = this->create_service(servicePrefix + "detect_more_loop_closures", std::bind(&CoreWrapper::detectMoreLoopClosuresCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + globalBundleAdjustmentSrv_ = this->create_service(servicePrefix + "global_bundle_adjustment", std::bind(&CoreWrapper::globalBundleAdjustmentCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + cleanupLocalGridsSrv_ = this->create_service(servicePrefix + "cleanup_local_grids", std::bind(&CoreWrapper::cleanupLocalGridsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setModeLocalizationSrv_ = this->create_service(servicePrefix + "set_mode_localization", std::bind(&CoreWrapper::setModeLocalizationCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setModeMappingSrv_ = this->create_service(servicePrefix + "set_mode_mapping", std::bind(&CoreWrapper::setModeMappingCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getNodeDataSrv_ = this->create_service(servicePrefix + "get_node_data", std::bind(&CoreWrapper::getNodeDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getMapDataSrv_ = this->create_service(servicePrefix + "get_map_data", std::bind(&CoreWrapper::getMapDataCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getMapData2Srv_ = this->create_service(servicePrefix + "get_map_data2", std::bind(&CoreWrapper::getMapData2Callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getMapSrv_ = this->create_service(servicePrefix + "get_map", std::bind(&CoreWrapper::getMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getProbMapSrv_ = this->create_service(servicePrefix + "get_prob_map", std::bind(&CoreWrapper::getProbMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + publishMapDataSrv_ = this->create_service(servicePrefix + "publish_map", std::bind(&CoreWrapper::publishMapCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getPlanSrv_ = this->create_service(servicePrefix + "get_plan", std::bind(&CoreWrapper::getPlanCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getPlanNodesSrv_ = this->create_service(servicePrefix + "get_plan_nodes", std::bind(&CoreWrapper::getPlanNodesCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setGoalSrv_ = this->create_service(servicePrefix + "set_goal", std::bind(&CoreWrapper::setGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + cancelGoalSrv_ = this->create_service(servicePrefix + "cancel_goal", std::bind(&CoreWrapper::cancelGoalCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setLabelSrv_ = this->create_service(servicePrefix + "set_label", std::bind(&CoreWrapper::setLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + listLabelsSrv_ = this->create_service(servicePrefix + "list_labels", std::bind(&CoreWrapper::listLabelsCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + removeLabelSrv_ = this->create_service(servicePrefix + "remove_label", std::bind(&CoreWrapper::removeLabelCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + addLinkSrv_ = this->create_service(servicePrefix + "add_link", std::bind(&CoreWrapper::addLinkCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + getNodesInRadiusSrv_ = this->create_service(servicePrefix + "get_nodes_in_radius", std::bind(&CoreWrapper::getNodesInRadiusCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); #ifdef WITH_OCTOMAP_MSGS #ifdef RTABMAP_OCTOMAP - octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + octomapBinarySrv_ = this->create_service(servicePrefix + "octomap_binary", std::bind(&CoreWrapper::octomapBinaryCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + octomapFullSrv_ = this->create_service(servicePrefix + "octomap_full", std::bind(&CoreWrapper::octomapFullCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); #endif #endif //private services - setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + setLogDebugSrv_ = this->create_service(servicePrefix + "log_debug", std::bind(&CoreWrapper::setLogDebug, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setLogInfoSrv_ = this->create_service(servicePrefix + "log_info", std::bind(&CoreWrapper::setLogInfo, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setLogWarnSrv_ = this->create_service(servicePrefix + "log_warning", std::bind(&CoreWrapper::setLogWarn, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); + setLogErrorSrv_ = this->create_service(servicePrefix + "log_error", std::bind(&CoreWrapper::setLogError, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3), rmw_qos_profile_services_default, processingCallbackGroup_); int optimizeIterations = 0; Parameters::parse(parameters_, Parameters::kOptimizerIterations(), optimizeIterations); @@ -700,9 +711,9 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : rclcpp::Rate r(1.0 / tfDelay); while(tfThreadRunning_) { + mapToOdomMutex_.lock(); if(!odomFrameId_.empty()) { - mapToOdomMutex_.lock(); rclcpp::Time tfExpiration = now() + rclcpp::Duration::from_seconds(tfTolerance); geometry_msgs::msg::TransformStamped msg; msg.child_frame_id = odomFrameId_; @@ -710,8 +721,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : msg.header.stamp = tfExpiration; rtabmap_conversions::transformToGeometryMsg(mapToOdom_, msg.transform); tfBroadcaster_->sendTransform(msg); - mapToOdomMutex_.unlock(); } + mapToOdomMutex_.unlock(); r.sleep(); } }); @@ -748,7 +759,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } auto node = rclcpp::Node::make_shared("rtabmap"); image_transport::TransportHints hints(this); - defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile()); + defaultSub_ = image_transport::create_subscription(node.get(), "image", std::bind(&CoreWrapper::defaultCallback, this, std::placeholders::_1), hints.getTransport(), rclcpp::QoS(this->getTopicQueueSize()).reliability((rmw_qos_reliability_policy_t)qosImage_).get_rmw_qos_profile(), subOptions); RCLCPP_INFO(this->get_logger(), "\n%s subscribed to:\n %s", get_name(), defaultSub_.getTopic().c_str()); @@ -834,25 +845,36 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : } this->set_parameters(rosParameters); + // Setup callback groups for any subscriptions that should not be affected by main processing thread. + userDataAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions userDataAsyncSubOptions; + rclcpp::SubscriptionOptions landmarkSubOptions; + rclcpp::SubscriptionOptions imuSubOptions; + userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_; + landmarkSubOptions.callback_group = imuCallbackGroup_; + imuSubOptions.callback_group = imuCallbackGroup_; + int qosGPS = 0; int qosIMU = 0; qosGPS = this->declare_parameter("qos_gps", qosGPS); qosIMU = this->declare_parameter("qos_imu", qosIMU); - userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1)); - globalPoseAsyncSub_ = this->create_subscription("global_pose", 5, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1)); - gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionSub_ = this->create_subscription("landmark_detection", 5, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1)); - landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 5, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1)); + userDataAsyncSub_ = this->create_subscription("user_data_async", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosUserData_), std::bind(&CoreWrapper::userDataAsyncCallback, this, std::placeholders::_1), userDataAsyncSubOptions); + globalPoseAsyncSub_ = this->create_subscription("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), subOptions); + gpsFixAsyncSub_ = this->create_subscription("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), subOptions); + landmarkDetectionSub_ = this->create_subscription("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); + landmarkDetectionsSub_ = this->create_subscription("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #ifdef WITH_APRILTAG_MSGS - tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1)); + tagDetectionsSub_ = this->create_subscription("tag_detections", 5, std::bind(&CoreWrapper::tagDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif #ifdef WITH_FIDUCIAL_MSGS - fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1)); + fiducialTransfromsSub_ = this->create_subscription("fiducial_transforms", 5, std::bind(&CoreWrapper::fiducialDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions); #endif - imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1)); - republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 5, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1)); + imuSub_ = this->create_subscription("imu", rclcpp::QoS(100).reliability((rmw_qos_reliability_policy_t)qosIMU), std::bind(&CoreWrapper::imuAsyncCallback, this, std::placeholders::_1), imuSubOptions); + republishNodeDataSub_ = this->create_subscription(servicePrefix+"republish_node_data", 1, std::bind(&CoreWrapper::republishNodeDataCallback, this, std::placeholders::_1), subOptions); - parametersClient_ = std::make_shared(this); + parametersClient_ = std::make_shared(this, std::string(), rmw_qos_profile_parameters, processingCallbackGroup_); auto on_parameter_event_callback = [this](const rcl_interfaces::msg::ParameterEvent::SharedPtr event) -> void { @@ -908,7 +930,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) : }; // Setup callback for changes to parameters. - parameterEventSub_ = parametersClient_->on_parameter_event(on_parameter_event_callback); + rclcpp::SubscriptionOptionsWithAllocator> paramOptions; + paramOptions.callback_group = processingCallbackGroup_; + parameterEventSub_ = parametersClient_->on_parameter_event( + on_parameter_event_callback, + rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_parameter_events)), + paramOptions); } CoreWrapper::~CoreWrapper() @@ -1071,11 +1098,13 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti } } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && !odom.isNull() && (odom.isIdentity() || (odomMsg.pose.covariance[0] >= BAD_COVARIANCE && odomMsg.twist.covariance[0] >= BAD_COVARIANCE))) { UWARN("Odometry is reset (identity pose or high variance (%f) detected). Increment map id!", MAX(odomMsg.pose.covariance[0], odomMsg.twist.covariance[0])); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1110,9 +1139,9 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti covariance.at(0,0)>0.0) { // Use largest covariance error (to be independent of the odometry frame rate) - if(covariance_.empty() || covariance.at(0,0) > covariance_.at(0,0)) + if(lastPoseCovariance_.empty() || covariance.at(0,0) > lastPoseCovariance_.at(0,0)) { - covariance_ = covariance; + lastPoseCovariance_ = covariance; } } } @@ -1142,32 +1171,30 @@ bool CoreWrapper::odomUpdate(const nav_msgs::msg::Odometry & odomMsg, rclcpp::Ti return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } return false; } -bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) +bool CoreWrapper::odomTFUpdate(const std::string & odomFrameId, const rclcpp::Time & stamp) { if(!paused_) { // Odom TF ready? - Transform odom = rtabmap_conversions::getTransform(odomFrameId_, frameId_, stamp, *tfBuffer_, waitForTransform_); + Transform odom = rtabmap_conversions::getTransform(odomFrameId, frameId_, stamp, *tfBuffer_, waitForTransform_); if(odom.isNull()) { return false; } + UScopeMutex lock(lastPoseMutex_); + if(!lastPose_.isIdentity() && odom.isIdentity()) { UWARN("Odometry is reset (identity pose detected). Increment map id!"); - rtabmap_.triggerNewMap(); - covariance_ = cv::Mat(); + triggerNewMapBeforeNextUpdate_ = true; + lastPoseCovariance_ = cv::Mat(); } lastPoseIntermediate_ = false; @@ -1199,10 +1226,6 @@ bool CoreWrapper::odomTFUpdate(const rclcpp::Time & stamp) return false; } } - else if(!ignoreFrame) - { - previousStamp_ = stamp; - } return true; } @@ -1224,7 +1247,7 @@ void CoreWrapper::commonMultiCameraCallback( const std::vector > & localPoints3d, const std::vector & localDescriptors) { - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1247,38 +1270,53 @@ void CoreWrapper::commonMultiCameraCallback( return; } } - else if(!scan2dMsg.ranges.empty()) + else { - if(!odomTFUpdate(scan2dMsg.header.stamp)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) { - return; + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } } - } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(odomFrameId, cameraInfoMsgs[0].header.stamp)) { return; } } - else if(cameraInfoMsgs.size() == 0 || !odomTFUpdate(cameraInfoMsgs[0].header.stamp)) + + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - return; + UScopeMutex lock(lastPoseMutex_); + commonMultiCameraCallbackImpl(odomFrameId, + userDataMsg, + imageMsgs, + depthMsgs, + cameraInfoMsgs, + depthCameraInfoMsgs, + scan2dMsg, + scan3dMsg, + odomInfoMsg, + globalDescriptorMsgs, + localKeyPoints, + localPoints3d, + localDescriptors); + + if(syncData_.valid) { + syncTimer_->reset(); + } + syncDataMutex_.unlock(); } - - commonMultiCameraCallbackImpl(odomFrameId, - userDataMsg, - imageMsgs, - depthMsgs, - cameraInfoMsgs, - depthCameraInfoMsgs, - scan2dMsg, - scan3dMsg, - odomInfoMsg, - globalDescriptorMsgs, - localKeyPoints, - localPoints3d, - localDescriptors); } void CoreWrapper::commonMultiCameraCallbackImpl( @@ -1528,10 +1566,9 @@ void CoreWrapper::commonMultiCameraCallbackImpl( userData_ = cv::Mat(); } - SensorData data; if(!stereoCameraModels.empty()) { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1542,7 +1579,7 @@ void CoreWrapper::commonMultiCameraCallbackImpl( } else { - data = SensorData( + syncData_.data = SensorData( scan, rgb, depth, @@ -1560,25 +1597,31 @@ void CoreWrapper::commonMultiCameraCallbackImpl( if(!globalDescriptorMsgs.empty()) { - data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); + syncData_.data.setGlobalDescriptors(rtabmap_conversions::globalDescriptorsFromROS(globalDescriptorMsgs)); } if(!keypoints.empty()) { UASSERT(points.empty() || points.size() == keypoints.size()); UASSERT(descriptors.empty() || descriptors.rows == (int)keypoints.size()); - data.setFeatures(keypoints, points, descriptors); - } - - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); - covariance_ = cv::Mat(); + syncData_.data.setFeatures(keypoints, points, descriptors); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); } void CoreWrapper::commonLaserScanCallback( @@ -1590,7 +1633,7 @@ void CoreWrapper::commonLaserScanCallback( const rtabmap_msgs::msg::GlobalDescriptor & globalDescriptor) { UTimer timerConversion; - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1613,110 +1656,128 @@ void CoreWrapper::commonLaserScanCallback( return; } } - else if(!scan2dMsg.ranges.empty()) + else { - if(!odomTFUpdate(scan2dMsg.header.stamp)) + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!scan2dMsg.ranges.empty()) { - return; + if(!odomTFUpdate(odomFrameId, scan2dMsg.header.stamp)) + { + return; + } } - } - else if(!scan3dMsg.data.empty()) - { - if(!odomTFUpdate(scan3dMsg.header.stamp)) + else if(!scan3dMsg.data.empty()) + { + if(!odomTFUpdate(odomFrameId, scan3dMsg.header.stamp)) + { + return; + } + } + else { return; } } - else - { - return; - } - LaserScan scan; - if(!scan2dMsg.ranges.empty()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - if(!rtabmap_conversions::convertScanMsg( - scan2dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - // backward compatibility, project 2D scan in /base_link frame - rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + UScopeMutex lock(lastPoseMutex_); + LaserScan scan; + if(!scan2dMsg.ranges.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); - return; + if(!rtabmap_conversions::convertScanMsg( + scan2dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + // backward compatibility, project 2D scan in /base_link frame + rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update..."); + return; + } } - } - else if(!scan3dMsg.data.empty()) - { - if(!rtabmap_conversions::convertScan3dMsg( - scan3dMsg, - frameId_, - odomSensorSync_?odomFrameId:"", - lastPoseStamp_, - scan, - *tfBuffer_, - waitForTransform_, - scanCloudMaxPoints_, - 0, - scanCloudIs2d_)) + else if(!scan3dMsg.data.empty()) { - RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); - return; + if(!rtabmap_conversions::convertScan3dMsg( + scan3dMsg, + frameId_, + odomSensorSync_?odomFrameId:"", + lastPoseStamp_, + scan, + *tfBuffer_, + waitForTransform_, + scanCloudMaxPoints_, + 0, + scanCloudIs2d_)) + { + RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update..."); + return; + } } - } - cv::Mat userData; - if(userDataMsg.get()) - { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + cv::Mat userData; + if(userDataMsg.get()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } - } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - SensorData data( - scan, - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); + syncData_.data = SensorData( + scan, + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } - if(!globalDescriptor.data.empty()) - { - data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); - } + if(!globalDescriptor.data.empty()) + { + syncData_.data.addGlobalDescriptor(rtabmap_conversions::globalDescriptorFromROS(globalDescriptor)); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } - covariance_ = cv::Mat(); + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); + } } void CoreWrapper::commonOdomCallback( @@ -1726,56 +1787,66 @@ void CoreWrapper::commonOdomCallback( { UTimer timerConversion; UASSERT(odomMsg.get()); - std::string odomFrameId = odomFrameId_; - - odomFrameId = odomMsg->header.frame_id; + std::string odomFrameId = odomMsg->header.frame_id; if(!odomUpdate(*odomMsg, odomMsg->header.stamp)) { return; } - cv::Mat userData; - if(userDataMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - userData = rtabmap_conversions::userDataFromROS(*userDataMsg); - UScopeMutex lock(userDataMutex_); - if(!userData_.empty()) + UScopeMutex lock(lastPoseMutex_); + cv::Mat userData; + if(userDataMsg.get()) { - RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData = rtabmap_conversions::userDataFromROS(*userDataMsg); + UScopeMutex lock(userDataMutex_); + if(!userData_.empty()) + { + RCLCPP_WARN(this->get_logger(), "Synchronized and asynchronized user data topics cannot be used at the same time. Async user data dropped!"); + userData_ = cv::Mat(); + } + } + else + { + UScopeMutex lock(userDataMutex_); + userData = userData_; userData_ = cv::Mat(); } - } - else - { - UScopeMutex lock(userDataMutex_); - userData = userData_; - userData_ = cv::Mat(); - } - SensorData data( - cv::Mat(), - cv::Mat(), - rtabmap::CameraModel(), - lastPoseIntermediate_?-1:0, - rtabmap_conversions::timestampFromROS(lastPoseStamp_), - userData); + syncData_.data = SensorData( + cv::Mat(), + cv::Mat(), + rtabmap::CameraModel(), + lastPoseIntermediate_?-1:0, + rtabmap_conversions::timestampFromROS(lastPoseStamp_), + userData); - OdometryInfo odomInfo; - if(odomInfoMsg.get()) - { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); - } + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); - covariance_ = cv::Mat(); + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); + } } void CoreWrapper::commonSensorDataCallback( @@ -1785,7 +1856,7 @@ void CoreWrapper::commonSensorDataCallback( { UTimer timerConversion; UASSERT(sensorDataMsg.get()); - std::string odomFrameId = odomFrameId_; + std::string odomFrameId; if(odomMsg.get()) { odomFrameId = odomMsg->header.frame_id; @@ -1794,30 +1865,73 @@ void CoreWrapper::commonSensorDataCallback( return; } } - else if(!odomTFUpdate(sensorDataMsg->header.stamp)) + else { - return; + mapToOdomMutex_.lock(); + odomFrameId = odomFrameId_; + mapToOdomMutex_.unlock(); + if(!odomTFUpdate(odomFrameId, sensorDataMsg->header.stamp)) + { + return; + } } - SensorData data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); - data.setId(lastPoseIntermediate_?-1:0); - - OdometryInfo odomInfo; - if(odomInfoMsg.get()) + if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0) { - odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg); + UScopeMutex lock(lastPoseMutex_); + syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg); + syncData_.data.setId(lastPoseIntermediate_?-1:0); + + OdometryInfo odomInfo; + if(odomInfoMsg.get()) + { + odomInfo = rtabmap_conversions::odomInfoFromROS(*odomInfoMsg, true); + } + + syncData_.valid = true; + syncData_.stamp = lastPoseStamp_; + syncData_.odom = lastPose_; + syncData_.odomVelocity = lastPoseVelocity_; + syncData_.odomFrameId = odomFrameId; + syncData_.odomCovariance = lastPoseCovariance_; + syncData_.odomInfo = odomInfo; + syncData_.timeMsgConversion = timerConversion.ticks(); + + if(!lastPoseIntermediate_) + { + previousStamp_ = lastPoseStamp_; + } + + lastPoseCovariance_ = cv::Mat(); + + syncTimer_->reset(); + syncDataMutex_.unlock(); } +} - process(lastPoseStamp_, - data, - lastPose_, - lastPoseVelocity_, - odomFrameId, - covariance_, - odomInfo, - timerConversion.ticks()); +void CoreWrapper::processAsync() +{ + UScopeMutex lock(syncDataMutex_); + + if(triggerNewMapBeforeNextUpdate_) + { + rtabmap_.triggerNewMap(); + triggerNewMapBeforeNextUpdate_ = false; + } - covariance_ = cv::Mat(); + if(syncData_.valid) + { + process(syncData_.stamp, + syncData_.data, + syncData_.odom, + syncData_.odomVelocity, + syncData_.odomFrameId, + syncData_.odomCovariance, + syncData_.odomInfo, + syncData_.timeMsgConversion); + syncData_.valid=false; + } + syncTimer_->cancel(); } void CoreWrapper::process( @@ -1836,7 +1950,7 @@ void CoreWrapper::process( // Add intermediate nodes? for(std::list >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();) { - if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < lastPoseStamp_) + if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp) { Transform interOdom; if(!rtabmap_.getLocalOptimizedPoses().empty()) @@ -1925,7 +2039,7 @@ void CoreWrapper::process( } interOdoms_.erase(iter++); } - else if(iter->first.header.stamp == lastPoseStamp_) + else if(iter->first.header.stamp == stamp) { interOdoms_.erase(iter++); break; @@ -1940,7 +2054,7 @@ void CoreWrapper::process( Transform groundTruthPose; if(!groundTruthFrameId_.empty()) { - groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, lastPoseStamp_, *tfBuffer_, waitForTransform_); + groundTruthPose = rtabmap_conversions::getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp, *tfBuffer_, waitForTransform_); } data.setGroundTruth(groundTruthPose); @@ -1951,7 +2065,7 @@ void CoreWrapper::process( Transform sensorToBase = rtabmap_conversions::getTransform( globalPose_.header.frame_id, frameId_, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_); if(!sensorToBase.isNull()) @@ -1963,7 +2077,7 @@ void CoreWrapper::process( Transform correction = rtabmap_conversions::getMovingTransform( frameId_, odomFrameId, - lastPoseStamp_, + stamp, rclcpp::Time(globalPose_.header.stamp.sec, globalPose_.header.stamp.nanosec), *tfBuffer_, waitForTransform_); @@ -1990,27 +2104,31 @@ void CoreWrapper::process( gps_ = rtabmap::GPS(); //tag detections + landmarksMutex_.lock(); Landmarks landmarks = rtabmap_conversions::landmarksFromROS( landmarks_, frameId_, odomFrameId, - lastPoseStamp_, + stamp, *tfBuffer_, waitForTransform_, landmarkDefaultLinVariance_, landmarkDefaultAngVariance_); landmarks_.clear(); + landmarksMutex_.unlock(); if(!landmarks.empty()) { data.setLandmarks(landmarks); } // IMU + imuMutex_.lock(); if(!imus_.empty()) { Transform t = Transform::getTransform(imus_, data.stamp()); if(!t.isNull()) { + imuMutex_.unlock(); // get local transform rtabmap::Transform localTransform; if(frameId_.compare(imuFrameId_) != 0) @@ -2034,10 +2152,15 @@ void CoreWrapper::process( else { RCLCPP_WARN(this->get_logger(), "We are receiving imu data (buffer=%d), but cannot interpolate " - "imu transform at time %f. IMU won't be added to graph.", - (int)imus_.size(), data.stamp()); + "imu transform at time %f (latest imu received with stamp %f). IMU won't be added to graph.", + (int)imus_.size(), data.stamp(), imus_.rbegin()->first); + imuMutex_.unlock(); } } + else + { + imuMutex_.unlock(); + } double timeRtabmap = 0.0; double timeUpdateMaps = 0.0; @@ -2103,7 +2226,7 @@ void CoreWrapper::process( timeRtabmap = timer.ticks(); mapToOdomMutex_.lock(); mapToOdom_ = rtabmap_.getMapCorrection(); - + Transform mapToOdomSafe = mapToOdom_.clone(); if(!odomFrameId.empty() && !odomFrameId_.empty() && odomFrameId_.compare(odomFrameId)!=0) { RCLCPP_ERROR(get_logger(), "Odometry received doesn't have same frame_id " @@ -2132,7 +2255,7 @@ void CoreWrapper::process( geometry_msgs::msg::PoseWithCovarianceStamped poseMsg; poseMsg.header.frame_id = mapFrameId_; poseMsg.header.stamp = stamp; - rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); + rtabmap_conversions::transformToPoseMsg(mapToOdomSafe*odom, poseMsg.pose.pose); if(!rtabmap_.getStatistics().localizationCovariance().empty()) { const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance(); @@ -2165,12 +2288,12 @@ void CoreWrapper::process( SensorData tmpData = data; tmpData.setId(0); tmpSignature.insert(std::make_pair(0, Signature(0, -1, 0, data.stamp(), "", odom, Transform(), tmpData))); - filteredPoses.insert(std::make_pair(0, mapToOdom_*odom)); + filteredPoses.insert(std::make_pair(0, mapToOdomSafe*odom)); } if((mappingMaxNodes_ > 0 || mappingAltitudeDelta_>0.0) && filteredPoses.size()>1) { - std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdom_*odom); + std::map nearestPoses = filterNodesToAssemble(filteredPoses, mapToOdomSafe*odom); //add latest/zero and make sure those on a planned path are not filtered std::set onPath; if(rtabmap_.getPath().size()) @@ -2324,7 +2447,7 @@ void CoreWrapper::process( timeRtabmap, timeUpdateMaps, timePublishMaps, - (now() - lastPoseStamp_).seconds(), + (now() - stamp).seconds(), (int)rtabmap_.getLocalOptimizedPoses().size(), rtabmap_.getWMSize()+rtabmap_.getSTMSize()); rtabmapROSStats_.insert(std::make_pair(std::string("RtabmapROS/HasSubscribers/"), mapsManager_.hasSubscribers()?1:0)); @@ -2431,6 +2554,7 @@ void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::Landma geometry_msgs::msg::PoseWithCovarianceStamped p; p.header = landmarkDetection->header; p.pose = landmarkDetection->pose; + UScopeMutex lock(landmarksMutex_); uInsert(landmarks_, std::make_pair(landmarkDetection->id, std::make_pair(p, landmarkDetection->size))); @@ -2441,6 +2565,7 @@ void CoreWrapper::landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::Landm { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; ilandmarks.size(); ++i) { geometry_msgs::msg::PoseWithCovarianceStamped p; @@ -2458,6 +2583,7 @@ void CoreWrapper::tagDetectionsAsyncCallback(const apriltag_msgs::msg::AprilTagD { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; idetections.size(); ++i) { std::string tagFrameId = tagDetections->detections[i].family+":"+uNumber2Str(tagDetections->detections[i].id); @@ -2493,6 +2619,7 @@ void CoreWrapper::fiducialDetectionsAsyncCallback(const fiducial_msgs::msg::Fidu { if(!paused_) { + UScopeMutex lock(landmarksMutex_); for(unsigned int i=0; iorientation.x, msg->orientation.y, msg->orientation.z, msg->orientation.w); imus_.insert(std::make_pair(rtabmap_conversions::timestampFromROS(msg->header.stamp), orientation)); if(imus_.size() > 1000) @@ -2829,10 +2957,15 @@ void CoreWrapper::resetRtabmapCallback( { RCLCPP_INFO(this->get_logger(), "rtabmap: Reset"); rtabmap_.resetMemory(); - covariance_ = cv::Mat(); + + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2842,12 +2975,16 @@ void CoreWrapper::resetRtabmapCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -2923,10 +3060,14 @@ void CoreWrapper::loadDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(get_logger(), "LoadDatabase: Saving current map (%s, %ld MB)... done!", databasePath_.c_str(), UFile::length(databasePath_)/(1024*1024)); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -2936,12 +3077,16 @@ void CoreWrapper::loadDatabaseCallback( previousStamp_ = rclcpp::Time(0); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); userDataMutex_.lock(); userData_ = cv::Mat(); userDataMutex_.unlock(); + imuMutex_.lock(); imus_.clear(); imuFrameId_.clear(); + imuMutex_.unlock(); interOdoms_.clear(); mapToOdomMutex_.lock(); mapToOdom_.setIdentity(); @@ -3050,9 +3195,14 @@ void CoreWrapper::backupDatabaseCallback( rtabmap_.close(); RCLCPP_INFO(this->get_logger(), "Backup: Saving memory... done!"); - covariance_ = cv::Mat(); + lastPoseMutex_.lock(); + lastPoseCovariance_ = cv::Mat(); lastPose_.setIdentity(); + lastPoseStamp_ = rclcpp::Time(); lastPoseVelocity_.clear(); + lastPoseIntermediate_ = false; + lastPoseMutex_.unlock(); + currentMetricGoal_.setNull(); lastPublishedMetricGoal_.setNull(); goalFrameId_.clear(); @@ -3063,7 +3213,9 @@ void CoreWrapper::backupDatabaseCallback( userDataMutex_.unlock(); globalPose_.header.stamp = rclcpp::Time(0); gps_ = rtabmap::GPS(); + landmarksMutex_.lock(); landmarks_.clear(); + landmarksMutex_.unlock(); RCLCPP_INFO(this->get_logger(), "Backup: Saving \"%s\" to \"%s\"...", databasePath_.c_str(), (databasePath_+".back").c_str()); UFile::copy(databasePath_, databasePath_+".back"); @@ -3388,11 +3540,15 @@ void CoreWrapper::getMapDataCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3432,11 +3588,15 @@ void CoreWrapper::getMapData2Callback( req->with_words, req->with_global_descriptors); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + //RGB-D SLAM data rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, res->data); res->data.header.stamp = now(); @@ -3555,6 +3715,10 @@ void CoreWrapper::publishMapCallback( !req->graph_only, !req->graph_only); + mapToOdomMutex_.lock(); + Transform mapToOdomSafe = mapToOdom_.clone(); + mapToOdomMutex_.unlock(); + if(mapDataPub_->get_subscription_count()) { rtabmap_msgs::msg::MapData::UniquePtr msg(new rtabmap_msgs::msg::MapData); @@ -3564,7 +3728,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapDataToROS(poses, constraints, signatures, - mapToOdom_, + mapToOdomSafe, *msg); mapDataPub_->publish(std::move(msg)); @@ -3578,7 +3742,7 @@ void CoreWrapper::publishMapCallback( rtabmap_conversions::mapGraphToROS(poses, constraints, - mapToOdom_, + mapToOdomSafe, *msg); mapGraphPub_->publish(std::move(msg)); diff --git a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h index f8c58a691..20e5f2b4e 100644 --- a/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h +++ b/rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h @@ -117,26 +117,27 @@ class CommonDataSubscriber { const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg) = 0; - void commonSingleCameraCallback( - const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, - const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, - const cv_bridge::CvImageConstPtr & imageMsg, - const cv_bridge::CvImageConstPtr & depthMsg, - const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, - const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, - const sensor_msgs::msg::LaserScan & scanMsg, - const sensor_msgs::msg::PointCloud2 & scan3dMsg, - const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, - const std::vector & globalDescriptorMsgs = std::vector(), - const std::vector & localKeyPoints = std::vector(), - const std::vector & localPoints3d = std::vector(), - const cv::Mat & localDescriptors = cv::Mat()); - void tick(const rclcpp::Time & stamp, double targetFrequency = 0); private: + void commonSingleCameraCallback( + const nav_msgs::msg::Odometry::ConstSharedPtr & odomMsg, + const rtabmap_msgs::msg::UserData::ConstSharedPtr & userDataMsg, + const cv_bridge::CvImageConstPtr & imageMsg, + const cv_bridge::CvImageConstPtr & depthMsg, + const sensor_msgs::msg::CameraInfo & rgbCameraInfoMsg, + const sensor_msgs::msg::CameraInfo & depthCameraInfoMsg, + const sensor_msgs::msg::LaserScan & scanMsg, + const sensor_msgs::msg::PointCloud2 & scan3dMsg, + const rtabmap_msgs::msg::OdomInfo::ConstSharedPtr& odomInfoMsg, + const std::vector & globalDescriptorMsgs = std::vector(), + const std::vector & localKeyPoints = std::vector(), + const std::vector & localPoints3d = std::vector(), + const cv::Mat & localDescriptors = cv::Mat()); + void processSyncData(); void setupDepthCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -145,10 +146,12 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupStereoCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo); void setupRGBCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -157,6 +160,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBDCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -165,6 +169,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBDXCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -174,6 +179,7 @@ class CommonDataSubscriber { #ifdef RTABMAP_SYNC_MULTI_RGBD void setupRGBD2Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -182,6 +188,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBD3Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -190,6 +197,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBD4Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -198,6 +206,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBD5Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -206,6 +215,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupRGBD6Callbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeUserData, bool subscribeScan2d, @@ -215,10 +225,12 @@ class CommonDataSubscriber { #endif void setupSensorDataCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo); void setupScanCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeScan2d, bool subscribeScanDesc, bool subscribeOdom, @@ -226,6 +238,7 @@ class CommonDataSubscriber { bool subscribeOdomInfo); void setupOdomCallbacks( rclcpp::Node & node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, bool subscribeOdomInfo); @@ -257,6 +270,8 @@ class CommonDataSubscriber { int rgbdCameras_; std::string name_; + rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_; + //for depth and rgb-only callbacks image_transport::SubscriberFilter imageSub_; image_transport::SubscriberFilter imageDepthSub_; diff --git a/rtabmap_sync/src/CommonDataSubscriber.cpp b/rtabmap_sync/src/CommonDataSubscriber.cpp index 746510a5a..c714e9305 100644 --- a/rtabmap_sync/src/CommonDataSubscriber.cpp +++ b/rtabmap_sync/src/CommonDataSubscriber.cpp @@ -363,6 +363,8 @@ CommonDataSubscriber::CommonDataSubscriber(rclcpp::Node & node, bool gui) : { name_ = node.get_name(); + syncCallbackGroup_ = node.create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + // ROS related parameters (private) // ros2: should be declared in the constructor to be used by inherited classes in their constructor subscribedToDepth_ = node.declare_parameter("subscribe_depth", subscribedToDepth_); @@ -539,11 +541,15 @@ void CommonDataSubscriber::setupCallbacks( RCLCPP_INFO(node.get_logger(), "%s: qos_user_data = %d", name_.c_str(), qosUserData_); RCLCPP_INFO(node.get_logger(), "%s: approx_sync = %s", name_.c_str(), approxSync_?"true":"false"); + rclcpp::SubscriptionOptions callbackOptions; + callbackOptions.callback_group = syncCallbackGroup_; + subscribedToOdom_ = odomFrameId_.empty() && subscribedToOdom_; if(subscribedToDepth_) { setupDepthCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -555,6 +561,7 @@ void CommonDataSubscriber::setupCallbacks( { setupStereoCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToOdomInfo_); } @@ -562,6 +569,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -583,6 +591,7 @@ void CommonDataSubscriber::setupCallbacks( setupRGBD6Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -594,6 +603,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD5Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -605,6 +615,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD4Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -616,6 +627,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD3Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -627,6 +639,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBD2Callbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -647,6 +660,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBDXCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -658,6 +672,7 @@ void CommonDataSubscriber::setupCallbacks( { setupRGBDCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToUserData_, subscribedToScan2d_, @@ -670,6 +685,7 @@ void CommonDataSubscriber::setupCallbacks( { setupScanCallbacks( node, + callbackOptions, subscribedToScan2d_, subscribedToScanDescriptor_, subscribedToOdom_, @@ -680,6 +696,7 @@ void CommonDataSubscriber::setupCallbacks( { setupSensorDataCallbacks( node, + callbackOptions, subscribedToOdom_, subscribedToOdomInfo_); } @@ -687,6 +704,7 @@ void CommonDataSubscriber::setupCallbacks( { setupOdomCallbacks( node, + callbackOptions, subscribedToUserData_, subscribedToOdomInfo_); } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp index 62782fea6..30046ed05 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberDepth.cpp @@ -457,6 +457,7 @@ void CommonDataSubscriber::depthOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupDepthCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -471,24 +472,24 @@ void CommonDataSubscriber::setupDepthCallbacks( RCLCPP_INFO(node.get_logger(), "Setup depth callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageDepthSub_.subscribe(&node, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -499,11 +500,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -514,11 +515,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, depthOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -529,7 +530,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -541,16 +542,16 @@ void CommonDataSubscriber::setupDepthCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -561,11 +562,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -576,11 +577,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -591,7 +592,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -602,17 +603,17 @@ void CommonDataSubscriber::setupDepthCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -623,12 +624,12 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -639,11 +640,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, depthDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -654,7 +655,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else @@ -668,11 +669,11 @@ void CommonDataSubscriber::setupDepthCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScanDescInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -683,11 +684,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScan2dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -698,11 +699,11 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, depthScan3dInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -713,7 +714,7 @@ void CommonDataSubscriber::setupDepthCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, depthInfo, approxSync_, syncQueueSize_, imageSub_, imageDepthSub_, cameraInfoSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp index 573b49cef..6edc2f497 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberOdom.cpp @@ -65,6 +65,7 @@ void CommonDataSubscriber::odomDataInfoCallback( void CommonDataSubscriber::setupOdomCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeUserData, bool subscribeOdomInfo) { @@ -72,16 +73,16 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeUserData || subscribeOdomInfo) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, odomInfoSub_); } else @@ -94,7 +95,7 @@ void CommonDataSubscriber::setupOdomCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, odomInfo, approxSync_, syncQueueSize_, odomSub_, odomInfoSub_); } } diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp index e21b80ae6..f6a02e26b 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGB.cpp @@ -457,6 +457,7 @@ void CommonDataSubscriber::rgbOdomDataScanDescInfoCallback( void CommonDataSubscriber::setupRGBCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -471,23 +472,23 @@ void CommonDataSubscriber::setupRGBCallbacks( RCLCPP_INFO(node.get_logger(), "Setup rgb-only callback"); image_transport::TransportHints hints(&node); - imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageSub_.subscribe(&node, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoSub_.subscribe(&node, "rgb/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -498,11 +499,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -513,11 +514,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbOdomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -528,7 +529,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -540,16 +541,16 @@ void CommonDataSubscriber::setupRGBCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -560,11 +561,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -575,11 +576,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbOdomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -590,7 +591,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -601,17 +602,17 @@ void CommonDataSubscriber::setupRGBCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -622,12 +623,12 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -638,11 +639,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbDataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -653,7 +654,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbDataInfo, approxSync_, syncQueueSize_, userDataSub_, imageSub_, cameraInfoSub_, odomInfoSub_); } else @@ -667,11 +668,11 @@ void CommonDataSubscriber::setupRGBCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScanDescInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanDescSub_, odomInfoSub_); } else @@ -682,11 +683,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScan2dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scanSub_, odomInfoSub_); } else @@ -697,11 +698,11 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbScan3dInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, scan3dSub_, odomInfoSub_); } else @@ -712,7 +713,7 @@ void CommonDataSubscriber::setupRGBCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbInfo, approxSync_, syncQueueSize_, imageSub_, cameraInfoSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp index 7cd6eb23c..6e81e8278 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD.cpp @@ -531,6 +531,7 @@ void CommonDataSubscriber::rgbdOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -555,17 +556,17 @@ void CommonDataSubscriber::setupRGBDCallbacks( { rgbdSubs_.resize(1); rgbdSubs_[0] = new message_filters::Subscriber; - rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[0]->subscribe(&node, "rgbd_image", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -576,7 +577,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -587,7 +588,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -598,7 +599,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbdOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -610,11 +611,11 @@ void CommonDataSubscriber::setupRGBDCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -625,7 +626,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -636,7 +637,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -647,7 +648,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdOdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -658,11 +659,11 @@ void CommonDataSubscriber::setupRGBDCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -673,7 +674,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -684,7 +685,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -695,7 +696,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdDataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), odomInfoSub_); } else @@ -709,7 +710,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -720,7 +721,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -731,7 +732,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -742,7 +743,7 @@ void CommonDataSubscriber::setupRGBDCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, rgbdInfo, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp index fe7e1a5b3..5f708f434 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD2.cpp @@ -343,6 +343,7 @@ void CommonDataSubscriber::rgbd2OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD2Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -360,17 +361,17 @@ void CommonDataSubscriber::setupRGBD2Callbacks( for(int i=0; i<2; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -381,7 +382,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -392,7 +393,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -403,7 +404,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd2OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -415,11 +416,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -430,7 +431,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -441,7 +442,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -452,7 +453,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd2OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -463,11 +464,11 @@ void CommonDataSubscriber::setupRGBD2Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -478,7 +479,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -489,7 +490,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -500,7 +501,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd2DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else @@ -514,7 +515,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -525,7 +526,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -536,7 +537,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -547,7 +548,7 @@ void CommonDataSubscriber::setupRGBD2Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbd2Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp index 811167642..e175357fc 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD3.cpp @@ -431,6 +431,7 @@ void CommonDataSubscriber::rgbd3OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD3Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -448,17 +449,17 @@ void CommonDataSubscriber::setupRGBD3Callbacks( for(int i=0; i<3; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -469,7 +470,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -480,7 +481,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -491,7 +492,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd3OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -503,11 +504,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -518,7 +519,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -529,7 +530,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -540,7 +541,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd3OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -551,11 +552,11 @@ void CommonDataSubscriber::setupRGBD3Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -566,7 +567,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -577,7 +578,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -588,7 +589,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd3DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else @@ -602,7 +603,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( if(subscribeScanDescriptor) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -613,7 +614,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -624,7 +625,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; @@ -636,7 +637,7 @@ void CommonDataSubscriber::setupRGBD3Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbd3Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp index 6c2768094..edd5704f3 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD4.cpp @@ -400,6 +400,7 @@ void CommonDataSubscriber::rgbd4OdomDataInfoCallback( void CommonDataSubscriber::setupRGBD4Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -417,17 +418,17 @@ void CommonDataSubscriber::setupRGBD4Callbacks( for(int i=0; i<4; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -438,7 +439,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -449,7 +450,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -460,7 +461,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd4OdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -472,11 +473,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -487,7 +488,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -498,7 +499,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -509,7 +510,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd4OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -520,11 +521,11 @@ void CommonDataSubscriber::setupRGBD4Callbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -535,7 +536,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -546,7 +547,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -557,7 +558,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd4DataInfo, approxSync_, syncQueueSize_, userDataSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else @@ -571,7 +572,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -582,7 +583,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -593,7 +594,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -604,7 +605,7 @@ void CommonDataSubscriber::setupRGBD4Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, rgbd4Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp index 16befb5a6..e14dd9156 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD5.cpp @@ -256,6 +256,7 @@ void CommonDataSubscriber::rgbd5OdomInfoCallback( void CommonDataSubscriber::setupRGBD5Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, @@ -269,15 +270,15 @@ void CommonDataSubscriber::setupRGBD5Callbacks( for(int i=0; i<5; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -288,7 +289,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -299,7 +300,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -310,7 +311,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd5OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else @@ -323,7 +324,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -334,7 +335,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -345,7 +346,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -356,7 +357,7 @@ void CommonDataSubscriber::setupRGBD5Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, rgbd5Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp index a5cd0b8cb..320cdfb29 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBD6.cpp @@ -274,6 +274,7 @@ void CommonDataSubscriber::rgbd6OdomInfoCallback( void CommonDataSubscriber::setupRGBD6Callbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool /*subscribeUserData*/, bool subscribeScan2d, @@ -287,15 +288,15 @@ void CommonDataSubscriber::setupRGBD6Callbacks( for(int i=0; i<6; ++i) { rgbdSubs_[i] = new message_filters::Subscriber; - rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdSubs_[i]->subscribe(&node, uFormat("rgbd_image%d", i), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); } if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -306,7 +307,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -317,7 +318,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -328,7 +329,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL8(CommonDataSubscriber, rgbd6OdomInfo, approxSync_, syncQueueSize_, odomSub_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else @@ -341,7 +342,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -352,7 +353,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -363,7 +364,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -374,7 +375,7 @@ void CommonDataSubscriber::setupRGBD6Callbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL7(CommonDataSubscriber, rgbd6Info, approxSync_, syncQueueSize_, (*rgbdSubs_[0]), (*rgbdSubs_[1]), (*rgbdSubs_[2]), (*rgbdSubs_[3]), (*rgbdSubs_[4]), (*rgbdSubs_[5]), odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp index 66af728ca..02affd032 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberRGBDX.cpp @@ -319,6 +319,7 @@ void CommonDataSubscriber::rgbdXOdomDataInfoCallback( void CommonDataSubscriber::setupRGBDXCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, #ifdef RTABMAP_SYNC_USER_DATA bool subscribeUserData, @@ -332,16 +333,16 @@ void CommonDataSubscriber::setupRGBDXCallbacks( { RCLCPP_INFO(node.get_logger(), "Setup rgbdX callback"); - rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); + rgbdXSub_.subscribe(&node, "rgbd_images", rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -352,7 +353,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -363,7 +364,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -374,7 +375,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, rgbdXOdomDataInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, rgbdXSub_, odomInfoSub_); } else @@ -386,11 +387,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -401,7 +402,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -412,7 +413,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -423,7 +424,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdXOdomInfo, approxSync_, syncQueueSize_, odomSub_, rgbdXSub_, odomInfoSub_); } else @@ -434,11 +435,11 @@ void CommonDataSubscriber::setupRGBDXCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -449,7 +450,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -460,7 +461,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -471,7 +472,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, rgbdXDataInfo, approxSync_, syncQueueSize_, userDataSub_, rgbdXSub_, odomInfoSub_); } else @@ -485,7 +486,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( if(subscribeScanDesc) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -496,7 +497,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan2d) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -507,7 +508,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeScan3d) { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = false; @@ -518,7 +519,7 @@ void CommonDataSubscriber::setupRGBDXCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, rgbdXInfo, approxSync_, syncQueueSize_, rgbdXSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp index cf8dd7587..8f8cd3905 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberScan.cpp @@ -245,6 +245,7 @@ void CommonDataSubscriber::odomDataScanDescInfoCallback( void CommonDataSubscriber::setupScanCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool scan2dTopic, bool scanDescTopic, bool subscribeOdom, @@ -266,31 +267,31 @@ void CommonDataSubscriber::setupScanCallbacks( if(scanDescTopic) { subscribedToScanDescriptor_ = true; - scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanDescSub_.subscribe(&node, "scan_descriptor", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else if(scan2dTopic) { subscribedToScan2d_ = true; - scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scanSub_.subscribe(&node, "scan", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } else { subscribedToScan3d_ = true; - scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile()); + scan3dSub_.subscribe(&node, "scan_cloud", rclcpp::QoS(topicQueueSize_).reliability(qosScan_).get_rmw_qos_profile(), options); } #ifdef RTABMAP_SYNC_USER_DATA if(subscribeOdom && subscribeUserData) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScanDescInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanDescSub_, odomInfoSub_); } else @@ -303,7 +304,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScan2dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scanSub_, odomInfoSub_); } else @@ -316,7 +317,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL4(CommonDataSubscriber, odomDataScan3dInfo, approxSync_, syncQueueSize_, odomSub_, userDataSub_, scan3dSub_, odomInfoSub_); } else @@ -329,14 +330,14 @@ void CommonDataSubscriber::setupScanCallbacks( #endif if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScanDescInfo, approxSync_, syncQueueSize_, odomSub_, scanDescSub_, odomInfoSub_); } else @@ -349,7 +350,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScan2dInfo, approxSync_, syncQueueSize_, odomSub_, scanSub_, odomInfoSub_); } else @@ -362,7 +363,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, odomScan3dInfo, approxSync_, syncQueueSize_, odomSub_, scan3dSub_, odomInfoSub_); } else @@ -374,14 +375,14 @@ void CommonDataSubscriber::setupScanCallbacks( #ifdef RTABMAP_SYNC_USER_DATA else if(subscribeUserData) { - userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile()); + userDataSub_.subscribe(&node, "user_data", rclcpp::QoS(topicQueueSize_).reliability(qosUserData_).get_rmw_qos_profile(), options); if(scanDescTopic) { if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScanDescInfo, approxSync_, syncQueueSize_, userDataSub_, scanDescSub_, odomInfoSub_); } else @@ -394,7 +395,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScan2dInfo, approxSync_, syncQueueSize_, userDataSub_, scanSub_, odomInfoSub_); } else @@ -407,7 +408,7 @@ void CommonDataSubscriber::setupScanCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, dataScan3dInfo, approxSync_, syncQueueSize_, userDataSub_, scan3dSub_, odomInfoSub_); } else @@ -420,7 +421,7 @@ void CommonDataSubscriber::setupScanCallbacks( else if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(scanDescTopic) { SYNC_DECL2(CommonDataSubscriber, scanDescInfo, approxSync_, syncQueueSize_, scanDescSub_, odomInfoSub_); diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp index 5c537a512..fff29579c 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberSensorData.cpp @@ -65,19 +65,20 @@ void CommonDataSubscriber::sensorDataOdomInfoCallback( void CommonDataSubscriber::setupSensorDataCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup SensorData callback"); - sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile()); + sensorDataSub_.subscribe(&node, "sensor_data", rclcpp::QoS(topicQueueSize_).reliability(qosSensorData_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL3(CommonDataSubscriber, sensorDataOdomInfo, approxSync_, syncQueueSize_, odomSub_, sensorDataSub_, odomInfoSub_); } else @@ -90,7 +91,7 @@ void CommonDataSubscriber::setupSensorDataCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL2(CommonDataSubscriber, sensorDataInfo, approxSync_, syncQueueSize_, sensorDataSub_, odomInfoSub_); } else diff --git a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp index 2832ca247..13ced404e 100644 --- a/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp +++ b/rtabmap_sync/src/impl/CommonDataSubscriberStereo.cpp @@ -87,25 +87,26 @@ void CommonDataSubscriber::stereoOdomInfoCallback( void CommonDataSubscriber::setupStereoCallbacks( rclcpp::Node& node, + const rclcpp::SubscriptionOptions & options, bool subscribeOdom, bool subscribeOdomInfo) { RCLCPP_INFO(node.get_logger(), "Setup stereo callback"); image_transport::TransportHints hints(&node); - imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile()); - cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); - cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile()); + imageRectLeft_.subscribe(&node, "left/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + imageRectRight_.subscribe(&node, "right/image_rect", hints.getTransport(), rclcpp::QoS(topicQueueSize_).reliability(qosImage_).get_rmw_qos_profile(), options); + cameraInfoLeft_.subscribe(&node, "left/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); + cameraInfoRight_.subscribe(&node, "right/camera_info", rclcpp::QoS(topicQueueSize_).reliability(qosCameraInfo_).get_rmw_qos_profile(), options); if(subscribeOdom) { - odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomSub_.subscribe(&node, "odom", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL6(CommonDataSubscriber, stereoOdomInfo, approxSync_, syncQueueSize_, odomSub_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else @@ -118,7 +119,7 @@ void CommonDataSubscriber::setupStereoCallbacks( if(subscribeOdomInfo) { subscribedToOdomInfo_ = true; - odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile()); + odomInfoSub_.subscribe(&node, "odom_info", rclcpp::QoS(topicQueueSize_).reliability(qosOdom_).get_rmw_qos_profile(), options); SYNC_DECL5(CommonDataSubscriber, stereoInfo, approxSync_, syncQueueSize_, imageRectLeft_, imageRectRight_, cameraInfoLeft_, cameraInfoRight_, odomInfoSub_); } else