i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 08:47:45 +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_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())