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

merged master->ros2

Browse Source
This commit is contained in:
matlabbe
2024-01-03 18:11:16 -08:00
parent 2f365ab7e1
commit 80ae448d1b
14 changed files with 493 additions and 131 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.github/workflows/ros1.yml
+1
View File
@@ -36,6 +36,7 @@ jobs:
sudo apt-get update sudo apt-get update
sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros python3-catkin-tools
sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
sudo pip3 uninstall empy --yes
- name: Setup catkin workspace - name: Setup catkin workspace
run: | run: |
rtabmap_conversions/src/MsgConversion.cpp
+6
View File
@@ -2850,6 +2850,12 @@ bool deskew_impl(
if(offsetTime < 0) if(offsetTime < 0)
{ {
UERROR("Input cloud doesn't have \"t\", \"time\", \"stamps\" or \"timestamp\" field!"); UERROR("Input cloud doesn't have \"t\", \"time\", \"stamps\" or \"timestamp\" field!");
std::string fieldsReceived;
for(size_t i=0; i<input.fields.size(); ++i)
{
fieldsReceived += input.fields[i].name + " ";
}
UERROR("Input cloud has these fields: %s", fieldsReceived.c_str());
return false; return false;
} }
if(offsetX < 0) if(offsetX < 0)
rtabmap_demos/launch/demo_unitree_quadruped_robot.launch
-95
View File
@@ -1,95 +0,0 @@
<?xml version="1.0"?>
<launch>
<!-- See https://github.com/unitreerobotics/unitree_guide to bringup simulation.
Fix Cx/Cy of the cameras by setting them to 464 and 400 respectively in unitree_ros/robots/go1_description/xacro/depthCamera.xacro
Ideally, build rtabmap with OpenGV support.
Would work better if simulated environment has a lot of visual texture, see https://github.com/introlab/rtabmap_ros/issues/1031#issuecomment-1722322305
Launch:
$ roslaunch unitree_guide gazeboSim.launch wname:=apt
$ roslaunch rtabmap_demos demo_unitree_quadruped_robot.launch
$ ~/catkin_ws/devel/lib/unitree_guide/junior_ctrl
Press 2 to get up, press 4 to move (w,a,s,d) and rotate (j,l)
-->
<arg name="localization" default="false"/>
<arg if="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory false"/>
<arg unless="$(arg localization)" name="rtabmap_args" default="--Mem/IncrementalMemory true --delete_db_on_start"/>
<!-- sync rgb/depth images and camera info per camera -->
<group ns="camera_face">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>
<group ns="camera_left">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>
<group ns="camera_right">
<node pkg="rtabmap_sync" type="rgbd_sync" name="rgbd_sync">
<remap from="rgb/image" to="color/image_raw"/>
<remap from="depth/image" to="depth/image_raw"/>
<remap from="rgb/camera_info" to="color/camera_info"/>
<param name="approx_sync" value="false"/>
</node>
</group>
<group ns="rtabmap">
<!-- sync all cameras together -->
<node pkg="rtabmap_sync" type="rgbdx_sync" name="rgbdx_sync" output="screen">
<remap from="rgbd_image0" to="/camera_left/rgbd_image"/>
<remap from="rgbd_image1" to="/camera_face/rgbd_image"/>
<remap from="rgbd_image2" to="/camera_right/rgbd_image"/>
<param name="rgbd_cameras" type="int" value="3"/>
</node>
<!-- Odometry -->
<node pkg="rtabmap_odom" type="rgbd_odometry" name="rgbd_odometry" output="screen">
<remap from="imu" to="/trunk_imu"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="frame_id" type="string" value="base"/>
<param name="rgbd_cameras" type="int" value="0"/>
<param name="wait_for_imu_to_init" type="bool" value="true"/>
<param name="Odom/ImageDecimation" type="int" value="2"/>
</node>
<!-- Visual SLAM -->
<node name="rtabmap" pkg="rtabmap_slam" type="rtabmap" output="screen" args="$(arg rtabmap_args)">
<remap from="imu" to="/trunk_imu"/>
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="rgbd_cameras" type="int" value="0"/>
<param name="frame_id" type="string" value="base"/>
<param name="Grid/RangeMin" type="string" value="0.1"/> <!-- to avoid adding legs as obstacle in occupancy grid map -->
<param name="Grid/MaxObstacleHeight" type="string" value="1"/>
<param name="Mem/ImagePreDecimation" type="string" value="2"/>
<param name="Mem/ImagePostDecimation" type="string" value="2"/>
</node>
<!-- Visualisation RTAB-Map -->
<node pkg="rtabmap_viz" type="rtabmap_viz" name="rtabmap_viz" args="-d $(find rtabmap_demos)/launch/config/rgbd_gui.ini" output="screen">
<param name="subscribe_depth" type="bool" value="false"/>
<param name="subscribe_rgbd" type="bool" value="true"/>
<param name="subscribe_odom_info" type="bool" value="true"/>
<param name="frame_id" type="string" value="base"/>
<param name="rgbd_cameras" type="int" value="0"/>
</node>
</group>
</launch>
rtabmap_odom/include/rtabmap_odom/rgbd_odometry.hpp
+13
View File
@@ -95,6 +95,14 @@ private:
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5); const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
void callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
protected: protected:
virtual void flushCallbacks(); virtual void flushCallbacks();
@@ -110,6 +118,7 @@ private:
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -131,6 +140,10 @@ private:
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_; message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy; typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_; message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_; int queueSize_;
bool keepColor_; bool keepColor_;
}; };
rtabmap_odom/include/rtabmap_odom/stereo_odometry.hpp
+23
View File
@@ -84,6 +84,19 @@ private:
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3, const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4); const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4);
void callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5);
void callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6);
protected: protected:
@@ -101,6 +114,8 @@ private:
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image2_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image3_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_; message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image4_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image5_sub_;
message_filters::Subscriber<rtabmap_msgs::msg::RGBDImage> rgbd_image6_sub_;
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy; typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image, sensor_msgs::msg::Image, sensor_msgs::msg::CameraInfo, sensor_msgs::msg::CameraInfo> MyApproxSyncPolicy;
message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_; message_filters::Synchronizer<MyApproxSyncPolicy> * approxSync_;
@@ -118,6 +133,14 @@ private:
message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_; message_filters::Synchronizer<MyApproxSync4Policy> * approxSync4_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy; typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync4Policy;
message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_; message_filters::Synchronizer<MyExactSync4Policy> * exactSync4_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync5Policy;
message_filters::Synchronizer<MyApproxSync5Policy> * approxSync5_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync5Policy;
message_filters::Synchronizer<MyExactSync5Policy> * exactSync5_;
typedef message_filters::sync_policies::ApproximateTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyApproxSync6Policy;
message_filters::Synchronizer<MyApproxSync6Policy> * approxSync6_;
typedef message_filters::sync_policies::ExactTime<rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage, rtabmap_msgs::msg::RGBDImage> MyExactSync6Policy;
message_filters::Synchronizer<MyExactSync6Policy> * exactSync6_;
int queueSize_; int queueSize_;
bool keepColor_; bool keepColor_;
rtabmap_odom/src/nodelets/icp_odometry.cpp
+1
View File
@@ -317,6 +317,7 @@ void ICPOdometry::callbackScan(const sensor_msgs::msg::LaserScan::SharedPtr scan
*scanMsg, *scanMsg,
scanOut, scanOut,
this->tfBuffer(), this->tfBuffer(),
-1.0f,
laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp); laser_geometry::channel_option::Intensity | laser_geometry::channel_option::Timestamp);
if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull()) if(guessFrameId().empty() && previousStamp() > 0 && !velocityGuess().isNull())
rtabmap_odom/src/nodelets/rgbd_odometry.cpp
+112 -4
View File
@@ -57,6 +57,8 @@ RGBDOdometry::RGBDOdometry(const rclcpp::NodeOptions & options) :
exactSync4_(0), exactSync4_(0),
approxSync5_(0), approxSync5_(0),
exactSync5_(0), exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5), queueSize_(5),
keepColor_(false) keepColor_(false)
{ {
@@ -75,6 +77,8 @@ RGBDOdometry::~RGBDOdometry()
delete exactSync4_; delete exactSync4_;
delete approxSync5_; delete approxSync5_;
delete exactSync5_; delete exactSync5_;
delete approxSync6_;
delete exactSync6_;
} }
void RGBDOdometry::onOdomInit() void RGBDOdometry::onOdomInit()
@@ -93,10 +97,6 @@ void RGBDOdometry::onOdomInit()
{ {
rgbdCameras = 1; rgbdCameras = 1;
} }
if(rgbdCameras > 5)
{
RCLCPP_FATAL(this->get_logger(), "Only 5 cameras maximum supported yet. Set 0 to use rgbd_images input (for which rgbdx_sync node can sync up to 8 cameras).");
}
keepColor_ = this->declare_parameter("keep_color", keepColor_); keepColor_ = this->declare_parameter("keep_color", keepColor_);
RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false"); RCLCPP_INFO(this->get_logger(), "RGBDOdometry: approx_sync = %s", approxSync?"true":"false");
@@ -129,6 +129,10 @@ void RGBDOdometry::onOdomInit()
{ {
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
} }
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2) if(rgbdCameras == 2)
{ {
@@ -256,6 +260,54 @@ void RGBDOdometry::onOdomInit()
rgbd_image4_sub_.getSubscriber()->get_topic_name(), rgbd_image4_sub_.getSubscriber()->get_topic_name(),
rgbd_image5_sub_.getSubscriber()->get_topic_name()); rgbd_image5_sub_.getSubscriber()->get_topic_name());
} }
else if(rgbdCameras == 6)
{
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str(),
rgbd_image5_sub_.getTopic().c_str(),
rgbd_image6_sub_.getTopic().c_str());
}
else
{
RCLCPP_FATAL(this->get_logger(),
"%s doesn't support more than 6 cameras (rgbd_cameras=%d) with "
"internal synchronization interface, set rgbd_cameras=0 and use "
"rgbd_images input topic instead for more cameras (for which "
"rgbdx_sync node can sync up to 8 cameras).",
get_name(), rgbdCameras);
}
} }
else if(rgbdCameras == 0) else if(rgbdCameras == 0)
{ {
@@ -649,6 +701,36 @@ void RGBDOdometry::callbackRGBD5(
} }
} }
void RGBDOdometry::callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> imageMsgs(6);
std::vector<cv_bridge::CvImageConstPtr> depthMsgs(6);
std::vector<sensor_msgs::msg::CameraInfo> infoMsgs;
rtabmap_conversions::toCvShare(image, imageMsgs[0], depthMsgs[0]);
rtabmap_conversions::toCvShare(image2, imageMsgs[1], depthMsgs[1]);
rtabmap_conversions::toCvShare(image3, imageMsgs[2], depthMsgs[2]);
rtabmap_conversions::toCvShare(image4, imageMsgs[3], depthMsgs[3]);
rtabmap_conversions::toCvShare(image5, imageMsgs[4], depthMsgs[4]);
rtabmap_conversions::toCvShare(image6, imageMsgs[5], depthMsgs[5]);
infoMsgs.push_back(image->rgb_camera_info);
infoMsgs.push_back(image2->rgb_camera_info);
infoMsgs.push_back(image3->rgb_camera_info);
infoMsgs.push_back(image4->rgb_camera_info);
infoMsgs.push_back(image5->rgb_camera_info);
infoMsgs.push_back(image6->rgb_camera_info);
this->commonCallback(imageMsgs, depthMsgs, infoMsgs);
}
}
void RGBDOdometry::flushCallbacks() void RGBDOdometry::flushCallbacks()
{ {
// flush callbacks // flush callbacks
@@ -748,6 +830,32 @@ void RGBDOdometry::flushCallbacks()
rgbd_image5_sub_); rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5)); exactSync5_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
} }
if(approxSync6_)
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
approxSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
if(exactSync6_)
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&RGBDOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
} }
} }
rtabmap_odom/src/nodelets/stereo_odometry.cpp
+223 -1
View File
@@ -55,6 +55,10 @@ StereoOdometry::StereoOdometry(const rclcpp::NodeOptions & options) :
exactSync3_(0), exactSync3_(0),
approxSync4_(0), approxSync4_(0),
exactSync4_(0), exactSync4_(0),
approxSync5_(0),
exactSync5_(0),
approxSync6_(0),
exactSync6_(0),
queueSize_(5), queueSize_(5),
keepColor_(false) keepColor_(false)
{ {
@@ -65,6 +69,16 @@ StereoOdometry::~StereoOdometry()
{ {
delete approxSync_; delete approxSync_;
delete exactSync_; delete exactSync_;
delete approxSync2_;
delete exactSync2_;
delete approxSync3_;
delete exactSync3_;
delete approxSync4_;
delete exactSync4_;
delete approxSync5_;
delete exactSync5_;
delete approxSync6_;
delete exactSync6_;
} }
void StereoOdometry::onOdomInit() void StereoOdometry::onOdomInit()
@@ -106,6 +120,14 @@ void StereoOdometry::onOdomInit()
{ {
rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile()); rgbd_image4_sub_.subscribe(this, "rgbd_image3", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
} }
if(rgbdCameras >= 5)
{
rgbd_image5_sub_.subscribe(this, "rgbd_image4", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras >= 6)
{
rgbd_image6_sub_.subscribe(this, "rgbd_image5", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos()).get_rmw_qos_profile());
}
if(rgbdCameras == 2) if(rgbdCameras == 2)
{ {
@@ -197,9 +219,89 @@ void StereoOdometry::onOdomInit()
rgbd_image3_sub_.getTopic().c_str(), rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str()); rgbd_image4_sub_.getTopic().c_str());
} }
else if(rgbdCameras == 5)
{
if(approxSync)
{
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync5_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
else
{
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str(),
rgbd_image5_sub_.getTopic().c_str());
}
else if(rgbdCameras == 6)
{
if(approxSync)
{
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
if(approxSyncMaxInterval > 0.0)
approxSync6_->setMaxIntervalDuration(rclcpp::Duration::from_seconds(approxSyncMaxInterval));
approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
else
{
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
subscribedTopicsMsg = uFormat("\n%s subscribed to (%s sync%s):\n %s \\\n %s \\\n %s \\\n %s \\\n %s \\\n %s",
get_name(),
approxSync?"approx":"exact",
approxSync&&approxSyncMaxInterval!=0.0?uFormat(", max interval=%fs", approxSyncMaxInterval).c_str():"",
rgbd_image1_sub_.getTopic().c_str(),
rgbd_image2_sub_.getTopic().c_str(),
rgbd_image3_sub_.getTopic().c_str(),
rgbd_image4_sub_.getTopic().c_str(),
rgbd_image5_sub_.getTopic().c_str(),
rgbd_image6_sub_.getTopic().c_str());
}
else else
{ {
RCLCPP_FATAL(this->get_logger(), "%s doesn't support more than 4 cameras (rgbd_cameras=%d) with internal synchronization interface, set rgbd_cameras=0 and use rgbd_images input topic instead for more cameras.", get_name(), rgbdCameras); RCLCPP_FATAL(this->get_logger(),
"%s doesn't support more than 6 cameras (rgbd_cameras=%d) "
"with internal synchronization interface, set rgbd_cameras=0 and use "
"rgbd_images input topic instead for more cameras (for which "
"rgbdx_sync node can sync up to 8 cameras).",
get_name(), rgbdCameras);
} }
} }
@@ -735,6 +837,76 @@ void StereoOdometry::callbackRGBD4(
} }
} }
void StereoOdometry::callbackRGBD5(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(5);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(5);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
leftInfoMsgs.push_back(image4->rgb_camera_info);
leftInfoMsgs.push_back(image5->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
rightInfoMsgs.push_back(image4->depth_camera_info);
rightInfoMsgs.push_back(image5->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::callbackRGBD6(
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image2,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image3,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image4,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image5,
const rtabmap_msgs::msg::RGBDImage::ConstSharedPtr image6)
{
if(!this->isPaused())
{
std::vector<cv_bridge::CvImageConstPtr> leftMsgs(6);
std::vector<cv_bridge::CvImageConstPtr> rightMsgs(6);
std::vector<sensor_msgs::msg::CameraInfo> leftInfoMsgs;
std::vector<sensor_msgs::msg::CameraInfo> rightInfoMsgs;
rtabmap_conversions::toCvShare(image, leftMsgs[0], rightMsgs[0]);
rtabmap_conversions::toCvShare(image2, leftMsgs[1], rightMsgs[1]);
rtabmap_conversions::toCvShare(image3, leftMsgs[2], rightMsgs[2]);
rtabmap_conversions::toCvShare(image4, leftMsgs[3], rightMsgs[3]);
rtabmap_conversions::toCvShare(image5, leftMsgs[4], rightMsgs[4]);
rtabmap_conversions::toCvShare(image6, leftMsgs[5], rightMsgs[5]);
leftInfoMsgs.push_back(image->rgb_camera_info);
leftInfoMsgs.push_back(image2->rgb_camera_info);
leftInfoMsgs.push_back(image3->rgb_camera_info);
leftInfoMsgs.push_back(image4->rgb_camera_info);
leftInfoMsgs.push_back(image5->rgb_camera_info);
leftInfoMsgs.push_back(image6->rgb_camera_info);
rightInfoMsgs.push_back(image->depth_camera_info);
rightInfoMsgs.push_back(image2->depth_camera_info);
rightInfoMsgs.push_back(image3->depth_camera_info);
rightInfoMsgs.push_back(image4->depth_camera_info);
rightInfoMsgs.push_back(image5->depth_camera_info);
rightInfoMsgs.push_back(image6->depth_camera_info);
this->commonCallback(leftMsgs, rightMsgs, leftInfoMsgs, rightInfoMsgs);
}
}
void StereoOdometry::flushCallbacks() void StereoOdometry::flushCallbacks()
{ {
//flush callbacks //flush callbacks
@@ -810,6 +982,56 @@ void StereoOdometry::flushCallbacks()
rgbd_image4_sub_); rgbd_image4_sub_);
exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4)); exactSync4_->registerCallback(std::bind(&StereoOdometry::callbackRGBD4, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
} }
if(approxSync5_)
{
delete approxSync5_;
approxSync5_ = new message_filters::Synchronizer<MyApproxSync5Policy>(
MyApproxSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
approxSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(exactSync5_)
{
delete exactSync5_;
exactSync5_ = new message_filters::Synchronizer<MyExactSync5Policy>(
MyExactSync5Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_);
exactSync5_->registerCallback(std::bind(&StereoOdometry::callbackRGBD5, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5));
}
if(approxSync6_)
{
delete approxSync6_;
approxSync6_ = new message_filters::Synchronizer<MyApproxSync6Policy>(
MyApproxSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
approxSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
if(exactSync6_)
{
delete exactSync6_;
exactSync6_ = new message_filters::Synchronizer<MyExactSync6Policy>(
MyExactSync6Policy(queueSize_),
rgbd_image1_sub_,
rgbd_image2_sub_,
rgbd_image3_sub_,
rgbd_image4_sub_,
rgbd_image5_sub_,
rgbd_image6_sub_);
exactSync6_->registerCallback(std::bind(&StereoOdometry::callbackRGBD6, this, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, std::placeholders::_4, std::placeholders::_5, std::placeholders::_6));
}
} }
} }
rtabmap_slam/CMakeLists.txt
-11
View File
@@ -33,7 +33,6 @@ IF(${nav2_msgs_VERSION_MAJOR} EQUAL 0)
ENDIF() ENDIF()
#optional #optional
find_package(octomap_msgs)
find_package(apriltag_msgs) find_package(apriltag_msgs)
IF(WIN32) IF(WIN32)
@@ -71,16 +70,6 @@ SET(rtabmap_slam_plugins_lib_src
src/CoreWrapper.cpp src/CoreWrapper.cpp
) )
# If octomap is found, add definition
IF(octomap_msgs_FOUND)
MESSAGE(STATUS "WITH octomap_msgs")
ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
SET(Libraries
${Libraries}
octomap_msgs
)
ENDIF(octomap_msgs_FOUND)
# If apriltag_msgs is found, add definition # If apriltag_msgs is found, add definition
IF(apriltag_msgs_FOUND) IF(apriltag_msgs_FOUND)
MESSAGE(STATUS "WITH apriltag_msgs") MESSAGE(STATUS "WITH apriltag_msgs")
rtabmap_util/CMakeLists.txt
+22 -1
View File
@@ -29,6 +29,7 @@ find_package(rtabmap_conversions REQUIRED)
# Optional components # Optional components
find_package(octomap_msgs) find_package(octomap_msgs)
find_package(grid_map_ros)
include_directories( include_directories(
${CMAKE_CURRENT_SOURCE_DIR}/include ${CMAKE_CURRENT_SOURCE_DIR}/include
@@ -75,7 +76,7 @@ SET(rtabmap_util_plugins_lib_src
) )
# If octomap is found, add definition # If octomap is found, add dependency
IF(octomap_msgs_FOUND) IF(octomap_msgs_FOUND)
MESSAGE(STATUS "WITH octomap_msgs") MESSAGE(STATUS "WITH octomap_msgs")
include_directories( include_directories(
@@ -88,6 +89,18 @@ SET(Libraries
ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS") ADD_DEFINITIONS("-DWITH_OCTOMAP_MSGS")
ENDIF(octomap_msgs_FOUND) ENDIF(octomap_msgs_FOUND)
# If grid_map is found, add dependency
IF(grid_map_ros_FOUND)
MESSAGE(STATUS "WITH grid_map_ros")
include_directories(
${grid_map_ros_INCLUDE_DIRS}
)
SET(Libraries
grid_map_ros
${Libraries}
)
ENDIF(grid_map_ros_FOUND)
############################ ############################
## Declare a cpp library ## Declare a cpp library
############################ ############################
@@ -100,6 +113,14 @@ target_include_directories(rtabmap_util_plugins
$<INSTALL_INTERFACE:include> $<INSTALL_INTERFACE:include>
) )
IF(octomap_msgs_FOUND)
target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_OCTOMAP_MSGS)
ENDIF(octomap_msgs_FOUND)
IF(grid_map_ros_FOUND)
target_compile_definitions(rtabmap_util_plugins PUBLIC -DWITH_GRID_MAP_ROS)
ENDIF(grid_map_ros_FOUND)
ament_target_dependencies(rtabmap_util_plugins ${Libraries}) ament_target_dependencies(rtabmap_util_plugins ${Libraries})
rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay") rclcpp_components_register_nodes(rtabmap_util_plugins "rtabmap_util::RGBDRelay")
rtabmap_util/include/rtabmap_util/MapsManager.h
+13 -2
View File
@@ -38,10 +38,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/msg/point_cloud2.hpp> #include <sensor_msgs/msg/point_cloud2.hpp>
#include <nav_msgs/msg/occupancy_grid.hpp> #include <nav_msgs/msg/occupancy_grid.hpp>
#ifdef RTABMAP_OCTOMAP #if defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP)
#ifdef WITH_OCTOMAP_MSGS
#include <octomap_msgs/msg/octomap.hpp> #include <octomap_msgs/msg/octomap.hpp>
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
#include <grid_map_msgs/msg/grid_map.hpp>
#endif #endif
namespace rtabmap { namespace rtabmap {
@@ -49,6 +51,7 @@ class OctoMap;
class Memory; class Memory;
class OccupancyGrid; class OccupancyGrid;
class LocalGridMaker; class LocalGridMaker;
class GridMap;
} // namespace rtabmap } // namespace rtabmap
@@ -126,6 +129,9 @@ private:
rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapEmptySpace_; rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr octoMapEmptySpace_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr octoMapProj_; rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr octoMapProj_;
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
rclcpp::Publisher<grid_map_msgs::msg::GridMap>::SharedPtr elevationMapPub_;
#endif
std::map<int, rtabmap::Transform> assembledGroundPoses_; std::map<int, rtabmap::Transform> assembledGroundPoses_;
std::map<int, rtabmap::Transform> assembledObstaclePoses_; std::map<int, rtabmap::Transform> assembledObstaclePoses_;
@@ -148,6 +154,11 @@ private:
int octomapTreeDepth_; int octomapTreeDepth_;
bool octomapUpdated_; bool octomapUpdated_;
#ifdef RTABMAP_GRIDMAP
rtabmap::GridMap * elevationMap_;
#endif
bool elevationMapUpdated_;
rtabmap::ParametersMap parameters_; rtabmap::ParametersMap parameters_;
bool latching_; bool latching_;
rtabmap_util/package.xml
+1
View File
@@ -30,6 +30,7 @@
<depend>message_filters</depend> <depend>message_filters</depend>
<depend>rtabmap_msgs</depend> <depend>rtabmap_msgs</depend>
<depend>rtabmap_conversions</depend> <depend>rtabmap_conversions</depend>
<depend>grid_map_ros</depend>
<export> <export>
<build_type>ament_cmake</build_type> <build_type>ament_cmake</build_type>
rtabmap_util/src/MapsManager.cpp
+76 -14
View File
@@ -51,6 +51,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/OctoMap.h> #include <rtabmap/core/OctoMap.h>
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
#include <grid_map_ros/GridMapRosConverter.hpp>
#include <rtabmap/core/global_map/GridMap.h>
#endif
using namespace rtabmap; using namespace rtabmap;
namespace rtabmap_util { namespace rtabmap_util {
@@ -74,6 +79,10 @@ MapsManager::MapsManager() :
#endif #endif
octomapTreeDepth_(16), octomapTreeDepth_(16),
octomapUpdated_(true), octomapUpdated_(true),
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_(new GridMap(&localMaps_)),
#endif
elevationMapUpdated_(true),
latching_(true) latching_(true)
{ {
} }
@@ -96,6 +105,7 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
// connect // connect
latching_ = node.declare_parameter("latch", rclcpp::ParameterValue(latching_)).get<bool>(); latching_ = node.declare_parameter("latch", rclcpp::ParameterValue(latching_)).get<bool>();
RCLCPP_INFO(node.get_logger(), "%s(maps): latch = %s", name.c_str(), latching_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_); RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_radius = %f", name.c_str(), mapFilterRadius_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_); RCLCPP_INFO(node.get_logger(), "%s(maps): map_filter_angle = %f", name.c_str(), mapFilterAngle_);
RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): map_cleanup = %s", name.c_str(), mapCacheCleanup_?"true":"false");
@@ -105,7 +115,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false"); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering = %s", name.c_str(), cloudSubtractFiltering_?"true":"false");
RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_); RCLCPP_INFO(node.get_logger(), "%s(maps): cloud_subtract_filtering_min_neighbors = %d", name.c_str(), cloudSubtractFilteringMinNeighbors_);
#ifdef WITH_OCTOMAP_MSGS
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
octomapTreeDepth_ = node.declare_parameter("octomap_tree_depth", rclcpp::ParameterValue(octomapTreeDepth_)).get<int>(); octomapTreeDepth_ = node.declare_parameter("octomap_tree_depth", rclcpp::ParameterValue(octomapTreeDepth_)).get<int>();
if(octomapTreeDepth_ > 16) if(octomapTreeDepth_ > 16)
@@ -120,9 +129,6 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
} }
RCLCPP_INFO(node.get_logger(), "%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_); RCLCPP_INFO(node.get_logger(), "%s(maps): octomap_tree_depth = %d", name.c_str(), octomapTreeDepth_);
#endif #endif
#endif
// mapping topics // mapping topics
latched_.clear(); latched_.clear();
@@ -157,6 +163,11 @@ void MapsManager::init(rclcpp::Node & node, const std::string & name, bool)
octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE)); octoMapProj_ = node.create_publisher<nav_msgs::msg::OccupancyGrid>("octomap_grid", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&octoMapProj_, false)); latched_.insert(std::make_pair((void*)&octoMapProj_, false));
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMapPub_ = node.create_publisher<grid_map_msgs::msg::GridMap>("elevation_map", rclcpp::QoS(1).reliable().durability(latching_?RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL:RMW_QOS_POLICY_DURABILITY_VOLATILE));
latched_.insert(std::make_pair((void*)&elevationMapPub_, false));
#endif
} }
MapsManager::~MapsManager() { MapsManager::~MapsManager() {
@@ -168,6 +179,9 @@ MapsManager::~MapsManager() {
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
delete octomap_; delete octomap_;
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
delete elevationMap_;
#endif
} }
void parameterMoved( void parameterMoved(
@@ -236,13 +250,16 @@ void MapsManager::setParameters(const rtabmap::ParametersMap & parameters)
parameters_ = parameters; parameters_ = parameters;
delete occupancyGrid_; delete occupancyGrid_;
occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_); occupancyGrid_ = new OccupancyGrid(&localMaps_, parameters_);
localMapMaker_->parseParameters(parameters_); localMapMaker_->parseParameters(parameters_);
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
delete octomap_; delete octomap_;
octomap_ = new OctoMap(&localMaps_, parameters_); octomap_ = new OctoMap(&localMaps_, parameters_);
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
delete elevationMap_;
elevationMap_ = new GridMap(&localMaps_, parameters_);
#endif
} }
void MapsManager::set2DMap( void MapsManager::set2DMap(
@@ -299,9 +316,15 @@ void MapsManager::clear()
groundClouds_.clear(); groundClouds_.clear();
obstacleClouds_.clear(); obstacleClouds_.clear();
occupancyGrid_->clear(); occupancyGrid_->clear();
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
octomap_->clear(); octomap_->clear();
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
elevationMap_->clear();
#endif
for(std::map<void*, bool>::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter) for(std::map<void*, bool>::iterator iter=latched_.begin(); iter!=latched_.end(); ++iter)
{ {
iter->second = false; iter->second = false;
@@ -327,6 +350,9 @@ bool MapsManager::hasSubscribers() const
octoMapGroundCloud_->get_subscription_count() != 0 || octoMapGroundCloud_->get_subscription_count() != 0 ||
octoMapEmptySpace_->get_subscription_count() != 0 || octoMapEmptySpace_->get_subscription_count() != 0 ||
octoMapProj_->get_subscription_count() != 0 octoMapProj_->get_subscription_count() != 0
#endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
|| elevationMapPub_->get_subscription_count() != 0
#endif #endif
; ;
} }
@@ -361,7 +387,8 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
const std::map<int, rtabmap::Signature> & signatures) const std::map<int, rtabmap::Signature> & signatures)
{ {
bool updateGridCache = updateGrid || updateOctomap; bool updateGridCache = updateGrid || updateOctomap;
if(!updateGrid && !updateOctomap) bool updateElevation = false;
if(!updateGrid && !updateOctomap && !updateOctomap)
{ {
// all false, update only those where we have subscribers // all false, update only those where we have subscribers
#ifdef RTABMAP_OCTOMAP #ifdef RTABMAP_OCTOMAP
@@ -378,21 +405,29 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
octoMapProj_->get_subscription_count() != 0; octoMapProj_->get_subscription_count() != 0;
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
updateElevation = elevationMapPub_->get_subscription_count() != 0;
#endif
updateGrid = gridMapPub_->get_subscription_count() != 0 || updateGrid = gridMapPub_->get_subscription_count() != 0 ||
gridProbMapPub_->get_subscription_count() != 0; gridProbMapPub_->get_subscription_count() != 0;
updateGridCache = updateOctomap || updateGrid || updateGridCache = updateOctomap || updateGrid || updateElevation ||
cloudMapPub_->get_subscription_count() != 0 || cloudMapPub_->get_subscription_count() != 0 ||
cloudObstaclesPub_->get_subscription_count() != 0 || cloudObstaclesPub_->get_subscription_count() != 0 ||
cloudGroundPub_->get_subscription_count() != 0; cloudGroundPub_->get_subscription_count() != 0;
} }
#if !defined(WITH_OCTOMAP_MSGS) and !defined(RTABMAP_OCTOMAP) #if not (defined(WITH_OCTOMAP_MSGS) and defined(RTABMAP_OCTOMAP))
updateOctomap = false; updateOctomap = false;
#endif #endif
#if not (defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP))
updateElevation = false;
#endif
gridUpdated_ = updateGrid; gridUpdated_ = updateGrid;
octomapUpdated_ = updateOctomap; octomapUpdated_ = updateOctomap;
elevationMapUpdated_ = updateElevation;
UDEBUG("Updating map caches..."); UDEBUG("Updating map caches...");
@@ -595,6 +630,18 @@ std::map<int, rtabmap::Transform> MapsManager::updateMapCaches(
} }
#endif #endif
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if(updateElevation)
{
UTimer time;
elevationMapUpdated_ = elevationMap_->update(filteredPoses);
UINFO("GridMap (elevation map) update time = %fs", time.ticks());
}
#endif
localMaps_.clear(true);
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin(); for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr >::iterator iter=groundClouds_.begin();
iter!=groundClouds_.end();) iter!=groundClouds_.end();)
{ {
@@ -796,10 +843,6 @@ void MapsManager::publishMaps(
} }
++countObstacles; ++countObstacles;
} }
else
{
//std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::iterator jter = gridMaps_.find(iter->first);
}
} }
} }
double addingPointsTime = t.ticks(); double addingPointsTime = t.ticks();
@@ -1218,7 +1261,6 @@ void MapsManager::publishMaps(
{ {
latched_.at(&octoMapProj_) = false; latched_.at(&octoMapProj_) = false;
} }
#endif #endif
if( gridUpdated_ || if( gridUpdated_ ||
@@ -1314,7 +1356,27 @@ void MapsManager::publishMaps(
{ {
latched_.at(&gridProbMapPub_) = false; latched_.at(&gridProbMapPub_) = false;
} }
#if defined(WITH_GRID_MAP_ROS) and defined(RTABMAP_GRIDMAP)
if( elevationMapUpdated_ ||
!latching_ ||
(elevationMapPub_->get_subscription_count() && !latched_.at(&elevationMapPub_)))
{
grid_map_msgs::msg::GridMap::UniquePtr msg;
msg = grid_map::GridMapRosConverter::toMessage(elevationMap_->gridMap());
msg->header.frame_id = mapFrameId;
msg->header.stamp = stamp;
elevationMapPub_->publish(std::move(msg));
}
if(elevationMapPub_->get_subscription_count() == 0)
{
latched_.at(&elevationMapPub_) = false;
}
if( mapCacheCleanup_ &&
elevationMapPub_->get_subscription_count() == 0)
{
elevationMap_->clear();
}
#endif
if(!this->hasSubscribers() && mapCacheCleanup_) if(!this->hasSubscribers() && mapCacheCleanup_)
{ {
if(!localMaps_.empty()) if(!localMaps_.empty())
rtabmap_util/src/nodelets/obstacles_detection.cpp
+1 -2
View File
@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h> #include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h> #include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h> #include <pcl/filters/filter.h>
#include <rtabmap/core/LocalGridMaker.h>
#include <rtabmap_conversions/MsgConversion.h> #include <rtabmap_conversions/MsgConversion.h>
@@ -85,8 +86,6 @@ ObstaclesDetection::ObstaclesDetection(const rclcpp::NodeOptions & options) :
cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1)); cloudSub_ = create_subscription<sensor_msgs::msg::PointCloud2>("cloud", rclcpp::QoS(1).reliability((rmw_qos_reliability_policy_t)qos), std::bind(&ObstaclesDetection::callback, this, std::placeholders::_1));
} }
void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg) void ObstaclesDetection::callback(const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloudMsg)
{ {
rclcpp::Time time = now(); rclcpp::Time time = now();