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

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_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/camera_info.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/laser_scan.hpp>
#include <sensor_msgs/msg/point_cloud2.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/transform_broadcaster.h>
#include <rtabmap_msgs/msg/rgbd_image.hpp>
#include <rtabmap/core/DBReader.h>
#include <rtabmap/core/OdometryEvent.h>
namespace rtabmap_util
{
@@ -57,13 +60,15 @@ public:
RTABMAP_UTIL_PUBLIC
explicit DbPlayer(const rclcpp::NodeOptions & options);
virtual ~DbPlayer();
bool publishNextFrame();
bool isPaused() const {return paused_;}
void setPaused(bool enabled) {paused_ = enabled;}
bool publishNextFrame();
bool isPaused() const {return paused_;}
void setPaused(bool enabled) {paused_ = enabled;}
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 initializePublishers(const rtabmap::OdometryEvent & odom);
bool cvImageToROS(const cv::Mat & image, sensor_msgs::msg::Image & rosImage);
private:
bool paused_;
@@ -74,7 +79,15 @@ private:
std::string scanFrameId_;
std::string gtFrameId_;
std::string gtBaseFrameId_;
std::string imuFrameId_;
int qos_;
int qosCameraInfo_;
int qosOdom_;
int qosScan_;
int qosScanCloud_;
int qosGlobalPose_;
int qosGps_;
int qosImu_;
double scanAngleMin_;
double scanAngleMax_;
double scanAngleIncrement_;
@@ -89,15 +102,17 @@ private:
image_transport::Publisher depthPub_;
image_transport::Publisher leftPub_;
image_transport::Publisher rightPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rightInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr leftInfoPub_;
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<sensor_msgs::msg::LaserScan>::SharedPtr scanPub_;
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<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
+403 -256
View File
@@ -53,7 +53,7 @@ namespace rtabmap_util
{
DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
rclcpp::Node("db_player", options),
rclcpp::Node("db_player", options),
paused_(false),
frameId_("base_link"),
odomFrameId_("odom"),
@@ -61,93 +61,110 @@ DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
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);
//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;
}
}
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;
double rate = 1.0f;
std::string databasePath = "";
bool publishTf = true;
bool ignoreOdom = false;
int startId = 0;
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_);
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);
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(), "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(), "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_);
if(databasePath.empty())
{
RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
exit(-1);
}
{
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());
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);
}
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));
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);
}
{
clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
}
}
DbPlayer::~DbPlayer(){}
@@ -158,14 +175,14 @@ void DbPlayer::pauseCallback(
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
if(paused_)
{
RCLCPP_WARN(get_logger(), "Already paused!");
}
else
{
paused_ = true;
RCLCPP_INFO(get_logger(), "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>,
@@ -173,140 +190,105 @@ void DbPlayer::resumeCallback(
std::shared_ptr<std_srvs::srv::Empty::Response>)
{
if(!paused_)
{
RCLCPP_WARN(get_logger(), "Already running!");
}
else
{
paused_ = false;
RCLCPP_INFO(get_logger(), "resumed!");
}
{
RCLCPP_WARN(get_logger(), "Already running!");
}
else
{
paused_ = false;
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().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
{
//depth
if(odom.data().cameraModels().size())
{
camInfoA.d.resize(5,0);
camInfoA.p[0] = odom.data().cameraModels()[0].fx();
camInfoA.k[0] = odom.data().cameraModels()[0].fx();
camInfoA.p[5] = odom.data().cameraModels()[0].fy();
camInfoA.k[4] = odom.data().cameraModels()[0].fy();
camInfoA.p[2] = odom.data().cameraModels()[0].cx();
camInfoA.k[2] = odom.data().cameraModels()[0].cx();
camInfoA.p[6] = odom.data().cameraModels()[0].cy();
camInfoA.k[5] = odom.data().cameraModels()[0].cy();
camInfoB = camInfoA;
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());
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()) {
depthPub_ = image_transport::create_publisher(this, "depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
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());
}
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)
{
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
{
//stereo
if(odom.data().stereoCameraModels()[0].isValidForProjection())
{
camInfoA.d.resize(8,0);
camInfoA.p[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.k[0] = odom.data().stereoCameraModels()[0].left().fx();
camInfoA.p[5] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.k[4] = odom.data().stereoCameraModels()[0].left().fy();
camInfoA.p[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.k[2] = odom.data().stereoCameraModels()[0].left().cx();
camInfoA.p[6] = odom.data().stereoCameraModels()[0].left().cy();
camInfoA.k[5] = odom.data().stereoCameraModels()[0].left().cy();
camInfoB = camInfoA;
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());
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()) {
rightPub_ = image_transport::create_publisher(this, "right/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos_).get_rmw_qos_profile());
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());
}
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(!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)
{
RCLCPP_INFO(get_logger(), "Scan will be published.");
RCLCPP_INFO(get_logger(), "LaserScan \"%s\" will be published.", scanPub_->get_topic_name());
}
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_max=%f", scanAngleMax_);
RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_);
@@ -316,8 +298,8 @@ bool DbPlayer::publishNextFrame()
}
else if(!scanCloudPub_.get())
{
scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 1);
RCLCPP_INFO(get_logger(), "Scan cloud will be published.");
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());
}
}
@@ -327,41 +309,121 @@ bool DbPlayer::publishNextFrame()
{
if(!globalPosePub_.get())
{
globalPosePub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1);
RCLCPP_INFO(get_logger(), "Global pose will be published.");
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(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", 1);
RCLCPP_INFO(get_logger(), "GPS will be published.");
}
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(!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())
{
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;
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;
baseToCamera.child_frame_id = cameraFrameId_;
baseToCamera.header.frame_id = frameId_;
baseToCamera.header.stamp = time;
rtabmap_conversions::transformToGeometryMsg(localTransform, baseToCamera.transform);
transforms.push_back(baseToCamera);
for(const auto & cam: odom.data().stereoCameraModels()) {
stereoModels.push_back(cam.left());
stereoModels.push_back(cam.right());
}
models = &stereoModels;
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())
@@ -392,26 +454,32 @@ bool DbPlayer::publishNextFrame()
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
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);
}
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);
if(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);
}
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)
@@ -444,86 +512,164 @@ bool DbPlayer::publishNextFrame()
gpsFixPub_->publish(msg);
}
if( (imagePub_.getNumSubscribers()) ||
(rgbPub_.getNumSubscribers()) ||
(leftPub_.getNumSubscribers()))
if( imuPub_.get() &&
imuPub_->get_subscription_count() > 0 &&
!odom.data().imu().empty())
{
cv_bridge::CvImage img;
if(odom.data().imageRaw().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
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;
sensor_msgs::msg::Imu msg;
rtabmap_conversions::imuToROS(odom.data().imu(), msg);
msg.header.frame_id = imuFrameId_;
msg.header.stamp = time;
imuPub_->publish(msg);
}
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);
rgbInfoPub_->publish(camInfoA);
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(leftPub_.getNumSubscribers())
if(!odom.data().rightRaw().empty() && rightPub_.getNumSubscribers())
{
leftPub_.publish(imageRosMsg);
leftInfoPub_->publish(camInfoA);
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);
}
}
}
if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty())
// Multi-cameras
if(!odom.data().imageRaw().empty())
{
cv_bridge::CvImage img;
if(odom.data().depthRaw().type() == CV_32FC1)
std::vector<rtabmap_msgs::msg::RGBDImage> rgbdImages;
if(odom.data().cameraModels().size() > 1)
{
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
}
else
{
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
}
img.image = odom.data().depthRaw();
sensor_msgs::msg::Image imageRosMsg;
img.toImageMsg(imageRosMsg);
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
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);
depthPub_.publish(imageRosMsg);
depthInfoPub_->publish(camInfoB);
}
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(rightPub_.getNumSubscribers() && !odom.data().rightRaw().empty())
{
cv_bridge::CvImage img;
if(odom.data().imageRaw().channels() == 1)
{
img.encoding = sensor_msgs::image_encodings::MONO8;
}
else
{
img.encoding = sensor_msgs::image_encodings::BGR8;
}
img.image = odom.data().rightRaw();
sensor_msgs::msg::Image imageRosMsg;
img.toImageMsg(imageRosMsg);
imageRosMsg.header.frame_id = cameraFrameId_;
imageRosMsg.header.stamp = time;
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;
}
rightPub_.publish(imageRosMsg);
rightInfoPub_->publish(camInfoB);
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())
if(scanPub_.get() &&
scanPub_->get_subscription_count() &&
odom.data().laserScanRaw().is2d())
{
//inspired from pointcloud_to_laserscan package
sensor_msgs::msg::LaserScan msg;
@@ -570,7 +716,8 @@ bool DbPlayer::publishNextFrame()
scanPub_->publish(msg);
}
else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count())
else if(scanCloudPub_.get() &&
scanCloudPub_->get_subscription_count())
{
sensor_msgs::msg::PointCloud2 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);
#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);
depthInfoPub_->publish(outputCameraInfo);
}