i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-09-12 22:30:19 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of github.com:introlab/rtabmap_ros into ros2

Browse Source
This commit is contained in:
matlabbe
2025-11-11 18:32:18 -08:00
parent e245b7751e 2f258bdae0
commit 4b970c2ca0
6 changed files with 97 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_launch/launch/rtabmap.launch.py
+5
View File
@@ -63,6 +63,7 @@ def launch_setup(context, *args, **kwargs):
DeclareLaunchArgument('qos_user_data', default_value=LaunchConfiguration('qos'), description='Specific QoS used for user input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_imu', default_value=LaunchConfiguration('qos'), description='Specific QoS used for imu input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_gps', default_value=LaunchConfiguration('qos'), description='Specific QoS used for gps input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('qos_env_sensor', default_value=LaunchConfiguration('qos'), description='Specific QoS used for env sensor input data: 0=system default, 1=Reliable, 2=Best Effort.'),
DeclareLaunchArgument('odom_log_level', default_value=LaunchConfiguration('log_level'), description='Specific ROS logger level for odometry node.'),
@@ -316,6 +317,7 @@ def launch_setup(context, *args, **kwargs):
"qos_camera_info": LaunchConfiguration('qos_camera_info'),
"qos_imu": LaunchConfiguration('qos_imu'),
"qos_gps": LaunchConfiguration('qos_gps'),
"qos_env_sensor": LaunchConfiguration('qos_env_sensor'),
"qos_user_data": LaunchConfiguration('qos_user_data'),
"scan_normal_k": LaunchConfiguration('scan_normal_k'),
"landmark_linear_variance": LaunchConfiguration('tag_linear_variance'),
@@ -340,6 +342,7 @@ def launch_setup(context, *args, **kwargs):
("gps/fix", LaunchConfiguration('gps_topic')),
("tag_detections", LaunchConfiguration('tag_topic')),
("fiducial_transforms", LaunchConfiguration('fiducial_topic')),
("env_sensor", LaunchConfiguration('env_sensor_topic')),
("odom", LaunchConfiguration('odom_topic')),
("imu", LaunchConfiguration('imu_topic')),
("goal_out", LaunchConfiguration('output_goal_topic'))],
@@ -529,6 +532,8 @@ def generate_launch_description():
DeclareLaunchArgument('tag_linear_variance', default_value='0.0001', description=''),
DeclareLaunchArgument('tag_angular_variance', default_value='9999.0', description='>=9999 means rotation is ignored in optimization, when rotation estimation of the tag is not reliable or not computed.'),
DeclareLaunchArgument('fiducial_topic', default_value='/fiducial_transforms', description='aruco_detect async subscription, use tag_linear_variance and tag_angular_variance to set covariance.'),
DeclareLaunchArgument('env_sensor_topic', default_value='/env_sensor', description='A rtabmap_msgs/EnvSensor topic.'),
OpaqueFunction(function=launch_setup)
])
rtabmap_msgs/msg/EnvSensor.msg
+22 -1
View File
@@ -1,6 +1,27 @@
std_msgs/Header header
# EnvSensor
# Environmental sensor
# built-in types
int32 TYPE_UNDEFINED=0
int32 TYPE_WIFI_SIGNAL_STRENGTH=1 # dBm
int32 TYPE_AMBIENT_TEMPERATURE=2 # Celcius
int32 TYPE_AMBIENT_AIR_PRESSURE=3 # hPa
int32 TYPE_AMBIENT_LIGHT=4 # lx
int32 TYPE_AMBIENT_RELATIVE_HUMIDITY=5 # %
# user types
int32 TYPE_CUSTOM1=100
int32 TYPE_CUSTOM2=101
int32 TYPE_CUSTOM3=102
int32 TYPE_CUSTOM4=103
int32 TYPE_CUSTOM5=104
int32 TYPE_CUSTOM6=105
int32 TYPE_CUSTOM7=106
int32 TYPE_CUSTOM8=107
int32 TYPE_CUSTOM9=108
int32 type
float64 value
rtabmap_slam/include/rtabmap_slam/CoreWrapper.h
+7
View File
@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap_msgs/msg/info.hpp"
#include "rtabmap_msgs/msg/landmark_detection.hpp"
#include "rtabmap_msgs/msg/landmark_detections.hpp"
#include "rtabmap_msgs/msg/env_sensor.h"
#include "rtabmap_msgs/srv/get_nodes_in_radius.hpp"
#include "rtabmap_msgs/srv/load_database.hpp"
#include "rtabmap_msgs/srv/detect_more_loop_closures.hpp"
@@ -191,6 +192,7 @@ private:
void userDataAsyncCallback(const rtabmap_msgs::msg::UserData::SharedPtr dataMsg);
void globalPoseAsyncCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr globalPoseMsg);
void gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedPtr gpsFixMsg);
void envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg);
void landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection);
void landmarkDetectionsAsyncCallback(const rtabmap_msgs::msg::LandmarkDetections::SharedPtr landmarkDetections);
#ifdef WITH_APRILTAG_MSGS
@@ -445,6 +447,11 @@ private:
std::map<double, rtabmap::GPS> gps_;
UMutex gpsMutex_;
rclcpp::CallbackGroup::SharedPtr envSensorAsyncCallbackGroup_;
rclcpp::Subscription<rtabmap_msgs::msg::EnvSensor>::SharedPtr envSensorAsyncSub_;
rtabmap::EnvSensors envSensors_;
UMutex envSensorMutex_;
rclcpp::CallbackGroup::SharedPtr landmarkCallbackGroup_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetection>::SharedPtr landmarkDetectionSub_;
rclcpp::Subscription<rtabmap_msgs::msg::LandmarkDetections>::SharedPtr landmarkDetectionsSub_;
rtabmap_slam/src/CoreWrapper.cpp
+36
View File
@@ -872,24 +872,30 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
gpsAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
landmarkCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
imuCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
envSensorAsyncCallbackGroup_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions userDataAsyncSubOptions;
rclcpp::SubscriptionOptions globalPoseAsyncSubOptions;
rclcpp::SubscriptionOptions gpsAsyncSubOptions;
rclcpp::SubscriptionOptions landmarkSubOptions;
rclcpp::SubscriptionOptions imuSubOptions;
rclcpp::SubscriptionOptions envSensorAsyncSubOptions;
userDataAsyncSubOptions.callback_group = userDataAsyncCallbackGroup_;
globalPoseAsyncSubOptions.callback_group = globalPoseAsyncCallbackGroup_;
gpsAsyncSubOptions.callback_group = gpsAsyncCallbackGroup_;
landmarkSubOptions.callback_group = imuCallbackGroup_;
imuSubOptions.callback_group = imuCallbackGroup_;
envSensorAsyncSubOptions.callback_group = envSensorAsyncCallbackGroup_;
int qosGPS = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
int qosIMU = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
int qosEnvSensor = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qosGPS = this->declare_parameter("qos_gps", qosGPS);
qosIMU = this->declare_parameter("qos_imu", qosIMU);
qosEnvSensor = this->declare_parameter("qos_env_sensor", qosEnvSensor);
userDataAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::UserData>("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<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1, std::bind(&CoreWrapper::globalPoseAsyncCallback, this, std::placeholders::_1), globalPoseAsyncSubOptions);
gpsFixAsyncSub_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGPS), std::bind(&CoreWrapper::gpsFixAsyncCallback, this, std::placeholders::_1), gpsAsyncSubOptions);
envSensorAsyncSub_ = this->create_subscription<rtabmap_msgs::msg::EnvSensor>("env_sensor", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosEnvSensor), std::bind(&CoreWrapper::envSensorAsyncCallback, this, std::placeholders::_1), envSensorAsyncSubOptions);
landmarkDetectionSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetection>("landmark_detection", 1, std::bind(&CoreWrapper::landmarkDetectionAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
landmarkDetectionsSub_ = this->create_subscription<rtabmap_msgs::msg::LandmarkDetections>("landmark_detections", 1, std::bind(&CoreWrapper::landmarkDetectionsAsyncCallback, this, std::placeholders::_1), landmarkSubOptions);
#ifdef WITH_APRILTAG_MSGS
@@ -2256,6 +2262,16 @@ void CoreWrapper::process(
data.setLandmarks(landmarks);
}
// Env sensors
{
UScopeMutex lock(envSensorMutex_);
if(!envSensors_.empty())
{
data.setEnvSensors(envSensors_);
envSensors_.clear();
}
}
// IMU
imuMutex_.lock();
if(!imus_.empty())
@@ -2695,6 +2711,17 @@ void CoreWrapper::gpsFixAsyncCallback(const sensor_msgs::msg::NavSatFix::SharedP
}
}
void CoreWrapper::envSensorAsyncCallback(const rtabmap_msgs::msg::EnvSensor::SharedPtr envSensorMsg)
{
if(!paused_)
{
// Can only insert one value for each type per node, keep the most recent
EnvSensor value = rtabmap_conversions::envSensorFromROS(*envSensorMsg);
UScopeMutex lock(envSensorMutex_);
uInsert(envSensors_, std::make_pair(value.type(), value));
}
}
void CoreWrapper::landmarkDetectionAsyncCallback(const rtabmap_msgs::msg::LandmarkDetection::SharedPtr landmarkDetection)
{
if(!paused_)
@@ -3252,6 +3279,9 @@ void CoreWrapper::resetRtabmapCallback(
userDataMutex_.lock();
userData_ = cv::Mat();
userDataMutex_.unlock();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
imuMutex_.lock();
imus_.clear();
imuFrameId_.clear();
@@ -3354,6 +3384,9 @@ void CoreWrapper::loadDatabaseCallback(
userDataMutex_.lock();
userData_ = cv::Mat();
userDataMutex_.unlock();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
imuMutex_.lock();
imus_.clear();
imuFrameId_.clear();
@@ -3502,6 +3535,9 @@ void CoreWrapper::backupDatabaseCallback(
userDataMutex_.unlock();
globalPoses_.clear();
gps_.clear();
envSensorMutex_.lock();
envSensors_.clear();
envSensorMutex_.unlock();
landmarksMutex_.lock();
landmarks_.clear();
landmarksMutex_.unlock();
rtabmap_util/include/rtabmap_util/db_player.hpp
+3
View File
@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tf2_ros/transform_broadcaster.h>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap_msgs/msg/env_sensor.hpp>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
@@ -88,6 +89,7 @@ private:
int qosGlobalPose_;
int qosGps_;
int qosImu_;
int qosEnvSensor_;
double scanAngleMin_;
double scanAngleMax_;
double scanAngleIncrement_;
@@ -112,6 +114,7 @@ private:
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanCloudPub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPosePub_;
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixPub_;
rclcpp::Publisher<rtabmap_msgs::msg::EnvSensor>::SharedPtr envSensorPub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPub_;
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clockPub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
rtabmap_util/src/nodelets/db_player.cpp
+24
View File
@@ -132,6 +132,7 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(get_logger(), " qos_global_pose = %d", qosGlobalPose_);
RCLCPP_INFO(get_logger(), " qos_gps = %d", qosGps_);
RCLCPP_INFO(get_logger(), " qos_imu = %d", qosImu_);
RCLCPP_INFO(get_logger(), " qos_env_sensor = %d", qosEnvSensor_);
if(databasePath.empty())
{
@@ -320,6 +321,12 @@ void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
RCLCPP_INFO(get_logger(), "GPS \"%s\" will be published.", gpsFixPub_->get_topic_name());
}
if(!envSensorPub_.get() && !odom.data().envSensors().empty())
{
envSensorPub_ = this->create_publisher<rtabmap_msgs::msg::EnvSensor>("env_sensor", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosEnvSensor_));
RCLCPP_INFO(get_logger(), "Env sensor \"%s\" will be published.", envSensorPub_->get_topic_name());
}
if(!odometryPub_.get() && !odom.pose().isNull())
{
odometryPub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosOdom_));
@@ -512,6 +519,23 @@ bool DbPlayer::publishNextFrame()
gpsFixPub_->publish(msg);
}
if( envSensorPub_.get() &&
envSensorPub_->get_subscription_count() > 0 &&
!odom.data().envSensors().empty())
{
rtabmap_msgs::msg::EnvSensor msg;
for(rtabmap::EnvSensors::const_iterator iter=odom.data().envSensors().begin(); iter!=odom.data().envSensors().end(); ++iter)
{
rtabmap_msgs::msg::EnvSensor msg;
rtabmap_conversions::envSensorToROS(iter->second, msg);
msg.header.frame_id = frameId_;
if(iter->second.stamp() == 0.0) {
msg.header.stamp = rtabmap_conversions::timestampToROS(data.stamp());
}
envSensorPub_->publish(msg);
}
}
if( imuPub_.get() &&
imuPub_->get_subscription_count() > 0 &&
!odom.data().imu().empty())