From 724673fae28fedcec95b504816ecaf6863f26b43 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 9 Apr 2023 17:37:45 -0700 Subject: [PATCH 01/20] Updating demos/examples using lidar with new Icp/ReciprocalCorrespondences param --- ...urdf.xacro => husky_velodyne_gpu_extra.urdf.xacro} | 2 +- rtabmap_demos/launch/demo_hector_mapping.launch | 4 ++-- rtabmap_demos/launch/demo_husky.launch | 11 ++++++++--- rtabmap_examples/launch/test_k4a_cloud_only.launch | 1 + rtabmap_examples/launch/test_ouster.launch | 1 + rtabmap_examples/launch/test_ouster_gen2.launch | 1 + rtabmap_examples/launch/test_velodyne.launch | 2 +- .../launch/test_velodyne_d435i_deskewing.launch | 2 +- .../launch/test_velodyne_t265_deskewing.launch | 2 +- 9 files changed, 17 insertions(+), 9 deletions(-) rename rtabmap_demos/launch/config/{husky_velodyne_extra.urdf.xacro => husky_velodyne_gpu_extra.urdf.xacro} (66%) diff --git a/rtabmap_demos/launch/config/husky_velodyne_extra.urdf.xacro b/rtabmap_demos/launch/config/husky_velodyne_gpu_extra.urdf.xacro similarity index 66% rename from rtabmap_demos/launch/config/husky_velodyne_extra.urdf.xacro rename to rtabmap_demos/launch/config/husky_velodyne_gpu_extra.urdf.xacro index 8c97c1a4..dc1d56f1 100644 --- a/rtabmap_demos/launch/config/husky_velodyne_extra.urdf.xacro +++ b/rtabmap_demos/launch/config/husky_velodyne_gpu_extra.urdf.xacro @@ -2,7 +2,7 @@ - + diff --git a/rtabmap_demos/launch/demo_hector_mapping.launch b/rtabmap_demos/launch/demo_hector_mapping.launch index 36fa3f84..9156e8ea 100644 --- a/rtabmap_demos/launch/demo_hector_mapping.launch +++ b/rtabmap_demos/launch/demo_hector_mapping.launch @@ -23,7 +23,7 @@ - + @@ -91,7 +91,7 @@ - + diff --git a/rtabmap_demos/launch/demo_husky.launch b/rtabmap_demos/launch/demo_husky.launch index 8b44b526..ee5c4e55 100644 --- a/rtabmap_demos/launch/demo_husky.launch +++ b/rtabmap_demos/launch/demo_husky.launch @@ -3,8 +3,11 @@ - + diff --git a/rtabmap_examples/launch/test_k4a_cloud_only.launch b/rtabmap_examples/launch/test_k4a_cloud_only.launch index 0eaa9d0a..8f451cca 100644 --- a/rtabmap_examples/launch/test_k4a_cloud_only.launch +++ b/rtabmap_examples/launch/test_k4a_cloud_only.launch @@ -35,6 +35,7 @@ + diff --git a/rtabmap_examples/launch/test_ouster.launch b/rtabmap_examples/launch/test_ouster.launch index 2440b016..8fb0e805 100644 --- a/rtabmap_examples/launch/test_ouster.launch +++ b/rtabmap_examples/launch/test_ouster.launch @@ -73,6 +73,7 @@ + diff --git a/rtabmap_examples/launch/test_ouster_gen2.launch b/rtabmap_examples/launch/test_ouster_gen2.launch index 6ee40375..4de086d7 100644 --- a/rtabmap_examples/launch/test_ouster_gen2.launch +++ b/rtabmap_examples/launch/test_ouster_gen2.launch @@ -114,6 +114,7 @@ + diff --git a/rtabmap_examples/launch/test_velodyne.launch b/rtabmap_examples/launch/test_velodyne.launch index b156d9fa..8251f85d 100644 --- a/rtabmap_examples/launch/test_velodyne.launch +++ b/rtabmap_examples/launch/test_velodyne.launch @@ -37,7 +37,7 @@ - + diff --git a/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch b/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch index 5b9ed512..49163e3a 100644 --- a/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch +++ b/rtabmap_examples/launch/test_velodyne_d435i_deskewing.launch @@ -36,7 +36,7 @@ - + diff --git a/rtabmap_examples/launch/test_velodyne_t265_deskewing.launch b/rtabmap_examples/launch/test_velodyne_t265_deskewing.launch index bc878b24..ff118e1a 100644 --- a/rtabmap_examples/launch/test_velodyne_t265_deskewing.launch +++ b/rtabmap_examples/launch/test_velodyne_t265_deskewing.launch @@ -36,7 +36,7 @@ - + From 9f2253b84f6826e999613ba2b73fab02005867da Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Thu, 13 Apr 2023 09:37:32 -0700 Subject: [PATCH 02/20] Add dependency on nav_msgs (#942) * Add dependency on nav_msgs Signed-off-by: Shane Loretz * Also find_package(nav_msgs) Signed-off-by: Shane Loretz --------- Signed-off-by: Shane Loretz --- rtabmap_util/CMakeLists.txt | 4 ++-- rtabmap_util/package.xml | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index 8c12256e..e060af2f 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_util) find_package(catkin REQUIRED COMPONENTS - cv_bridge image_transport roscpp sensor_msgs std_msgs + cv_bridge image_transport roscpp nav_msgs sensor_msgs std_msgs tf laser_geometry pcl_conversions pcl_ros nodelet message_filters pluginlib rtabmap_msgs rtabmap_conversions ) @@ -18,7 +18,7 @@ ENDIF(octomap_msgs_FOUND) catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_util_plugins - CATKIN_DEPENDS cv_bridge image_transport roscpp sensor_msgs std_msgs + CATKIN_DEPENDS cv_bridge image_transport roscpp nav_msgs sensor_msgs std_msgs tf laser_geometry pcl_conversions pcl_ros nodelet message_filters pluginlib rtabmap_msgs rtabmap_conversions ${optional_dependencies} ) diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index b425e844..7e1a1380 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -14,6 +14,7 @@ cv_bridge image_transport roscpp + nav_msgs octomap_msgs sensor_msgs std_msgs From 1eea301cbdc13ac6cc3722af5aaba4d4b741ba5b Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 13 Apr 2023 10:28:26 -0700 Subject: [PATCH 03/20] rtabmap_util: added missing stereo_msgs dep --- rtabmap_util/CMakeLists.txt | 4 ++-- rtabmap_util/package.xml | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rtabmap_util/CMakeLists.txt b/rtabmap_util/CMakeLists.txt index e060af2f..99f33ee8 100644 --- a/rtabmap_util/CMakeLists.txt +++ b/rtabmap_util/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_util) find_package(catkin REQUIRED COMPONENTS - cv_bridge image_transport roscpp nav_msgs sensor_msgs std_msgs + cv_bridge image_transport roscpp nav_msgs sensor_msgs stereo_msgs std_msgs tf laser_geometry pcl_conversions pcl_ros nodelet message_filters pluginlib rtabmap_msgs rtabmap_conversions ) @@ -18,7 +18,7 @@ ENDIF(octomap_msgs_FOUND) catkin_package( INCLUDE_DIRS include LIBRARIES rtabmap_util_plugins - CATKIN_DEPENDS cv_bridge image_transport roscpp nav_msgs sensor_msgs std_msgs + CATKIN_DEPENDS cv_bridge image_transport roscpp nav_msgs sensor_msgs stereo_msgs std_msgs tf laser_geometry pcl_conversions pcl_ros nodelet message_filters pluginlib rtabmap_msgs rtabmap_conversions ${optional_dependencies} ) diff --git a/rtabmap_util/package.xml b/rtabmap_util/package.xml index 7e1a1380..cf55452b 100644 --- a/rtabmap_util/package.xml +++ b/rtabmap_util/package.xml @@ -17,6 +17,7 @@ nav_msgs octomap_msgs sensor_msgs + stereo_msgs std_msgs tf laser_geometry From 6b0edfd62c68bf3e3f8c4abdb744995966f19189 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Apr 2023 10:36:26 -0700 Subject: [PATCH 04/20] fixed https://github.com/introlab/rtabmap_ros/issues/946 --- rtabmap_demos/CMakeLists.txt | 4 ++-- rtabmap_demos/package.xml | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/rtabmap_demos/CMakeLists.txt b/rtabmap_demos/CMakeLists.txt index 9d15c8db..d84e4305 100644 --- a/rtabmap_demos/CMakeLists.txt +++ b/rtabmap_demos/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_demos) find_package(catkin REQUIRED COMPONENTS - rtabmap_conversions rtabmap_msgs + rtabmap_conversions rtabmap_msgs visualization_msgs ) # Optional components @@ -14,7 +14,7 @@ IF(find_object_2d_FOUND) ENDIF(find_object_2d_FOUND) catkin_package( - CATKIN_DEPENDS rtabmap_conversions rtabmap_msgs ${optional_dependencies} + CATKIN_DEPENDS rtabmap_conversions rtabmap_msgs visualization_msgs ${optional_dependencies} ) include_directories(${catkin_INCLUDE_DIRS}) diff --git a/rtabmap_demos/package.xml b/rtabmap_demos/package.xml index 7f785a2b..020559ac 100644 --- a/rtabmap_demos/package.xml +++ b/rtabmap_demos/package.xml @@ -14,6 +14,7 @@ find_object_2d rtabmap_conversions rtabmap_msgs + visualization_msgs rtabmap_costmap_plugins rtabmap_launch From 95e763cd7ef6b43775247ae28575c3891c140fbe Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Apr 2023 10:53:11 -0700 Subject: [PATCH 05/20] Added possible missing deps --- rtabmap_examples/CMakeLists.txt | 4 ++-- rtabmap_examples/package.xml | 1 + rtabmap_legacy/CMakeLists.txt | 4 ++-- rtabmap_legacy/package.xml | 1 + 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/rtabmap_examples/CMakeLists.txt b/rtabmap_examples/CMakeLists.txt index 2bd837de..58755640 100644 --- a/rtabmap_examples/CMakeLists.txt +++ b/rtabmap_examples/CMakeLists.txt @@ -2,11 +2,11 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_examples) find_package(catkin REQUIRED COMPONENTS - roscpp rtabmap_conversions + roscpp rtabmap_conversions message_filters ) catkin_package( - CATKIN_DEPENDS roscpp rtabmap_conversions + CATKIN_DEPENDS roscpp rtabmap_conversions message_filters ) ########### diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index 9f692ed1..d4367abf 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -13,6 +13,7 @@ roscpp rtabmap_conversions + message_filters rtabmap_costmap_plugins rtabmap_launch diff --git a/rtabmap_legacy/CMakeLists.txt b/rtabmap_legacy/CMakeLists.txt index 9ce58f26..0eded438 100644 --- a/rtabmap_legacy/CMakeLists.txt +++ b/rtabmap_legacy/CMakeLists.txt @@ -1,14 +1,14 @@ cmake_minimum_required(VERSION 3.5) project(rtabmap_legacy) -find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_msgs +find_package(catkin REQUIRED COMPONENTS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_util nodelet ) generate_dynamic_reconfigure_options(cfg/Camera.cfg) catkin_package( LIBRARIES rtabmap_legacy_plugins - CATKIN_DEPENDS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_msgs + CATKIN_DEPENDS dynamic_reconfigure image_transport rtabmap_conversions rtabmap_msgs rtabmap_util ) ########### diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 6d817995..32fb3f6e 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -18,6 +18,7 @@ image_transport rtabmap_conversions rtabmap_msgs + rtabmap_util dynamic_reconfigure image_transport From e1e08aa07a9f5ca9ad2d0642bd74109555c09fcc Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Apr 2023 10:54:55 -0700 Subject: [PATCH 06/20] missing exec_depend --- rtabmap_legacy/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_legacy/package.xml b/rtabmap_legacy/package.xml index 32fb3f6e..e34ed242 100644 --- a/rtabmap_legacy/package.xml +++ b/rtabmap_legacy/package.xml @@ -30,6 +30,7 @@ rtabmap_odom rtabmap_slam rtabmap_viz + rtabmap_util From 3e140a04e6d4827cb913825c19a06dc8626f790c Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 15 Apr 2023 16:29:02 -0700 Subject: [PATCH 07/20] Update test_ouster_gen2.launch suggestion from #566 --- rtabmap_examples/launch/test_ouster_gen2.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rtabmap_examples/launch/test_ouster_gen2.launch b/rtabmap_examples/launch/test_ouster_gen2.launch index 4de086d7..9e194248 100644 --- a/rtabmap_examples/launch/test_ouster_gen2.launch +++ b/rtabmap_examples/launch/test_ouster_gen2.launch @@ -59,7 +59,8 @@ - + + From e0cae222be08786f57e88b7a0b1512fa91aec193 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 16 Apr 2023 12:35:28 -0700 Subject: [PATCH 08/20] euroc: fixed static TF issue with V2 datasets --- rtabmap_examples/launch/euroc_datasets.launch | 25 ++++++++++++++----- 1 file changed, 19 insertions(+), 6 deletions(-) diff --git a/rtabmap_examples/launch/euroc_datasets.launch b/rtabmap_examples/launch/euroc_datasets.launch index d97fd2dc..5cbdb787 100644 --- a/rtabmap_examples/launch/euroc_datasets.launch +++ b/rtabmap_examples/launch/euroc_datasets.launch @@ -71,13 +71,23 @@ Examples: - - - + + + + + + + + + - - + + + + + + @@ -89,7 +99,9 @@ Examples: - + + + @@ -104,6 +116,7 @@ Examples: + From 3766d036028b97125d7d62869a15f6c74caf167d Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Apr 2023 14:44:02 -0700 Subject: [PATCH 09/20] Updated docker usage examples --- docker/README.md | 77 +++++++++++++++++++----------------------------- 1 file changed, 31 insertions(+), 46 deletions(-) diff --git a/docker/README.md b/docker/README.md index e0700711..bb889184 100644 --- a/docker/README.md +++ b/docker/README.md @@ -7,57 +7,42 @@ melodic, melodic-latest noetic, noetic-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. -* The following example show how to launch a camera on host computer and run our pre-built rtabmap container. All examples from [RGB-D tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/HandHeldMapping) and [stereo tutorial](http://wiki.ros.org/rtabmap_ros/Tutorials/StereoHandHeldMapping) should work using rtabmap from the container instead. Launch camera on host computer (set ROS_IP as the IP used for docker): - ```bash - $ export ROS_IP=172.17.0.1 && roslaunch openni2_launch openni2.launch depth_registration:=true - # In another terminal, launch rtabmapviz or RVIZ. We do visualization - # on host computer to avoid GPU problems with the container (see "with gui" below to launch rtabmapviz from the container): - $ export ROS_NAMESPACE=rtabmap && rosrun rtabmap_ros rtabmapviz _frame_id:=camera_link - ``` - * Launch `rtabmap` from inside the container (**no gui**), saving the database on host `~/.ros/rtabmap.db`: ```bash - $ docker run -it --rm \ - --env ROS_MASTER_URI=http://172.17.0.1:11311 --env ROS_IP=172.17.0.2 \ - -v ~/.ros:/root \ - introlab3it/rtabmap_ros:kinetic-latest \ - roslaunch rtabmap_ros rtabmap.launch rtabmapviz:=false database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start" + docker run -it --rm \ + --user $UID \ + -e ROS_HOME=/tmp \ + --network host \ + -v ~/.ros:/tmp \ + introlab3it/rtabmap_ros:noetic-latest \ + roslaunch --wait rtabmap_launch rtabmap.launch rtabmap_viz:=false database_path:=/tmp/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` from inside the container (**with gui**), saving the database on host `~/.ros/rtabmap.db`, using [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2): - * With [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2), we would have this Dockerfile: - ```docker - FROM introlab3it/rtabmap_ros:noetic + ```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 . - - # 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 \ - --env ROS_MASTER_URI=http://172.17.0.1:11311 --env ROS_IP=172.17.0.2 \ - -v ~/.ros:/root \ - rtabmap_ros3d \ - roslaunch rtabmap_ros rtabmap.launch database_path:=/root/rtabmap.db rtabmap_args:="--delete_db_on_start" - ``` + docker run -it --rm \ + --privileged \ + --user $UID \ + -e ROS_HOME=/tmp \ + -e DISPLAY=$DISPLAY \ + -e QT_X11_NO_MITSHM=1 \ + -e NVIDIA_VISIBLE_DEVICES=all \ + -e NVIDIA_DRIVER_CAPABILITIES=all \ + -e XAUTHORITY=$XAUTH \ + --runtime=nvidia \ + --network host \ + -v $XAUTH:$XAUTH \ + -v /tmp/.X11-unix:/tmp/.X11-unix \ + -v ~/.ros:/tmp \ + introlab3it/rtabmap_ros:noetic-latest \ + roslaunch --wait rtabmap_launch rtabmap.launch database_path:=/tmp/rtabmap.db gui_cfg:=/tmp/rtabmap_gui.ini rtabmap_args:="--delete_db_on_start" + ``` From 1d01511a050e36ca8a6f8b9e8381297c2ba23d05 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Apr 2023 19:14:10 -0700 Subject: [PATCH 10/20] rtabmap_examples: updated usyd_dataset.launch --- rtabmap_examples/launch/usyd_dataset.launch | 25 ++++++++++++--------- 1 file changed, 15 insertions(+), 10 deletions(-) diff --git a/rtabmap_examples/launch/usyd_dataset.launch b/rtabmap_examples/launch/usyd_dataset.launch index ce69a9b1..a50089dc 100644 --- a/rtabmap_examples/launch/usyd_dataset.launch +++ b/rtabmap_examples/launch/usyd_dataset.launch @@ -9,15 +9,20 @@ roslaunch rtabmap_examples usyd_dataset.launch cameras:=true --> - - + + - + - - - + + + + + + + + @@ -91,12 +96,12 @@ + - @@ -105,7 +110,7 @@ - + @@ -132,6 +137,7 @@ + @@ -157,10 +163,9 @@ - + - From ae67b249e56dc9dea3fc27c18276786d50905e76 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 23 Apr 2023 21:12:13 -0700 Subject: [PATCH 11/20] updated docker example --- docker/README.md | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/docker/README.md b/docker/README.md index bb889184..fd6e652e 100644 --- a/docker/README.md +++ b/docker/README.md @@ -14,14 +14,14 @@ ```bash docker run -it --rm \ --user $UID \ - -e ROS_HOME=/tmp \ + -e ROS_HOME=/tmp/.ros \ --network host \ - -v ~/.ros:/tmp \ + -v ~/.ros:/tmp/.ros \ introlab3it/rtabmap_ros:noetic-latest \ - roslaunch --wait rtabmap_launch rtabmap.launch rtabmap_viz:=false database_path:=/tmp/rtabmap.db rtabmap_args:="--delete_db_on_start" + roslaunch rtabmap_launch rtabmap.launch rtabmap_viz:=false database_path:=/tmp/.ros/rtabmap.db rtabmap_args:="--delete_db_on_start" ``` -* Launch `rtabmap` from inside the container (**with gui**), saving the database on host `~/.ros/rtabmap.db`, using [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2): +* Launch `rtabmap_viz` from inside the container, using [nvidia-docker2 approach](http://wiki.ros.org/docker/Tutorials/Hardware%20Acceleration#nvidia-docker2): ```bash # those following 3 lines would need to be done only one time @@ -31,8 +31,6 @@ docker run -it --rm \ --privileged \ - --user $UID \ - -e ROS_HOME=/tmp \ -e DISPLAY=$DISPLAY \ -e QT_X11_NO_MITSHM=1 \ -e NVIDIA_VISIBLE_DEVICES=all \ @@ -42,7 +40,6 @@ --network host \ -v $XAUTH:$XAUTH \ -v /tmp/.X11-unix:/tmp/.X11-unix \ - -v ~/.ros:/tmp \ introlab3it/rtabmap_ros:noetic-latest \ - roslaunch --wait rtabmap_launch rtabmap.launch database_path:=/tmp/rtabmap.db gui_cfg:=/tmp/rtabmap_gui.ini rtabmap_args:="--delete_db_on_start" + /bin/bash -c "export ROS_NAMESPACE=rtabmap && rosrun rtabmap_viz rtabmap_viz" ``` From 0bd2190f77a5be191b1cebd438dea4c61d960c99 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 24 Apr 2023 11:37:13 -0700 Subject: [PATCH 12/20] rtabmap_examples: Added missing exec_depend --- rtabmap_examples/package.xml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/rtabmap_examples/package.xml b/rtabmap_examples/package.xml index d4367abf..36d974aa 100644 --- a/rtabmap_examples/package.xml +++ b/rtabmap_examples/package.xml @@ -26,5 +26,8 @@ rtabmap_demos robot_localization + imu_filter_madgwick + realsense2_camera + velodyne_pointcloud From 65a8faf78ef51fde49f13f98947b8f67f291a037 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 30 Apr 2023 14:36:38 -0700 Subject: [PATCH 13/20] usyd_dataset: updated visual parameters --- rtabmap_examples/launch/usyd_dataset.launch | 20 ++++++++++++++++++-- 1 file changed, 18 insertions(+), 2 deletions(-) diff --git a/rtabmap_examples/launch/usyd_dataset.launch b/rtabmap_examples/launch/usyd_dataset.launch index a50089dc..51539b43 100644 --- a/rtabmap_examples/launch/usyd_dataset.launch +++ b/rtabmap_examples/launch/usyd_dataset.launch @@ -2,6 +2,10 @@ - - + + @@ -111,6 +117,7 @@ + @@ -147,6 +154,8 @@ + + @@ -159,6 +168,13 @@ + + + + + + + From a4f7afac84c739702e3ca34801ab0b6a2ca6ec89 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 6 May 2023 14:03:56 -0700 Subject: [PATCH 14/20] docker: added rosdep install step to make sure all new dependencies are installed for examples --- docker/noetic/latest/Dockerfile | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docker/noetic/latest/Dockerfile b/docker/noetic/latest/Dockerfile index 5ae90e62..260c344e 100644 --- a/docker/noetic/latest/Dockerfile +++ b/docker/noetic/latest/Dockerfile @@ -9,6 +9,9 @@ COPY . catkin_ws/src/rtabmap_ros RUN source /ros_entrypoint.sh && \ cd catkin_ws && \ + apt update && \ + rosdep install --from-paths src --ignore-src -y && \ + 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 From 6b5cf04dad1d98856612c2910a665eda808e22a9 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sat, 6 May 2023 16:56:49 -0700 Subject: [PATCH 15/20] Fixed rgb_sync node not installed --- rtabmap_sync/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/rtabmap_sync/CMakeLists.txt b/rtabmap_sync/CMakeLists.txt index f15d4419..9e02c70a 100644 --- a/rtabmap_sync/CMakeLists.txt +++ b/rtabmap_sync/CMakeLists.txt @@ -120,6 +120,7 @@ install(TARGETS rtabmap_rgbd_sync rtabmap_rgbdx_sync rtabmap_stereo_sync + rtabmap_rgb_sync ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} From 2399b8cf01f1d8187a8342a6426ee9d52c950ab6 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Wed, 10 May 2023 09:37:01 -0700 Subject: [PATCH 16/20] docker: updated noetic --- docker/noetic/latest/Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docker/noetic/latest/Dockerfile b/docker/noetic/latest/Dockerfile index 260c344e..a9d2c6d9 100644 --- a/docker/noetic/latest/Dockerfile +++ b/docker/noetic/latest/Dockerfile @@ -11,7 +11,7 @@ RUN source /ros_entrypoint.sh && \ cd catkin_ws && \ apt update && \ rosdep install --from-paths src --ignore-src -y && \ - rm -rf /var/lib/apt/lists/* && \ + 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 From aa3ada9c42273c38ae8dd1a21fe55bc114566667 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 11 May 2023 21:00:46 -0700 Subject: [PATCH 17/20] docker/noetic/latest: fixed wrong rtabmap version used --- README.md | 1 + docker/noetic/latest/Dockerfile | 1 + 2 files changed, 2 insertions(+) diff --git a/README.md b/README.md index bf3e009c..09ea4210 100644 --- a/README.md +++ b/README.md @@ -130,6 +130,7 @@ This section shows how to install RTAB-Map ros-pkg on **ROS Melodic/Noetic** (Ca * Add `-DRTABMAP_SYNC_USER_DATA=ON` to `catkin_make` if you plan to use user data synchronized topics. ## Build from source for Nvidia Jetson + * For latest jetpack, see this [post](https://github.com/introlab/rtabmap_ros/issues/943) to use docker (you will save a lot of time using the already built docker images). * For **Jetpack 4** (Ubuntu 18.04 with ROS Melodic), see this [post](https://github.com/introlab/rtabmap/issues/427#issuecomment-608052821). * For **Jetpack 3** (Ubuntu 16.04 with ROS Kinetic), see this [post](https://github.com/introlab/rtabmap_ros/issues/655). diff --git a/docker/noetic/latest/Dockerfile b/docker/noetic/latest/Dockerfile index a9d2c6d9..523dab67 100644 --- a/docker/noetic/latest/Dockerfile +++ b/docker/noetic/latest/Dockerfile @@ -11,6 +11,7 @@ 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 && \ From 1bf27fb2d9160d0cfb86c30430862264843edf43 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Mon, 15 May 2023 10:45:50 -0700 Subject: [PATCH 18/20] Update README.md --- README.md | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/README.md b/README.md index 09ea4210..ecd18510 100644 --- a/README.md +++ b/README.md @@ -54,6 +54,13 @@ For the RTAB-Map libraries and standalone application, visit [RTAB-Map's home pa Rolling Build Status + + Docker + + rtabmap_ros + + Docker Pulls + From 1a87f1598df2a40d01b230808074b45c9a26039e Mon Sep 17 00:00:00 2001 From: matlabbe Date: Tue, 6 Jun 2023 13:40:07 -0700 Subject: [PATCH 19/20] added pub_loc_pose_only_when_localizing option (#980) Addressing https://github.com/introlab/rtabmap_ros/issues/979 --- .../include/rtabmap_slam/CoreWrapper.h | 1 + rtabmap_slam/src/CoreWrapper.cpp | 42 ++++++++++++++----- 2 files changed, 33 insertions(+), 10 deletions(-) diff --git a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h index 80319a78..8c58132f 100644 --- a/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h +++ b/rtabmap_slam/include/rtabmap_slam/CoreWrapper.h @@ -256,6 +256,7 @@ private: rtabmap::Transform currentMetricGoal_; rtabmap::Transform lastPublishedMetricGoal_; bool latestNodeWasReached_; + bool pubLocPoseOnlyWhenLocalizing_; bool graphLatched_; rtabmap::ParametersMap parameters_; std::map rtabmapROSStats_; diff --git a/rtabmap_slam/src/CoreWrapper.cpp b/rtabmap_slam/src/CoreWrapper.cpp index dbb25693..e430b338 100644 --- a/rtabmap_slam/src/CoreWrapper.cpp +++ b/rtabmap_slam/src/CoreWrapper.cpp @@ -89,6 +89,7 @@ CoreWrapper::CoreWrapper() : lastPose_(Transform::getIdentity()), lastPoseIntermediate_(false), latestNodeWasReached_(false), + pubLocPoseOnlyWhenLocalizing_(false), graphLatched_(false), frameId_("base_link"), odomFrameId_(""), @@ -192,6 +193,7 @@ void CoreWrapper::onInit() pnh.param("odom_tf_linear_variance", odomDefaultLinVariance_, odomDefaultLinVariance_); pnh.param("landmark_angular_variance", landmarkDefaultAngVariance_, landmarkDefaultAngVariance_); pnh.param("landmark_linear_variance", landmarkDefaultLinVariance_, landmarkDefaultLinVariance_); + pnh.param("pub_loc_pose_only_when_localizing", pubLocPoseOnlyWhenLocalizing_,pubLocPoseOnlyWhenLocalizing_); pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); pnh.param("wait_for_transform_duration", waitForTransformDuration_, waitForTransformDuration_); pnh.param("initial_pose", initialPoseStr, initialPoseStr); @@ -244,6 +246,7 @@ void CoreWrapper::onInit() NODELET_INFO("rtabmap: tf_delay = %f", tfDelay); NODELET_INFO("rtabmap: tf_tolerance = %f", tfTolerance); NODELET_INFO("rtabmap: odom_sensor_sync = %s", odomSensorSync_?"true":"false"); + NODELET_INFO("rtabmap: pub_loc_pose_only_when_localizing = %s", pubLocPoseOnlyWhenLocalizing_?"true":"false"); bool subscribeStereo = false; pnh.param("subscribe_stereo", subscribeStereo, subscribeStereo); if(subscribeStereo) @@ -2043,17 +2046,36 @@ void CoreWrapper::process( } else { - if(localizationPosePub_.getNumSubscribers() && - !rtabmap_.getStatistics().localizationCovariance().empty()) + if(localizationPosePub_.getNumSubscribers()) { - geometry_msgs::PoseWithCovarianceStamped poseMsg; - poseMsg.header.frame_id = mapFrameId_; - poseMsg.header.stamp = stamp; - rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); - poseMsg.pose.covariance; - 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(uValue(rtabmap_.getStatistics().data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f))!=0; + + if(localized || !pubLocPoseOnlyWhenLocalizing_) + { + geometry_msgs::PoseWithCovarianceStamped poseMsg; + poseMsg.header.frame_id = mapFrameId_; + poseMsg.header.stamp = stamp; + rtabmap_conversions::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose); + poseMsg.pose.covariance; + 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 filteredPoses(rtabmap_.getLocalOptimizedPoses().lower_bound(1), rtabmap_.getLocalOptimizedPoses().end()); From 4cbdb2d14b3cb40c7fb32079262cf45367b1a9f0 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 16 Jun 2023 16:32:01 -0700 Subject: [PATCH 20/20] rtabmap_viz: Added subscribe_info_only parameter (to subscribe only to rtabmap's info message.) --- rtabmap_viz/include/rtabmap_viz/GuiWrapper.h | 2 + rtabmap_viz/src/GuiWrapper.cpp | 54 +++++++++++++++++--- 2 files changed, 49 insertions(+), 7 deletions(-) diff --git a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h index 9e070f2d..7761f3d3 100644 --- a/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h +++ b/rtabmap_viz/include/rtabmap_viz/GuiWrapper.h @@ -65,6 +65,7 @@ protected: private: void infoMapCallback(const rtabmap_msgs::InfoConstPtr & infoMsg, const rtabmap_msgs::MapDataConstPtr & mapMsg); + void infoCallback(const rtabmap_msgs::InfoConstPtr & infoMsg); void goalPathCallback(const rtabmap_msgs::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg); void goalReachedCallback(const std_msgs::BoolConstPtr & value); @@ -133,6 +134,7 @@ private: message_filters::Subscriber infoTopic_; message_filters::Subscriber mapDataTopic_; + ros::Subscriber infoOnlyTopic_; message_filters::Subscriber goalTopic_; message_filters::Subscriber pathTopic_; diff --git a/rtabmap_viz/src/GuiWrapper.cpp b/rtabmap_viz/src/GuiWrapper.cpp index bff9ba8d..c0cd3f34 100644 --- a/rtabmap_viz/src/GuiWrapper.cpp +++ b/rtabmap_viz/src/GuiWrapper.cpp @@ -112,6 +112,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : // To receive odometry events std::string initCachePath; + bool subscribeInfoOnly = false; pnh.param("frame_id", frameId_, frameId_); pnh.param("odom_frame_id", odomFrameId_, odomFrameId_); // set to use odom from TF pnh.param("wait_for_transform", waitForTransform_, waitForTransform_); @@ -119,6 +120,7 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : pnh.param("odom_sensor_sync", odomSensorSync_, odomSensorSync_); pnh.param("max_odom_update_rate", maxOdomUpdateRate_, maxOdomUpdateRate_); pnh.param("camera_node_name", cameraNodeName_, cameraNodeName_); // used to pause the rtabmap_conversions/camera when pausing the process + pnh.param("subscribe_info_only", subscribeInfoOnly, subscribeInfoOnly); pnh.param("init_cache_path", initCachePath, initCachePath); if(initCachePath.size()) { @@ -157,13 +159,21 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) : republishNodeDataPub_ = nh.advertise("republish_node_data", 1); - infoTopic_.subscribe(nh, "info", 1); - mapDataTopic_.subscribe(nh, "mapData", 1); - infoMapSync_ = new message_filters::Synchronizer( - MyInfoMapSyncPolicy(this->getQueueSize()), - infoTopic_, - mapDataTopic_); - infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + if(subscribeInfoOnly) + { + ROS_INFO("subscribe_info_only=true"); + infoOnlyTopic_ = nh.subscribe("info", 1, &GuiWrapper::infoCallback, this); + } + else + { + infoTopic_.subscribe(nh, "info", 1); + mapDataTopic_.subscribe(nh, "mapData", 1); + infoMapSync_ = new message_filters::Synchronizer( + MyInfoMapSyncPolicy(this->getQueueSize()), + infoTopic_, + mapDataTopic_); + infoMapSync_->registerCallback(boost::bind(&GuiWrapper::infoMapCallback, this, boost::placeholders::_1, boost::placeholders::_2)); + } goalTopic_.subscribe(nh, "goal_node", 1); pathTopic_.subscribe(nh, "global_path", 1); @@ -213,6 +223,36 @@ void GuiWrapper::infoMapCallback( this->post(new RtabmapEvent(stat)); } +void GuiWrapper::infoCallback( + const rtabmap_msgs::InfoConstPtr & 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::GoalConstPtr & goalMsg, const nav_msgs::PathConstPtr & pathMsg)