Skip to content

Commit 1486d42

Browse files
authored
Add amcl3 demo (#9)
* Add amcl3 demo Signed-off-by: Pablo Vela <pablovela@naver.com> * Add jazzy image Signed-off-by: Pablo Vela <pablovela@naver.com> * Add crop filter Signed-off-by: Pablo Vela <pablovela@naver.com> * Improve jazzy instructions Signed-off-by: Pablo Vela <pablovela@naver.com> * Add ignore vdb files Signed-off-by: Pablo Vela <pablovela@naver.com> * Change files permission Signed-off-by: Pablo Vela <pablovela@naver.com> * Add EOF Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix nit and add particle markers Signed-off-by: Pablo Vela <pablovela@naver.com> * Add EOF Signed-off-by: Pablo Vela <pablovela@naver.com> * Change to rosdep dependencies Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix covariance calculation Signed-off-by: Pablo Vela <pablovela@naver.com> * Activate particle markers Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix formatting Signed-off-by: Pablo Vela <pablovela@naver.com> * Change beluga repo path Signed-off-by: Pablo Vela <pablovela@naver.com> * Update to beluga_vdb Signed-off-by: Pablo Vela <pablovela@naver.com> * Remove hard-coded map path Signed-off-by: Pablo Vela <pablovela@naver.com> * Remove test map Signed-off-by: Pablo Vela <pablovela@naver.com> * Update readme Signed-off-by: Pablo Vela <pablovela@naver.com> * Move tf to rosbag Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix files not rebased Signed-off-by: Pablo Vela <pablovela@naver.com> * Update instructions Signed-off-by: Pablo Vela <pablovela@naver.com> * Add author Signed-off-by: Pablo Vela <pablovela@naver.com> * Remove rosbags from the repository Signed-off-by: Pablo Vela <pablovela@naver.com> * Add documentation amcl3 demo Signed-off-by: Pablo Vela <pablovela@naver.com> * Update documentation amcl3 demo Signed-off-by: Pablo Vela <pablovela@naver.com> * Update to humble Signed-off-by: Pablo Vela <pablovela@naver.com> * Change the particles to 2D Signed-off-by: Pablo Vela <pablovela@naver.com> * Update readme Signed-off-by: Pablo Vela <pablovela@naver.com> * Update readme Signed-off-by: Pablo Vela <pablovela@naver.com> * Update to last beluga version Signed-off-by: Pablo Vela <pablovela@naver.com> * Update to humble Signed-off-by: Pablo Vela <pablovela@naver.com> * Install updated version pcl_perception Signed-off-by: Pablo Vela <pablovela@naver.com> * Update to humble Signed-off-by: Pablo Vela <pablovela@naver.com> * Remove old version of openvdb Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix not running dpkg as root Signed-off-by: Pablo Vela <pablovela@naver.com> * Remove commented code Signed-off-by: Pablo Vela <pablovela@naver.com> * Fix callback type Signed-off-by: Pablo Vela <pablovela@naver.com> --------- Signed-off-by: Pablo Vela <pablovela@naver.com>
1 parent dbc6e32 commit 1486d42

16 files changed

Lines changed: 1325 additions & 12 deletions

File tree

.gitignore

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1 +1,5 @@
1+
rosbags/
12
.vscode
3+
*.db3
4+
*.mcap
5+
*.vdb

README.md

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,8 @@ Step into the repository directory and start the docker image. The first time yo
3333

3434
```bash
3535
cd beluga-demos
36-
./docker/run.sh --build
36+
ROSDISTRO=humble docker/run.sh # ROS2 Humble
37+
ROSDISTRO=jazzy docker/run.sh # ROS2 Jazzy
3738
```
3839

3940
#### Step 3: Build the demo software code
@@ -70,15 +71,16 @@ You can close the demo by pressing `Ctrl+C` in the terminal where you initially
7071

7172
This table contains the list of predefined demos that you can run:
7273

73-
| Alias command | Description |
74-
| ------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- |
75-
| `lidar_beam_model_hallway_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node to localize in a world built out of the [Cartographer Magazino](https://github.com/magazino/cartographer_magazino?tab=readme-ov-file#data) dataset map. The node is configured to use the `beam` sensor model configuration. |
76-
| `lidar_likelihood_model_hallway_demo`| Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node to localize in a world built out of the [Cartographer Magazino](https://github.com/magazino/cartographer_magazino?tab=readme-ov-file#data) dataset map. The node is configured to use the `likelihood` sensor model configuration. |
77-
| `lidar_beam_model_office_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node moving around a large office cluttered with unmapped obstacles. The configured sensor model is `beam`. |
78-
| `lidar_likelihood_model_office_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node moving around a large office cluttered with unmapped obstacles. Sensor model is `likelihood`. |
74+
| Alias command | Description |
75+
|--------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
76+
| `lidar_beam_model_hallway_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node to localize in a world built out of the [Cartographer Magazino](https://github.com/magazino/cartographer_magazino?tab=readme-ov-file#data) dataset map. The node is configured to use the `beam` sensor model configuration. |
77+
| `lidar_likelihood_model_hallway_demo`| Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node to localize in a world built out of the [Cartographer Magazino](https://github.com/magazino/cartographer_magazino?tab=readme-ov-file#data) dataset map. The node is configured to use the `likelihood` sensor model configuration. |
78+
| `lidar_beam_model_office_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node moving around a large office cluttered with unmapped obstacles. The configured sensor model is `beam`. |
79+
| `lidar_likelihood_model_office_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node moving around a large office cluttered with unmapped obstacles. Sensor model is `likelihood`. |
7980
| `apriltags_localization_demo` | Simple custom localization node using the `beluga` library to localize the robot within a large $10m \times 10m$ area using Apriltag markers as landmarks. The code of this localization node can be found within this repository [here](https://github.com/Ekumen-OS/beluga-demos/blob/main/localization/beluga_demo_fiducial_localization/src/beluga_lmcl_demo.cpp). |
8081
| `light_beacons_localization_demo` | Simple custom localization node using the `beluga` library to localize the robot within a large $10m \times 10m$ area using light sources as landmarks. The code of this localization node can be found within this repository [here](https://github.com/Ekumen-OS/beluga-demos/blob/main/localization/beluga_demo_bearing_localization/src/beluga_bmcl_demo.cpp). |
81-
| `nav2_integration_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node in lieu of the [_nav2_amcl_](https://github.com/ros-navigation/navigation2/tree/main/nav2_amcl) in a [Nav2](https://docs.nav2.org/) stack to navigate around a large office cluttered with unmapped obstacles. |
82+
| `nav2_integration_demo` | Demo using the [_beluga_amcl_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_amcl) node in lieu of the [_nav2_amcl_](https://github.com/ros-navigation/navigation2/tree/main/nav2_amcl) in a [Nav2](https://docs.nav2.org/) stack to navigate around a large office cluttered with unmapped obstacles. |
83+
| `amcl3_localization_demo` | Custom 3D localization node using the [_beluga_vdb_](https://github.com/Ekumen-OS/beluga/tree/main/beluga_vdb) library to localize the robot in a botanic garden [dataset](https://github.com/robot-pesg/BotanicGarden). The code of this localization node is based on `OpenVDB` and can be found within this repository [here](https://github.com/Ekumen-OS/beluga/blob/main/beluga_vdb/include/beluga_vdb/sensor/vdb_likelihood_field_model.hpp). To download the dataset and run this demo please follow the [INSTRUCTIONS](localization/beluga_demo_amcl3_localization/doc/README.md). |
8284

8385
## Under the hood
8486

@@ -90,7 +92,7 @@ The `demo_build` alias command is a wrapper around the `colcon` build tool that
9092

9193
```bash
9294
cd ~/ws
93-
colcon build --symlink-install
95+
colcon build --symlink-install --packages-up-to-regex beluga_demo --cmake-args -DCMAKE_BUILD_TYPE=Release
9496
source install/setup.bash
9597
```
9698

docker/files/DOTaliases

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
alias demo_build='cd ~/ws \
2-
&& colcon build --symlink-install --packages-up-to-regex beluga_demo \
2+
&& colcon build --symlink-install --packages-up-to-regex beluga_demo --cmake-args -DCMAKE_BUILD_TYPE=Release \
33
&& source install/setup.bash'
44

55
alias lidar_beam_model_hallway_demo='cd ~/ws \
@@ -29,3 +29,7 @@ alias light_beacons_localization_demo='cd ~/ws \
2929
alias nav2_integration_demo='cd ~/ws \
3030
&& source install/setup.bash \
3131
&& ros2 launch beluga_demo_nav2_integration demo_office_navigation.launch.py'
32+
33+
alias amcl3_localization_demo='cd ~/ws \
34+
&& source install/setup.bash \
35+
&& ros2 launch beluga_demo_amcl3_localization demo_botanic_garden_amcl3_localization.launch.py'

docker/files/humble_base.repos

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ repositories:
22
beluga:
33
type: git
44
url: https://github.com/Ekumen-OS/beluga.git
5-
version: b6d5ee387cd196aa3c11617c993aa6300c7a9484
5+
version: a0cf83f70f91c9f95424c39e28c1ee71e72c5dff
66
apriltag_ros:
77
type: git
88
url: https://github.com/Adlink-ROS/apriltag_ros.git

docker/files/jazzy_base.repos

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@ repositories:
22
beluga:
33
type: git
44
url: https://github.com/Ekumen-OS/beluga.git
5-
version: b6d5ee387cd196aa3c11617c993aa6300c7a9484
5+
version: a0cf83f70f91c9f95424c39e28c1ee71e72c5dff
66
apriltag_ros:
77
type: git
88
url: https://github.com/Adlink-ROS/apriltag_ros.git

docker/images/humble/Dockerfile

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,44 @@ RUN mkdir -p /tmp/ws/src \
1919
&& find ./ -name "*.sdf" | xargs cp --parents -t /tmp/ws/src \
2020
|| true
2121

22+
FROM ros:humble-ros-base-jammy AS openvdb-builder
23+
24+
RUN apt-get update \
25+
&& apt-get install --no-install-recommends -y \
26+
git \
27+
libboost-all-dev \
28+
libtbb-dev \
29+
libblosc-dev \
30+
&& rm -rf /var/lib/apt/lists/*
31+
32+
WORKDIR /opt
33+
34+
RUN git clone -b v8.2.0 https://github.com/AcademySoftwareFoundation/openvdb.git \
35+
&& cd openvdb \
36+
&& mkdir build && cd build \
37+
&& cmake .. \
38+
&& make -j$(nproc)
39+
40+
#
41+
# Build pcl_perception here to avoid installing boost again
42+
RUN mkdir -p /opt/perception/src
43+
44+
RUN apt-get update \
45+
&& apt-get install --no-install-recommends -y \
46+
libeigen3-dev \
47+
libpcl-dev \
48+
&& rm -rf /var/lib/apt/lists/*
49+
50+
WORKDIR /opt/perception/src
51+
52+
RUN git clone -b 2.6.1 https://github.com/ros-perception/perception_pcl.git \
53+
&& cd .. \
54+
&& sudo apt-get update \
55+
&& . /opt/ros/humble/setup.sh \
56+
&& rosdep update \
57+
&& rosdep install -i -y --from-path src \
58+
&& colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
59+
2260
FROM ros:humble-ros-base-jammy AS builder
2361

2462
SHELL ["/bin/bash", "-c"]
@@ -97,6 +135,16 @@ COPY docker/files/patches/nav2_minimal_turtlebot_simulation.patch /tmp/nav2_mini
97135
RUN cd src/external-deps/nav2_minimal_turtlebot_simulation \
98136
&& cat /tmp/nav2_minimal_turtlebot_simulation.patch | git apply -p1
99137

138+
#
139+
# Install fixed pcl_perception before installing it with rosdep
140+
141+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/pcl_ros/include/pcl_ros/ /opt/ros/humble/include/
142+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/pcl_ros/lib/. /opt/ros/humble/lib/.
143+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/pcl_ros/share/pcl_ros /opt/ros/humble/share
144+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/pcl_conversions/include/pcl_conversions/ /opt/ros/humble/include/
145+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/pcl_conversions/share/pcl_conversions /opt/ros/humble/share
146+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/perception/install/perception_pcl/share/perception_pcl /opt/ros/humble/share
147+
100148
#
101149
# Install project dependencies
102150

@@ -143,4 +191,13 @@ RUN find src -name '*.world' -or -name '*.sdf' | \
143191

144192
ENV GZ_SIM_RESOURCE_PATH=/home/$USER/.gazebo/models
145193

194+
#
195+
# Install OpenVDB files
196+
COPY --from=openvdb-builder --chown=$USER:$GROUP /opt/openvdb/ /opt/openvdb
197+
RUN cd /opt/openvdb/build \
198+
&& sudo make install \
199+
&& sudo rm -rf /opt/openvdb
200+
RUN sudo dpkg -r libopenvdb8.1 \
201+
libopenvdb-dev
202+
146203
ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"]
Lines changed: 120 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,120 @@
1+
# Copyright 2024 Ekumen, Inc.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# http://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
################################################################################
16+
# Set minimum required version of cmake, project name and compile options
17+
################################################################################
18+
19+
cmake_minimum_required(VERSION 3.5)
20+
project(beluga_demo_amcl3_localization)
21+
22+
set(OPENVDB_CMAKE_MODULE_PATH
23+
${OPENVDB_CMAKE_MODULE_PATH}
24+
CACHE PATH "Path to OpenVDB CMake module")
25+
26+
if(NOT OPENVDB_CMAKE_MODULE_PATH)
27+
file(GLOB_RECURSE OPENVDB_MODULES /usr/lib/*/FindOpenVDB.cmake
28+
/usr/local/lib/*/FindOpenVDB.cmake)
29+
list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES)
30+
if (NUM_OPENVDB_MODULES EQUAL 1)
31+
list(GET OPENVDB_MODULES 0 OPENVDB_MODULE)
32+
get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE}
33+
DIRECTORY)
34+
endif()
35+
unset(NUM_OPENVDB_MODULES)
36+
unset(OPENVDB_MODULES)
37+
endif()
38+
if(OPENVDB_CMAKE_MODULE_PATH)
39+
list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH})
40+
endif()
41+
42+
################################################################################
43+
# Find ament packages and libraries for ament and system dependencies
44+
################################################################################
45+
46+
find_package(ament_cmake REQUIRED)
47+
find_package(rclcpp REQUIRED)
48+
find_package(range-v3 REQUIRED)
49+
find_package(Sophus REQUIRED)
50+
find_package(Eigen3 REQUIRED NO_MODULE)
51+
find_package(beluga REQUIRED)
52+
find_package(beluga_ros REQUIRED)
53+
find_package(beluga_vdb REQUIRED)
54+
find_package(rclcpp_components REQUIRED)
55+
find_package(tf2 REQUIRED)
56+
find_package(tf2_eigen REQUIRED)
57+
find_package(tf2_geometry_msgs REQUIRED)
58+
find_package(message_filters REQUIRED)
59+
find_package(geometry_msgs REQUIRED)
60+
find_package(sensor_msgs REQUIRED)
61+
find_package(visualization_msgs REQUIRED)
62+
find_package(std_msgs REQUIRED)
63+
find_package(OpenVDB REQUIRED)
64+
65+
66+
################################################################################
67+
# Build
68+
################################################################################
69+
70+
add_library(beluga_amcl3_demo_component SHARED)
71+
target_sources(beluga_amcl3_demo_component PRIVATE src/beluga_amcl3_demo.cpp)
72+
target_compile_features(beluga_amcl3_demo_component PUBLIC cxx_std_17)
73+
target_link_libraries(beluga_amcl3_demo_component
74+
PUBLIC beluga::beluga beluga_ros::beluga_ros beluga_vdb::beluga_vdb)
75+
ament_target_dependencies(
76+
beluga_amcl3_demo_component
77+
PUBLIC ament_index_cpp
78+
geometry_msgs
79+
visualization_msgs
80+
std_msgs
81+
sensor_msgs
82+
message_filters
83+
rclcpp
84+
rclcpp_components
85+
tf2
86+
tf2_eigen
87+
tf2_geometry_msgs
88+
tf2_ros
89+
beluga
90+
beluga_ros
91+
beluga_vdb)
92+
rclcpp_components_register_node(
93+
beluga_amcl3_demo_component
94+
PLUGIN
95+
"beluga_demo_amcl3_localization::Amcl3Node"
96+
EXECUTABLE beluga_amcl3_demo_node)
97+
98+
99+
################################################################################
100+
# Install
101+
################################################################################
102+
103+
install(
104+
TARGETS beluga_amcl3_demo_component
105+
ARCHIVE DESTINATION lib
106+
LIBRARY DESTINATION lib
107+
RUNTIME DESTINATION bin)
108+
install(TARGETS beluga_amcl3_demo_node DESTINATION lib/${PROJECT_NAME})
109+
110+
# Install Python executables
111+
install(PROGRAMS
112+
scripts/marker_publisher.py
113+
DESTINATION lib/${PROJECT_NAME}
114+
)
115+
116+
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
117+
install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
118+
install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
119+
120+
ament_package()
Lines changed: 81 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
# Beluga VDB: AMCL3 Localization Demo!
2+
3+
This package provides 2 maps based on the [botanic garden dataset](https://github.com/robot-pesg/BotanicGarden). The rosbags from the botanic garden dataset were modified (`/odom` topic was added and several topics were removed) to be able to run on this demo. Currently, only 2 sequences can be tested: 1005-07 and 1006-01.
4+
5+
<div align="center">
6+
<img src="./images/1005-07.png" alt="Sequence 1005-07" height="200" hspace="5"/>
7+
<img src="./images/1006-01.png" alt="Sequence 1006-01" height="200" />
8+
</div>
9+
10+
## Pre-requisites
11+
12+
#### Request map and bags from the team
13+
14+
Request the maps and bags from the team and store the maps under `beluga_demo/localization/beluga_demo_amcl3_localization/maps` and the bags under `beluga_demo/localization/beluga_demo_amcl3_localization/rosbags`.
15+
16+
#### Local machine (Only Humble)
17+
18+
If you want to run this example on your local machine and not in the provided docker image, you need to download and compile the [pcl_perception repo release 2.6.1](https://github.com/ros-perception/perception_pcl) by yourself. Because the filters provided for `humble` are not compiled as plugins.
19+
20+
## Quick start
21+
22+
#### Step 1: File verification
23+
24+
Inside the docker container verify that you have the following file structure:
25+
26+
```bash
27+
beluga_demo_amcl3_localization/
28+
├── maps/
29+
│ ├── map_1005_07.vdb
30+
│ └── map_1006_01.vdb
31+
├── rosbags/
32+
│ ├── botanic_garden_tf_1005_07/
33+
│ │ ├── botanic_garden_tf_1005_07.db3
34+
│ │ └── metadata.yaml
35+
│ └── botanic_garden_tf_1006_01/
36+
│ ├── botanic_garden_tf_1006_01.db3
37+
│ └── metadata.yaml
38+
├── doc/
39+
├── launch/
40+
├── rviz/
41+
├── scripts/
42+
└── src/
43+
```
44+
45+
#### Step 2: Build the demo software code
46+
47+
Now you need to build the demo software stack:
48+
49+
```bash
50+
demo_build
51+
```
52+
53+
#### Step 3: Launch the AMCL3 node
54+
55+
You can now launch the AMCL3 node and Rviz using:
56+
57+
```bash
58+
amcl3_localization_demo
59+
```
60+
61+
#### Step 4: Run the bag
62+
63+
Finally, play the rosbag:
64+
65+
```bash
66+
ros2 bag play ~/ws/src/beluga_demo/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_tf_1005_07/botanic_garden_tf_1005_07.db3
67+
```
68+
69+
Once the bag starts playing you'll see that Rviz shows the pointcloud, pose estimation, particle filter belief, etc (see the screen capture below):
70+
71+
![Demo running](images/demo_amcl3_running.png)
72+
73+
You can stop the demo by pressing `Ctrl+C` in the terminal where rviz and the rosbag are running.
74+
75+
## Maps
76+
77+
To test a different map the following variable in `src/beluga_amcl3_demo.cpp` must be changed accordingly:
78+
79+
```cpp
80+
const std::string kMapFile = ament_index_cpp::get_package_share_directory("beluga_demo_amcl3_localization") + "/maps/map_1005_07.vdb";
81+
```
921 KB
Loading
940 KB
Loading

0 commit comments

Comments
 (0)