merged master->ros2

2026-09-16 08:10:19 +08:00 · 2023-06-17 18:17:20 -07:00
parent 7d84f83d96 4cbdb2d14b
commit 49316bff00
11 changed files with 131 additions and 59 deletions
@@ -50,6 +50,13 @@ RTAB-Map's ROS2 package (branch `ros2`). **ROS2 Foxy minimum required**: current
            <td>Rolling</td>
            <td><a href="http://build.ros2.org/job/Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary/"><img src="http://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__rtabmap_ros__ubuntu_jammy_amd64__binary" alt="Build Status"/></td>
        </tr>
+        <tr>
+           <td>Docker</td>
+           <td>
+             <a href="https://hub.docker.com/r/introlab3it/rtabmap_ros">rtabmap_ros</a>
+           </td>
+           <td><img src="https://img.shields.io/docker/pulls/introlab3it/rtabmap_ros.svg?label=pulls" alt="Docker Pulls"/></td>
+        </tr>
    </tbody>
 </table>

@@ -5,49 +5,45 @@
    foxy, foxy-latest
    humble, humble-latest
    ```
-    * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including GTSAM and libpointmatcher dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. 
+    * The `-latest` images are automatically built from latest version of `rtabmap` and `rtabmap_ros` from source (including dependencies that are not available with ROS binaries). The other images have the same version than the binaries released on ROS. 


 * Launch `rtabmap` from inside the container (**no gui**), saving the database on host `~/.ros/rtabmap.db`:
    ```bash
-    $ docker run -it --rm \
+    docker run -it --rm \
+     --user $UID \
+     -e ROS_HOME=/tmp/.ros \
     --network=host \
-     -v ~/.ros:/root  \
+     --ipc=host \
+     -v ~/.ros:/tmp/.ros \
     introlab3it/rtabmap_ros:humble-latest \
-     ros2 launch rtabmap_ros rtabmap.launch.py rtabmapviz:=false database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start"
+     ros2 launch rtabmap_launch rtabmap.launch.py rtabmap_viz:=false database_path:=/tmp/.ros/rtabmap.db rtabmap_args:="--delete_db_on_start"
   ```
   
- * Launch `rtabmap` from inside the container (**with gui**, if you have a nvidia GPU, follow those [instructions](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2) with nvidia-docker2 using one of the images above instead of the ros image example, or for other GPUs try the intel/AMD [instructions](http://wiki.ros.org/action/fullsearch/docker/Tutorials/Hardware%20Acceleration) to build a `rtabmap_ros3d` image with your GPU driver), saving the database on host `~/.ros/rtabmap.db`:
+* Launch `rtabmap_viz` from inside the container, using [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2):

-   * With [nvidia-docker2 approach](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html), we would have this Dockerfile:
-       ```docker
-       FROM introlab3it/rtabmap_ros:humble-latest
+   ```bash
+    # those following 3 lines would need to be done only one time
+    XAUTH=/tmp/.docker.xauth
+    touch $XAUTH
+    xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -

-       # nvidia-container-runtime
-       ENV NVIDIA_VISIBLE_DEVICES \
-           ${NVIDIA_VISIBLE_DEVICES:-all}
-       ENV NVIDIA_DRIVER_CAPABILITIES \
-           ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics
-       ```
-       Building the image and run it:
-       ```bash
-       $ docker build -t rtabmap_ros3d .
+    docker run -it --rm \
+      --privileged \
+      -e DISPLAY=$DISPLAY \
+      -e QT_X11_NO_MITSHM=1 \
+      -e NVIDIA_VISIBLE_DEVICES=all \
+      -e NVIDIA_DRIVER_CAPABILITIES=all \
+      -e XAUTHORITY=$XAUTH \
+      --user $UID \
+      -e ROS_HOME=/tmp/.ros \
+      -v ~/.ros:/tmp/.ros \
+      --runtime=nvidia \
+      --network=host \
+      --ipc=host \
+      -v $XAUTH:$XAUTH \
+      -v /tmp/.X11-unix:/tmp/.X11-unix \
+      introlab3it/rtabmap_ros:humble-latest \
+      /bin/bash -c "ros2 run rtabmap_viz rtabmap_viz --ros-args -r __ns:=/rtabmap"
+    ```

