diff --git a/.gitignore b/.gitignore index 722d5e7..4dc51f0 100644 --- a/.gitignore +++ b/.gitignore @@ -1 +1,3 @@ .vscode +*.db3 +*.vdb diff --git a/README.md b/README.md index 3a54e45..0566ebc 100644 --- a/README.md +++ b/README.md @@ -33,7 +33,8 @@ Step into the repository directory and start the docker image. The first time yo ```bash cd beluga-demos -./docker/run.sh --build +ROSDISTRO=humble docker/run.sh # ROS2 Humble +ROSDISTRO=jazzy docker/run.sh # ROS2 Jazzy ``` #### 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 This table contains the list of predefined demos that you can run: -| Alias command | Description | -| ------------------------------------ | ---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | -| `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. | -| `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. | -| `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`. | -| `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`. | -| `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). | -| `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). | -| `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. | +| Alias command | Distro | Description | +| ------------------------------------ | --------------------------------------------- | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| `lidar_beam_model_hallway_demo` | Humble | 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. | +| `lidar_likelihood_model_hallway_demo`| Humble | 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. | +| `lidar_beam_model_office_demo` | Humble | 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`. | +| `lidar_likelihood_model_office_demo` | Humble | 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`. | +| `apriltags_localization_demo` | Humble | 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). | +| `light_beacons_localization_demo` | Humble | 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). | +| `nav2_integration_demo` | Humble | 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. +| `amcl3_localization_demo` | Jazzy | Custom 3D localization node using the `beluga` 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/pvela2017/beluga/blob/likelihood_field_3d_model/beluga/include/beluga/sensor/likelihood_field_model3.hpp). | ## Under the hood @@ -89,9 +91,15 @@ The usual ROS 2 tooling can be used to build, launch and examine the demos insid The `demo_build` alias command is a wrapper around the `colcon` build tool that builds the workspace and sources it. It's equivalent to running the following commands: ```bash +# Humble cd ~/ws colcon build --symlink-install source install/setup.bash + +# Jazzy +cd ~/ws +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -DUSE_OPENVDB=ON +source install/setup.bash ``` The individual demo alias commands are just wrappers around the `ros2 launch` command. For instance, the `lidar_likelihood_model_hallway_demo` alias is equivalent to running the following command after having built the workspace: diff --git a/docker/docker-compose.yml b/docker/docker-compose.yml index ff3867c..a15ad20 100644 --- a/docker/docker-compose.yml +++ b/docker/docker-compose.yml @@ -3,7 +3,9 @@ services: build: context: .. dockerfile: docker/images/${ROSDISTRO:-humble}/Dockerfile - container_name: beluga-demo-dev + image: ekumenlabs/beluga-demo-${ROSDISTRO:-humble}-dev + hostname: beluga-demo-${ROSDISTRO:-humble}-dev + container_name: beluga-demo-${ROSDISTRO:-humble}-dev environment: - DISPLAY - QT_X11_NO_MITSHM=1 @@ -15,7 +17,6 @@ services: volumes: - /tmp/.X11-unix:/tmp/.X11-unix:rw - /tmp/.docker.xauth:/tmp/.docker.xauth - - ..:/home/developer/ws/src/beluga_demo - beluga_demos_ccache:/home/developer/.ccache - /dev:/dev volumes: diff --git a/docker/files/DOTaliases b/docker/files/DOTaliases index 9a076ab..01cfcbf 100644 --- a/docker/files/DOTaliases +++ b/docker/files/DOTaliases @@ -1,31 +1,42 @@ -alias demo_build='cd ~/ws \ - && colcon build --symlink-install \ - && source install/setup.bash' +if [ "${ROSDISTRO}" == "humble" ]; then + alias demo_build='cd ~/ws \ + && colcon build --symlink-install \ + && source install/setup.bash' -alias lidar_beam_model_hallway_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_lidar_localization demo_hallway_beam_localization.launch.py' + alias lidar_beam_model_hallway_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_lidar_localization demo_hallway_beam_localization.launch.py' -alias lidar_likelihood_model_hallway_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_lidar_localization demo_hallway_likelihood_localization.launch.py' + alias lidar_likelihood_model_hallway_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_lidar_localization demo_hallway_likelihood_localization.launch.py' -alias lidar_beam_model_office_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_lidar_localization demo_office_beam_localization.launch.py' + alias lidar_beam_model_office_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_lidar_localization demo_office_beam_localization.launch.py' -alias lidar_likelihood_model_office_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_lidar_localization demo_office_likelihood_localization.launch.py' + alias lidar_likelihood_model_office_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_lidar_localization demo_office_likelihood_localization.launch.py' -alias apriltags_localization_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_fiducial_localization demo_apriltags_based_localization.launch.py' + alias apriltags_localization_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_fiducial_localization demo_apriltags_based_localization.launch.py' -alias light_beacons_localization_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_bearing_localization demo_light_beacons_based_localization.launch.py' + alias light_beacons_localization_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_bearing_localization demo_light_beacons_based_localization.launch.py' -alias nav2_integration_demo='cd ~/ws \ - && source install/setup.bash \ - && ros2 launch beluga_demo_nav2_integration demo_office_navigation.launch.py' + alias nav2_integration_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_nav2_integration demo_office_navigation.launch.py' + +else + alias demo_build='cd ~/ws \ + && colcon build --symlink-install --packages-up-to beluga_demo_amcl3_localization --cmake-args -DCMAKE_BUILD_TYPE=Release -DUSE_OPENVDB=ON \ + && source install/setup.bash' + + alias amcl3_localization_demo='cd ~/ws \ + && source install/setup.bash \ + && ros2 launch beluga_demo_amcl3_localization demo_botanic_garden_amcl3_localization.launch.py' +fi diff --git a/docker/files/jazzy_base.repos b/docker/files/jazzy_base.repos new file mode 100644 index 0000000..64c29eb --- /dev/null +++ b/docker/files/jazzy_base.repos @@ -0,0 +1,5 @@ +repositories: + beluga: + type: git + url: https://github.com/pvela2017/beluga.git + version: likelihood_field_3d_model diff --git a/docker/images/jazzy/Dockerfile b/docker/images/jazzy/Dockerfile new file mode 100644 index 0000000..3818735 --- /dev/null +++ b/docker/images/jazzy/Dockerfile @@ -0,0 +1,105 @@ +FROM ros:jazzy-ros-base AS cacher + +WORKDIR /ws/src + +COPY localization/beluga_demo_amcl3_localization beluga_demo/localization/ + +RUN mkdir -p /tmp/ws/src \ + && find ./ -name "package.xml" | xargs cp --parents -t /tmp/ws/src \ + && find ./ -name "COLCON_IGNORE" | xargs cp --parents -t /tmp/ws/src \ + || true + +FROM ros:jazzy-ros-base AS builder + +ENV DEBIAN_FRONTEND noninteractive + +RUN apt-get update \ + && apt-get install --no-install-recommends -y \ + ccache \ + curl \ + gdb \ + git \ + python3-pip \ + mc \ + tmux \ + && rm -rf /var/lib/apt/lists/* + +ENV PIP_BREAK_SYSTEM_PACKAGES 1 + +RUN pip install \ + pre-commit==2.20.0 + +ARG USER=developer +ARG GROUP=ekumen + +# Drop ubuntu user to avoid fixuid clashes. +RUN deluser ubuntu + +RUN addgroup --gid 1000 $GROUP \ + && adduser --uid 1000 --ingroup $GROUP --home /home/$USER --shell /bin/sh --disabled-password --gecos "" $USER \ + && adduser $USER sudo \ + && adduser $USER dialout \ + && echo "$USER ALL=NOPASSWD: ALL" >> /etc/sudoers.d/$USER + +COPY docker/files/fixuid_config.yml /etc/fixuid/config.yml +RUN curl -SsL https://github.com/boxboat/fixuid/releases/download/v0.4/fixuid-0.4-linux-amd64.tar.gz | tar -C /usr/local/bin -xzf - \ + && chmod 4755 /usr/local/bin/fixuid \ + && cd /etc/fixuid \ + && sed -i "s/_USER_/$USER/" config.yml \ + && sed -i "s/_GROUP_/$GROUP/" config.yml + +USER $USER:$GROUP + +ENV USER_WS /home/$USER/ws +RUN mkdir -p /$USER_WS + +WORKDIR /tmp + +WORKDIR $USER_WS + +RUN colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml \ + && colcon mixin update default +COPY --chown=$USER:$GROUP docker/files/colcon_defaults.yaml /home/$USER/.colcon/defaults.yaml +RUN mkdir -p /home/$USER/.ccache $USER_WS/src + +# +# Install base system external dependencies + +COPY docker/files/jazzy_base.repos /tmp/jazzy_base.repos +RUN cd src/ \ + && mkdir -p external-deps \ + && cd external-deps \ + && vcs import < /tmp/jazzy_base.repos + +RUN sudo apt-get update \ + && . /opt/ros/jazzy/setup.sh \ + && rosdep update \ + && rosdep install -i -y --from-path src \ + && sudo rm -rf /var/lib/apt/lists/* + +# +# Install project dependencies + +USER root +ENV PIP_BREAK_SYSTEM_PACKAGES 1 + +COPY --from=cacher --chown=$USER:$GROUP /tmp/ws/ $USER_WS/ +RUN sudo apt-get update \ + && . /opt/ros/jazzy/setup.sh \ + && rosdep update \ + && rosdep install -i -y --from-path src \ + && sudo rm -rf /var/lib/apt/lists/* + +# +# Install aliases and setup environment + +COPY --chown=$USER:$GROUP docker/files/DOTaliases /home/$USER/.bash_aliases + +RUN bash -c "echo '. /opt/ros/jazzy/setup.bash' >> /home/$USER/.bashrc" +RUN bash -c "echo 'ros2 daemon start' >> /home/$USER/.bashrc" + +ENV WITHIN_DEV 1 +ENV MAKEFLAGS "-j 2" + +ENV SHELL /bin/bash +ENTRYPOINT ["fixuid", "-q", "/ros_entrypoint.sh", "/bin/bash"] diff --git a/docker/run.sh b/docker/run.sh index 4ca895a..b987728 100755 --- a/docker/run.sh +++ b/docker/run.sh @@ -66,10 +66,31 @@ while [[ "$1" != "" ]]; do esac done +DOCKER_EXTRA_ARGS="" +BASE_PATH="$(cd .. && pwd)" + +if [ "${ROSDISTRO}" == "humble" ]; then + DOCKER_EXTRA_ARGS="${DOCKER_EXTRA_ARGS} -v /home/developer/ws/src/beluga_demo/localization/beluga_demo_amcl3_localization" +fi + +if [ "${ROSDISTRO}" == "jazzy" ]; then + EXCLUDE_FOLDERS=("common" + "integration" + "misc" + "localization/beluga_demo_bearing_localization" + "localization/beluga_demo_fiducial_localization" + "localization/beluga_demo_lidar_localization" + ) + + for dir in ${EXCLUDE_FOLDERS[@]}; do + DOCKER_EXTRA_ARGS="${DOCKER_EXTRA_ARGS} -v /home/developer/ws/src/beluga_demo/${dir}" + done +fi + # Note: The `--build` flag was added to docker compose run after # https://github.com/docker/compose/releases/tag/v2.13.0. # We have this for convenience and compatibility with previous versions. # Otherwise, we could just forward the script arguments to the run verb. [[ "$BUILD" = true ]] && docker compose build beluga-demo-dev -PRIVILEGED_CONTAINER=$PRIVILEGED_CONTAINER USERID=$(id -u) GROUPID=dialout docker compose run --rm beluga-demo-dev +PRIVILEGED_CONTAINER=$PRIVILEGED_CONTAINER USERID=$(id -u) GROUPID=dialout docker compose run -v ${BASE_PATH}:/home/developer/ws/src/beluga_demo ${DOCKER_EXTRA_ARGS} --rm beluga-demo-dev diff --git a/localization/beluga_demo_amcl3_localization/CMakeLists.txt b/localization/beluga_demo_amcl3_localization/CMakeLists.txt new file mode 100644 index 0000000..9b5ca0f --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/CMakeLists.txt @@ -0,0 +1,118 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +################################################################################ +# Set minimum required version of cmake, project name and compile options +################################################################################ + +cmake_minimum_required(VERSION 3.5) +project(beluga_demo_amcl3_localization) + +set(OPENVDB_CMAKE_MODULE_PATH + ${OPENVDB_CMAKE_MODULE_PATH} + CACHE PATH "Path to OpenVDB CMake module") + +if(NOT OPENVDB_CMAKE_MODULE_PATH) + file(GLOB_RECURSE OPENVDB_MODULES /usr/lib/*/FindOpenVDB.cmake + /usr/local/lib/*/FindOpenVDB.cmake) + list(LENGTH OPENVDB_MODULES NUM_OPENVDB_MODULES) + if (NUM_OPENVDB_MODULES EQUAL 1) + list(GET OPENVDB_MODULES 0 OPENVDB_MODULE) + get_filename_component(OPENVDB_CMAKE_MODULE_PATH ${OPENVDB_MODULE} + DIRECTORY) + endif() + unset(NUM_OPENVDB_MODULES) + unset(OPENVDB_MODULES) +endif() +if(OPENVDB_CMAKE_MODULE_PATH) + list(APPEND CMAKE_MODULE_PATH ${OPENVDB_CMAKE_MODULE_PATH}) +endif() + +################################################################################ +# Find ament packages and libraries for ament and system dependencies +################################################################################ + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(range-v3 REQUIRED) +find_package(Sophus REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) +find_package(beluga REQUIRED) +find_package(beluga_ros REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(message_filters REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(OpenVDB REQUIRED) + + +################################################################################ +# Build +################################################################################ + +add_library(beluga_amcl3_demo_component SHARED) +target_sources(beluga_amcl3_demo_component PRIVATE src/beluga_amcl3_demo.cpp) +target_compile_features(beluga_amcl3_demo_component PUBLIC cxx_std_17) +target_link_libraries(beluga_amcl3_demo_component + PUBLIC beluga::beluga beluga_ros::beluga_ros) +ament_target_dependencies( + beluga_amcl3_demo_component + PUBLIC geometry_msgs + visualization_msgs + std_msgs + sensor_msgs + message_filters + rclcpp + rclcpp_components + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + beluga + beluga_ros) +rclcpp_components_register_node( + beluga_amcl3_demo_component + PLUGIN + "beluga_demo_amcl3_localization::Amcl3Node" + EXECUTABLE beluga_amcl3_demo_node) + + +################################################################################ +# Install +################################################################################ + +install( + TARGETS beluga_amcl3_demo_component + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) +install(TARGETS beluga_amcl3_demo_node DESTINATION lib/${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + scripts/tf_publisher.py + scripts/marker_publisher.py + DESTINATION lib/${PROJECT_NAME} +) + +install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/localization/beluga_demo_amcl3_localization/launch/demo_botanic_garden_amcl3_localization.launch.py b/localization/beluga_demo_amcl3_localization/launch/demo_botanic_garden_amcl3_localization.launch.py new file mode 100644 index 0000000..ff04307 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/launch/demo_botanic_garden_amcl3_localization.launch.py @@ -0,0 +1,137 @@ +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node + +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + ################################################################################ + # CONFIGURATION FILES + ################################################################################ + # Packages names + amcl3_package_name = "beluga_demo_amcl3_localization" + + # Rviz config file + rviz_config_file = PathJoinSubstitution( + [FindPackageShare(amcl3_package_name), "rviz", "rviz.rviz"] + ) + + ################################################################################ + # NODES DEFINITION + ################################################################################ + beluga_amcl3 = Node( + package="beluga_demo_amcl3_localization", + executable="beluga_amcl3_demo_node", + name="beluga_amcl3_demo", + parameters=[{'use_sim_time': True}], + output="screen", + ) + + static_tf = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='velodyne_in_xsense', + arguments=[ + '0.0584867781527745', + '0.00840419966766332', + '0.168915521980526', + '0.0078031', + '0.0015042', + '-0.0252884', + 'base_link', + 'velodyne', + ], + parameters=[{'use_sim_time': True}], + ) + + tf = Node( + package='beluga_demo_amcl3_localization', + executable='tf_publisher.py', + name='odom_to_base_link', + parameters=[{'use_sim_time': True}], + ) + + markers = Node( + package='beluga_demo_amcl3_localization', + executable='marker_publisher.py', + name='pose_marker_node', + parameters=[{'use_sim_time': True}], + ) + + voxel_filter = Node( + package="pcl_ros", + executable="filter_voxel_grid_node", + name="filter_voxel_grid_node", + parameters=[{'use_sim_time': True, 'leaf_size': 0.1}], + remappings=[ + ('/input', '/velodyne_points'), + # ('/output', '/pointcloud'), + ('/output', '/output_voxel_filter'), + ], + output="screen", + ) + + crop_box_filter = Node( + package="pcl_ros", + executable="filter_crop_box_node", + name="filter_crop_box_node", + parameters=[ + { + 'use_sim_time': True, + 'min_x': -20.0, + 'max_x': 20.0, + 'min_y': -20.0, + 'max_y': 20.0, + 'min_z': -20.0, + 'max_z': 20.0, + } + ], + remappings=[ + ('/input', '/output_voxel_filter'), + ('/output', '/pointcloud'), + ], + output="screen", + ) + + rviz = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + parameters=[{'use_sim_time': True}], + arguments=["-d", rviz_config_file], + ) + + ################################################################################ + # LAUNCH NODES + ################################################################################ + nodes = [ + rviz, + static_tf, + tf, + markers, + voxel_filter, + crop_box_filter, + RegisterEventHandler( + event_handler=OnProcessStart(target_action=rviz, on_start=[beluga_amcl3]) + ), + ] + + return LaunchDescription(nodes) diff --git a/localization/beluga_demo_amcl3_localization/maps/map.vdb b/localization/beluga_demo_amcl3_localization/maps/map.vdb new file mode 100755 index 0000000..00885cb Binary files /dev/null and b/localization/beluga_demo_amcl3_localization/maps/map.vdb differ diff --git a/localization/beluga_demo_amcl3_localization/package.xml b/localization/beluga_demo_amcl3_localization/package.xml new file mode 100644 index 0000000..ae396a7 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/package.xml @@ -0,0 +1,23 @@ + + + + beluga_demo_amcl3_localization + 1.0.0 + Demos of Beluga AMCL 3D based on a botanic garden dataset. + Gerardo Puga + Apache License 2.0 + + ament_cmake + + rclcpp + beluga + beluga_ros + libboost-iostreams-dev + libboost-thread-dev + libopenvdb-dev + pcl_ros + + + ament_cmake + + diff --git a/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1005_07/metadata.yaml b/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1005_07/metadata.yaml new file mode 100644 index 0000000..ecc95f1 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1005_07/metadata.yaml @@ -0,0 +1,43 @@ +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: {} + duration: + nanoseconds: 561580161683 + files: + - duration: + nanoseconds: 561580161683 + message_count: 121055 + path: 1005_07.db3 + starting_time: + nanoseconds_since_epoch: 1664954558092533000 + message_count: 121055 + relative_file_paths: + - 1005_07.db3 + ros_distro: rosbags + starting_time: + nanoseconds_since_epoch: 1664954558092533000 + storage_identifier: sqlite3 + topics_with_message_count: + - message_count: 5474 + topic_metadata: + name: /velodyne_points + offered_qos_profiles: '' + serialization_format: cdr + type: sensor_msgs/msg/PointCloud2 + type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5 + - message_count: 110212 + topic_metadata: + name: /odom + offered_qos_profiles: '' + serialization_format: cdr + type: nav_msgs/msg/Odometry + type_description_hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0 + - message_count: 5369 + topic_metadata: + name: /gt_poses + offered_qos_profiles: '' + serialization_format: cdr + type: geometry_msgs/msg/PoseStamped + type_description_hash: RIHS01_10f3786d7d40fd2b54367835614bff85d4ad3b5dab62bf8bca0cc232d73b4cd8 + version: 8 diff --git a/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1006_01/metadata.yaml b/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1006_01/metadata.yaml new file mode 100644 index 0000000..fbcc74d --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/rosbags/botanic_garden_1006_01/metadata.yaml @@ -0,0 +1,43 @@ +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: {} + duration: + nanoseconds: 762370098240 + files: + - duration: + nanoseconds: 762370098240 + message_count: 164818 + path: 1006_01.db3 + starting_time: + nanoseconds_since_epoch: 1665024607612136000 + message_count: 164818 + relative_file_paths: + - 1006_01.db3 + ros_distro: rosbags + starting_time: + nanoseconds_since_epoch: 1665024607612136000 + storage_identifier: sqlite3 + topics_with_message_count: + - message_count: 7445 + topic_metadata: + name: /velodyne_points + offered_qos_profiles: '' + serialization_format: cdr + type: sensor_msgs/msg/PointCloud2 + type_description_hash: RIHS01_9198cabf7da3796ae6fe19c4cb3bdd3525492988c70522628af5daa124bae2b5 + - message_count: 150048 + topic_metadata: + name: /odom + offered_qos_profiles: '' + serialization_format: cdr + type: nav_msgs/msg/Odometry + type_description_hash: RIHS01_3cc97dc7fb7502f8714462c526d369e35b603cfc34d946e3f2eda2766dfec6e0 + - message_count: 7325 + topic_metadata: + name: /gt_poses + offered_qos_profiles: '' + serialization_format: cdr + type: geometry_msgs/msg/PoseStamped + type_description_hash: RIHS01_10f3786d7d40fd2b54367835614bff85d4ad3b5dab62bf8bca0cc232d73b4cd8 + version: 8 diff --git a/localization/beluga_demo_amcl3_localization/rviz/rviz.rviz b/localization/beluga_demo_amcl3_localization/rviz/rviz.rviz new file mode 100644 index 0000000..e6a56fd --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/rviz/rviz.rviz @@ -0,0 +1,279 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /TF1/Frames1 + - /Particles1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 725 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 100 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /velodyne_points + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + map: + Value: true + odom: + Value: true + velodyne: + Value: true + Marker Scale: 5 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + map: + odom: + base_link: + velodyne: + {} + Update Interval: 0 + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Map + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vdb_map_visualization + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: GT Path + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /gt_pose_markers + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: AMCL3 path + Namespaces: + "": true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /amcl3_pose_markers + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 0; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 100 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: Filter + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.25 + Style: Boxes + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Particles + Namespaces: + bodies: true + heads: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /particle_markers + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 42.181148529052734 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.02468675933778286 + Y: 1.8904190063476562 + Z: -1.0710196495056152 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.8453980684280396 + Target Frame: + Value: Orbit (rviz) + Yaw: 1.0603970289230347 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035efc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000035e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b0000035e000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d00650100000000000007380000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004c70000035e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1848 + X: 72 + Y: 27 diff --git a/localization/beluga_demo_amcl3_localization/scripts/marker_publisher.py b/localization/beluga_demo_amcl3_localization/scripts/marker_publisher.py new file mode 100755 index 0000000..cdebf72 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/scripts/marker_publisher.py @@ -0,0 +1,120 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import PoseStamped, PoseWithCovarianceStamped +from visualization_msgs.msg import Marker, MarkerArray +from std_msgs.msg import ColorRGBA +from geometry_msgs.msg import Point + + +class PoseVisualizer(Node): + def __init__(self): + """Class constructor.""" + super().__init__('pose_marker_node') + self.gt_pose_subscriber = self.create_subscription( + PoseStamped, '/gt_poses', self.gt_pose_callback, 10 + ) + + self.amcl3_pose_subscriber = self.create_subscription( + PoseWithCovarianceStamped, '/amcl3_poses', self.amcl3_pose_callback, 10 + ) + + self.gt_marker_publisher = self.create_publisher( + MarkerArray, '/gt_pose_markers', 10 + ) + self.amcl3_marker_publisher = self.create_publisher( + MarkerArray, '/amcl3_pose_markers', 10 + ) + self.gt_points = [] + self.amcl3_points = [] + + def amcl3_pose_callback(self, msg: PoseWithCovarianceStamped): + """Amcl poses callback.""" + position = msg.pose.pose.position + # Create a point for visualization + point = Point() + point.x = position.x + point.y = position.y + point.z = position.z + self.amcl3_points.append(point) + + # Create MarkerArray to hold the Marker + marker_array = MarkerArray() + + # Create the LineStrip marker + line_strip_marker = Marker() + line_strip_marker.header.frame_id = 'map' + line_strip_marker.header.stamp = self.get_clock().now().to_msg() + line_strip_marker.id = 0 + line_strip_marker.type = Marker.LINE_STRIP + line_strip_marker.action = Marker.ADD + line_strip_marker.scale.x = 0.1 + line_strip_marker.color = ColorRGBA(r=0.0, g=1.0, b=0.0, a=1.0) + + # Add points to the LineStrip marker + line_strip_marker.points = self.amcl3_points + + # Add the marker to the MarkerArray + marker_array.markers.append(line_strip_marker) + + # Publish the MarkerArray + self.amcl3_marker_publisher.publish(marker_array) + + def gt_pose_callback(self, msg: PoseStamped): + """Ground truth poses callback.""" + position = msg.pose.position + # Create a point for visualization + point = Point() + point.x = position.x + point.y = position.y + point.z = position.z + self.gt_points.append(point) + + # Create MarkerArray to hold the Marker + marker_array = MarkerArray() + + # Create the LineStrip marker + line_strip_marker = Marker() + line_strip_marker.header.frame_id = 'map' + line_strip_marker.header.stamp = self.get_clock().now().to_msg() + line_strip_marker.id = 0 + line_strip_marker.type = Marker.LINE_STRIP + line_strip_marker.action = Marker.ADD + line_strip_marker.scale.x = 0.1 + line_strip_marker.color = ColorRGBA(r=1.0, g=0.0, b=0.0, a=1.0) + + # Add points to the LineStrip marker + line_strip_marker.points = self.gt_points + + # Add the marker to the MarkerArray + marker_array.markers.append(line_strip_marker) + + # Publish the MarkerArray + self.gt_marker_publisher.publish(marker_array) + + +def main(args=None): + rclpy.init(args=args) + pose_visualizer = PoseVisualizer() + rclpy.spin(pose_visualizer) + pose_visualizer.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/localization/beluga_demo_amcl3_localization/scripts/tf_publisher.py b/localization/beluga_demo_amcl3_localization/scripts/tf_publisher.py new file mode 100755 index 0000000..8efb3d0 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/scripts/tf_publisher.py @@ -0,0 +1,62 @@ +#!/usr/bin/env python3 + +# Copyright 2024 Ekumen, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import rclpy +from rclpy.node import Node +from nav_msgs.msg import Odometry +from tf2_ros import TransformBroadcaster +from geometry_msgs.msg import TransformStamped + + +class TfPublisher(Node): + def __init__(self): + """Class constructor.""" + super().__init__('tf_republisher_node') + + # Subscribe to the input Odometry topic + self.subscription = self.create_subscription( + Odometry, 'odom', self.odom_callback, 10 + ) + + # Create a TransformBroadcaster to publish tf + self.tf_broadcaster = TransformBroadcaster(self) + + def odom_callback(self, msg): + """Odom callback & TF publisher.""" + # Create and publish the TransformStamped based on the PoseStamped + transform = TransformStamped() + transform.header.stamp = msg.header.stamp + transform.header.frame_id = msg.header.frame_id + transform.child_frame_id = msg.child_frame_id + transform.transform.translation.x = msg.pose.pose.position.x + transform.transform.translation.y = msg.pose.pose.position.y + transform.transform.translation.z = msg.pose.pose.position.z + transform.transform.rotation = msg.pose.pose.orientation + + # Broadcast the transform + self.tf_broadcaster.sendTransform(transform) + + +def main(args=None): + rclpy.init(args=args) + node = TfPublisher() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp b/localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp new file mode 100644 index 0000000..b40a670 --- /dev/null +++ b/localization/beluga_demo_amcl3_localization/src/beluga_amcl3_demo.cpp @@ -0,0 +1,517 @@ +// Copyright 2024 Ekumen, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace beluga_demo_amcl3_localization { + +// Weighted SE(2) state particle type. +using particle_type = std::tuple; +// Motion model variant type for runtime selection support. +using motion_model = beluga::DifferentialDriveModel3d; +// Sensor model variant type for runtime selection support. +using sensor_model = + beluga::LikelihoodFieldModel3>; +// Execution policy variant type for runtime selection support. +using execution_policy = std::execution::parallel_policy; + +class Amcl3 { +public: + explicit Amcl3(motion_model &&motion, sensor_model &&sensor, + const Sophus::SE3d &pose, const Sophus::Matrix6d &covariance, + execution_policy policy = std::execution::par) + : motion_model_{std::move(motion)}, sensor_model_{std::move(sensor)}, + execution_policy_{std::move(policy)}, + spatial_hasher_{kSpatialResolutionLineal, kSpatialResolutionAngular}, + update_policy_{beluga::policies::on_motion(kUpdateMinD, + kUpdateMinA)}, + resample_policy_{beluga::policies::every_n(kResampleInterval)} { + if (kRecoveryAlphaFast) { + resample_policy_ = + resample_policy_ && beluga::policies::on_effective_size_drop; + } + // Initialize particles using a custom distribution. + particles_ = + beluga::views::sample(std::move( + beluga::MultivariateNormalDistribution{pose, covariance})) | + ranges::views::transform(beluga::make_from_state) | + ranges::views::take_exactly(kMaxParticles) | + ranges::to; + force_update_ = true; + } + + auto update(Sophus::SE3d base_pose_in_odom, + beluga_ros::SparsePointCloud3 pointcloud) + -> std::optional> { + if (particles_.empty()) { + return std::nullopt; + } + + if (!update_policy_(base_pose_in_odom) && !force_update_) { + return std::nullopt; + } + + particles_ |= + beluga::actions::propagate( + execution_policy_, + motion_model_(control_action_window_ << base_pose_in_odom)) | + beluga::actions::reweight(execution_policy_, + sensor_model_(std::move(pointcloud))) | + beluga::actions::normalize(execution_policy_); + + if (resample_policy_(particles_)) { + + particles_ |= beluga::views::sample | + beluga::views::take_while_kld(spatial_hasher_, // + kMinParticles, // + kMaxParticles, // + kLdEpsilon, // + kLdZ) | + beluga::actions::assign; + } + + force_update_ = false; + return beluga::estimate(beluga::views::states(particles_), + beluga::views::weights(particles_)); + } + + // Returns a reference to the current set of particles. + const auto &particles() const { return particles_; } + +private: + beluga::TupleVector particles_; + motion_model motion_model_; + sensor_model sensor_model_; + execution_policy execution_policy_; + beluga::spatial_hash spatial_hasher_; + beluga::any_policy update_policy_; + beluga::any_policy resample_policy_; + beluga::RollingWindow control_action_window_; + bool force_update_{true}; + + // Parameters + // Particle filter + static constexpr double kUpdateMinD = 0.25; + static constexpr double kUpdateMinA = 0.2; + static constexpr size_t kResampleInterval = 10; + static constexpr int kMinParticles = 500; + static constexpr int kMaxParticles = 1500; + static constexpr double kLdEpsilon = 0.05; + static constexpr double kLdZ = 3.0; + static constexpr double kRecoveryAlphaSlow = 0.001; + static constexpr double kRecoveryAlphaFast = 0.1; + static constexpr double kSpatialResolutionLineal = 0.07; + static constexpr double kSpatialResolutionAngular = 0.1; +}; + +class Amcl3Node : public rclcpp::Node { +public: + // Constructor. + explicit Amcl3Node(const rclcpp::NodeOptions &options) + : rclcpp::Node("amcl3_example_node", options) { + // Initialize OpenVDB + openvdb::initialize(); + // Load OpenVDB map + openvdb::io::File file(kMapFile); + // Open the file. This reads the file header, but not any grids. + file.open(); + // Read the entire contents of the file and return a list of grid pointers. + openvdb::GridPtrVecPtr grids = file.getGrids(); + // Close the file + file.close(); + // Cast the generic grid pointer to a FloatGrid pointer. + openvdb::FloatGrid::Ptr grid = + openvdb::gridPtrCast((*grids)[0]); + + // Create Likelihood 3D Field Sensor Model + auto params_sm = beluga::LikelihoodFieldModel3Param{}; + params_sm.max_obstacle_distance = kLaserLikelihoodMaxDist; + params_sm.max_laser_distance = kLaserMaxRange; + params_sm.z_hit = kZHit; + params_sm.z_random = kZRand; + params_sm.sigma_hit = kSigmaHit; + + // Create Differential Motion Model + auto params_mm = beluga::DifferentialDriveModelParam{}; + params_mm.rotation_noise_from_rotation = kAlpha1; + params_mm.rotation_noise_from_translation = kAlpha2; + params_mm.translation_noise_from_translation = kAlpha3; + params_mm.translation_noise_from_rotation = kAlpha4; + + // Initial position + const auto pose = Sophus::SE3d{ + Sophus::SO3d{}, + Eigen::Vector3d{ + kInitialPoseX, + kInitialPoseY, + kInitialPoseZ, + }, + }; + + Eigen::Matrix covariance = + Eigen::Matrix::Zero(); + covariance.coeffRef(0, 0) = kInitialPoseCovarianceX; + covariance.coeffRef(1, 1) = kInitialPoseCovarianceY; + covariance.coeffRef(2, 2) = kInitialPoseCovarianceZ; + covariance.coeffRef(3, 3) = kInitialPoseCovarianceYaw; + covariance.coeffRef(4, 4) = kInitialPoseCovariancePitch; + covariance.coeffRef(5, 5) = kInitialPoseCovarianceRoll; + + // Create filter + particle_filter_ = std::make_unique(motion_model{params_mm}, + sensor_model{params_sm, *grid}, + pose, covariance); + + // TFs + tf_buffer_ = std::make_unique(get_clock()); + tf_buffer_->setCreateTimerInterface( + std::make_shared(get_node_base_interface(), + get_node_timers_interface())); + + tf_listener_ = std::make_unique( + *tf_buffer_, this, !kUseDedicatedThread); + tf_broadcaster_ = std::make_unique(this); + + // Callback options + callback_group_ = + this->create_callback_group(rclcpp::CallbackGroupType::Reentrant); + rclcpp::SubscriptionOptions common_subscription_options; + common_subscription_options.callback_group = callback_group_; + + // Pointcloud subcription + pointcloud_sub_ = std::make_unique< + message_filters::Subscriber>( + this, kPointcloudTopic, rmw_qos_profile_sensor_data, + common_subscription_options); + + // Message filter that caches pointcloud readings until it is possible to + // transform from lidar frame to odom frame and update the particle filter. + pointcloud_filter_ = + std::make_unique>( + *pointcloud_sub_, *tf_buffer_, kOdomFrameId, 10, + get_node_logging_interface(), get_node_clock_interface(), + tf2::durationFromSec(kTransformTolerance)); + + pointcloud_connection_ = pointcloud_filter_->registerCallback(std::bind( + &Amcl3Node::pointcloud_callback, this, std::placeholders::_1)); + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), + "Subscribed to pointcloud_topic: %s", + pointcloud_sub_->getTopic().c_str()); + + pose_pub_ = create_publisher( + "amcl3_poses", rclcpp::SystemDefaultsQoS()); + + particle_markers_pub_ = + create_publisher( + "particle_markers", rclcpp::SensorDataQoS()); + + timer_ = create_wall_timer(std::chrono_literals::operator""ms(200), + std::bind(&Amcl3Node::timer_callback, this), + callback_group_); + + if (kDisplayMap) { + m_visualization_marker_pub_ = + create_publisher( + "vdb_map_visualization", rclcpp::SystemDefaultsQoS()); + display_map(grid); + } + } + +private: + // Callback for laser scan updates. + void + pointcloud_callback(sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud) { + if (!particle_filter_) { + RCLCPP_WARN_THROTTLE(rclcpp::get_logger("rclcpp"), *get_clock(), 2000, + "Ignoring pointcloud data because the particle " + "filter has not been initialized"); + return; + } + + auto base_pose_in_odom = Sophus::SE3d{}; + try { + // Use the lookupTransform overload with no timeout since we're not using + // a dedicated tf thread. The message filter we are using avoids the need + // for it. + tf2::convert(tf_buffer_ + ->lookupTransform(kOdomFrameId, kBaseFrameId, + tf2_ros::fromMsg(cloud->header.stamp)) + .transform, + base_pose_in_odom); + } catch (const tf2::TransformException &error) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Could not transform from odom to base: %s", error.what()); + return; + } + + auto lidar_pose_in_base = Sophus::SE3d{}; + try { + tf2::convert(tf_buffer_ + ->lookupTransform(kBaseFrameId, cloud->header.frame_id, + tf2_ros::fromMsg(cloud->header.stamp)) + .transform, + lidar_pose_in_base); + } catch (const tf2::TransformException &error) { + RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), + "Could not transform from base to lidar: %s", error.what()); + return; + } + + const auto update_start_time = std::chrono::high_resolution_clock::now(); + const auto new_estimate = particle_filter_->update( + base_pose_in_odom, // + beluga_ros::SparsePointCloud3{cloud, lidar_pose_in_base}); + const auto update_stop_time = std::chrono::high_resolution_clock::now(); + const auto update_duration = update_stop_time - update_start_time; + + if (new_estimate.has_value()) { + const auto &[base_pose_in_map, _] = new_estimate.value(); + last_known_odom_transform_in_map_ = + base_pose_in_map * base_pose_in_odom.inverse(); + last_known_estimate_ = new_estimate.value(); + + RCLCPP_INFO( + rclcpp::get_logger("rclcpp"), + "Particle filter update iteration stats: %ld particles - %.3fms", + particle_filter_->particles().size(), + std::chrono::duration(update_duration).count()); + } + + // Transforms are always published to keep them current. + if (kTfBroadcast) { + if (last_known_odom_transform_in_map_.has_value()) { + auto message = geometry_msgs::msg::TransformStamped{}; + // Sending a transform that is valid into the future so that odom can be + // used. + const auto expiration_stamp = tf2_ros::fromMsg(cloud->header.stamp) + + tf2::durationFromSec(kTransformTolerance); + message.header.stamp = tf2_ros::toMsg(expiration_stamp); + message.header.frame_id = kGlobalFrameId; + message.child_frame_id = kOdomFrameId; + message.transform = tf2::toMsg(*last_known_odom_transform_in_map_); + tf_broadcaster_->sendTransform(message); + } + } + + // New pose messages are only published on updates to the filter. + if (new_estimate.has_value()) { + auto message = geometry_msgs::msg::PoseWithCovarianceStamped{}; + message.header.stamp = cloud->header.stamp; + message.header.frame_id = kGlobalFrameId; + const auto &[base_pose_in_map, base_pose_covariance] = + new_estimate.value(); + message.pose = tf2::toMsg(base_pose_in_map, base_pose_covariance); + pose_pub_->publish(message); + } + } + + void timer_callback() { + if (!particle_filter_) { + return; + } + + if (particle_markers_pub_->get_subscription_count() > 0) { + auto message = visualization_msgs::msg::MarkerArray{}; + beluga_ros::assign_particle_cloud(particle_filter_->particles(), message); + beluga_ros::stamp_message(kGlobalFrameId, this->get_clock()->now(), + message); + particle_markers_pub_->publish(message); + } + } + + // Map displayer + // Adapted from: + // https://github.com/fzi-forschungszentrum-informatik/vdb_mapping_ros + void display_map(const openvdb::FloatGrid::Ptr grid) const { + visualization_msgs::msg::Marker marker_msg; + const auto point_color = []() { + std_msgs::msg::ColorRGBA msg; + msg.r = 1.0; + msg.g = 1.0; + msg.b = 1.0; + msg.a = 1.0; + return msg; + }(); + const openvdb::CoordBBox bbox = grid->evalActiveVoxelBoundingBox(); + const openvdb::Vec3d min_world_coord = grid->indexToWorld(bbox.getStart()); + const openvdb::Vec3d max_world_coord = grid->indexToWorld(bbox.getEnd()); + const double min_z = min_world_coord.z(); + const double max_z = max_world_coord.z(); + + for (openvdb::FloatGrid::ValueOnCIter iter = grid->cbeginValueOn(); iter; + ++iter) { + openvdb::Vec3d world_coord = grid->indexToWorld(iter.getCoord()); + if (world_coord.z() < min_z || world_coord.z() > max_z) { + continue; + } + + geometry_msgs::msg::Point cube_center; + cube_center.x = world_coord.x(); + cube_center.y = world_coord.y(); + cube_center.z = world_coord.z(); + marker_msg.points.push_back(cube_center); + marker_msg.colors.push_back(point_color); + } + + const double size = grid->transform().voxelSize()[0]; + marker_msg.header.frame_id = kGlobalFrameId; + marker_msg.header.stamp = this->get_clock()->now(); + marker_msg.id = 0; + marker_msg.type = visualization_msgs::msg::Marker::CUBE_LIST; + marker_msg.scale.x = size; + marker_msg.scale.y = size; + marker_msg.scale.z = size; + marker_msg.color.a = 1.0; + marker_msg.pose.orientation.w = 1.0; + marker_msg.frame_locked = true; + + if (marker_msg.points.size() > 0) { + marker_msg.action = visualization_msgs::msg::Marker::ADD; + } else { + marker_msg.action = visualization_msgs::msg::Marker::DELETE; + } + + m_visualization_marker_pub_->publish(marker_msg); + } + + // Callback group + rclcpp::CallbackGroup::SharedPtr callback_group_; + // Timer + rclcpp::TimerBase::SharedPtr timer_; + // Pointcloud updates subscription. + std::unique_ptr> + pointcloud_sub_; + // Transform synchronization filter for pointcloud updates. + std::unique_ptr> + pointcloud_filter_; + // Map publisher + rclcpp::Publisher::SharedPtr + m_visualization_marker_pub_; + // Connection for pointcloud updates filter and callback. + message_filters::Connection pointcloud_connection_; + // Particle marker publisher + rclcpp::Publisher::SharedPtr + particle_markers_pub_; + // Particle filter instance. + std::unique_ptr particle_filter_; + // Last known pose estimate, if any. + std::pair> last_known_estimate_; + // Estimated pose publisher. + rclcpp::Publisher::SharedPtr + pose_pub_; + // Transforms buffer. + std::unique_ptr tf_buffer_; + // Transforms broadcaster. + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_listener_; + // Last known map to odom correction estimate, if any. + std::optional last_known_odom_transform_in_map_; + + // Parameters + // Constructor + static constexpr bool kUseDedicatedThread = true; + static constexpr bool kDisplayMap = false; + const std::string kPointcloudTopic = "/pointcloud"; + const std::string kOdomFrameId = "odom"; + const std::string kMapFile = + "/home/developer/ws/src/beluga_demo/localization/" + "beluga_demo_amcl3_localization/maps/map.vdb"; + + // Get Initial Estimate + static constexpr double kInitialPoseX = 0.0; + static constexpr double kInitialPoseY = 0.0; + static constexpr double kInitialPoseZ = 0.0; + static constexpr double kInitialPoseYaw = 0.0; + static constexpr double kInitialPosePitch = 0.0; + static constexpr double kInitialPoseRoll = 0.0; + + static constexpr double kInitialPoseCovarianceX = 0.25; + static constexpr double kInitialPoseCovarianceY = 0.25; + static constexpr double kInitialPoseCovarianceZ = 0.25; + static constexpr double kInitialPoseCovarianceYaw = 0.15; + static constexpr double kInitialPoseCovariancePitch = 0.15; + static constexpr double kInitialPoseCovarianceRoll = 0.15; + + // Get Motion Model + static constexpr double kAlpha1 = 0.1; + static constexpr double kAlpha2 = 0.05; + static constexpr double kAlpha3 = 0.1; + static constexpr double kAlpha4 = 0.05; + + // Get Sensor Model + static constexpr double kLaserLikelihoodMaxDist = 100; + static constexpr double kLaserMaxRange = 100; + static constexpr double kZHit = 0.5; + static constexpr double kZRand = 0.5; + static constexpr double kSigmaHit = 0.2; + + // Pointcloud Callback + const std::string kBaseFrameId = "base_link"; + const std::string kGlobalFrameId = "map"; + static constexpr bool kTfBroadcast = true; + static constexpr double kTransformTolerance = 0.3; +}; + +} // namespace beluga_demo_amcl3_localization + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(beluga_demo_amcl3_localization::Amcl3Node)