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

rtabmap_slam tests and doc (#1460)

Browse Source 
* rtabmap_slam tests and doc

* another round of review of the doc

* Disable by default use_intra_process_comms on latched/transient publishers

* Added test to catch not unlocked mutex from early error exit

* updated coverage settings

* Using UScopeMutex on all tryLock()
This commit is contained in:
matlabbe
2026-09-28 13:09:51 -07:00
committed by GitHub
parent 853a434fe9
commit 5207dab7c2
30 changed files with 5832 additions and 118 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_slam/src/CoreWrapper.cpp
+45 -21
View File
@@ -139,7 +139,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
tfThreadRunning_(false),
interOdomSync_(0),
stereoToDepth_(false),
odomSensorSync_(false),
odomSensorSync_(true),
rate_(Parameters::defaultRtabmapDetectionRate()),
createIntermediateNodes_(Parameters::defaultRtabmapCreateIntermediateNodes()),
mappingMaxNodes_(Parameters::defaultGridGlobalMaxNodes()),
@@ -232,6 +232,8 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
stereoToDepth_ = this->declare_parameter("stereo_to_depth", stereoToDepth_);
odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
bool interOdomInfo = false;
interOdomInfo = this->declare_parameter("subscribe_inter_odom_info", interOdomInfo);
RCLCPP_INFO(this->get_logger(), "rtabmap: frame_id = \"%s\"", frameId_.c_str());
RCLCPP_INFO(this->get_logger(), "rtabmap: odom_frame_id = \"%s\"", odomFrameId_.c_str());
@@ -289,7 +291,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
infoPub_ = this->create_publisher<rtabmap_msgs::msg::Info>("info", 1);
mapDataPub_ = this->create_publisher<rtabmap_msgs::msg::MapData>("mapData", 1);
mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
// Intra-process communication doesn't support transient local durability: when latching,
// disable it on this publisher, otherwise keep the node's setting.
rclcpp::PublisherOptions latchedPubOptions;
if(mapsManager_.isLatching())
{
latchedPubOptions.use_intra_process_comm = rclcpp::IntraProcessSetting::Disable;
}
mapGraphPub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapGraph", rclcpp::QoS(1).reliable().durability(mapsManager_.isLatching()?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE), latchedPubOptions);
odomCachePub_ = this->create_publisher<rtabmap_msgs::msg::MapGraph>("mapOdomCache", 1);
landmarksPub_ = this->create_publisher<geometry_msgs::msg::PoseArray>("landmarks", 1);
labelsPub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("labels", 1);
@@ -418,11 +427,12 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
iter!=Parameters::getRemovedParameters().end();
++iter)
{
// Old names are never declared, so they can only be found among the overrides.
std::string paramValue;
rclcpp::Parameter parameter;
if(get_parameter(iter->first, parameter))
std::map<std::string, rclcpp::ParameterValue>::const_iterator oter = overrides.find(iter->first);
if(oter != overrides.end() && oter->second.get_type() == rclcpp::ParameterType::PARAMETER_STRING)
{
paramValue = parameter.as_string();
paramValue = oter->second.get<std::string>();
}
if(!paramValue.empty())
{
@@ -563,8 +573,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "Create intermediate nodes");
if(rate_ == 0.0f)
{
bool interOdomInfo = false;
if(get_parameter("subscribe_inter_odom_info", interOdomInfo))
if(interOdomInfo)
{
RCLCPP_INFO(this->get_logger(), "Subscribe to inter odom + info messages");
interOdomSync_ = new message_filters::Synchronizer<MyExactInterOdomSyncPolicy>(MyExactInterOdomSyncPolicy(100), interOdomSyncSub_, interOdomInfoSyncSub_);
@@ -808,13 +817,14 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
if(modifiedParameters.find(Parameters::kRGBDProximityPathMaxNeighbors()) == modifiedParameters.end())
{
if(this->isSubscribedToScan2d())
if(this->isSubscribedToScan2d() || (this->isSubscribedToScan3d() && scanCloudIs2d_))
{
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"subscribe_scan\" is "
RCLCPP_WARN(this->get_logger(), "Setting \"%s\" parameter to 10 (default 0) as \"%s\" is "
"true and \"%s\" uses ICP. Proximity detection by space will be also done by merging close "
"scans. To disable, set \"%s\" to 0. To suppress this warning, "
"add <param name=\"%s\" type=\"string\" value=\"10\"/>",
Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
this->isSubscribedToScan2d()?"subscribe_scan":"scan_cloud_is_2d",
Parameters::kRegStrategy().c_str(),
Parameters::kRGBDProximityPathMaxNeighbors().c_str(),
Parameters::kRGBDProximityPathMaxNeighbors().c_str());
@@ -1392,7 +1402,8 @@ void CoreWrapper::commonMultiCameraCallback(
}
}
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
UScopeMutex syncDataLock(syncDataMutex_, false);
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
{
UScopeMutex lock(lastPoseMutex_);
commonMultiCameraCallbackImpl(odomFrameId,
@@ -1412,7 +1423,6 @@ void CoreWrapper::commonMultiCameraCallback(
if(syncData_.valid) {
syncTimer_->reset();
}
syncDataMutex_.unlock();
}
}
@@ -1783,7 +1793,8 @@ void CoreWrapper::commonLaserScanCallback(
}
}
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
UScopeMutex syncDataLock(syncDataMutex_, false);
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
{
UScopeMutex lock(lastPoseMutex_);
LaserScan scan;
@@ -1801,7 +1812,6 @@ void CoreWrapper::commonLaserScanCallback(
rtabmap_.getMemory() && uStrNumCmp(rtabmap_.getMemory()->getDatabaseVersion(), "0.11.10") < 0))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert laser scan msg! Aborting rtabmap update...");
syncDataMutex_.unlock();
return;
}
}
@@ -1820,7 +1830,6 @@ void CoreWrapper::commonLaserScanCallback(
scanCloudIs2d_))
{
RCLCPP_ERROR(this->get_logger(), "Could not convert 3d laser scan msg! Aborting rtabmap update...");
syncDataMutex_.unlock();
return;
}
}
@@ -1880,7 +1889,6 @@ void CoreWrapper::commonLaserScanCallback(
lastPoseCovariance_ = cv::Mat();
syncTimer_->reset();
syncDataMutex_.unlock();
}
}
@@ -1897,7 +1905,8 @@ void CoreWrapper::commonOdomCallback(
return;
}
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
UScopeMutex syncDataLock(syncDataMutex_, false);
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
{
UScopeMutex lock(lastPoseMutex_);
cv::Mat userData;
@@ -1949,7 +1958,6 @@ void CoreWrapper::commonOdomCallback(
lastPoseCovariance_ = cv::Mat();
syncTimer_->reset();
syncDataMutex_.unlock();
}
}
@@ -1985,12 +1993,29 @@ void CoreWrapper::commonSensorDataCallback(
}
}
if(syncTimer_->is_canceled() && syncDataMutex_.lockTry() == 0)
UScopeMutex syncDataLock(syncDataMutex_, false);
if(syncTimer_->is_canceled() && syncDataLock.lockTry() == 0)
{
UScopeMutex lock(lastPoseMutex_);
syncData_.data = rtabmap_conversions::sensorDataFromROS(*sensorDataMsg);
syncData_.data.setId(lastPoseIntermediate_?-1:0);
{
UScopeMutex lock(userDataMutex_);
if(!userData_.empty())
{
if(!syncData_.data.userDataRaw().empty() || !syncData_.data.userDataCompressed().empty())
{
RCLCPP_WARN(this->get_logger(), "Sensor data received already contains user data. Async user data dropped!");
}
else
{
syncData_.data.setUserData(userData_);
}
userData_ = cv::Mat();
}
}
OdometryInfo odomInfo;
if(odomInfoMsg.get())
{
@@ -2014,7 +2039,6 @@ void CoreWrapper::commonSensorDataCallback(
lastPoseCovariance_ = cv::Mat();
syncTimer_->reset();
syncDataMutex_.unlock();
}
}
@@ -2059,7 +2083,7 @@ void CoreWrapper::process(
// Add intermediate nodes?
for(std::list<std::pair<nav_msgs::msg::Odometry, rtabmap_msgs::msg::OdomInfo> >::iterator iter=interOdoms_.begin(); iter!=interOdoms_.end();)
{
if(rclcpp::Time(iter->first.header.stamp.sec, iter->first.header.stamp.nanosec) < stamp)
if(rclcpp::Time(iter->first.header.stamp) < stamp)
{
Transform interOdom;
if(!rtabmap_.getLocalOptimizedPoses().empty())
@@ -2208,8 +2232,8 @@ void CoreWrapper::process(
Transform correction = rtabmap_conversions::getMovingTransform(
frameId_,
odomFrameId,
stamp,
rclcpp::Time(globalPoseMsg.header.stamp.sec, globalPoseMsg.header.stamp.nanosec),
stamp,
*tfBuffer_,
waitForTransform_);
if(!correction.isNull())