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

ROS2 various QOL updates (preparing new binary release) (#1225)

Browse Source 
* Updating examples and README

* updated main readme

* odom: Added always_check_imu_tf parameter. sync: increased default sync_queue_size from 2 to 5 (to be more flexible to different hardware), added input and output diagnostics. rtabmap_viz: fixed node with same name redeclared warning.

* update readme

* Added husky demo

* converted demo_robot_mapping.launch to ros2

* Converted stereo_outdoor demo from ros1 to ros2

* Converted multi-session demo to ros2

* Converted demo_find_object launch to ros2

* renamed files

* uniformized default qos, increased sync_queue_size default to 10

* Added vlp16 + imu example

* Added husky 3D lidar demos

* moved demos in subdir

* Added vlp16+zed example

* rtabmap_viz: ignore odom update if tf not ready when not using topic (to avoid showing red screen). Added champ VSLAM demo.

* forwarded camera_model arg for zed examples, removed qos for turtlebot4 demo

* pointcloud_to_depthimage: added more error logs, removed output camera_info published twice and match ros1 namespaces

* Created two base general examples for 3D lidar usage, then create hardware specific examples on top of them.

* Added isaac sim demo

* Final update of all demos. Fixed MapCloud rviz plugin crashing when floor/ceiling filtering is used and resulting cloud is empty.

* Fixed 2d scan deskewing output frame, moved nav2 params under "params" folder, added number of topics processed/dropped for odometry, added icp_odometry option for turtlebot3 scan-only demo.

* rtabmap_demos: added README with examples

* Added TOC

* bump version to 0.21.9
This commit is contained in:
matlabbe
2024-11-30 17:28:16 -08:00
committed by GitHub
parent e9aa8ed082
commit 3f6adad463
116 changed files with 6102 additions and 575 deletions
Download Patch File Download Diff File Expand all files Collapse all files
rtabmap_util/include/rtabmap_util/pointcloud_to_depthimage.hpp
+2 -2
View File
@@ -57,8 +57,8 @@ private:
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoMsg);
private:
image_transport::CameraPublisher depthImage16Pub_;
image_transport::CameraPublisher depthImage32Pub_;
image_transport::Publisher depthImage16Pub_;
image_transport::Publisher depthImage32Pub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo16Pub_;
rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr cameraInfo32Pub_;
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pointCloudTransformedPub_;
rtabmap_util/package.xml
+1 -1
View File
@@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>rtabmap_util</name>
<version>0.21.5</version>
<version>0.21.9</version>
<description>RTAB-Map's various useful nodes and nodelets.</description>
<maintainer email="[email protected]">Mathieu Labbe</maintainer>
<author>Mathieu Labbe</author>
rtabmap_util/src/PointCloudAssemblerNode.cpp
+2
View File
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudAssembler>(rclcpp::NodeOptions()));
rclcpp::shutdown();
rtabmap_util/src/PointCloudToDepthImageNode.cpp
+3
View File
@@ -25,10 +25,13 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include <rtabmap/utilite/ULogger.h>
#include "rtabmap_util/pointcloud_to_depthimage.hpp"
int main(int argc, char **argv)
{
ULogger::setType(ULogger::kTypeConsole);
ULogger::setLevel(ULogger::kWarning);
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<rtabmap_util::PointCloudToDepthImage>(rclcpp::NodeOptions()));
rclcpp::shutdown();
rtabmap_util/src/nodelets/disparity_to_depth.cpp
+1 -1
View File
@@ -42,7 +42,7 @@ namespace rtabmap_util
DisparityToDepth::DisparityToDepth(const rclcpp::NodeOptions & options) :
rclcpp::Node("disparity_to_depth", options)
{
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
pub32f_ = image_transport::create_publisher(this, "depth", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
rtabmap_util/src/nodelets/imu_to_tf.cpp
+1 -1
View File
@@ -42,7 +42,7 @@ ImuToTF::ImuToTF(const rclcpp::NodeOptions & options) :
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
tfBroadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
baseFrameId_ = this->declare_parameter("base_frame_id", baseFrameId_);
qos = this->declare_parameter("qos", qos);
rtabmap_util/src/nodelets/lidar_deskewing.cpp
+2 -5
View File
@@ -14,14 +14,10 @@ LidarDeskewing::LidarDeskewing(const rclcpp::NodeOptions & options) :
slerp_(false)
{
tfBuffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
//auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
// this->get_node_base_interface(),
// this->get_node_timers_interface());
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int queueSize = 5;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
queueSize = this->declare_parameter("queue_size", queueSize);
qos = this->declare_parameter("qos", qos);
fixedFrameId_ = this->declare_parameter("fixed_frame_id", fixedFrameId_);
@@ -77,6 +73,7 @@ void LidarDeskewing::callbackScan(const sensor_msgs::msg::LaserScan::ConstShared
sensor_msgs::msg::PointCloud2 scanOutDeskewed;
rtabmap_conversions::transformPointCloud(t.toEigen4f(), scanOut, scanOutDeskewed);
scanOutDeskewed.header.frame_id = msg->header.frame_id;
pubScan_->publish(scanOutDeskewed);
}
rtabmap_util/src/nodelets/obstacles_detection.cpp
+1 -1
View File
@@ -55,7 +55,7 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
frameId_ = this->declare_parameter("frame_id", frameId_);
mapFrameId_ = this->declare_parameter("map_frame_id", mapFrameId_);
waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
rtabmap::ParametersMap gridParameters = rtabmap::Parameters::getDefaultParameters("Grid");
rtabmap_util/src/nodelets/point_cloud_aggregator.cpp
+1 -1
View File
@@ -64,7 +64,7 @@ PointCloudAggregator::PointCloudAggregator(const rclcpp::NodeOptions & options)
int count = 2;
bool approx=true;
double approxSyncMaxInterval = 0.0;
int qos=0;
int qos=RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
rtabmap_util/src/nodelets/point_cloud_assembler.cpp
+3 -3
View File
@@ -73,9 +73,9 @@ PointCloudAssembler::PointCloudAssembler(const rclcpp::NodeOptions & options) :
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int topicQueueSize = 1;
int syncQueueSize = 5;
int qos = 0;
int topicQueueSize = 10;
int syncQueueSize = 10;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool subscribeOdomInfo = false;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
rtabmap_util/src/nodelets/point_cloud_xyz.cpp
+1 -1
View File
@@ -68,7 +68,7 @@ PointCloudXYZ::PointCloudXYZ(const rclcpp::NodeOptions & options) :
{
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool approxSync = true;
std::string roiStr;
double approxSyncMaxInterval = 0.0;
rtabmap_util/src/nodelets/point_cloud_xyzrgb.cpp
+1 -1
View File
@@ -75,7 +75,7 @@ PointCloudXYZRGB::PointCloudXYZRGB(const rclcpp::NodeOptions & options) :
std::string roiStr;
int topicQueueSize = 1;
int syncQueueSize = 10;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
double approxSyncMaxInterval = 0.0;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
rtabmap_util/src/nodelets/pointcloud_to_depthimage.cpp
+13 -6
View File
@@ -60,9 +60,9 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
//tfBuffer_->setCreateTimerInterface(timer_interface);
tfListener_ = std::make_shared<tf2_ros::TransformListener>(*tfBuffer_);
int topicQueueSize = 1;
int topicQueueSize = 10;
int syncQueueSize = 10;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
bool approx = true;
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
@@ -107,8 +107,8 @@ PointCloudToDepthImage::PointCloudToDepthImage(const rclcpp::NodeOptions & optio
RCLCPP_INFO(this->get_logger(), " decimation=%d", decimation_);
RCLCPP_INFO(this->get_logger(), " upscale=%s (upscale_depth_error_ratio=%f)", upscale_?"true":"false", upscaleDepthErrorRatio_);
depthImage16Pub_ = image_transport::create_camera_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_camera_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
depthImage16Pub_ = image_transport::create_publisher(this, "image_raw", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile()); // 16 bits unsigned in mm
depthImage32Pub_ = image_transport::create_publisher(this, "image", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());// 32 bits float in meters
pointCloudTransformedPub_ = create_publisher<sensor_msgs::msg::PointCloud2>("cloud_transformed", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos));
cameraInfo16Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage16Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
cameraInfo32Pub_ = create_publisher<sensor_msgs::msg::CameraInfo>(depthImage32Pub_.getTopic()+"/camera_info", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qosCamInfo));
@@ -159,6 +159,10 @@ void PointCloudToDepthImage::callback(
if(cloudDisplacement.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, accordingly to %s, aborting!",
pointCloud2Msg->header.frame_id.c_str(),
cameraInfoMsg->header.frame_id.c_str(),
fixedFrameId_.c_str());
return;
}
@@ -171,6 +175,9 @@ void PointCloudToDepthImage::callback(
if(cloudToCamera.isNull())
{
RCLCPP_ERROR(this->get_logger(), "Could not find transform between %s and %s, aborting!",
pointCloud2Msg->header.frame_id.c_str(),
cameraInfoMsg->header.frame_id.c_str());
return;
}
rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
@@ -239,7 +246,7 @@ void PointCloudToDepthImage::callback(
if(depthImage32Pub_.getNumSubscribers())
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
depthImage32Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
depthImage32Pub_.publish(depthImage.toImageMsg());
if(cameraInfo32Pub_->get_subscription_count())
{
cameraInfo32Pub_->publish(cameraInfoMsgOut);
@@ -250,7 +257,7 @@ void PointCloudToDepthImage::callback(
{
depthImage.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
depthImage.image = rtabmap::util2d::cvtDepthFromFloat(depthImage.image);
depthImage16Pub_.publish(depthImage.toImageMsg(), cameraInfoMsg);
depthImage16Pub_.publish(depthImage.toImageMsg());
if(cameraInfo16Pub_->get_subscription_count())
{
cameraInfo16Pub_->publish(cameraInfoMsgOut);
rtabmap_util/src/nodelets/rgbd_relay.cpp
+1 -1
View File
@@ -52,7 +52,7 @@ RGBDRelay::RGBDRelay(const rclcpp::NodeOptions & options) :
compress_(false),
uncompress_(false)
{
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
compress_ = this->declare_parameter("compress", compress_);
uncompress_ = this->declare_parameter("uncompress", uncompress_);
rtabmap_util/src/nodelets/rgbd_split.cpp
+1 -1
View File
@@ -39,7 +39,7 @@ namespace rtabmap_util
RGBDSplit::RGBDSplit(const rclcpp::NodeOptions & options) :
Node("rgbd_split", options)
{
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
qos = this->declare_parameter("qos", qos);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);