mirror of
https://github.com/introlab/rtabmap_ros.git
synced 2026-10-12 12:39:50 +08:00
Ported data_player node to ROS2. Fixed camera publisher namespace in rgbd_split.
This commit is contained in:
matlabbe
@@ -76,6 +76,7 @@ SET(rtabmap_util_plugins_lib_src
|
|||||||
src/nodelets/point_cloud_aggregator.cpp
|
src/nodelets/point_cloud_aggregator.cpp
|
||||||
src/nodelets/point_cloud_assembler.cpp
|
src/nodelets/point_cloud_assembler.cpp
|
||||||
src/nodelets/imu_to_tf.cpp
|
src/nodelets/imu_to_tf.cpp
|
||||||
|
src/nodelets/db_player.cpp
|
||||||
src/nodelets/lidar_deskewing.cpp
|
src/nodelets/lidar_deskewing.cpp
|
||||||
src/nodelets/rgbd_relay.cpp
|
src/nodelets/rgbd_relay.cpp
|
||||||
src/nodelets/rgbd_split.cpp
|
src/nodelets/rgbd_split.cpp
|
||||||
@@ -134,6 +135,7 @@ rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay")
|
|||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDSplit")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DisparityToDepth")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DisparityToDepth")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ImuToTF")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::ImuToTF")
|
||||||
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::DbPlayer")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::LidarDeskewing")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::LidarDeskewing")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZ")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZ")
|
||||||
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZRGB")
|
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::PointCloudXYZRGB")
|
||||||
@@ -188,10 +190,10 @@ ament_target_dependencies(rtabmap_point_cloud_xyzrgb ${Libraries})
|
|||||||
target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins)
|
target_link_libraries(rtabmap_point_cloud_xyzrgb rtabmap_util_plugins)
|
||||||
set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb")
|
set_target_properties(rtabmap_point_cloud_xyzrgb PROPERTIES OUTPUT_NAME "point_cloud_xyzrgb")
|
||||||
|
|
||||||
#add_executable(rtabmap_data_player src/DbPlayerNode.cpp)
|
add_executable(rtabmap_data_player src/DbPlayerNode.cpp)
|
||||||
#ament_target_dependencies(rtabmap_data_player ${Libraries})
|
ament_target_dependencies(rtabmap_data_player ${Libraries})
|
||||||
#target_link_libraries(rtabmap_data_player rtabmap_util_plugins)
|
target_link_libraries(rtabmap_data_player rtabmap_util_plugins)
|
||||||
#set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player")
|
set_target_properties(rtabmap_data_player PROPERTIES OUTPUT_NAME "data_player")
|
||||||
|
|
||||||
#add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp)
|
#add_executable(rtabmap_odom_msg_to_tf src/OdomMsgToTFNode.cpp)
|
||||||
#ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries})
|
#ament_target_dependencies(rtabmap_odom_msg_to_tf ${Libraries})
|
||||||
@@ -250,7 +252,7 @@ install(TARGETS
|
|||||||
)
|
)
|
||||||
install(TARGETS
|
install(TARGETS
|
||||||
# rtabmap_map_optimizer
|
# rtabmap_map_optimizer
|
||||||
# rtabmap_data_player
|
rtabmap_data_player
|
||||||
# rtabmap_odom_msg_to_tf
|
# rtabmap_odom_msg_to_tf
|
||||||
rtabmap_imu_to_tf
|
rtabmap_imu_to_tf
|
||||||
rtabmap_disparity_to_depth
|
rtabmap_disparity_to_depth
|
||||||
|
|||||||
@@ -0,0 +1,105 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtabmap_util/visibility.h>
|
||||||
|
#include "rclcpp/rclcpp.hpp"
|
||||||
|
|
||||||
|
#include <sensor_msgs/msg/image.hpp>
|
||||||
|
#include <sensor_msgs/msg/camera_info.hpp>
|
||||||
|
#include <sensor_msgs/msg/laser_scan.hpp>
|
||||||
|
#include <sensor_msgs/msg/point_cloud2.hpp>
|
||||||
|
#include <sensor_msgs/msg/nav_sat_fix.hpp>
|
||||||
|
#include <image_transport/image_transport.hpp>
|
||||||
|
|
||||||
|
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||||
|
|
||||||
|
#include <nav_msgs/msg/odometry.hpp>
|
||||||
|
|
||||||
|
#include <rosgraph_msgs/msg/clock.hpp>
|
||||||
|
|
||||||
|
#include <std_srvs/srv/empty.hpp>
|
||||||
|
|
||||||
|
#include <tf2_ros/buffer.h>
|
||||||
|
#include <tf2_ros/transform_broadcaster.h>
|
||||||
|
|
||||||
|
#include <rtabmap/core/DBReader.h>
|
||||||
|
|
||||||
|
namespace rtabmap_util
|
||||||
|
{
|
||||||
|
|
||||||
|
class DbPlayer : public rclcpp::Node
|
||||||
|
{
|
||||||
|
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;}
|
||||||
|
|
||||||
|
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 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>);
|
||||||
|
|
||||||
|
private:
|
||||||
|
bool paused_;
|
||||||
|
std::shared_ptr<rtabmap::DBReader> reader_;
|
||||||
|
std::string frameId_;
|
||||||
|
std::string odomFrameId_;
|
||||||
|
std::string cameraFrameId_;
|
||||||
|
std::string scanFrameId_;
|
||||||
|
std::string gtFrameId_;
|
||||||
|
std::string gtBaseFrameId_;
|
||||||
|
int qos_;
|
||||||
|
double scanAngleMin_;
|
||||||
|
double scanAngleMax_;
|
||||||
|
double scanAngleIncrement_;
|
||||||
|
double scanRangeMin_;
|
||||||
|
double scanRangeMax_;
|
||||||
|
|
||||||
|
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr pauseSrv_;
|
||||||
|
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr resumeSrv_;
|
||||||
|
|
||||||
|
image_transport::Publisher imagePub_;
|
||||||
|
image_transport::Publisher rgbPub_;
|
||||||
|
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<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<rosgraph_msgs::msg::Clock>::SharedPtr clockPub_;
|
||||||
|
std::shared_ptr<tf2_ros::TransformBroadcaster> tfBroadcaster_;
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
@@ -50,8 +50,10 @@ public:
|
|||||||
private:
|
private:
|
||||||
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
|
rclcpp::Subscription<rtabmap_msgs::msg::RGBDImage>::SharedPtr rgbdImageSub_;
|
||||||
|
|
||||||
image_transport::CameraPublisher rgbPub_;
|
image_transport::Publisher rgbPub_;
|
||||||
image_transport::CameraPublisher depthPub_;
|
image_transport::Publisher depthPub_;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr rgbInfoPub_;
|
||||||
|
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr depthInfoPub_;
|
||||||
};
|
};
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,5 +1,5 @@
|
|||||||
/*
|
/*
|
||||||
Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
All rights reserved.
|
All rights reserved.
|
||||||
|
|
||||||
Redistribution and use in source and binary forms, with or without
|
Redistribution and use in source and binary forms, with or without
|
||||||
@@ -25,36 +25,9 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|||||||
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include "rtabmap_util/db_player.hpp"
|
||||||
#include <sensor_msgs/Image.h>
|
#include "rtabmap/utilite/ULogger.h"
|
||||||
#include <sensor_msgs/image_encodings.h>
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include <sensor_msgs/PointCloud2.h>
|
|
||||||
#include <sensor_msgs/LaserScan.h>
|
|
||||||
#include <sensor_msgs/CameraInfo.h>
|
|
||||||
#include <sensor_msgs/NavSatFix.h>
|
|
||||||
#include <geometry_msgs/PoseWithCovarianceStamped.h>
|
|
||||||
#include <rosgraph_msgs/Clock.h>
|
|
||||||
#include <pcl_conversions/pcl_conversions.h>
|
|
||||||
#include <nav_msgs/Odometry.h>
|
|
||||||
#ifdef PRE_ROS_IRON
|
|
||||||
#include <cv_bridge/cv_bridge.h>
|
|
||||||
#else
|
|
||||||
#include <cv_bridge/cv_bridge.hpp>
|
|
||||||
#endif
|
|
||||||
#include <image_transport/image_transport.h>
|
|
||||||
#include <tf2_ros/transform_broadcaster.h>
|
|
||||||
#include <std_srvs/Empty.h>
|
|
||||||
#include <rtabmap_conversions/MsgConversion.h>
|
|
||||||
#include <rtabmap_msgs/SetGoal.h>
|
|
||||||
#include <rtabmap/utilite/ULogger.h>
|
|
||||||
#include <rtabmap/utilite/UStl.h>
|
|
||||||
#include <rtabmap/utilite/UThread.h>
|
|
||||||
#include <rtabmap/utilite/UDirectory.h>
|
|
||||||
#include <rtabmap/utilite/UConversion.h>
|
|
||||||
#include <rtabmap/core/util3d.h>
|
|
||||||
#include <rtabmap/core/DBReader.h>
|
|
||||||
#include <rtabmap/core/OdometryEvent.h>
|
|
||||||
#include <cmath>
|
|
||||||
|
|
||||||
#ifndef _WIN32
|
#ifndef _WIN32
|
||||||
#include <sys/ioctl.h>
|
#include <sys/ioctl.h>
|
||||||
@@ -96,604 +69,58 @@ bool spacehit()
|
|||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
bool paused = false;
|
int main(int argc, char **argv)
|
||||||
bool pauseCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
|
||||||
{
|
{
|
||||||
if(paused)
|
ULogger::setType(ULogger::kTypeConsole);
|
||||||
{
|
|
||||||
ROS_WARN("Already paused!");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
paused = true;
|
|
||||||
ROS_INFO("paused!");
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
bool resumeCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
|
std::vector<std::string> arguments;
|
||||||
{
|
|
||||||
if(!paused)
|
|
||||||
{
|
|
||||||
ROS_WARN("Already running!");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
paused = false;
|
|
||||||
ROS_INFO("resumed!");
|
|
||||||
}
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
int main(int argc, char** argv)
|
|
||||||
{
|
|
||||||
ros::init(argc, argv, "data_player");
|
|
||||||
|
|
||||||
//ULogger::setType(ULogger::kTypeConsole);
|
|
||||||
//ULogger::setLevel(ULogger::kDebug);
|
|
||||||
//ULogger::setEventLevel(ULogger::kWarning);
|
|
||||||
|
|
||||||
bool publishClock = false;
|
|
||||||
for(int i=1;i<argc;++i)
|
for(int i=1;i<argc;++i)
|
||||||
{
|
{
|
||||||
if(strcmp(argv[i], "--clock") == 0)
|
arguments.push_back(argv[i]);
|
||||||
{
|
|
||||||
publishClock = true;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
ros::NodeHandle nh;
|
rclcpp::init(argc, argv);
|
||||||
ros::NodeHandle pnh("~");
|
rclcpp::NodeOptions options;
|
||||||
|
options.arguments(arguments);
|
||||||
|
|
||||||
std::string frameId = "base_link";
|
auto node = std::make_shared<rtabmap_util::DbPlayer>(options);
|
||||||
std::string odomFrameId = "odom";
|
|
||||||
std::string cameraFrameId = "camera_optical_link";
|
|
||||||
std::string scanFrameId = "base_laser_link";
|
|
||||||
std::string gtFrameId = "world";
|
|
||||||
std::string gtBaseFrameId = "base_link_gt";
|
|
||||||
double rate = 1.0f;
|
|
||||||
std::string databasePath = "";
|
|
||||||
bool publishTf = true;
|
|
||||||
int startId = 0;
|
|
||||||
bool useDbStamps = true;
|
|
||||||
|
|
||||||
pnh.param("frame_id", frameId, frameId);
|
rclcpp::Rate pauseRate(10);
|
||||||
pnh.param("odom_frame_id", odomFrameId, odomFrameId);
|
|
||||||
pnh.param("camera_frame_id", cameraFrameId, cameraFrameId);
|
|
||||||
pnh.param("scan_frame_id", scanFrameId, scanFrameId);
|
|
||||||
pnh.param("ground_truth_frame_id", gtFrameId, gtFrameId);
|
|
||||||
pnh.param("ground_truth_base_frame_id", gtBaseFrameId, gtBaseFrameId);
|
|
||||||
pnh.param("rate", rate, rate); // Ratio of the database stamps
|
|
||||||
pnh.param("database", databasePath, databasePath);
|
|
||||||
pnh.param("publish_tf", publishTf, publishTf);
|
|
||||||
pnh.param("start_id", startId, startId);
|
|
||||||
|
|
||||||
// A general 360 lidar with 0.5 deg increment
|
while(rclcpp::ok())
|
||||||
double scanAngleMin, scanAngleMax, scanAngleIncrement, scanRangeMin, scanRangeMax;
|
|
||||||
pnh.param<double>("scan_angle_min", scanAngleMin, -M_PI);
|
|
||||||
pnh.param<double>("scan_angle_max", scanAngleMax, M_PI);
|
|
||||||
pnh.param<double>("scan_angle_increment", scanAngleIncrement, M_PI / 720.0);
|
|
||||||
pnh.param<double>("scan_range_min", scanRangeMin, 0.0);
|
|
||||||
pnh.param<double>("scan_range_max", scanRangeMax, 60);
|
|
||||||
|
|
||||||
ROS_INFO("frame_id = %s", frameId.c_str());
|
|
||||||
ROS_INFO("odom_frame_id = %s", odomFrameId.c_str());
|
|
||||||
ROS_INFO("camera_frame_id = %s", cameraFrameId.c_str());
|
|
||||||
ROS_INFO("scan_frame_id = %s", scanFrameId.c_str());
|
|
||||||
ROS_INFO("ground_truth_frame_id = %s", gtFrameId.c_str());
|
|
||||||
ROS_INFO("rate (factor) = %f", rate);
|
|
||||||
ROS_INFO("publish_tf = %s", publishTf?"true":"false");
|
|
||||||
ROS_INFO("start_id = %d", startId);
|
|
||||||
ROS_INFO("Publish clock (--clock): %s", publishClock?"true":"false");
|
|
||||||
|
|
||||||
if(databasePath.empty())
|
|
||||||
{
|
{
|
||||||
ROS_ERROR("Parameter \"database\" must be set (path to a RTAB-Map database).");
|
if(!node->publishNextFrame()) {
|
||||||
return -1;
|
// end of file, exit
|
||||||
}
|
RCLCPP_INFO(node->get_logger(), "Last frame published, exiting!");
|
||||||
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
|
break;
|
||||||
if(databasePath.size() && databasePath.at(0) != '/')
|
|
||||||
{
|
|
||||||
databasePath = UDirectory::currentDir(true) + databasePath;
|
|
||||||
}
|
|
||||||
ROS_INFO("database = %s", databasePath.c_str());
|
|
||||||
|
|
||||||
rtabmap::DBReader reader(databasePath, -rate, false, false, false, startId);
|
|
||||||
if(!reader.init())
|
|
||||||
{
|
|
||||||
ROS_ERROR("Cannot open database \"%s\".", databasePath.c_str());
|
|
||||||
return -1;
|
|
||||||
}
|
|
||||||
|
|
||||||
ros::ServiceServer pauseSrv = pnh.advertiseService("pause", pauseCallback);
|
|
||||||
ros::ServiceServer resumeSrv = pnh.advertiseService("resume", resumeCallback);
|
|
||||||
|
|
||||||
image_transport::ImageTransport it(nh);
|
|
||||||
image_transport::Publisher imagePub;
|
|
||||||
image_transport::Publisher rgbPub;
|
|
||||||
image_transport::Publisher depthPub;
|
|
||||||
image_transport::Publisher leftPub;
|
|
||||||
image_transport::Publisher rightPub;
|
|
||||||
ros::Publisher rgbCamInfoPub;
|
|
||||||
ros::Publisher depthCamInfoPub;
|
|
||||||
ros::Publisher leftCamInfoPub;
|
|
||||||
ros::Publisher rightCamInfoPub;
|
|
||||||
ros::Publisher odometryPub;
|
|
||||||
ros::Publisher scanPub;
|
|
||||||
ros::Publisher scanCloudPub;
|
|
||||||
ros::Publisher globalPosePub;
|
|
||||||
ros::Publisher gpsFixPub;
|
|
||||||
ros::Publisher clockPub;
|
|
||||||
tf2_ros::TransformBroadcaster tfBroadcaster;
|
|
||||||
|
|
||||||
if(publishClock)
|
|
||||||
{
|
|
||||||
clockPub = nh.advertise<rosgraph_msgs::Clock>("/clock", 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
UTimer timer;
|
|
||||||
rtabmap::SensorCaptureInfo cameraInfo;
|
|
||||||
rtabmap::SensorData data = reader.takeImage(&cameraInfo);
|
|
||||||
rtabmap::OdometryInfo odomInfo;
|
|
||||||
odomInfo.reg.covariance = cameraInfo.odomCovariance;
|
|
||||||
rtabmap::OdometryEvent odom(data, cameraInfo.odomPose, odomInfo);
|
|
||||||
double acquisitionTime = timer.ticks();
|
|
||||||
while(ros::ok() && odom.data().id())
|
|
||||||
{
|
|
||||||
ROS_INFO("Reading sensor data %d...", odom.data().id());
|
|
||||||
|
|
||||||
ros::Time time(odom.data().stamp());
|
|
||||||
|
|
||||||
if(publishClock)
|
|
||||||
{
|
|
||||||
rosgraph_msgs::Clock msg;
|
|
||||||
msg.clock = time;
|
|
||||||
clockPub.publish(msg);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
sensor_msgs::CameraInfo camInfoA; //rgb or left
|
while(rclcpp::ok())
|
||||||
sensor_msgs::CameraInfo camInfoB; //depth or right
|
|
||||||
|
|
||||||
camInfoA.K.assign(0);
|
|
||||||
camInfoA.K[0] = camInfoA.K[4] = camInfoA.K[8] = 1;
|
|
||||||
camInfoA.R.assign(0);
|
|
||||||
camInfoA.R[0] = camInfoA.R[4] = camInfoA.R[8] = 1;
|
|
||||||
camInfoA.P.assign(0);
|
|
||||||
camInfoA.P[10] = 1;
|
|
||||||
|
|
||||||
camInfoA.header.frame_id = cameraFrameId;
|
|
||||||
camInfoA.header.stamp = time;
|
|
||||||
|
|
||||||
camInfoB = camInfoA;
|
|
||||||
|
|
||||||
int type = -1;
|
|
||||||
if(!odom.data().depthRaw().empty() && (odom.data().depthRaw().type() == CV_32FC1 || odom.data().depthRaw().type() == CV_16UC1))
|
|
||||||
{
|
|
||||||
if(odom.data().cameraModels().size() > 1)
|
|
||||||
{
|
|
||||||
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
|
|
||||||
}
|
|
||||||
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;
|
|
||||||
}
|
|
||||||
|
|
||||||
type=0;
|
|
||||||
|
|
||||||
if(rgbPub.getTopic().empty()) rgbPub = it.advertise("rgb/image", 1);
|
|
||||||
if(depthPub.getTopic().empty()) depthPub = it.advertise("depth_registered/image", 1);
|
|
||||||
if(rgbCamInfoPub.getTopic().empty()) rgbCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("rgb/camera_info", 1);
|
|
||||||
if(depthCamInfoPub.getTopic().empty()) depthCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("depth_registered/camera_info", 1);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(!odom.data().rightRaw().empty() && odom.data().rightRaw().type() == CV_8U)
|
|
||||||
{
|
|
||||||
if(odom.data().stereoCameraModels().size() > 1)
|
|
||||||
{
|
|
||||||
ROS_WARN("Multi-cameras detected in database but this node cannot send multi-images yet...");
|
|
||||||
}
|
|
||||||
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
|
|
||||||
}
|
|
||||||
|
|
||||||
type=1;
|
|
||||||
|
|
||||||
if(leftPub.getTopic().empty()) leftPub = it.advertise("left/image", 1);
|
|
||||||
if(rightPub.getTopic().empty()) rightPub = it.advertise("right/image", 1);
|
|
||||||
if(leftCamInfoPub.getTopic().empty()) leftCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("left/camera_info", 1);
|
|
||||||
if(rightCamInfoPub.getTopic().empty()) rightCamInfoPub = nh.advertise<sensor_msgs::CameraInfo>("right/camera_info", 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
if(imagePub.getTopic().empty()) imagePub = it.advertise("image", 1);
|
|
||||||
}
|
|
||||||
|
|
||||||
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.getTopic().empty() && odom.data().laserScanRaw().is2d())
|
|
||||||
{
|
|
||||||
scanPub = nh.advertise<sensor_msgs::LaserScan>("scan", 1);
|
|
||||||
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
|
|
||||||
{
|
|
||||||
ROS_INFO("Scan will be published.");
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
ROS_INFO("Scan will be published with those parameters:");
|
|
||||||
ROS_INFO(" scan_angle_min=%f", scanAngleMin);
|
|
||||||
ROS_INFO(" scan_angle_max=%f", scanAngleMax);
|
|
||||||
ROS_INFO(" scan_angle_increment=%f", scanAngleIncrement);
|
|
||||||
ROS_INFO(" scan_range_min=%f", scanRangeMin);
|
|
||||||
ROS_INFO(" scan_range_max=%f", scanRangeMax);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
else if(scanCloudPub.getTopic().empty())
|
|
||||||
{
|
|
||||||
scanCloudPub = nh.advertise<sensor_msgs::PointCloud2>("scan_cloud", 1);
|
|
||||||
ROS_INFO("Scan cloud will be published.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!odom.data().globalPose().isNull() &&
|
|
||||||
odom.data().globalPoseCovariance().cols==6 &&
|
|
||||||
odom.data().globalPoseCovariance().rows==6)
|
|
||||||
{
|
|
||||||
if(globalPosePub.getTopic().empty())
|
|
||||||
{
|
|
||||||
globalPosePub = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("global_pose", 1);
|
|
||||||
ROS_INFO("Global pose will be published.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(odom.data().gps().stamp() > 0.0)
|
|
||||||
{
|
|
||||||
if(gpsFixPub.getTopic().empty())
|
|
||||||
{
|
|
||||||
gpsFixPub = nh.advertise<sensor_msgs::NavSatFix>("gps/fix", 1);
|
|
||||||
ROS_INFO("GPS will be published.");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// publish transforms first
|
|
||||||
if(publishTf)
|
|
||||||
{
|
|
||||||
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::TransformStamped> transforms;
|
|
||||||
if(!localTransform.isNull())
|
|
||||||
{
|
|
||||||
geometry_msgs::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);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!odom.pose().isNull())
|
|
||||||
{
|
|
||||||
geometry_msgs::TransformStamped odomToBase;
|
|
||||||
odomToBase.child_frame_id = frameId;
|
|
||||||
odomToBase.header.frame_id = odomFrameId;
|
|
||||||
odomToBase.header.stamp = time;
|
|
||||||
rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
|
|
||||||
transforms.push_back(odomToBase);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!scanPub.getTopic().empty() || !scanCloudPub.getTopic().empty())
|
|
||||||
{
|
|
||||||
geometry_msgs::TransformStamped baseToLaserScan;
|
|
||||||
baseToLaserScan.child_frame_id = scanFrameId;
|
|
||||||
baseToLaserScan.header.frame_id = frameId;
|
|
||||||
baseToLaserScan.header.stamp = time;
|
|
||||||
rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
|
|
||||||
transforms.push_back(baseToLaserScan);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!odom.data().groundTruth().isNull()) {
|
|
||||||
geometry_msgs::TransformStamped worldToBase;
|
|
||||||
worldToBase.child_frame_id = gtBaseFrameId;
|
|
||||||
worldToBase.header.frame_id = gtFrameId;
|
|
||||||
worldToBase.header.stamp = time;
|
|
||||||
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
|
|
||||||
transforms.push_back(worldToBase);
|
|
||||||
}
|
|
||||||
tfBroadcaster.sendTransform(transforms);
|
|
||||||
}
|
|
||||||
if(!odom.pose().isNull())
|
|
||||||
{
|
|
||||||
if(odometryPub.getTopic().empty()) odometryPub = nh.advertise<nav_msgs::Odometry>("odom", 1);
|
|
||||||
|
|
||||||
if(odometryPub.getNumSubscribers())
|
|
||||||
{
|
|
||||||
nav_msgs::Odometry odomMsg;
|
|
||||||
odomMsg.child_frame_id = frameId;
|
|
||||||
odomMsg.header.frame_id = odomFrameId;
|
|
||||||
odomMsg.header.stamp = time;
|
|
||||||
rtabmap_conversions::transformToPoseMsg(odom.pose(), odomMsg.pose.pose);
|
|
||||||
UASSERT(odomMsg.pose.covariance.size() == 36 &&
|
|
||||||
odom.covariance().total() == 36 &&
|
|
||||||
odom.covariance().type() == CV_64FC1);
|
|
||||||
memcpy(odomMsg.pose.covariance.begin(), odom.covariance().data, 36*sizeof(double));
|
|
||||||
odometryPub.publish(odomMsg);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Publish async topics first (so that they can catched by rtabmap before the image topics)
|
|
||||||
if(globalPosePub.getNumSubscribers() > 0 &&
|
|
||||||
!odom.data().globalPose().isNull() &&
|
|
||||||
odom.data().globalPoseCovariance().cols==6 &&
|
|
||||||
odom.data().globalPoseCovariance().rows==6)
|
|
||||||
{
|
|
||||||
geometry_msgs::PoseWithCovarianceStamped msg;
|
|
||||||
rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
|
|
||||||
memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
|
|
||||||
msg.header.frame_id = frameId;
|
|
||||||
msg.header.stamp = time;
|
|
||||||
globalPosePub.publish(msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(odom.data().gps().stamp() > 0.0)
|
|
||||||
{
|
|
||||||
sensor_msgs::NavSatFix msg;
|
|
||||||
msg.longitude = odom.data().gps().longitude();
|
|
||||||
msg.latitude = odom.data().gps().latitude();
|
|
||||||
msg.altitude = odom.data().gps().altitude();
|
|
||||||
msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
|
|
||||||
msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
|
|
||||||
msg.header.frame_id = frameId;
|
|
||||||
msg.header.stamp.fromSec(odom.data().gps().stamp());
|
|
||||||
gpsFixPub.publish(msg);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(type >= 0)
|
|
||||||
{
|
|
||||||
if(rgbCamInfoPub.getNumSubscribers() && type == 0)
|
|
||||||
{
|
|
||||||
rgbCamInfoPub.publish(camInfoA);
|
|
||||||
}
|
|
||||||
if(leftCamInfoPub.getNumSubscribers() && type == 1)
|
|
||||||
{
|
|
||||||
leftCamInfoPub.publish(camInfoA);
|
|
||||||
}
|
|
||||||
if(depthCamInfoPub.getNumSubscribers() && type == 0)
|
|
||||||
{
|
|
||||||
depthCamInfoPub.publish(camInfoB);
|
|
||||||
}
|
|
||||||
if(rightCamInfoPub.getNumSubscribers() && type == 1)
|
|
||||||
{
|
|
||||||
rightCamInfoPub.publish(camInfoB);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(imagePub.getNumSubscribers() || rgbPub.getNumSubscribers() || leftPub.getNumSubscribers())
|
|
||||||
{
|
|
||||||
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::ImagePtr imageRosMsg = img.toImageMsg();
|
|
||||||
imageRosMsg->header.frame_id = cameraFrameId;
|
|
||||||
imageRosMsg->header.stamp = time;
|
|
||||||
|
|
||||||
if(imagePub.getNumSubscribers())
|
|
||||||
{
|
|
||||||
imagePub.publish(imageRosMsg);
|
|
||||||
}
|
|
||||||
if(rgbPub.getNumSubscribers() && type == 0)
|
|
||||||
{
|
|
||||||
rgbPub.publish(imageRosMsg);
|
|
||||||
}
|
|
||||||
if(leftPub.getNumSubscribers() && type == 1)
|
|
||||||
{
|
|
||||||
leftPub.publish(imageRosMsg);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(depthPub.getNumSubscribers() && !odom.data().depthRaw().empty() && type==0)
|
|
||||||
{
|
|
||||||
cv_bridge::CvImage img;
|
|
||||||
if(odom.data().depthRaw().type() == CV_32FC1)
|
|
||||||
{
|
|
||||||
img.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
img.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
|
|
||||||
}
|
|
||||||
img.image = odom.data().depthRaw();
|
|
||||||
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
|
|
||||||
imageRosMsg->header.frame_id = cameraFrameId;
|
|
||||||
imageRosMsg->header.stamp = time;
|
|
||||||
|
|
||||||
depthPub.publish(imageRosMsg);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(rightPub.getNumSubscribers() && !odom.data().rightRaw().empty() && type==1)
|
|
||||||
{
|
|
||||||
cv_bridge::CvImage img;
|
|
||||||
img.encoding = sensor_msgs::image_encodings::MONO8;
|
|
||||||
img.image = odom.data().rightRaw();
|
|
||||||
sensor_msgs::ImagePtr imageRosMsg = img.toImageMsg();
|
|
||||||
imageRosMsg->header.frame_id = cameraFrameId;
|
|
||||||
imageRosMsg->header.stamp = time;
|
|
||||||
|
|
||||||
rightPub.publish(imageRosMsg);
|
|
||||||
}
|
|
||||||
|
|
||||||
if(!odom.data().laserScanRaw().isEmpty())
|
|
||||||
{
|
|
||||||
if(scanPub.getNumSubscribers() && odom.data().laserScanRaw().is2d())
|
|
||||||
{
|
|
||||||
//inspired from pointcloud_to_laserscan package
|
|
||||||
sensor_msgs::LaserScan msg;
|
|
||||||
msg.header.frame_id = scanFrameId;
|
|
||||||
msg.header.stamp = time;
|
|
||||||
|
|
||||||
msg.angle_min = scanAngleMin;
|
|
||||||
msg.angle_max = scanAngleMax;
|
|
||||||
msg.angle_increment = scanAngleIncrement;
|
|
||||||
msg.time_increment = 0.0;
|
|
||||||
msg.scan_time = 0;
|
|
||||||
msg.range_min = scanRangeMin;
|
|
||||||
msg.range_max = scanRangeMax;
|
|
||||||
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
|
|
||||||
{
|
|
||||||
msg.angle_min = odom.data().laserScanRaw().angleMin();
|
|
||||||
msg.angle_max = odom.data().laserScanRaw().angleMax();
|
|
||||||
msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
|
|
||||||
msg.range_min = odom.data().laserScanRaw().rangeMin();
|
|
||||||
msg.range_max = odom.data().laserScanRaw().rangeMax();
|
|
||||||
}
|
|
||||||
|
|
||||||
uint32_t rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
|
|
||||||
msg.ranges.assign(rangesSize, 0.0);
|
|
||||||
|
|
||||||
const cv::Mat & scan = odom.data().laserScanRaw().data();
|
|
||||||
for (int i=0; i<scan.cols; ++i)
|
|
||||||
{
|
|
||||||
const float * ptr = scan.ptr<float>(0,i);
|
|
||||||
double range = hypot(ptr[0], ptr[1]);
|
|
||||||
if (range >= msg.range_min && range <=msg.range_max)
|
|
||||||
{
|
|
||||||
double angle = atan2(ptr[1], ptr[0]);
|
|
||||||
if (angle >= msg.angle_min && angle <= msg.angle_max)
|
|
||||||
{
|
|
||||||
int index = (angle - msg.angle_min) / msg.angle_increment;
|
|
||||||
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
|
|
||||||
{
|
|
||||||
msg.ranges[index] = range;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
scanPub.publish(msg);
|
|
||||||
}
|
|
||||||
else if(scanCloudPub.getNumSubscribers())
|
|
||||||
{
|
|
||||||
sensor_msgs::PointCloud2 msg;
|
|
||||||
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
|
|
||||||
msg.header.frame_id = scanFrameId;
|
|
||||||
msg.header.stamp = time;
|
|
||||||
scanCloudPub.publish(msg);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
if(odom.data().userDataRaw().type() == CV_8SC1 &&
|
|
||||||
odom.data().userDataRaw().cols >= 7 && // including null str ending
|
|
||||||
odom.data().userDataRaw().rows == 1 &&
|
|
||||||
memcmp(odom.data().userDataRaw().data, "GOAL:", 5) == 0)
|
|
||||||
{
|
|
||||||
//GOAL format detected, remove it from the user data and send it as goal event
|
|
||||||
std::string goalStr = (const char *)odom.data().userDataRaw().data;
|
|
||||||
if(!goalStr.empty())
|
|
||||||
{
|
|
||||||
std::list<std::string> strs = uSplit(goalStr, ':');
|
|
||||||
if(strs.size() == 2)
|
|
||||||
{
|
|
||||||
int goalId = atoi(strs.rbegin()->c_str());
|
|
||||||
|
|
||||||
if(goalId > 0)
|
|
||||||
{
|
|
||||||
ROS_WARN("Goal %d detected, calling rtabmap's set_goal service!", goalId);
|
|
||||||
rtabmap_msgs::SetGoal setGoalSrv;
|
|
||||||
setGoalSrv.request.node_id = goalId;
|
|
||||||
setGoalSrv.request.node_label = "";
|
|
||||||
if(!ros::service::call("set_goal", setGoalSrv))
|
|
||||||
{
|
|
||||||
ROS_ERROR("Can't call \"set_goal\" service");
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
ros::spinOnce();
|
|
||||||
|
|
||||||
while(ros::ok())
|
|
||||||
{
|
{
|
||||||
#ifndef _WIN32
|
#ifndef _WIN32
|
||||||
if (spacehit()) {
|
if (spacehit()) {
|
||||||
paused = !paused;
|
node->setPaused(!node->isPaused());
|
||||||
if(paused)
|
if(node->isPaused())
|
||||||
{
|
{
|
||||||
ROS_INFO("paused!");
|
RCLCPP_INFO(node->get_logger(), "paused!");
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
ROS_INFO("resumed!");
|
RCLCPP_INFO(node->get_logger(), "resumed!");
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if(!paused)
|
if(!node->isPaused())
|
||||||
{
|
{
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
||||||
uSleep(100);
|
pauseRate.sleep();
|
||||||
ros::spinOnce();
|
rclcpp::spin_some(node);
|
||||||
}
|
}
|
||||||
|
|
||||||
timer.restart();
|
|
||||||
cameraInfo = rtabmap::CameraInfo();
|
|
||||||
data = reader.takeImage(&cameraInfo);
|
|
||||||
odomInfo.reg.covariance = cameraInfo.odomCovariance;
|
|
||||||
odom = rtabmap::OdometryEvent(data, cameraInfo.odomPose, odomInfo);
|
|
||||||
acquisitionTime = timer.ticks();
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,592 @@
|
|||||||
|
/*
|
||||||
|
Copyright (c) 2010-2025, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
|
||||||
|
All rights reserved.
|
||||||
|
|
||||||
|
Redistribution and use in source and binary forms, with or without
|
||||||
|
modification, are permitted provided that the following conditions are met:
|
||||||
|
* Redistributions of source code must retain the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer.
|
||||||
|
* Redistributions in binary form must reproduce the above copyright
|
||||||
|
notice, this list of conditions and the following disclaimer in the
|
||||||
|
documentation and/or other materials provided with the distribution.
|
||||||
|
* Neither the name of the Universite de Sherbrooke nor the
|
||||||
|
names of its contributors may be used to endorse or promote products
|
||||||
|
derived from this software without specific prior written permission.
|
||||||
|
|
||||||
|
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
|
||||||
|
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||||
|
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||||
|
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
|
||||||
|
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||||
|
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||||
|
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||||
|
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||||
|
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||||
|
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <rtabmap_util/db_player.hpp>
|
||||||
|
#include <sensor_msgs/image_encodings.hpp>
|
||||||
|
|
||||||
|
#include <image_transport/image_transport.hpp>
|
||||||
|
|
||||||
|
#include <rtabmap_conversions/MsgConversion.h>
|
||||||
|
|
||||||
|
#include <rtabmap/utilite/ULogger.h>
|
||||||
|
#include <rtabmap/utilite/UStl.h>
|
||||||
|
#include <rtabmap/utilite/UThread.h>
|
||||||
|
#include <rtabmap/utilite/UDirectory.h>
|
||||||
|
#include <rtabmap/utilite/UConversion.h>
|
||||||
|
#include <rtabmap/core/util3d.h>
|
||||||
|
#include <rtabmap/core/OdometryEvent.h>
|
||||||
|
#include <rtabmap/core/SensorData.h>
|
||||||
|
|
||||||
|
#include <pcl_conversions/pcl_conversions.h>
|
||||||
|
|
||||||
|
#ifdef PRE_ROS_IRON
|
||||||
|
#include <cv_bridge/cv_bridge.h>
|
||||||
|
#else
|
||||||
|
#include <cv_bridge/cv_bridge.hpp>
|
||||||
|
#endif
|
||||||
|
|
||||||
|
namespace rtabmap_util
|
||||||
|
{
|
||||||
|
|
||||||
|
DbPlayer::DbPlayer(const rclcpp::NodeOptions & options) :
|
||||||
|
rclcpp::Node("db_player", options),
|
||||||
|
paused_(false),
|
||||||
|
frameId_("base_link"),
|
||||||
|
odomFrameId_("odom"),
|
||||||
|
cameraFrameId_("camera_optical_link"),
|
||||||
|
scanFrameId_("base_laser_link"),
|
||||||
|
gtFrameId_("world"),
|
||||||
|
gtBaseFrameId_("base_link_gt"),
|
||||||
|
qos_(RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT)
|
||||||
|
{
|
||||||
|
//ULogger::setType(ULogger::kTypeConsole);
|
||||||
|
//ULogger::setLevel(ULogger::kDebug);
|
||||||
|
//ULogger::setEventLevel(ULogger::kWarning);
|
||||||
|
|
||||||
|
//parse input arguments
|
||||||
|
bool publishClock = false;
|
||||||
|
publishClock = this->declare_parameter("publish_clock", publishClock);
|
||||||
|
std::vector<std::string> tmpList = get_node_options().arguments();
|
||||||
|
std::vector<std::string> argList;
|
||||||
|
for(unsigned int i=0; i<tmpList.size(); ++i)
|
||||||
|
{
|
||||||
|
if(tmpList[i].compare("--clock") == 0)
|
||||||
|
{
|
||||||
|
publishClock = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
double rate = 1.0f;
|
||||||
|
std::string databasePath = "";
|
||||||
|
bool publishTf = true;
|
||||||
|
bool ignoreOdom = false;
|
||||||
|
int startId = 0;
|
||||||
|
frameId_ = this->declare_parameter("frame_id", frameId_);
|
||||||
|
odomFrameId_ = this->declare_parameter("odom_frame_id", odomFrameId_);
|
||||||
|
cameraFrameId_ = this->declare_parameter("camera_frame_id", cameraFrameId_);
|
||||||
|
scanFrameId_ = this->declare_parameter("scan_frame_id", scanFrameId_);
|
||||||
|
gtFrameId_ = this->declare_parameter("ground_truth_frame_id", gtFrameId_);
|
||||||
|
gtBaseFrameId_ = this->declare_parameter("ground_truth_base_frame_id", gtBaseFrameId_);
|
||||||
|
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_);
|
||||||
|
|
||||||
|
// A general 360 lidar with 0.5 deg increment
|
||||||
|
scanAngleMin_ = this->declare_parameter("scan_angle_min", -M_PI);
|
||||||
|
scanAngleMax_ = this->declare_parameter("scan_angle_max", M_PI);
|
||||||
|
scanAngleIncrement_ = this->declare_parameter("scan_angle_increment", M_PI / 720.0);
|
||||||
|
scanRangeMin_ = this->declare_parameter("scan_range_min", 0.0);
|
||||||
|
scanRangeMax_ = this->declare_parameter("scan_range_max", 60);
|
||||||
|
|
||||||
|
RCLCPP_INFO(get_logger(), "frame_id = %s", frameId_.c_str());
|
||||||
|
RCLCPP_INFO(get_logger(), "odom_frame_id = %s", odomFrameId_.c_str());
|
||||||
|
RCLCPP_INFO(get_logger(), "camera_frame_id = %s", cameraFrameId_.c_str());
|
||||||
|
RCLCPP_INFO(get_logger(), "scan_frame_id = %s", scanFrameId_.c_str());
|
||||||
|
RCLCPP_INFO(get_logger(), "ground_truth_frame_id = %s", gtFrameId_.c_str());
|
||||||
|
RCLCPP_INFO(get_logger(), "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_);
|
||||||
|
|
||||||
|
if(databasePath.empty())
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(get_logger(), "Parameter \"database\" must be set (path to a RTAB-Map database).");
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
databasePath = uReplaceChar(databasePath, '~', UDirectory::homeDir());
|
||||||
|
if(databasePath.size() && databasePath.at(0) != '/')
|
||||||
|
{
|
||||||
|
databasePath = UDirectory::currentDir(true) + databasePath;
|
||||||
|
}
|
||||||
|
RCLCPP_INFO(get_logger(), "database = %s", databasePath.c_str());
|
||||||
|
|
||||||
|
reader_.reset(new rtabmap::DBReader(databasePath, -rate, ignoreOdom, false, false, startId));
|
||||||
|
if(!reader_->init())
|
||||||
|
{
|
||||||
|
RCLCPP_ERROR(get_logger(), "Cannot open database \"%s\".", databasePath.c_str());
|
||||||
|
exit(-1);
|
||||||
|
}
|
||||||
|
|
||||||
|
const std::string servicePrefix = get_name() + std::string("/");
|
||||||
|
pauseSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "pause", std::bind(&DbPlayer::pauseCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||||
|
resumeSrv_ = this->create_service<std_srvs::srv::Empty>(servicePrefix + "resume", std::bind(&DbPlayer::resumeCallback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
|
||||||
|
|
||||||
|
if(publishTf) {
|
||||||
|
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(publishClock)
|
||||||
|
{
|
||||||
|
clockPub_ = this->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
DbPlayer::~DbPlayer(){}
|
||||||
|
|
||||||
|
void DbPlayer::pauseCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<std_srvs::srv::Empty::Request>,
|
||||||
|
std::shared_ptr<std_srvs::srv::Empty::Response>)
|
||||||
|
{
|
||||||
|
if(paused_)
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(get_logger(), "Already paused!");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
paused_ = true;
|
||||||
|
RCLCPP_INFO(get_logger(), "paused!");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
void DbPlayer::resumeCallback(
|
||||||
|
const std::shared_ptr<rmw_request_id_t>,
|
||||||
|
const std::shared_ptr<std_srvs::srv::Empty::Request>,
|
||||||
|
std::shared_ptr<std_srvs::srv::Empty::Response>)
|
||||||
|
{
|
||||||
|
if(!paused_)
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(get_logger(), "Already running!");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
paused_ = false;
|
||||||
|
RCLCPP_INFO(get_logger(), "resumed!");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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());
|
||||||
|
|
||||||
|
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...");
|
||||||
|
}
|
||||||
|
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());
|
||||||
|
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)
|
||||||
|
{
|
||||||
|
if(odom.data().stereoCameraModels().size() > 1)
|
||||||
|
{
|
||||||
|
RCLCPP_WARN(get_logger(), "Multi-cameras detected in database but this node cannot send multi-images yet...");
|
||||||
|
}
|
||||||
|
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());
|
||||||
|
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
|
||||||
|
{
|
||||||
|
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());
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(get_logger(), "Scan will be published.");
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
RCLCPP_INFO(get_logger(), "Scan will be published with those parameters:");
|
||||||
|
RCLCPP_INFO(get_logger(), " scan_angle_min=%f", scanAngleMin_);
|
||||||
|
RCLCPP_INFO(get_logger(), " scan_angle_max=%f", scanAngleMax_);
|
||||||
|
RCLCPP_INFO(get_logger(), " scan_angle_increment=%f", scanAngleIncrement_);
|
||||||
|
RCLCPP_INFO(get_logger(), " scan_range_min=%f", scanRangeMin_);
|
||||||
|
RCLCPP_INFO(get_logger(), " scan_range_max=%f", scanRangeMax_);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(!scanCloudPub_.get())
|
||||||
|
{
|
||||||
|
scanCloudPub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>("scan_cloud", 1);
|
||||||
|
RCLCPP_INFO(get_logger(), "Scan cloud will be published.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!odom.data().globalPose().isNull() &&
|
||||||
|
odom.data().globalPoseCovariance().cols==6 &&
|
||||||
|
odom.data().globalPoseCovariance().rows==6)
|
||||||
|
{
|
||||||
|
if(!globalPosePub_.get())
|
||||||
|
{
|
||||||
|
globalPosePub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("global_pose", 1);
|
||||||
|
RCLCPP_INFO(get_logger(), "Global pose will be published.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(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.");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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())
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!odom.pose().isNull())
|
||||||
|
{
|
||||||
|
geometry_msgs::msg::TransformStamped odomToBase;
|
||||||
|
odomToBase.child_frame_id = frameId_;
|
||||||
|
odomToBase.header.frame_id = odomFrameId_;
|
||||||
|
odomToBase.header.stamp = time;
|
||||||
|
rtabmap_conversions::transformToGeometryMsg(odom.pose(), odomToBase.transform);
|
||||||
|
transforms.push_back(odomToBase);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(scanPub_.get() || scanCloudPub_.get())
|
||||||
|
{
|
||||||
|
geometry_msgs::msg::TransformStamped baseToLaserScan;
|
||||||
|
baseToLaserScan.child_frame_id = scanFrameId_;
|
||||||
|
baseToLaserScan.header.frame_id = frameId_;
|
||||||
|
baseToLaserScan.header.stamp = time;
|
||||||
|
rtabmap_conversions::transformToGeometryMsg(odom.data().laserScanCompressed().localTransform(), baseToLaserScan.transform);
|
||||||
|
transforms.push_back(baseToLaserScan);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!odom.data().groundTruth().isNull()) {
|
||||||
|
geometry_msgs::msg::TransformStamped worldToBase;
|
||||||
|
worldToBase.child_frame_id = gtBaseFrameId_;
|
||||||
|
worldToBase.header.frame_id = gtFrameId_;
|
||||||
|
worldToBase.header.stamp = time;
|
||||||
|
rtabmap_conversions::transformToGeometryMsg(odom.data().groundTruth(), worldToBase.transform);
|
||||||
|
transforms.push_back(worldToBase);
|
||||||
|
}
|
||||||
|
tfBroadcaster_->sendTransform(transforms);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!odom.pose().isNull())
|
||||||
|
{
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Publish async topics first (so that they can catched by rtabmap before the image topics)
|
||||||
|
if( globalPosePub_.get() &&
|
||||||
|
globalPosePub_->get_subscription_count() > 0 &&
|
||||||
|
!odom.data().globalPose().isNull() &&
|
||||||
|
odom.data().globalPoseCovariance().cols==6 &&
|
||||||
|
odom.data().globalPoseCovariance().rows==6)
|
||||||
|
{
|
||||||
|
geometry_msgs::msg::PoseWithCovarianceStamped msg;
|
||||||
|
rtabmap_conversions::transformToPoseMsg(odom.data().globalPose(), msg.pose.pose);
|
||||||
|
memcpy(msg.pose.covariance.data(), odom.data().globalPoseCovariance().data, 36*sizeof(double));
|
||||||
|
msg.header.frame_id = frameId_;
|
||||||
|
msg.header.stamp = time;
|
||||||
|
globalPosePub_->publish(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
if( gpsFixPub_.get() &&
|
||||||
|
gpsFixPub_->get_subscription_count() > 0 &&
|
||||||
|
odom.data().gps().stamp() > 0.0)
|
||||||
|
{
|
||||||
|
sensor_msgs::msg::NavSatFix msg;
|
||||||
|
msg.longitude = odom.data().gps().longitude();
|
||||||
|
msg.latitude = odom.data().gps().latitude();
|
||||||
|
msg.altitude = odom.data().gps().altitude();
|
||||||
|
msg.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_DIAGONAL_KNOWN;
|
||||||
|
msg.position_covariance.at(0) = msg.position_covariance.at(4) = msg.position_covariance.at(8)= odom.data().gps().error()* odom.data().gps().error();
|
||||||
|
msg.header.frame_id = frameId_;
|
||||||
|
msg.header.stamp = rtabmap_conversions::timestampToROS(odom.data().gps().stamp());
|
||||||
|
gpsFixPub_->publish(msg);
|
||||||
|
}
|
||||||
|
|
||||||
|
if( (imagePub_.getNumSubscribers()) ||
|
||||||
|
(rgbPub_.getNumSubscribers()) ||
|
||||||
|
(leftPub_.getNumSubscribers()))
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
|
||||||
|
if(imagePub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
imagePub_.publish(imageRosMsg);
|
||||||
|
}
|
||||||
|
if(rgbPub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
rgbPub_.publish(imageRosMsg);
|
||||||
|
rgbInfoPub_->publish(camInfoA);
|
||||||
|
}
|
||||||
|
if(leftPub_.getNumSubscribers())
|
||||||
|
{
|
||||||
|
leftPub_.publish(imageRosMsg);
|
||||||
|
leftInfoPub_->publish(camInfoA);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if(depthPub_.getNumSubscribers() && !odom.data().depthRaw().empty())
|
||||||
|
{
|
||||||
|
cv_bridge::CvImage img;
|
||||||
|
if(odom.data().depthRaw().type() == CV_32FC1)
|
||||||
|
{
|
||||||
|
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;
|
||||||
|
|
||||||
|
depthPub_.publish(imageRosMsg);
|
||||||
|
depthInfoPub_->publish(camInfoB);
|
||||||
|
}
|
||||||
|
|
||||||
|
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;
|
||||||
|
|
||||||
|
rightPub_.publish(imageRosMsg);
|
||||||
|
rightInfoPub_->publish(camInfoB);
|
||||||
|
}
|
||||||
|
|
||||||
|
if(!odom.data().laserScanRaw().isEmpty())
|
||||||
|
{
|
||||||
|
if(scanPub_.get() && scanPub_->get_subscription_count() && odom.data().laserScanRaw().is2d())
|
||||||
|
{
|
||||||
|
//inspired from pointcloud_to_laserscan package
|
||||||
|
sensor_msgs::msg::LaserScan msg;
|
||||||
|
msg.header.frame_id = scanFrameId_;
|
||||||
|
msg.header.stamp = time;
|
||||||
|
|
||||||
|
msg.angle_min = scanAngleMin_;
|
||||||
|
msg.angle_max = scanAngleMax_;
|
||||||
|
msg.angle_increment = scanAngleIncrement_;
|
||||||
|
msg.time_increment = 0.0;
|
||||||
|
msg.scan_time = 0;
|
||||||
|
msg.range_min = scanRangeMin_;
|
||||||
|
msg.range_max = scanRangeMax_;
|
||||||
|
if(odom.data().laserScanRaw().angleIncrement() > 0.0f)
|
||||||
|
{
|
||||||
|
msg.angle_min = odom.data().laserScanRaw().angleMin();
|
||||||
|
msg.angle_max = odom.data().laserScanRaw().angleMax();
|
||||||
|
msg.angle_increment = odom.data().laserScanRaw().angleIncrement();
|
||||||
|
msg.range_min = odom.data().laserScanRaw().rangeMin();
|
||||||
|
msg.range_max = odom.data().laserScanRaw().rangeMax();
|
||||||
|
}
|
||||||
|
|
||||||
|
int rangesSize = std::ceil((msg.angle_max - msg.angle_min) / msg.angle_increment);
|
||||||
|
msg.ranges.assign(rangesSize, 0.0);
|
||||||
|
|
||||||
|
const cv::Mat & scan = odom.data().laserScanRaw().data();
|
||||||
|
for (int i=0; i<scan.cols; ++i)
|
||||||
|
{
|
||||||
|
const float * ptr = scan.ptr<float>(0,i);
|
||||||
|
double range = hypot(ptr[0], ptr[1]);
|
||||||
|
if (range >= msg.range_min && range <=msg.range_max)
|
||||||
|
{
|
||||||
|
double angle = atan2(ptr[1], ptr[0]);
|
||||||
|
if (angle >= msg.angle_min && angle <= msg.angle_max)
|
||||||
|
{
|
||||||
|
int index = (angle - msg.angle_min) / msg.angle_increment;
|
||||||
|
if (index>=0 && index<rangesSize && (range < msg.ranges[index] || msg.ranges[index]==0))
|
||||||
|
{
|
||||||
|
msg.ranges[index] = range;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
scanPub_->publish(msg);
|
||||||
|
}
|
||||||
|
else if(scanCloudPub_.get() && scanCloudPub_->get_subscription_count())
|
||||||
|
{
|
||||||
|
sensor_msgs::msg::PointCloud2 msg;
|
||||||
|
pcl_conversions::moveFromPCL(*rtabmap::util3d::laserScanToPointCloud2(odom.data().laserScanRaw()), msg);
|
||||||
|
msg.header.frame_id = scanFrameId_;
|
||||||
|
msg.header.stamp = time;
|
||||||
|
scanCloudPub_->publish(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
#include "rclcpp_components/register_node_macro.hpp"
|
||||||
|
|
||||||
|
// Register the component with class_loader.
|
||||||
|
// This acts as a sort of entry point, allowing the component to be discoverable when its library
|
||||||
|
// is being loaded into a running process.
|
||||||
|
RCLCPP_COMPONENTS_REGISTER_NODE(rtabmap_util::DbPlayer)
|
||||||
@@ -46,8 +46,10 @@ RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
|
|||||||
|
|
||||||
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
|
rgbdImageSub_ = create_subscription<rtabmap_msgs::msg::RGBDImage>("rgbd_image", rclcpp::QoS(5).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&RGBDSplit::callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
rgbPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
rgbPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/rgb/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||||
depthPub_ = image_transport::create_camera_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
depthPub_ = image_transport::create_publisher(this, std::string(rgbdImageSub_->get_topic_name()) + "/depth/image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
|
||||||
|
rgbInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/rgb/camera_info", 1);
|
||||||
|
depthInfoPub_ = this->create_publisher<sensor_msgs::msg::CameraInfo>(std::string(rgbdImageSub_->get_topic_name()) + "/depth/camera_info", 1);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -73,7 +75,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
|||||||
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
|
cv_bridge::toCvCopy(input->rgb_compressed)->toImageMsg(outputImage);
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
rgbPub_.publish(outputImage, outputCameraInfo);
|
rgbPub_.publish(outputImage);
|
||||||
|
rgbInfoPub_->publish(outputCameraInfo);
|
||||||
}
|
}
|
||||||
|
|
||||||
if(depthPub_.getNumSubscribers())
|
if(depthPub_.getNumSubscribers())
|
||||||
@@ -96,7 +99,8 @@ void RGBDSplit::callback(const rtabmap_msgs::msg::RGBDImage::SharedPtr input) co
|
|||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
outputImage.header = outputCameraInfo.header = input->header;
|
outputImage.header = outputCameraInfo.header = input->header;
|
||||||
depthPub_.publish(outputImage, outputCameraInfo);
|
depthPub_.publish(outputImage);
|
||||||
|
depthInfoPub_->publish(outputCameraInfo);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user