i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37: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_conversions/src/MsgConversion.cpp
+13 -12
View File
@@ -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<msg.local_bundle_ids.size(); ++i)
{
std::vector<rtabmap::CameraModel> models;
for(size_t j=0; j<msg.local_bundle_models[i].models.size(); ++j)
{
models.push_back(cameraModelFromROS(msg.local_bundle_models[i].models[j].camera_info, transformFromGeometryMsg(msg.local_bundle_models[i].models[j].local_transform)));
}
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
}
info.keyFrameAdded = msg.key_frame_added;
info.timeEstimation = msg.time_estimation;
info.timeParticleFiltering = msg.time_particle_filtering;
@@ -1720,6 +1708,19 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_msgs::msg::OdomInfo & msg, b
if(!ignoreData)
{
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<msg.local_bundle_ids.size(); ++i)
{
std::vector<rtabmap::CameraModel> models;
for(size_t j=0; j<msg.local_bundle_models[i].models.size(); ++j)
{
models.push_back(cameraModelFromROS(msg.local_bundle_models[i].models[j].camera_info, transformFromGeometryMsg(msg.local_bundle_models[i].models[j].local_transform)));
}
info.localBundleModels.insert(std::make_pair(msg.local_bundle_ids[i], models));
info.localBundlePoses.insert(std::make_pair(msg.local_bundle_ids[i], transformFromPoseMsg(msg.local_bundle_poses[i])));
}
UASSERT(msg.words_keys.size() == msg.words_values.size());
for(unsigned int i=0; i<msg.words_keys.size(); ++i)
{