i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-08 02:37:45 +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_sync/src/nodelets/rgb_sync.cpp
+4 -3
View File
@@ -52,9 +52,9 @@ RGBSync::RGBSync(const rclcpp::NodeOptions & options) :
exactSync_(0)
{
int topicQueueSize = 10;
int syncQueueSize = 2;
int syncQueueSize = 10;
bool approxSync = true;
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);
@@ -138,7 +138,7 @@ void RGBSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr image,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
syncDiagnostic_->tick(image->header.stamp);
syncDiagnostic_->tickInput(image->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double stamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
@@ -188,6 +188,7 @@ void RGBSync::callback(
stamp, rtabmap_conversions::timestampFromROS(image->header.stamp));
}
}
syncDiagnostic_->tickOutput(image->header.stamp);
}
}
rtabmap_sync/src/nodelets/rgbd_sync.cpp
+19 -14
View File
@@ -50,16 +50,16 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
depthScale_(1.0),
decimation_(1),
compressedRate_(0),
approxSyncMaxInterval_(0.0),
approxSyncDepth_(0),
exactSyncDepth_(0)
{
int topicQueueSize = 10;
int syncQueueSize = 2;
int syncQueueSize = 10;
bool approxSync = true;
double approxSyncMaxInterval = 0.0;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
approxSyncMaxInterval_ = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval_);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
int queueSize = this->declare_parameter("queue_size", -1);
if(queueSize != -1)
@@ -84,7 +84,7 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
RCLCPP_INFO(this->get_logger(), "%s: approx_sync = %s", get_name(), approxSync?"true":"false");
if(approxSync)
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval);
RCLCPP_INFO(this->get_logger(), "%s: approx_sync_max_interval = %f", get_name(), approxSyncMaxInterval_);
RCLCPP_INFO(this->get_logger(), "%s: topic_queue_size = %d", get_name(), topicQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: sync_queue_size = %d", get_name(), syncQueueSize);
RCLCPP_INFO(this->get_logger(), "%s: qos = %d", get_name(), qos);
@@ -99,8 +99,8 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
if(approxSync)
{
approxSyncDepth_ = new message_filters::Synchronizer<MyApproxSyncDepthPolicy>(MyApproxSyncDepthPolicy(syncQueueSize), imageSub_, imageDepthSub_, cameraInfoSub_);
if(approxSyncMaxInterval > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
if(approxSyncMaxInterval_ > 0.0)
approxSyncDepth_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval_));
approxSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
else
@@ -109,15 +109,18 @@ RGBDSync::RGBDSync(const rclcpp::NodeOptions & options) :
exactSyncDepth_->registerCallback(std::bind(&RGBDSync::callback, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3));
}
image_transport::TransportHints hints(this);
imageSub_.subscribe(this, "rgb/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, "depth/image", hints.getTransport(), rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
std::string rgbImageTransport = this->declare_parameter<std::string>("rgb_image_transport", "raw");
std::string depthImageTransport = this->declare_parameter<std::string>("depth_image_transport", "raw");
std::string rgbTopic = this->get_node_topics_interface()->resolve_topic_name("rgb/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
std::string depthTopic = this->get_node_topics_interface()->resolve_topic_name("depth/image"); // Humble doesn't resolve base topic, fixed by https://github.com/ros-perception/image_common/commit/ea7589ae8c1f7ecb83d6aab7b4c890c2d630d27a
imageSub_.subscribe(this, rgbTopic, rgbImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
imageDepthSub_.subscribe(this, depthTopic, depthImageTransport, rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qos).get_rmw_qos_profile());
cameraInfoSub_.subscribe(this, "rgb/camera_info", rclcpp::QoS(topicQueueSize).reliability((rmw_qos_reliability_policy_t)qosCamInfo).get_rmw_qos_profile());
std::string subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s,\n %s,\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
approxSync&&approxSyncMaxInterval_!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval_).c_str():"",
imageSub_.getSubscriber().getTopic().c_str(),
imageDepthSub_.getSubscriber().getTopic().c_str(),
cameraInfoSub_.getSubscriber()->get_topic_name());
@@ -149,19 +152,20 @@ void RGBDSync::callback(
const sensor_msgs::msg::Image::ConstSharedPtr depth,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfo)
{
syncDiagnostic_->tick(image->header.stamp);
syncDiagnostic_->tickInput(image->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double rgbStamp = rtabmap_conversions::timestampFromROS(image->header.stamp);
double depthStamp = rtabmap_conversions::timestampFromROS(depth->header.stamp);
double stampDiff = fabs(rgbStamp - depthStamp);
if(stampDiff > 0.010)
if(stampDiff > 0.010 && approxSyncMaxInterval_ == 0.0)
{
RCLCPP_WARN(this->get_logger(), "The time difference between rgb and depth frames is "
"high (diff=%fs, rgb=%fs, depth=%fs). You may want "
"to set approx_sync_max_interval lower than 0.01s to reject spurious bad synchronizations or use "
"approx_sync=false if streams have all the exact same timestamp.",
"approx_sync=false if streams have all the exact same timestamp. Setting approx_sync_max_interval "
"will suppress this warning.",
stampDiff,
rgbStamp,
depthStamp);
@@ -273,6 +277,7 @@ void RGBDSync::callback(
depthStamp, rtabmap_conversions::timestampFromROS(depth->header.stamp));
}
}
syncDiagnostic_->tickOutput(image->header.stamp);
}
}
rtabmap_sync/src/nodelets/rgbdx_sync.cpp
+16 -9
View File
@@ -43,11 +43,11 @@ RGBDXSync::RGBDXSync(const rclcpp::NodeOptions & options) :
SYNC_INIT(rgbd8)
{
int topicQueueSize = 10;
int syncQueueSize = 2;
int syncQueueSize = 10;
bool approxSync = true;
int rgbdCameras = 2;
double approxSyncMaxInterval = 0.0;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
@@ -176,13 +176,14 @@ void RGBDXSync::rgbd2Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image0,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(2);
output.rgbd_images[0]=(*image0);
output.rgbd_images[1]=(*image1);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd3Callback(
@@ -190,7 +191,7 @@ void RGBDXSync::rgbd3Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image1,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(3);
@@ -198,6 +199,7 @@ void RGBDXSync::rgbd3Callback(
output.rgbd_images[1]=(*image1);
output.rgbd_images[2]=(*image2);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd4Callback(
@@ -206,7 +208,7 @@ void RGBDXSync::rgbd4Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(4);
@@ -215,6 +217,7 @@ void RGBDXSync::rgbd4Callback(
output.rgbd_images[2]=(*image2);
output.rgbd_images[3]=(*image3);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd5Callback(
@@ -224,7 +227,7 @@ void RGBDXSync::rgbd5Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(5);
@@ -234,6 +237,7 @@ void RGBDXSync::rgbd5Callback(
output.rgbd_images[3]=(*image3);
output.rgbd_images[4]=(*image4);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd6Callback(
@@ -244,7 +248,7 @@ void RGBDXSync::rgbd6Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(6);
@@ -255,6 +259,7 @@ void RGBDXSync::rgbd6Callback(
output.rgbd_images[4]=(*image4);
output.rgbd_images[5]=(*image5);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd7Callback(
@@ -266,7 +271,7 @@ void RGBDXSync::rgbd7Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(7);
@@ -278,6 +283,7 @@ void RGBDXSync::rgbd7Callback(
output.rgbd_images[5]=(*image5);
output.rgbd_images[6]=(*image6);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
void RGBDXSync::rgbd8Callback(
@@ -290,7 +296,7 @@ void RGBDXSync::rgbd8Callback(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image7)
{
syncDiagnostic_->tick(image0->header.stamp);
syncDiagnostic_->tickInput(image0->header.stamp);
rtabmap_msgs::msg::RGBDImages output;
output.header = image0->header;
output.rgbd_images.resize(8);
@@ -303,6 +309,7 @@ void RGBDXSync::rgbd8Callback(
output.rgbd_images[6]=(*image6);
output.rgbd_images[7]=(*image7);
rgbdImagesPub_->publish(output);
syncDiagnostic_->tickOutput(image0->header.stamp);
}
}
rtabmap_sync/src/nodelets/stereo_sync.cpp
+4 -3
View File
@@ -51,10 +51,10 @@ StereoSync::StereoSync(const rclcpp::NodeOptions & options) :
exactSync_(0)
{
int topicQueueSize = 10;
int syncQueueSize = 2;
int syncQueueSize = 10;
bool approxSync = false;
double approxSyncMaxInterval = 0.0;
int qos = 0;
int qos = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
approxSync = this->declare_parameter("approx_sync", approxSync);
approxSyncMaxInterval = this->declare_parameter("approx_sync_max_interval", approxSyncMaxInterval);
topicQueueSize = this->declare_parameter("topic_queue_size", topicQueueSize);
@@ -139,7 +139,7 @@ void StereoSync::callback(
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoLeft,
const sensor_msgs::msg::CameraInfo::ConstSharedPtr cameraInfoRight)
{
syncDiagnostic_->tick(imageLeft->header.stamp);
syncDiagnostic_->tickInput(imageLeft->header.stamp);
if(rgbdImagePub_->get_subscription_count() || rgbdImageCompressedPub_->get_subscription_count())
{
double leftStamp = rtabmap_conversions::timestampFromROS(imageLeft->header.stamp);
@@ -209,6 +209,7 @@ void StereoSync::callback(
rightStamp, rtabmap_conversions::timestampFromROS(imageRight->header.stamp));
}
}
syncDiagnostic_->tickOutput(imageLeft->header.stamp);
}
}