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

Data player multicam support (#1365)

Browse Source 
* Data player multicam support

* viewer: fixed pause button
This commit is contained in:
matlabbe
2025-09-28 13:37:07 -07:00
committed by GitHub
parent 1c5df12674
commit bc5f0a7146
8 changed files with 804 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_conversions/src/MsgConversion.cpp
+13 -4
View File
@@ -869,7 +869,12 @@ void cameraModelToROS(
sensor_msgs::msg::CameraInfo & camInfo) sensor_msgs::msg::CameraInfo & camInfo)
{ {
UASSERT(model.K_raw().empty() || model.K_raw().total() == 9); UASSERT(model.K_raw().empty() || model.K_raw().total() == 9);
if(model.K_raw().empty()) UASSERT(model.P().empty() || model.P().total() == 12);
if(!model.P().empty())
{
model.P().colRange(0,3).copyTo(cv::Mat(3,3,CV_64FC1, camInfo.k.data()));
}
else if(model.K_raw().empty())
{ {
memset(camInfo.k.data(), 0.0, 9*sizeof(double)); memset(camInfo.k.data(), 0.0, 9*sizeof(double));
} }
@@ -878,7 +883,12 @@ void cameraModelToROS(
memcpy(camInfo.k.data(), model.K_raw().data, 9*sizeof(double)); memcpy(camInfo.k.data(), model.K_raw().data, 9*sizeof(double));
} }
if(model.D_raw().total() == 6) if(!model.P().empty()) {
camInfo.d = std::vector<double>(model.D().cols);
memcpy(camInfo.d.data(), model.D().data, model.D().cols*sizeof(double));
camInfo.distortion_model = "plumb_bob";
}
else if(model.D_raw().total() == 6)
{ {
camInfo.d = std::vector<double>(4); camInfo.d = std::vector<double>(4);
camInfo.d[0] = model.D_raw().at<double>(0,0); camInfo.d[0] = model.D_raw().at<double>(0,0);
@@ -902,7 +912,7 @@ void cameraModelToROS(
} }
UASSERT(model.R().empty() || model.R().total() == 9); UASSERT(model.R().empty() || model.R().total() == 9);
if(model.R().empty()) if(model.R().empty() || countNonZero(model.R()) == 0)
{ {
cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1); cv::Mat eye = cv::Mat::eye(3,3,CV_64FC1);
memcpy(camInfo.r.data(), eye.data, 9*sizeof(double)); memcpy(camInfo.r.data(), eye.data, 9*sizeof(double));
@@ -912,7 +922,6 @@ void cameraModelToROS(
memcpy(camInfo.r.data(), model.R().data, 9*sizeof(double)); memcpy(camInfo.r.data(), model.R().data, 9*sizeof(double));
} }
UASSERT(model.P().empty() || model.P().total() == 12);
if(model.P().empty()) if(model.P().empty())
{ {
memset(camInfo.p.data(), 0.0, 12*sizeof(double)); memset(camInfo.p.data(), 0.0, 12*sizeof(double));
rtabmap_util/include/rtabmap_util/db_player.hpp
+23 -8
View File
@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/image.hpp> #include <sensor_msgs/msg/image.hpp>
#include <sensor_msgs/msg/camera_info.hpp> #include <sensor_msgs/msg/camera_info.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp> #include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/msg/nav_sat_fix.hpp> #include <sensor_msgs/msg/nav_sat_fix.hpp>
@@ -46,7 +47,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <tf2_ros/buffer.h> #include <tf2_ros/buffer.h>
#include <tf2_ros/transform_broadcaster.h> #include <tf2_ros/transform_broadcaster.h>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap/core/DBReader.h> #include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
namespace rtabmap_util namespace rtabmap_util
{ {
@@ -57,13 +60,15 @@ public:
RTABMAP_UTIL_PUBLIC RTABMAP_UTIL_PUBLIC
explicit DbPlayer(const rclcpp::NodeOptions & options); explicit DbPlayer(const rclcpp::NodeOptions & options);
virtual ~DbPlayer(); virtual ~DbPlayer();
bool publishNextFrame(); bool publishNextFrame();
bool isPaused() const {return paused_;} bool isPaused() const {return paused_;}
void setPaused(bool enabled) {paused_ = enabled;} void setPaused(bool enabled) {paused_ = enabled;}
private: private:
void 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>); void 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>);
void 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>); void 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>);
void initializePublishers(const rtabmap::OdometryEvent & odom);
bool cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage);
private: private:
bool paused_; bool paused_;
@@ -74,7 +79,15 @@ private:
std::string scanFrameId_; std::string scanFrameId_;
std::string gtFrameId_; std::string gtFrameId_;
std::string gtBaseFrameId_; std::string gtBaseFrameId_;
std::string imuFrameId_;
int qos_; int qos_;
int qosCameraInfo_;
int qosOdom_;
int qosScan_;
int qosScanCloud_;
int qosGlobalPose_;
int qosGps_;
int qosImu_;
double scanAngleMin_; double scanAngleMin_;
double scanAngleMax_; double scanAngleMax_;
double scanAngleIncrement_; double scanAngleIncrement_;
@@ -89,15 +102,17 @@ private:
image_transport::Publisher depthPub_; image_transport::Publisher depthPub_;
image_transport::Publisher leftPub_; image_transport::Publisher leftPub_;
image_transport::Publisher rightPub_; image_transport::Publisher rightPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_; rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_; rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_; rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_; rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
std::vector<rclcpp::Publisher<rtabmap_msgs::msg::RGBDImage>::SharedPtr> rgbdImagePubs_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometryPub_; rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odometryPub_;
rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scanPub_; rclcpp::Publisher<sensor_msgs::msg::LaserScan>::SharedPtr scanPub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanCloudPub_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr scanCloudPub_;
rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPosePub_; rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr globalPosePub_;
rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixPub_; rclcpp::Publisher<sensor_msgs::msg::NavSatFix>::SharedPtr gpsFixPub_;
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr imuPub_;
rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clockPub_; rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clockPub_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_; std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
}; };
rtabmap_util/src/nodelets/db_player.cpp
+403 -256
View File
@@ -53,7 +53,7 @@ namespace rtabmap_util
{ {
DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) : DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
rclcpp::Node("db_player", options), rclcpp::Node("db_player", options),
paused_(false), paused_(false),
frameId_("base_link"), frameId_("base_link"),
odomFrameId_("odom"), odomFrameId_("odom"),
@@ -61,93 +61,110 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
scanFrameId_("base_laser_link"), scanFrameId_("base_laser_link"),
gtFrameId_("world"), gtFrameId_("world"),
gtBaseFrameId_("base_link_gt"), gtBaseFrameId_("base_link_gt"),
imuFrameId_("imu_link"),
qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT) qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)
{ {
//ULogger::setType(ULogger::kTypeConsole); //ULogger::setType(ULogger::kTypeConsole);
//ULogger::setLevel(ULogger::kDebug); //ULogger::setLevel(ULogger::kDebug);
//ULogger::setEventLevel(ULogger::kWarning); //ULogger::setEventLevel(ULogger::kWarning);
//parse input arguments //parse input arguments
bool publishClock = false; bool publishClock = false;
publishClock = this->declare_parameter("publish_clock", publishClock); publishClock = this->declare_parameter("publish_clock", publishClock);
std::vector<std::string> tmpList = get_node_options().arguments(); std::vector<std::string> tmpList = get_node_options().arguments();
std::vector<std::string> argList; std::vector<std::string> argList;
for(unsigned int i=0; i<tmpList.size(); ++i) for(unsigned int i=0; i<tmpList.size(); ++i)
{ {
if(tmpList[i].compare("--clock") == 0) if(tmpList[i].compare("--clock") == 0)
{ {
publishClock = true; publishClock = true;
} }
} }
double rate = 1.0f; double rate = 1.0f;
std::string databasePath = ""; std::string databasePath = "";
bool publishTf = true; bool publishTf = true;
bool ignoreOdom = false; bool ignoreOdom = false;
int startId = 0; int startId = 0;
frameId_ = this->declare_parameter("frame_id", frameId_); frameId_ = this->declare_parameter("frame_id", frameId_);
odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_); odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_);
cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_); cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_);
scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_); scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_);
gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_); gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_);
gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_); 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 rate = this->declare_parameter("rate", rate); // Ratio of the database stamps
databasePath = this->declare_parameter("database", databasePath); databasePath = this->declare_parameter("database", databasePath);
publishTf = this->declare_parameter("publish_tf", publishTf); publishTf = this->declare_parameter("publish_tf", publishTf);
ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom); ignoreOdom = this->declare_parameter("ignore_odom", ignoreOdom);
startId = this->declare_parameter("start_id", startId); startId = this->declare_parameter("start_id", startId);
qos_ = this->declare_parameter("qos", qos_); 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 // A general 360 lidar with 0.5 deg increment
scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI); scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI);
scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI); scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI);
scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0); scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0);
scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0); scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0);
scanRangeMax_ = this->declare_parameter("scan_range_max", 60); scanRangeMax_ = this->declare_parameter("scan_range_max", 60);
RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str()); 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(), "odom_frame_id = %s", odomFrameId_.c_str());
RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.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(), "scan_frame_id = %s", scanFrameId_.c_str());
RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str()); RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str());
RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate); RCLCPP_INFO(get_logger(), "imu_frame_id = %s", imuFrameId_.c_str());
RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false"); RCLCPP_INFO(get_logger(), "rate (factor) = %f", rate);
RCLCPP_INFO(get_logger(), "start_id = %d", startId); RCLCPP_INFO(get_logger(), "publish_tf = %s", publishTf?"true":"false");
RCLCPP_INFO(get_logger(), "Publish clock (--clock): %s", publishClock?"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 = %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_);
if(databasePath.empty()) if(databasePath.empty())
{ {
RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database)."); RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
exit(-1); exit(-1);
} }
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir()); databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
if(databasePath.size() && databasePath.at(0) != '/') if(databasePath.size() && databasePath.at(0) != '/')
{ {
databasePath = UDirectory::currentDir(true) + databasePath; databasePath = UDirectory::currentDir(true) + databasePath;
} }
RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str()); RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str());
reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId)); reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId));
if(!reader_->init()) if(!reader_->init())
{ {
RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str()); RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
exit(-1); exit(-1);
} }
const std::string servicePrefix = get_name() + std::string("/"); 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)); 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)); 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) { if(publishTf) {
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this); tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
} }
if(publishClock) if(publishClock)
{ {
clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1); clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
} }
} }
DbPlayer::~DbPlayer(){} DbPlayer::~DbPlayer(){}
@@ -158,14 +175,14 @@ void DbPlayer::pauseCallback(
std::shared_ptr<std_srvs::srv::Empty::Response>) std::shared_ptr<std_srvs::srv::Empty::Response>)
{ {
if(paused_) if(paused_)
{ {
RCLCPP_WARN(get_logger(), "Already paused!"); RCLCPP_WARN(get_logger(), "Already paused!");
} }
else else
{ {
paused_ = true; paused_ = true;
RCLCPP_INFO(get_logger(), "paused!"); RCLCPP_INFO(get_logger(), "paused!");
} }
} }
void DbPlayer::resumeCallback( void DbPlayer::resumeCallback(
const std::shared_ptr<rmw_request_id_t>, const std::shared_ptr<rmw_request_id_t>,
@@ -173,140 +190,105 @@ void DbPlayer::resumeCallback(
std::shared_ptr<std_srvs::srv::Empty::Response>) std::shared_ptr<std_srvs::srv::Empty::Response>)
{ {
if(!paused_) if(!paused_)
{ {
RCLCPP_WARN(get_logger(), "Already running!"); RCLCPP_WARN(get_logger(), "Already running!");
} }
else else
{ {
paused_ = false; paused_ = false;
RCLCPP_INFO(get_logger(), "resumed!"); RCLCPP_INFO(get_logger(), "resumed!");
} }
} }
bool DbPlayer::publishNextFrame() void DbPlayer::initializePublishers(const rtabmap::OdometryEvent & odom)
{ {
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());
if(clockPub_.get())
{
rosgraph_msgs::msg::Clock msg;
msg.clock = time;
clockPub_->publish(msg);
}
sensor_msgs::msg::CameraInfo camInfoA; //rgb or left
sensor_msgs::msg::CameraInfo camInfoB; //depth or right
camInfoA.k.fill(0);
camInfoA.k[0] = camInfoA.k[4] = camInfoA.k[8] = 1;
camInfoA.r.fill(0);
camInfoA.r[0] = camInfoA.r[4] = camInfoA.r[8] = 1;
camInfoA.p.fill(0);
camInfoA.p[10] = 1;
camInfoA.header.frame_id = cameraFrameId_;
camInfoA.header.stamp = time;
camInfoB = camInfoA;
if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1)) if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
{ {
if(odom.data().cameraModels().size() > 1) if(odom.data().cameraModels().size() > 1)
{ {
RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); 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 else
{ {
//depth if(rgbPub_.getTopic().empty()) {
if(odom.data().cameraModels().size()) rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
{ RCLCPP_INFO(get_logger(), "Gray/RGB image \"%s\" will be published.", rgbPub_.getTopic().c_str());
camInfoA.d.resize(5,0); }
if(!rgbInfoPub_.get()) {
camInfoA.p[0] = odom.data().cameraModels()[0].fx(); rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
camInfoA.k[0] = odom.data().cameraModels()[0].fx(); RCLCPP_INFO(get_logger(), "Gray/RGB calibration \"%s\" will be published.", rgbInfoPub_->get_topic_name());
camInfoA.p[5] = odom.data().cameraModels()[0].fy(); }
camInfoA.k[4] = odom.data().cameraModels()[0].fy(); if(depthPub_.getTopic().empty()) {
camInfoA.p[2] = odom.data().cameraModels()[0].cx(); depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
camInfoA.k[2] = odom.data().cameraModels()[0].cx(); RCLCPP_INFO(get_logger(), "Depth image \"%s\" will be published.", depthPub_.getTopic().c_str());
camInfoA.p[6] = odom.data().cameraModels()[0].cy(); }
camInfoA.k[5] = odom.data().cameraModels()[0].cy(); if(!depthInfoPub_.get()) {
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
camInfoB = camInfoA; RCLCPP_INFO(get_logger(), "Depth calibration \"%s\" will be published.", depthInfoPub_->get_topic_name());
} }
if(rgbPub_.getTopic().empty()) rgbPub_ = image_transport::create_publisher(this, "rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
if(depthPub_.getTopic().empty()) depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
if(!rgbInfoPub_.get()) rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("rgb/camera_info", 1);
if(!depthInfoPub_.get()) depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("depth/camera_info", 1);
} }
} }
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U) 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(odom.data().stereoCameraModels().size() > 1)
{ {
RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet..."); 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 else
{ {
//stereo if(leftPub_.getTopic().empty()) {
if(odom.data().stereoCameraModels()[0].isValidForProjection()) leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
{ RCLCPP_INFO(get_logger(), "Left image \"%s\" will be published.", leftPub_.getTopic().c_str());
camInfoA.d.resize(8,0); }
if(!leftInfoPub_.get()) {
camInfoA.p[0] = odom.data().stereoCameraModels()[0].left().fx(); leftInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
camInfoA.k[0] = odom.data().stereoCameraModels()[0].left().fx(); RCLCPP_INFO(get_logger(), "Left calibration \"%s\" will be published.", leftInfoPub_->get_topic_name());
camInfoA.p[5] = odom.data().stereoCameraModels()[0].left().fy(); }
camInfoA.k[4] = odom.data().stereoCameraModels()[0].left().fy(); if(rightPub_.getTopic().empty()) {
camInfoA.p[2] = odom.data().stereoCameraModels()[0].left().cx(); rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
camInfoA.k[2] = odom.data().stereoCameraModels()[0].left().cx(); RCLCPP_INFO(get_logger(), "Right image \"%s\" will be published.", rightPub_.getTopic().c_str());
camInfoA.p[6] = odom.data().stereoCameraModels()[0].left().cy(); }
camInfoA.k[5] = odom.data().stereoCameraModels()[0].left().cy(); if(!rightInfoPub_.get()) {
rightInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCameraInfo_));
camInfoB = camInfoA; RCLCPP_INFO(get_logger(), "Right calibration \"%s\" will be published.", rightInfoPub_->get_topic_name());
camInfoB.p[3] = odom.data().stereoCameraModels()[0].right().Tx(); // Right_Tx = -baseline*fx
} }
if(leftPub_.getTopic().empty()) leftPub_ = image_transport::create_publisher(this, "left/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
if(rightPub_.getTopic().empty()) rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
if(!leftInfoPub_.get()) leftInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("left/camera_info", 1);
if(!rightInfoPub_.get()) rightInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>("right/camera_info", 1);
} }
} }
else else if(imagePub_.getTopic().empty())
{ {
if(imagePub_.getTopic().empty()) imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile()); imagePub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
RCLCPP_INFO(get_logger(), "Image \"%s\" without calibration will be published.", imagePub_.getTopic().c_str());
} }
camInfoA.height = odom.data().imageRaw().rows;
camInfoA.width = odom.data().imageRaw().cols;
camInfoB.height = odom.data().depthOrRightRaw().rows;
camInfoB.width = odom.data().depthOrRightRaw().cols;
if(!odom.data().laserScanRaw().isEmpty()) if(!odom.data().laserScanRaw().isEmpty())
{ {
if(!scanPub_.get() && odom.data().laserScanRaw().is2d()) if(!scanPub_.get() && odom.data().laserScanRaw().is2d())
{ {
scanPub_ = this->create_publisher<sensor_msgs::msg::LaserScan>("scan", 1); 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) if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
{ {
RCLCPP_INFO(get_logger(), "Scan will be published."); RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published.", scanPub_->get_topic_name());
} }
else else
{ {
RCLCPP_INFO(get_logger(), "Scan will be published with those parameters:"); 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_min=%f", scanAngleMin_);
RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_); RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_);
RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_); RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_);
@@ -316,8 +298,8 @@ bool DbPlayer::publishNextFrame()
} }
else if(!scanCloudPub_.get()) else if(!scanCloudPub_.get())
{ {
scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 1); scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosScanCloud_));
RCLCPP_INFO(get_logger(), "Scan cloud will be published."); RCLCPP_INFO(get_logger(), "PointCloud2 \"%s\" will be published.", scanCloudPub_->get_topic_name());
} }
} }
@@ -327,41 +309,121 @@ bool DbPlayer::publishNextFrame()
{ {
if(!globalPosePub_.get()) if(!globalPosePub_.get())
{ {
globalPosePub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1); 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 will be published."); RCLCPP_INFO(get_logger(), "Global pose \"%s\" will be published.", globalPosePub_->get_topic_name());
} }
} }
if(odom.data().gps().stamp() > 0.0) if(!gpsFixPub_.get() && odom.data().gps().stamp() > 0.0)
{ {
if(!gpsFixPub_.get()) 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());
gpsFixPub_ = this->create_publisher<sensor_msgs::msg::NavSatFix>("gps/fix", 1); }
RCLCPP_INFO(get_logger(), "GPS will be published.");
} 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 // publish transforms first
if(tfBroadcaster_.get()) if(tfBroadcaster_.get())
{ {
rtabmap::Transform localTransform;
if(odom.data().cameraModels().size() == 1)
{
localTransform = odom.data().cameraModels()[0].localTransform();
}
else if(odom.data().stereoCameraModels().size() == 1)
{
localTransform = odom.data().stereoCameraModels()[0].left().localTransform();
}
std::vector<geometry_msgs::msg::TransformStamped> transforms; std::vector<geometry_msgs::msg::TransformStamped> transforms;
if(!localTransform.isNull()) const std::vector<rtabmap::CameraModel> * models = &odom.data().cameraModels();
std::vector<rtabmap::CameraModel> stereoModels;
bool stereo = false;
if(odom.data().stereoCameraModels().size())
{ {
geometry_msgs::msg::TransformStamped baseToCamera; for(const auto & cam: odom.data().stereoCameraModels()) {
baseToCamera.child_frame_id = cameraFrameId_; stereoModels.push_back(cam.left());
baseToCamera.header.frame_id = frameId_; stereoModels.push_back(cam.right());
baseToCamera.header.stamp = time; }
rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform); models = &stereoModels;
transforms.push_back(baseToCamera); stereo = true;
}
int index = 0;
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_ + (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);
}
++index;
} }
if(!odom.pose().isNull()) if(!odom.pose().isNull())
@@ -392,26 +454,32 @@ bool DbPlayer::publishNextFrame()
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform); rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
transforms.push_back(worldToBase); transforms.push_back(worldToBase);
} }
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);
}
tfBroadcaster_->sendTransform(transforms); tfBroadcaster_->sendTransform(transforms);
} }
if(!odom.pose().isNull()) if( odometryPub_.get() &&
!odom.pose().isNull() &&
odometryPub_->get_subscription_count())
{ {
if(!odometryPub_.get()) odometryPub_ = this->create_publisher<nav_msgs::msg::Odometry>("odom", 1); nav_msgs::msg::Odometry odomMsg;
odomMsg.child_frame_id = frameId_;
if(odometryPub_->get_subscription_count()) odomMsg.header.frame_id = odomFrameId_;
{ odomMsg.header.stamp = time;
nav_msgs::msg::Odometry odomMsg; rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
odomMsg.child_frame_id = frameId_; UASSERT(odomMsg.pose.covariance.size() == 36 &&
odomMsg.header.frame_id = odomFrameId_; odom.covariance().total() == 36 &&
odomMsg.header.stamp = time; odom.covariance().type() == CV_64FC1);
rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose); memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
UASSERT(odomMsg.pose.covariance.size() == 36 && odometryPub_->publish(odomMsg);
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) // Publish async topics first (so that they can catched by rtabmap before the image topics)
@@ -444,86 +512,164 @@ bool DbPlayer::publishNextFrame()
gpsFixPub_->publish(msg); gpsFixPub_->publish(msg);
} }
if( (imagePub_.getNumSubscribers()) || if( imuPub_.get() &&
(rgbPub_.getNumSubscribers()) || imuPub_->get_subscription_count() > 0 &&
(leftPub_.getNumSubscribers())) !odom.data().imu().empty())
{ {
cv_bridge::CvImage img; sensor_msgs::msg::Imu msg;
if(odom.data().imageRaw().channels() == 1) rtabmap_conversions::imuToROS(odom.data().imu(), msg);
{ msg.header.frame_id = imuFrameId_;
img.encoding = sensor_msgs::image_encodings::MONO8; msg.header.stamp = time;
} imuPub_->publish(msg);
else }
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = odom.data().imageRaw();
sensor_msgs::msg::Image imageRosMsg;
img.toImageMsg(imageRosMsg);
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
if(imagePub_.getNumSubscribers()) // 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())))
{ {
imagePub_.publish(imageRosMsg); 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(rgbPub_.getNumSubscribers())
if(!odom.data().depthRaw().empty() && depthPub_.getNumSubscribers())
{ {
rgbPub_.publish(imageRosMsg); sensor_msgs::msg::Image imageRosMsg;
rgbInfoPub_->publish(camInfoA); 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(leftPub_.getNumSubscribers())
if(!odom.data().rightRaw().empty() && rightPub_.getNumSubscribers())
{ {
leftPub_.publish(imageRosMsg); sensor_msgs::msg::Image imageRosMsg;
leftInfoPub_->publish(camInfoA); 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);
}
} }
} }
if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty()) // Multi-cameras
if(!odom.data().imageRaw().empty())
{ {
cv_bridge::CvImage img; std::vector<rtabmap_msgs::msg::RGBDImage> rgbdImages;
if(odom.data().depthRaw().type() == CV_32FC1) if(odom.data().cameraModels().size() > 1)
{ {
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1; UASSERT(odom.data().cameraModels().size() == rgbdImagePubs_.size());
} int subRgbImageWidth = odom.data().imageRaw().cols / odom.data().cameraModels().size();
else int subDepthImageWidth = odom.data().depthRaw().cols / odom.data().cameraModels().size();
{ for(size_t i=0; i<rgbdImagePubs_.size(); ++i)
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1; {
} rtabmap_msgs::msg::RGBDImage msg;
img.image = odom.data().depthRaw(); msg.header.stamp = time;
sensor_msgs::msg::Image imageRosMsg; msg.header.frame_id = cameraFrameId_ + uNumber2Str((int)i);
img.toImageMsg(imageRosMsg);
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
depthPub_.publish(imageRosMsg); cvImageToROS(cv::Mat(odom.data().imageRaw(), cv::Range::all(), cv::Range(i*subRgbImageWidth, (i+1)*subRgbImageWidth)), msg.rgb);
depthInfoPub_->publish(camInfoB); msg.rgb.header = msg.header;
} rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.rgb_camera_info);
msg.rgb_camera_info.header = msg.header;
if(rightPub_.getNumSubscribers() && !odom.data().rightRaw().empty()) if(subDepthImageWidth) {
{ cvImageToROS(cv::Mat(odom.data().depthRaw(), cv::Range::all(), cv::Range(i*subDepthImageWidth, (i+1)*subDepthImageWidth)), msg.depth);
cv_bridge::CvImage img; msg.depth.header = msg.header;
if(odom.data().imageRaw().channels() == 1) UASSERT(subDepthImageWidth <= subRgbImageWidth);
{ if(subDepthImageWidth < subRgbImageWidth) {
img.encoding = sensor_msgs::image_encodings::MONO8; rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i].scaled(double(subDepthImageWidth)/double(subRgbImageWidth)), msg.depth_camera_info);
} }
else else {
{ rtabmap_conversions::cameraModelToROS(odom.data().cameraModels()[i], msg.depth_camera_info);
img.encoding = sensor_msgs::image_encodings::BGR8; }
} msg.depth_camera_info.header = msg.header;
img.image = odom.data().rightRaw(); }
sensor_msgs::msg::Image imageRosMsg;
img.toImageMsg(imageRosMsg);
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
rightPub_.publish(imageRosMsg); rgbdImagePubs_[i]->publish(msg);
rightInfoPub_->publish(camInfoB); }
}
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(!odom.data().laserScanRaw().isEmpty())
{ {
if(scanPub_.get() && scanPub_->get_subscription_count() && odom.data().laserScanRaw().is2d()) if(scanPub_.get() &&
scanPub_->get_subscription_count() &&
odom.data().laserScanRaw().is2d())
{ {
//inspired from pointcloud_to_laserscan package //inspired from pointcloud_to_laserscan package
sensor_msgs::msg::LaserScan msg; sensor_msgs::msg::LaserScan msg;
@@ -570,7 +716,8 @@ bool DbPlayer::publishNextFrame()
scanPub_->publish(msg); scanPub_->publish(msg);
} }
else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count()) else if(scanCloudPub_.get() &&
scanCloudPub_->get_subscription_count())
{ {
sensor_msgs::msg::PointCloud2 msg; sensor_msgs::msg::PointCloud2 msg;
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg); pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
rtabmap_util/src/nodelets/rgbd_split.cpp
+16 -1
View File
@@ -98,7 +98,22 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage); cv_bridge::toCvCopy(input->depth_compressed)->toImageMsg(outputImage);
#endif #endif
} }
outputImage.header = outputCameraInfo.header = input->header; if(outputCameraInfo.header.frame_id.empty()) {
if(outputImage.header.frame_id.empty()) {
outputCameraInfo.header = input->header;
}
else {
outputCameraInfo.header = outputImage.header;
}
}
if(outputImage.header.frame_id.empty()) {
if(outputCameraInfo.header.frame_id.empty()) {
outputImage.header = input->header;
}
else {
outputImage.header = outputCameraInfo.header;
}
}
depthPub_.publish(outputImage); depthPub_.publish(outputImage);
depthInfoPub_->publish(outputCameraInfo); depthInfoPub_->publish(outputCameraInfo);
} }
rtabmap_viz/CMakeLists.txt
+11
View File
@@ -68,12 +68,23 @@ SET_TARGET_PROPERTIES(
AUTORCC ON AUTORCC ON
) )
add_executable(rgbd_image_viewer src/RGBDImageViewerNode.cpp src/rgbd_image_viewer.cpp include/${PROJECT_NAME}/rgbd_image_viewer.hpp)
ament_target_dependencies(rgbd_image_viewer ${Libraries})
SET_TARGET_PROPERTIES(
rgbd_image_viewer
PROPERTIES
AUTOUIC ON
AUTOMOC ON
AUTORCC ON
)
############# #############
## Install ## ## Install ##
############# #############
install(TARGETS install(TARGETS
rtabmap_viz rtabmap_viz
rgbd_image_viewer
DESTINATION lib/${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}
) )
rtabmap_viz/include/rtabmap_viz/rgbd_image_viewer.hpp
+84
View File
@@ -0,0 +1,84 @@
/*
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.
*/
#ifndef RGBDIMAGEVIEWER_H_
#define RGBDIMAGEVIEWER_H_
#include <rtabmap_viz/visibility.h>
#include <rclcpp/rclcpp.hpp>
#include <QMainWindow>
#include "rtabmap_msgs/msg/rgbd_image.hpp"
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <rtabmap/core/Parameters.h>
#include <rtabmap/utilite/UEventsSender.h>
namespace rtabmap
{
class CameraViewer;
}
class QComboBox;
class QSpinBox;
class QLabel;
namespace rtabmap_viz {
class RGBDImageViewer : public QMainWindow, public UEventsSender
{
Q_OBJECT
public:
RTABMAP_VIZ_PUBLIC
explicit RGBDImageViewer(std::shared_ptr<rclcpp::Node> & node, const rtabmap::ParametersMap & parameters);
virtual ~RGBDImageViewer();
private Q_SLOTS:
void updateTopicList();
void topicSelected(const QString & topicName);
private:
void callback(const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg);
private:
QComboBox * topicComboBox_;
QComboBox * frameComboBox_;
QSpinBox * spinBox_;
QLabel * warningLabel_;
rtabmap::CameraViewer * cameraView_;
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
std::shared_ptr<rclcpp::Node> node_;
std::shared_ptr<tf2_ros::Buffer> tfBuffer_;
std::shared_ptr<tf2_ros::TransformListener> tfListener_;
std::mutex mutex_;
};
}
#endif /* RGBDIMAGEVIEWER_H_ */
rtabmap_viz/src/RGBDImageViewerNode.cpp
+83
View File
@@ -0,0 +1,83 @@
/*
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_viz/rgbd_image_viewer.hpp"
#include "rtabmap/utilite/ULogger.h"
#include <QApplication>
#include <rtabmap/gui/CameraViewer.h>
#include <rtabmap/utilite/ULogger.h>
#include <signal.h>
QApplication * app = 0;
void my_handler(int){
app->exit(-1);
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
app = new QApplication(argc, argv);
app->connect( app, SIGNAL( lastWindowClosed() ), app, SLOT( quit() ) );
int r;
{
auto node = std::make_shared<rclcpp::Node>("rgbd_image_viewer");
rtabmap::ParametersMap parameters = rtabmap::Parameters::parseArguments(argc, argv, true);
rtabmap_viz::RGBDImageViewer viewer(node, parameters);
viewer.show();
// Catch ctrl-c to close the gui
// (Place this after QApplication's constructor)
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = my_handler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
// Here start the ROS events loop
rclcpp::executors::SingleThreadedExecutor executor; //Use 1 thread
executor.add_node(node);
auto spin_executor = [&executor]() {
executor.spin();
};
// Launch executer
std::thread execution_thread(spin_executor);
// Now wait for application to finish
r = app->exec();// MUST be called by the Main Thread
rclcpp::shutdown();
execution_thread.join();
}
delete app;
return r;
}
rtabmap_viz/src/rgbd_image_viewer.cpp
+171
View File
@@ -0,0 +1,171 @@
/*
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_viz/rgbd_image_viewer.hpp"
#include <rtabmap_conversions/MsgConversion.h>
#include <rtabmap/gui/CameraViewer.h>
#include <rtabmap/core/SensorEvent.h>
#include <QComboBox>
#include <QHBoxLayout>
#include <QVBoxLayout>
#include <QPushButton>
#include <QSpinBox>
#include <QLabel>
namespace rtabmap_viz {
RGBDImageViewer::RGBDImageViewer(std::shared_ptr<rclcpp::Node> & node, const rtabmap::ParametersMap & parameters) :
node_(node)
{
this->setWindowTitle("rgbd_image_viewer");
topicComboBox_ = new QComboBox(this);
topicComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents);
topicComboBox_->setToolTip("Available rtabmap_msgs::RGBDImage topics");
frameComboBox_ = new QComboBox(this);
frameComboBox_->setSizeAdjustPolicy(QComboBox::SizeAdjustPolicy::AdjustToContents);
frameComboBox_->setToolTip("Base frame of the point cloud");
spinBox_ = new QSpinBox(this);
spinBox_->setMinimum(0);
spinBox_->setMaximum(1000);
spinBox_->setValue(10);
spinBox_->setSuffix(" ms");
spinBox_->setToolTip("Maximum time to wait for TF to transform in base frame (0 means latest available)");
warningLabel_ = new QLabel(this);
warningLabel_->setStyleSheet("QLabel { color : red; }");
cameraView_ = new rtabmap::CameraViewer(this, parameters);
cameraView_->registerToEventsManager();
QPushButton * refreshButton = new QPushButton(this);
refreshButton->setIcon(style()->standardIcon(QStyle::SP_BrowserReload));
refreshButton->setToolTip("Refresh topics and frames");
connect(topicComboBox_, SIGNAL(currentTextChanged(const QString &)), this, SLOT(topicSelected(const QString &)));
connect(cameraView_, SIGNAL(finished(int)), this, SLOT(close()));
connect(refreshButton, SIGNAL(clicked()), this, SLOT(updateTopicList()));
QWidget *centralWidget = new QWidget(this);
QVBoxLayout *layout = new QVBoxLayout(centralWidget);
QHBoxLayout *hlayout = new QHBoxLayout();
hlayout->addWidget(topicComboBox_);
hlayout->addWidget(frameComboBox_);
hlayout->addWidget(spinBox_);
hlayout->addWidget(refreshButton);
hlayout->addWidget(warningLabel_);
hlayout->addStretch();
layout->addLayout(hlayout);
layout->addWidget(cameraView_);
this->setCentralWidget(centralWidget);
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(node_->get_clock());
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
updateTopicList();
}
RGBDImageViewer::~RGBDImageViewer()
{
}
void RGBDImageViewer::updateTopicList() {
std::map<std::string, std::vector<std::string>> topicNames = node_->get_topic_names_and_types();
topicComboBox_->clear();
for(auto topic: topicNames) {
for(auto type: topic.second) {
if(type == "rtabmap_msgs/msg/RGBDImage") {
topicComboBox_->addItem(topic.first.c_str());
}
}
}
std::vector<std::string> frames = tfBuffer_->getAllFrameNames();
frameComboBox_->clear();
frameComboBox_->addItem("<camera>");
for(auto & frame: frames) {
frameComboBox_->addItem(frame.c_str());
}
}
void RGBDImageViewer::topicSelected(const QString & topicName) {
rgbdImageSub_.reset();
if(!topicName.isEmpty()) {
rgbdImageSub_ = node_->create_subscription<rtabmap_msgs::msg::RGBDImage>(topicName.toStdString(), rclcpp::QoS(1), std::bind(&RGBDImageViewer::callback, this, std::placeholders::_1));
}
}
void RGBDImageViewer::callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr msg)
{
bool warned = false;
rtabmap::SensorData data = rtabmap_conversions::rgbdImageFromROS(msg);
if(!frameComboBox_->currentText().isEmpty() && (!data.cameraModels().empty() || !data.stereoCameraModels().empty())) {
rtabmap::Transform localTransform;
if(frameComboBox_->currentText().compare("<camera>") == 0) {
localTransform = rtabmap::CameraModel::opticalRotation();
}
else {
localTransform = rtabmap_conversions::getTransform(
frameComboBox_->currentText().toStdString(),
msg->header.frame_id,
msg->header.stamp,
*tfBuffer_,
double(spinBox_->value()) / 1000.0);
}
if(localTransform.isNull())
{
QString log = QString("Could not get TF between \"%1\" and \"%2\" frames for stamp %3 after waiting %4 ms.")
.arg(frameComboBox_->currentText())
.arg(msg->header.frame_id.c_str())
.arg(QString::number(rclcpp::Time(msg->header.stamp).seconds(), 'f', 3))
.arg(spinBox_->value());
warningLabel_->setToolTip(log);
QMetaObject::invokeMethod(warningLabel_, "setText", Q_ARG(QString, log));
warned = true;
}
if(!data.cameraModels().empty()) {
rtabmap::CameraModel model = data.cameraModels()[0];
model.setLocalTransform(localTransform);
data.setCameraModel(model);
}
else {
rtabmap::StereoCameraModel model = data.stereoCameraModels()[0];
model.setLocalTransform(localTransform);
data.setStereoCameraModel(model);
}
}
if(!warned) {
warningLabel_->setToolTip("");
QMetaObject::invokeMethod(warningLabel_, "clear");
}
this->post(new rtabmap::SensorEvent(data));
}
}