i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-07 10:17: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/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);
}
}