i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-05 17:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity
Files
0b60e529145f2c878224a9c830185ca1d242299a
rtabmap_ros/rtabmap_util/src/nodelets/db_player.cpp
T

810 lines
37 KiB
C++
Raw Blame History

/*
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap_util/db_player.hpp>
#include <sensor_msgs/image_encodings.hpp>
#include <image_transport/image_transport.hpp>
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UThread.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/OdometryEvent.h>
#include <rtabmap/core/SensorData.h>
#include <pcl_conversions/pcl_conversions.h>
#ifdef PRE_ROS_IRON
#include <cv_bridge/cv_bridge.h>
#else
#include <cv_bridge/cv_bridge.hpp>
#endif
namespace rtabmap_util
{
DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
rclcpp::Node("db_player", options),
paused_(false),
frameId_("base_link"),
odomFrameId_("odom"),
cameraFrameId_("camera_optical_link"),
scanFrameId_("base_laser_link"),
gtFrameId_("world"),
gtBaseFrameId_("base_link_gt"),
imuFrameId_("imu_link"),
qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)
{
//ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug);
//ULogger::setEventLevel(ULogger::kWarning);
//parse input arguments
bool publishClock = false;
publishClock = this->declare_parameter("publish_clock", publishClock);
std::vector<std::string> tmpList = get_node_options().arguments();
std::vector<std::string> argList;
for(unsigned int i=0; i<tmpList.size(); ++i)
{
if(tmpList[i].compare("--clock") == 0)
{
publishClock = true;
}
}
double rate = 1.0f;
std::string databasePath = "";
bool publishTf = true;
bool ignoreOdom = false;
int startId = 0;
frameId_ = this->declare_parameter("frame_id", frameId_);
odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_);
cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_);
scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_);
gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_);
gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_);
imuFrameId_ = this->declare_parameter("imu_frame_id", imuFrameId_);
rate = this->declare_parameter("rate", rate); // Ratio of the database stamps
databasePath = this->declare_parameter("database", databasePath);
publishTf = this->declare_parameter("publish_tf", publishTf);
ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom);
startId = this->declare_parameter("start_id", startId);
qos_ = this->declare_parameter("qos", qos_);
qosCameraInfo_ = this->declare_parameter("qos_camera_info", qos_);
qosOdom_ = this->declare_parameter("qos_odom", qos_);
qosScan_ = this->declare_parameter("qos_scan", qos_);
qosScanCloud_ = this->declare_parameter("qos_scan_cloud", qos_);
qosGlobalPose_ = this->declare_parameter("qos_global_pose", qos_);
qosGps_ = this->declare_parameter("qos_gps", qos_);
qosImu_ = this->declare_parameter("qos_imu", qos_);
// A general 360 lidar with 0.5 deg increment
scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI);
scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI);
scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0);
scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0);
scanRangeMax_ = this->declare_parameter("scan_range_max", 60);
RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str());
RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str());
RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str());
RCLCPP_INFO(get_logger(), "imu_frame_id = %s", imuFrameId_.c_str());
RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate);
RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false");
RCLCPP_INFO(get_logger(), "start_id = %d", startId);
RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"true":"false");
RCLCPP_INFO(get_logger(), "qos = %d", qos_);
RCLCPP_INFO(get_logger(), " qos_camera_info = %d", qosCameraInfo_);
RCLCPP_INFO(get_logger(), " qos_odom = %d", qosOdom_);
RCLCPP_INFO(get_logger(), " qos_scan = %d", qosScan_);
RCLCPP_INFO(get_logger(), " qos_scan_cloud = %d", qosScanCloud_);
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())
{
RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
exit(-1);
}
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/')
{
databasePath = UDirectory::currentDir(true) + databasePath;
}
RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str());
reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId));
if(!reader_->init())
{
RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
exit(-1);
}
const std::string servicePrefix = get_name() + std::string("/");
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
if(publishTf) {
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(*this);
}
if(publishClock)
{
clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
}
}
DbPlayer::~DbPlayer(){}
void DbPlayer::pauseCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<std_srvs::srv::Empty::Request>,
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
if(paused_)
{
RCLCPP_WARN(get_logger(), "Already paused!");
}
else
{
paused_ = true;
RCLCPP_INFO(get_logger(), "paused!");
}
}
void DbPlayer::resumeCallback(
const std::shared_ptr<rmw_request_id_t>,
const std::shared_ptr<std_srvs::srv::Empty::Request>,
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
if(!paused_)
{
RCLCPP_WARN(get_logger(), "Already running!");
}
else
{
paused_ = false;
RCLCPP_INFO(get_logger(), "resumed!");
}
}
void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
{
if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
{
if(odom.data().cameraModels().size() > 1)
{
if(rgbdImagePubs_.empty()) {
for(size_t i=0;i<odom.data().cameraModels().size(); ++i) {
rgbdImagePubs_.push_back(this->create_publisher<rtabmap_msgs::msg::RGBDImage>(uFormat("rgbd_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)));
RCLCPP_INFO(get_logger(), "RGB-D image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name());
}
}
else {
UASSERT_MSG(rgbdImagePubs_.size() == odom.data().cameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().cameraModels().size()).c_str());
}
}
else
{
if(rgbPub_.getTopic().empty()) {
#ifdef PRE_ROS_LYRICAL
rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
#else
rgbPub_ = image_transport::create_publisher(*this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_));
#endif
RCLCPP_INFO(get_logger(), "Gray/RGB image \"%s\" will be published.", rgbPub_.getTopic().c_str());
}
if(!rgbInfoPub_.get()) {
rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
RCLCPP_INFO(get_logger(), "Gray/RGB calibration \"%s\" will be published.", rgbInfoPub_->get_topic_name());
}
if(depthPub_.getTopic().empty()) {
#ifdef PRE_ROS_LYRICAL
depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
#else
depthPub_ = image_transport::create_publisher(*this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_));
#endif
RCLCPP_INFO(get_logger(), "Depth image \"%s\" will be published.", depthPub_.getTopic().c_str());
}
if(!depthInfoPub_.get()) {
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
RCLCPP_INFO(get_logger(), "Depth calibration \"%s\" will be published.", depthInfoPub_->get_topic_name());
}
}
}
else if(!odom.data().rightRaw().empty() && (odom.data().rightRaw().type() == CV_8U || odom.data().rightRaw().type() == CV_8UC3))
{
if(odom.data().stereoCameraModels().size() > 1)
{
if(rgbdImagePubs_.empty()) {
for(size_t i=0;i<odom.data().stereoCameraModels().size(); ++i) {
rgbdImagePubs_.push_back(this->create_publisher<rtabmap_msgs::msg::RGBDImage>(uFormat("stereo_image%ld", i), rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_)));
RCLCPP_INFO(get_logger(), "Stereo image \"%s\" will be published.", rgbdImagePubs_.back()->get_topic_name());
}
}
else {
UASSERT_MSG(rgbdImagePubs_.size() == odom.data().stereoCameraModels().size(), uFormat("%ld versus %ld", rgbdImagePubs_.size(), odom.data().stereoCameraModels().size()).c_str());
}
}
else
{
if(leftPub_.getTopic().empty()) {
#ifdef PRE_ROS_LYRICAL
leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
#else
leftPub_ = image_transport::create_publisher(*this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_));
#endif
RCLCPP_INFO(get_logger(), "Left image \"%s\" will be published.", leftPub_.getTopic().c_str());
}
if(!leftInfoPub_.get()) {
leftInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
RCLCPP_INFO(get_logger(), "Left calibration \"%s\" will be published.", leftInfoPub_->get_topic_name());
}
if(rightPub_.getTopic().empty()) {
#ifdef PRE_ROS_LYRICAL
rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
#else
rightPub_ = image_transport::create_publisher(*this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_));
#endif
RCLCPP_INFO(get_logger(), "Right image \"%s\" will be published.", rightPub_.getTopic().c_str());
}
if(!rightInfoPub_.get()) {
rightInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
RCLCPP_INFO(get_logger(), "Right calibration \"%s\" will be published.", rightInfoPub_->get_topic_name());
}
}
}
else if(imagePub_.getTopic().empty())
{
#ifdef PRE_ROS_LYRICAL
imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
#else
imagePub_ = image_transport::create_publisher(*this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_));
#endif
RCLCPP_INFO(get_logger(), "Image \"%s\" without calibration will be published.", imagePub_.getTopic().c_str());
}
if(!odom.data().laserScanRaw().isEmpty())
{
if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
{
scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScan_));
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published.", scanPub_->get_topic_name());
}
else
{
RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published with those parameters:", scanPub_->get_topic_name());
RCLCPP_INFO(get_logger(), " scan_angle_min=%f", scanAngleMin_);
RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_);
RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_);
RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_);
RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_);
}
}
else if(!scanCloudPub_.get())
{
scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScanCloud_));
RCLCPP_INFO(get_logger(), "PointCloud2 \"%s\" will be published.", scanCloudPub_->get_topic_name());
}
}
if(!odom.data().globalPose().isNull() &&
odom.data().globalPoseCovariance().cols==6 &&
odom.data().globalPoseCovariance().rows==6)
{
if(!globalPosePub_.get())
{
globalPosePub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGlobalPose_));
RCLCPP_INFO(get_logger(), "Global pose \"%s\" will be published.", globalPosePub_->get_topic_name());
}
}
if(!gpsFixPub_.get() && odom.data().gps().stamp() > 0.0)
{
gpsFixPub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("gps/fix", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosGps_));
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_));
RCLCPP_INFO(get_logger(), "Odometry \"%s\" will be published.", odometryPub_->get_topic_name());
}
if(!imuPub_.get() && !odom.data().imu().empty())
{
imuPub_ = this->create_publisher<sensor_msgs::msg::Imu>("imu", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosImu_));
RCLCPP_INFO(get_logger(), "IMU \"%s\" will be published.", imuPub_->get_topic_name());
}
}
bool DbPlayer::cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage)
{
cv_bridge::CvImage img;
if(image.type() == CV_32FC1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
else if(image.type() == CV_16UC1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
else if(image.type() == CV_8UC1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else if(image.type() == CV_8UC3)
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
else {
RCLCPP_ERROR(get_logger(), "Unsupported image format: cv type = %d", image.type());
return false;
}
img.image = image;
img.toImageMsg(rosImage);
return true;
}
bool DbPlayer::publishNextFrame()
{
rtabmap::SensorCaptureInfo cameraInfo;
rtabmap::SensorData data = reader_->takeImage(&cameraInfo);
rtabmap::OdometryInfo odomInfo;
odomInfo.reg.covariance = cameraInfo.odomCovariance;
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
if(!odom.data().id())
{
return false;
}
RCLCPP_INFO(get_logger(), "Reading sensor data %d...", odom.data().id());
rclcpp::Time time = rtabmap_conversions::timestampToROS(odom.data().stamp());
///////////////////////
// Initialize publishers based on data in the database
///////////////////////
initializePublishers(odom); // called everytime in case some data like global pose, imu, odometry was not available at the beggining.
///////////////////////
// Publish topics
///////////////////////
if(clockPub_.get())
{
rosgraph_msgs::msg::Clock msg;
msg.clock = time;
clockPub_->publish(msg);
}
// publish transforms first
if(tfBroadcaster_.get())
{
std::vector<geometry_msgs::msg::TransformStamped> transforms;
const std::vector<rtabmap::CameraModel> * models = &odom.data().cameraModels();
std::vector<rtabmap::CameraModel> stereoModels;
bool stereo = false;
if(odom.data().stereoCameraModels().size())
{
for(const auto & cam: odom.data().stereoCameraModels()) {
stereoModels.push_back(cam.left());
stereoModels.push_back(cam.right());
}
models = &stereoModels;
stereo = true;
}
int index = 0;
static bool firstTimeCamMsg = true;
for(const auto & cam: *models) {
rtabmap::Transform localTransform = cam.localTransform();
if(!localTransform.isNull()) {
geometry_msgs::msg::TransformStamped baseToCamera;
baseToCamera.child_frame_id = (stereo?index%2==0?"left_":"right_":"") + cameraFrameId_ + (((stereo && models->size()>2) || (!stereo && models->size()>1))?uNumber2Str(index/(stereo?2:1)):"");
baseToCamera.header.frame_id = frameId_;
baseToCamera.header.stamp = time;
if(cam.Tx() != 0) {
localTransform *= rtabmap::Transform(-cam.Tx()/cam.fx(), 0, 0);
}
rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
transforms.push_back(baseToCamera);
if(firstTimeCamMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToCamera.header.frame_id.c_str(), baseToCamera.child_frame_id.c_str());
}
}
++index;
}
firstTimeCamMsg = firstTimeCamMsg && models->empty()?true:false;
if(!odom.pose().isNull())
{
geometry_msgs::msg::TransformStamped odomToBase;
odomToBase.child_frame_id = frameId_;
odomToBase.header.frame_id = odomFrameId_;
odomToBase.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
transforms.push_back(odomToBase);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", odomToBase.header.frame_id.c_str(), odomToBase.child_frame_id.c_str());
}
firstTimeMsg = false;
}
if(scanPub_.get() || scanCloudPub_.get())
{
geometry_msgs::msg::TransformStamped baseToLaserScan;
baseToLaserScan.child_frame_id = scanFrameId_;
baseToLaserScan.header.frame_id = frameId_;
baseToLaserScan.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
transforms.push_back(baseToLaserScan);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToLaserScan.header.frame_id.c_str(), baseToLaserScan.child_frame_id.c_str());
}
firstTimeMsg = false;
}
if(!odom.data().groundTruth().isNull()) {
geometry_msgs::msg::TransformStamped worldToBase;
worldToBase.child_frame_id = gtBaseFrameId_;
worldToBase.header.frame_id = gtFrameId_;
worldToBase.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
transforms.push_back(worldToBase);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", worldToBase.header.frame_id.c_str(), worldToBase.child_frame_id.c_str());
}
firstTimeMsg = false;
}
if(!odom.data().imu().empty()) {
geometry_msgs::msg::TransformStamped baseToImu;
baseToImu.child_frame_id = imuFrameId_;
baseToImu.header.frame_id = frameId_;
baseToImu.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(odom.data().imu().localTransform(), baseToImu.transform);
transforms.push_back(baseToImu);
static bool firstTimeMsg = true;
if(firstTimeMsg) {
RCLCPP_INFO(get_logger(), "Will publish tf %s -> %s", baseToImu.header.frame_id.c_str(), baseToImu.child_frame_id.c_str());
}
firstTimeMsg = false;
}
tfBroadcaster_->sendTransform(transforms);
}
if( odometryPub_.get() &&
!odom.pose().isNull() &&
odometryPub_->get_subscription_count())
{
nav_msgs::msg::Odometry odomMsg;
odomMsg.child_frame_id = frameId_;
odomMsg.header.frame_id = odomFrameId_;
odomMsg.header.stamp = time;
rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
UASSERT(odomMsg.pose.covariance.size() == 36 &&
odom.covariance().total() == 36 &&
odom.covariance().type() == CV_64FC1);
memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
odometryPub_->publish(odomMsg);
}
// Publish async topics first (so that they can catched by rtabmap before the image topics)
if( globalPosePub_.get() &&
globalPosePub_->get_subscription_count() > 0 &&
!odom.data().globalPose().isNull() &&
odom.data().globalPoseCovariance().cols==6 &&
odom.data().globalPoseCovariance().rows==6)
{
geometry_msgs::msg::PoseWithCovarianceStamped msg;
rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
msg.header.frame_id = frameId_;
msg.header.stamp = time;
globalPosePub_->publish(msg);
}
if( gpsFixPub_.get() &&
gpsFixPub_->get_subscription_count() > 0 &&
odom.data().gps().stamp() > 0.0)
{
sensor_msgs::msg::NavSatFix msg;
msg.longitude = odom.data().gps().longitude();
msg.latitude = odom.data().gps().latitude();
msg.altitude = odom.data().gps().altitude();
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
msg.header.frame_id = frameId_;
msg.header.stamp = rtabmap_conversions::timestampToROS(odom.data().gps().stamp());
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())
{
sensor_msgs::msg::Imu msg;
rtabmap_conversions::imuToROS(odom.data().imu(), msg);
msg.header.frame_id = imuFrameId_;
msg.header.stamp = time;
imuPub_->publish(msg);
}
// single camera
if(imagePub_.getNumSubscribers() || (odom.data().cameraModels().size() <= 1 && odom.data().stereoCameraModels().size() <= 1))
{
if(!odom.data().imageRaw().empty() &&
((imagePub_.getNumSubscribers()) ||
(rgbPub_.getNumSubscribers()) ||
(leftPub_.getNumSubscribers())))
{
sensor_msgs::msg::Image imageRosMsg;
if(cvImageToROS(odom.data().imageRaw(), imageRosMsg))
{
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
if(imagePub_.getNumSubscribers())
{
imagePub_.publish(imageRosMsg);
}
if(rgbPub_.getNumSubscribers())
{
rgbPub_.publish(imageRosMsg);
UASSERT(odom.data().cameraModels().size() == 1);
sensor_msgs::msg::CameraInfo info;
rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info);
info.header = imageRosMsg.header;
rgbInfoPub_->publish(info);
}
if(leftPub_.getNumSubscribers())
{
imageRosMsg.header.frame_id = "left_" + cameraFrameId_;
leftPub_.publish(imageRosMsg);
UASSERT(odom.data().stereoCameraModels().size() == 1);
sensor_msgs::msg::CameraInfo info;
rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].left(), info);
info.header = imageRosMsg.header;
leftInfoPub_->publish(info);
}
}
}
if(!odom.data().depthRaw().empty() && depthPub_.getNumSubscribers())
{
sensor_msgs::msg::Image imageRosMsg;
if(cvImageToROS(odom.data().depthRaw(), imageRosMsg))
{
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
depthPub_.publish(imageRosMsg);
UASSERT(odom.data().cameraModels().size() == 1);
sensor_msgs::msg::CameraInfo info;
// We assume depth is registered with the RGB camera, so they share same calibration and TF frame
rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[0], info);
info.header = imageRosMsg.header;
depthInfoPub_->publish(info);
}
}
if(!odom.data().rightRaw().empty() && rightPub_.getNumSubscribers())
{
sensor_msgs::msg::Image imageRosMsg;
if(cvImageToROS(odom.data().rightRaw(), imageRosMsg))
{
imageRosMsg.header.frame_id = "right_" + cameraFrameId_;
imageRosMsg.header.stamp = time;
rightPub_.publish(imageRosMsg);
UASSERT(odom.data().stereoCameraModels().size() == 1);
sensor_msgs::msg::CameraInfo info;
rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[0].right(), info);
info.header = imageRosMsg.header;
rightInfoPub_->publish(info);
}
}
}
// Multi-cameras
if(!odom.data().imageRaw().empty())
{
std::vector<rtabmap_msgs::msg::RGBDImage> rgbdImages;
if(odom.data().cameraModels().size() > 1)
{
UASSERT(odom.data().cameraModels().size() == rgbdImagePubs_.size());
int subRgbImageWidth = odom.data().imageRaw().cols / odom.data().cameraModels().size();
int subDepthImageWidth = odom.data().depthRaw().cols / odom.data().cameraModels().size();
for(size_t i=0; i<rgbdImagePubs_.size(); ++i)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.stamp = time;
msg.header.frame_id = cameraFrameId_ + uNumber2Str((int)i);
cvImageToROS(cv::Mat(odom.data().imageRaw(), cv::Range::all(), cv::Range(i*subRgbImageWidth, (i+1)*subRgbImageWidth)), msg.rgb);
msg.rgb.header = msg.header;
rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.rgb_camera_info);
msg.rgb_camera_info.header = msg.header;
if(subDepthImageWidth) {
cvImageToROS(cv::Mat(odom.data().depthRaw(), cv::Range::all(), cv::Range(i*subDepthImageWidth, (i+1)*subDepthImageWidth)), msg.depth);
msg.depth.header = msg.header;
UASSERT(subDepthImageWidth <= subRgbImageWidth);
if(subDepthImageWidth < subRgbImageWidth) {
rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i].scaled(double(subDepthImageWidth)/double(subRgbImageWidth)), msg.depth_camera_info);
}
else {
rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.depth_camera_info);
}
msg.depth_camera_info.header = msg.header;
}
rgbdImagePubs_[i]->publish(msg);
}
}
else if(odom.data().stereoCameraModels().size() > 1)
{
UASSERT(odom.data().stereoCameraModels().size() == rgbdImagePubs_.size());
int subImageWidth = odom.data().imageRaw().cols / odom.data().stereoCameraModels().size();
UASSERT(odom.data().imageRaw().cols == odom.data().rightRaw().cols);
for(size_t i=0; i<rgbdImagePubs_.size(); ++i)
{
rtabmap_msgs::msg::RGBDImage msg;
msg.header.stamp = time;
msg.header.frame_id = "left_" + cameraFrameId_ + uNumber2Str((int)i);
cvImageToROS(cv::Mat(odom.data().imageRaw(), cv::Range::all(), cv::Range(i*subImageWidth, (i+1)*subImageWidth)), msg.rgb);
msg.rgb.header = msg.header;
rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[i].left(), msg.rgb_camera_info);
msg.rgb_camera_info.header = msg.header;
std::string rightFrame = "right_" + cameraFrameId_ + uNumber2Str((int)i);
cvImageToROS(cv::Mat(odom.data().rightRaw(), cv::Range::all(), cv::Range(i*subImageWidth, (i+1)*subImageWidth)), msg.depth);
msg.depth.header = msg.header;
msg.depth.header.frame_id = rightFrame;
rtabmap_conversions::cameraModelToROS(odom.data().stereoCameraModels()[i].right(), msg.depth_camera_info);
msg.depth_camera_info.header = msg.depth.header;
rgbdImagePubs_[i]->publish(msg);
}
}
}
if(!odom.data().laserScanRaw().isEmpty())
{
if(scanPub_.get() &&
scanPub_->get_subscription_count() &&
odom.data().laserScanRaw().is2d())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::msg::LaserScan msg;
msg.header.frame_id = scanFrameId_;
msg.header.stamp = time;
msg.angle_min = scanAngleMin_;
msg.angle_max = scanAngleMax_;
msg.angle_increment = scanAngleIncrement_;
msg.time_increment = 0.0;
msg.scan_time = 0;
msg.range_min = scanRangeMin_;
msg.range_max = scanRangeMax_;
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{
msg.angle_min = odom.data().laserScanRaw().angleMin();
msg.angle_max = odom.data().laserScanRaw().angleMax();
msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
msg.range_min = odom.data().laserScanRaw().rangeMin();
msg.range_max = odom.data().laserScanRaw().rangeMax();
}
int rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
msg.ranges.assign(rangesSize, 0.0);
const cv::Mat & scan = odom.data().laserScanRaw().data();
for (int i=0; i<scan.cols; ++i)
{
const float * ptr = scan.ptr<float>(0,i);
double range = hypot(ptr[0], ptr[1]);
if (range >= msg.range_min && range <=msg.range_max)
{
double angle = atan2(ptr[1], ptr[0]);
if (angle >= msg.angle_min && angle <= msg.angle_max)
{
int index = (angle - msg.angle_min) / msg.angle_increment;
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
{
msg.ranges[index] = range;
}
}
}
}
scanPub_->publish(msg);
}
else if(scanCloudPub_.get() &&
scanCloudPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2 msg;
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
msg.header.frame_id = scanFrameId_;
msg.header.stamp = time;
scanCloudPub_->publish(msg);
}
}
return true;
}
}
#include "rclcpp_components/register_node_macro.hpp"
// Register the component with class_loader.
// This acts as a sort of entry point, allowing the component to be discoverable when its library
// is being loaded into a running process.
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::DbPlayer)
Reference in New Issue View Git Blame Copy Permalink