-       # those following 3 lines would need to be done only one time
-       $ XAUTH=/tmp/.docker.xauth
-       $ touch $XAUTH
-       $ xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge -
-    
-       $ docker run -it --rm \
-         --privileged \
-         --env="DISPLAY=$DISPLAY" \
-         --env="QT_X11_NO_MITSHM=1" \
-         --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
-         --env="XAUTHORITY=$XAUTH" \
-         --volume="$XAUTH:$XAUTH" \
-         --runtime=nvidia \
-         --network=host \
-         -v ~/.ros:/root  \
-         rtabmap_ros3d \
-         ros2 launch rtabmap_ros rtabmap.launch.py database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start"
-       ```
@@ -9,6 +9,10 @@ COPY . catkin_ws/src/rtabmap_ros

 RUN source /ros_entrypoint.sh && \
    cd catkin_ws && \
+    apt update && \
+    rosdep install --from-paths src --ignore-src -y && \
+    apt remove ros-$ROS_DISTRO-rtabmap* -y && \
+    apt-get clean && rm -rf /var/lib/apt/lists/ && \
    catkin_make -j1 -DRTABMAP_SYNC_MULTI_RGBD=ON -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \
    cd && \
    rm -rf catkin_ws
@@ -342,7 +342,7 @@ def launch_setup(context, *args, **kwargs):
                ("scan", LaunchConfiguration('scan_topic')),
                ("scan_cloud", LaunchConfiguration('scan_cloud_topic')),
                ("odom", LaunchConfiguration('odom_topic'))],
-            condition=IfCondition(LaunchConfiguration("rtabmapviz")),
+            condition=IfCondition(LaunchConfiguration("rtabmap_viz")),
            arguments=[LaunchConfiguration("gui_cfg")],
            prefix=LaunchConfiguration('launch_prefix'),
            namespace=LaunchConfiguration('namespace')),
@@ -383,14 +383,14 @@ def generate_launch_description():
        DeclareLaunchArgument('stereo', default_value='false', description='Use stereo input instead of RGB-D.'),

        DeclareLaunchArgument('localization', default_value='false', description='Launch in localization mode.'),
-        DeclareLaunchArgument('rtabmapviz',   default_value='true',  description='Launch RTAB-Map UI (optional).'),
+        DeclareLaunchArgument('rtabmap_viz',  default_value='true',  description='Launch RTAB-Map UI (optional).'),
        DeclareLaunchArgument('rviz',         default_value='false', description='Launch RVIZ (optional).'),

        DeclareLaunchArgument('use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),

        # Config files
        DeclareLaunchArgument('cfg',      default_value='',                        description='To change RTAB-Map\'s parameters, set the path of config file (*.ini) generated by the standalone app.'),
-        DeclareLaunchArgument('gui_cfg',  default_value='~/.ros/rtabmap_gui.ini',  description='Configuration path of rtabmapviz.'),
+        DeclareLaunchArgument('gui_cfg',  default_value='~/.ros/rtabmap_gui.ini',  description='Configuration path of rtabmap_viz.'),
        DeclareLaunchArgument('rviz_cfg', default_value=config_rviz,               description='Configuration path of rviz2.'),

        DeclareLaunchArgument('frame_id',       default_value='base_link',          description='Fixed frame id of the robot (base frame), you may set "base_link" or "base_footprint" if they are published. For camera-only config, this could be "camera_link".'),
@@ -263,6 +263,7 @@ private:
 	rtabmap::Transform currentMetricGoal_;
 	rtabmap::Transform lastPublishedMetricGoal_;
 	bool latestNodeWasReached_;
+	bool pubLocPoseOnlyWhenLocalizing_;
 	bool graphLatched_;
 	rtabmap::ParametersMap parameters_;
 	std::map<std::string, float> rtabmapROSStats_;
@@ -92,6 +92,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 		lastPose_(Transform::getIdentity()),
 		lastPoseIntermediate_(false),
 		latestNodeWasReached_(false),
+		pubLocPoseOnlyWhenLocalizing_(false),
 		graphLatched_(false),
 		frameId_("base_link"),
 		odomFrameId_(""),
@@ -190,6 +191,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	landmarkDefaultAngVariance_ = this->declare_parameter("landmark_angular_variance", landmarkDefaultAngVariance_);
 	landmarkDefaultLinVariance_ = this->declare_parameter("landmark_linear_variance", landmarkDefaultLinVariance_);
 	
+	pubLocPoseOnlyWhenLocalizing_ = this->declare_parameter("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_);
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
 	initialPoseStr = this->declare_parameter("initial_pose", initialPoseStr);
 	useActionForGoal_ = this->declare_parameter("use_action_for_goal", useActionForGoal_);
@@ -226,6 +228,7 @@ CoreWrapper::CoreWrapper(const rclcpp::NodeOptions & options) :
 	RCLCPP_INFO(this->get_logger(), "rtabmap: tf_delay      = %f", tfDelay);
 	RCLCPP_INFO(this->get_logger(), "rtabmap: tf_tolerance  = %f", tfTolerance);
 	RCLCPP_INFO(this->get_logger(), "rtabmap: odom_sensor_sync   = %s", odomSensorSync_?"true":"false");
+	RCLCPP_INFO(this->get_logger(), "rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false");
 	if(this->isSubscribedToStereo())
 	{
 		RCLCPP_INFO(this->get_logger(), "rtabmap: stereo_to_depth = %s", stereoToDepth_?"true":"false");
@@ -2018,16 +2021,35 @@ void CoreWrapper::process(
 			}
 			else
 			{
-				if(localizationPosePub_->get_subscription_count() &&
-					!rtabmap_.getStatistics().localizationCovariance().empty())
+				if(localizationPosePub_->get_subscription_count())
 				{
-					geometry_msgs::msg::PoseWithCovarianceStamped poseMsg;
-					poseMsg.header.frame_id = mapFrameId_;
-					poseMsg.header.stamp = stamp;
-					rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
-					const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
-					memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
-					localizationPosePub_->publish(poseMsg);
+				    bool localized = rtabmap_.getStatistics().loopClosureId()!=0 ||
+							rtabmap_.getStatistics().proximityDetectionId()!=0 ||
+							static_cast<int>(uValue(rtabmap_.getStatistics().data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f))!=0;
+
+                    if(localized || !pubLocPoseOnlyWhenLocalizing_)
+					{
+						geometry_msgs::msg::PoseWithCovarianceStamped poseMsg;
+					    poseMsg.header.frame_id = mapFrameId_;
+					    poseMsg.header.stamp = stamp;
+						rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
+						if(!rtabmap_.getStatistics().localizationCovariance().empty())
+						{
+							const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
+							memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
+						}
+						else
+						{
+							// Not yet localized, publish large covariance
+							poseMsg.pose.covariance.data()[0] = 9999;
+							poseMsg.pose.covariance.data()[7] = 9999;
+							poseMsg.pose.covariance.data()[14] = twoDMapping_?rtabmap::Registration::COVARIANCE_LINEAR_EPSILON:9999;
+							poseMsg.pose.covariance.data()[21] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
+							poseMsg.pose.covariance.data()[28] = twoDMapping_?rtabmap::Registration::COVARIANCE_ANGULAR_EPSILON:9999;
+							poseMsg.pose.covariance.data()[35] = 9999;
+						}
+						localizationPosePub_->publish(poseMsg);
+					}
 				}
 				std::map<int, rtabmap::Transform> filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end());

@@ -71,6 +71,7 @@ protected:

 private:
 	void infoMapCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg, const rtabmap_msgs::msg::MapData::ConstSharedPtr mapMsg);
+	void infoCallback(const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg);
 	void goalPathCallback(const rtabmap_msgs::msg::Goal::ConstSharedPtr goalMsg, const nav_msgs::msg::Path::ConstSharedPtr pathMsg);
 	void goalReachedCallback(const std_msgs::msg::Bool::ConstSharedPtr value);

@@ -141,6 +142,7 @@ private:

 	message_filters::Subscriber<rtabmap_msgs::msg::Info> infoTopic_;
 	message_filters::Subscriber<rtabmap_msgs::msg::MapData> mapDataTopic_;
+	rclcpp::Subscription<rtabmap_msgs::msg::Info>::SharedPtr infoOnlyTopic_;

 	message_filters::Subscriber<rtabmap_msgs::msg::Goal> goalTopic_;
 	message_filters::Subscriber<nav_msgs::msg::Path> pathTopic_;
@@ -112,12 +112,14 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :

 	// To receive odometry events
 	std::string initCachePath;
+	bool subscribeInfoOnly = false;
 	frameId_ = this->declare_parameter("frame_id", frameId_);
 	this->get_parameter_or("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF, ros2: already declared in CommonDataSubscriber
 	waitForTransform_ = this->declare_parameter("wait_for_transform", waitForTransform_);
 	odomSensorSync_ = this->declare_parameter("odom_sensor_sync", odomSensorSync_);
 	maxOdomUpdateRate_ = this->declare_parameter("max_odom_update_rate", maxOdomUpdateRate_);
 	cameraNodeName_ = this->declare_parameter("camera_node_name", cameraNodeName_); // used to pause the rtabmap_ros/camera when pausing the process
+	subscribeInfoOnly = this->declare_parameter("subscribe_info_only", subscribeInfoOnly);
 	initCachePath = this->declare_parameter("init_cache_path", initCachePath);
 	if(initCachePath.size())
 	{
@@ -142,13 +144,21 @@ GuiWrapper::GuiWrapper(const rclcpp::NodeOptions & options) :

 	republishNodeDataPub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("republish_node_data", 1);

-	infoTopic_.subscribe(this, "info");
-	mapDataTopic_.subscribe(this, "mapData");
-	infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
-			MyInfoMapSyncPolicy(this->getQueueSize()),
-			infoTopic_,
-			mapDataTopic_);
-	infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2));
+	if(subscribeInfoOnly)
+	{
+	    RCLCPP_INFO(this->get_logger(), "rtabmap_viz: subscribe_info_only=true");
+		infoOnlyTopic_ = this->create_subscription<rtabmap_msgs::msg::Info>("info", 1, std::bind(&GuiWrapper::infoCallback, this, std::placeholders::_1));
+	}
+	else
+	{
+		infoTopic_.subscribe(this, "info");
+	    mapDataTopic_.subscribe(this, "mapData");
+	    infoMapSync_ = new message_filters::Synchronizer<MyInfoMapSyncPolicy>(
+			    MyInfoMapSyncPolicy(this->getQueueSize()),
+			    infoTopic_,
+			    mapDataTopic_);
+	    infoMapSync_->registerCallback(std::bind(&GuiWrapper::infoMapCallback, this, std::placeholders::_1, std::placeholders::_2));
+	}

 	goalTopic_.subscribe(this, "goal_node");
 	pathTopic_.subscribe(this, "global_path");
@@ -198,6 +208,36 @@ void GuiWrapper::infoMapCallback(
 	this->post(new RtabmapEvent(stat));
 }

+void GuiWrapper::infoCallback(
+		const rtabmap_msgs::msg::Info::ConstSharedPtr infoMsg)
+{
+	rtabmap::Statistics stat;
+
+	// Info from ROS
+	rtabmap_conversions::infoFromROS(*infoMsg, stat);
+
+	// mapToOdom can be recovered from statistics
+	if(stat.data().find(Statistics::kLoopMapToOdom_x()) != stat.data().end() &&
+	   stat.data().find(Statistics::kLoopMapToOdom_y()) != stat.data().end() &&
+	   stat.data().find(Statistics::kLoopMapToOdom_z()) != stat.data().end() &&
+	   stat.data().find(Statistics::kLoopMapToOdom_roll()) != stat.data().end() &&
+	   stat.data().find(Statistics::kLoopMapToOdom_pitch()) != stat.data().end() &&
+	   stat.data().find(Statistics::kLoopMapToOdom_yaw()) != stat.data().end())
+	{
+		rtabmap::Transform mapToOdom;
+		mapToOdom = rtabmap::Transform(
+			stat.data().at(Statistics::kLoopMapToOdom_x()),
+			stat.data().at(Statistics::kLoopMapToOdom_y()),
+			stat.data().at(Statistics::kLoopMapToOdom_z()),
+			stat.data().at(Statistics::kLoopMapToOdom_roll())*M_PI/180.f,
+			stat.data().at(Statistics::kLoopMapToOdom_pitch())*M_PI/180.f,
+			stat.data().at(Statistics::kLoopMapToOdom_yaw())*M_PI/180.f);
+		stat.setMapCorrection(mapToOdom);
+	}
+
+	this->post(new RtabmapEvent(stat));
+}
+
 void GuiWrapper::goalPathCallback(
 		const rtabmap_msgs::msg::Goal::ConstSharedPtr goalMsg,
 		const nav_msgs::msg::Path::ConstSharedPtr pathMsg)