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

Multithreaded rtabmap node (#1214)

Browse Source 
* 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
This commit is contained in:
matlabbe
2024-10-15 21:00:35 -07:00
committed by GitHub
parent 1950b95bf5
commit ad97f56d0b
19 changed files with 785 additions and 537 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_sync/include/rtabmap_sync/CommonDataSubscriber.h
+30 -15
View File
@@ -117,26 +117,27 @@ protected:
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<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
const cv::Mat & localDescriptors = cv::Mat());
void tick(const rclcpp::Time & stamp, double targetFrequency = 0);
private:
void 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<rtabmap_msgs::msg::GlobalDescriptor> & globalDescriptorMsgs = std::vector<rtabmap_msgs::msg::GlobalDescriptor>(),
const std::vector<rtabmap_msgs::msg::KeyPoint> & localKeyPoints = std::vector<rtabmap_msgs::msg::KeyPoint>(),
const std::vector<rtabmap_msgs::msg::Point3f> & localPoints3d = std::vector<rtabmap_msgs::msg::Point3f>(),
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 @@ private:
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 @@ private:
bool subscribeOdomInfo);
void setupRGBDCallbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -165,6 +169,7 @@ private:
bool subscribeOdomInfo);
void setupRGBDXCallbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -174,6 +179,7 @@ private:
#ifdef RTABMAP_SYNC_MULTI_RGBD
void setupRGBD2Callbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -182,6 +188,7 @@ private:
bool subscribeOdomInfo);
void setupRGBD3Callbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -190,6 +197,7 @@ private:
bool subscribeOdomInfo);
void setupRGBD4Callbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -198,6 +206,7 @@ private:
bool subscribeOdomInfo);
void setupRGBD5Callbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -206,6 +215,7 @@ private:
bool subscribeOdomInfo);
void setupRGBD6Callbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeOdom,
bool subscribeUserData,
bool subscribeScan2d,
@@ -215,10 +225,12 @@ private:
#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 @@ private:
bool subscribeOdomInfo);
void setupOdomCallbacks(
rclcpp::Node & node,
const rclcpp::SubscriptionOptions & options,
bool subscribeUserData,
bool subscribeOdomInfo);
@@ -257,6 +270,8 @@ private:
int rgbdCameras_;
std::string name_;
rclcpp::CallbackGroup::SharedPtr syncCallbackGroup_;
//for depth and rgb-only callbacks
image_transport::SubscriberFilter imageSub_;
image_transport::SubscriberFilter imageDepthSub_;