Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add amcl3 demo #9

Open
wants to merge 19 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 13 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions .gitignore
Original file line number Diff line number Diff line change
@@ -1 +1,3 @@
.vscode
*.db3
*.vdb
28 changes: 18 additions & 10 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand All @@ -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:
Expand Down
5 changes: 3 additions & 2 deletions docker/docker-compose.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand Down
59 changes: 35 additions & 24 deletions docker/files/DOTaliases
Original file line number Diff line number Diff line change
@@ -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
5 changes: 5 additions & 0 deletions docker/files/jazzy_base.repos
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
repositories:
beluga:
type: git
url: https://github.com/pvela2017/beluga.git
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 this needs to change before merging.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This can now point to the main repository.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Changed in 623f842

version: likelihood_field_3d_model
105 changes: 105 additions & 0 deletions docker/images/jazzy/Dockerfile
Original file line number Diff line number Diff line change
@@ -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"]
23 changes: 22 additions & 1 deletion docker/run.sh
Original file line number Diff line number Diff line change
Expand Up @@ -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"
)
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@pvela2017 I'm a bit ambivalent about this, but I can't think of a better solution. CC @glpuga for thoughts.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Ping @glpuga

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't like it. I'm assuming these don't build or have missing dependencies in Jazzy.

Why supporting anything other than Humble, if this is a demos repository and we provide a docker to run it?

Copy link
Collaborator

@hidmic hidmic Jan 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why supporting anything other than Humble, if this is a demos repository and we provide a docker to run it?

Because OpenVDB packaging is broken in Ubuntu 22.04, see https://bugs.launchpad.net/ubuntu/+source/openvdb/+bug/1970108.

Nav2 folks realized about this and vendored it (see openvdb_vendor) but depending on their vendored version means we are (again) subject to their own release timeline to release ourselves. There is no good solution.

I'm assuming these don't build or have missing dependencies in Jazzy.

Thinking out loud, and this is out of scope for this PR but, Humble is the old LTS distro. Jazzy is new LTS distro. Having some our demos be incompatible with the latest LTS distro of ROS 2 is not good.

Copy link
Collaborator

@glpuga glpuga Jan 29, 2025

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Oh, I remember now.

Jazzy is new LTS distro. Having some our demos be incompatible with the latest LTS distro of ROS 2 is not good.

I'm not so absolute. People are still by far mostly using Humble, while Jazzy is at best "up-and-coming" right now. Core stuff should support both already, but leaf-stuff -like this self-contained demos repo- does not need to.

In any case, there's no reason I can think off the top of my head why these would not build in Jazzy. Is there any external dependency not available?

Looking at the package.xml files, everything seems either standard packages or packages with code we wrote ourselves. Indirect dependencies might be gazebo (classic?) and apriltag_ros which we use the "foxy-devel" branch (because there's no other).

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So, https://github.com/ROBOTIS-GIT/turtlebot3_simulations has not seen a release since Humble and it doesn't look like it will ever do. Nav2 folks spinned up https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation in place for it. Some of our demos already depend on Nav2 code, so I'll go ahead and use this for both Humble and Jazzy. It's source distributed anyways.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In any case, there's no reason I can think off the top of my head why these would not build in Jazzy. Is there any external dependency not available?

@pvela2017 ? I think it was Gazebo related, though my memory might be failing me. Isn't Gazebo Classic gone?

Edit: https://discourse.ros.org/t/gazebo-classic-end-of-life-ros-2-jazzy/36239 indeed.

I dont remember if I reached that step were Gazebo was an issue.

So, https://github.com/ROBOTIS-GIT/turtlebot3_simulations has not seen a release since Humble and it doesn't look like it will ever do. Nav2 folks spinned up https://github.com/ros-navigation/nav2_minimal_turtlebot_simulation in place for it. Some of our demos already depend on Nav2 code, so I'll go ahead and use this for both Humble and Jazzy. It's source distributed anyways.

I remember this was one of the issues. Also, I removed a lot of dependencies so the rosdep wouldn't pop errors.

So, Should we update everything to Gazebo ignition?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we update everything to Gazebo ignition?

@pvela2017 indeed. I'm on it.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we update everything to Gazebo ignition?

@pvela2017 indeed. I'm on it.

Can I help?

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Sure! I'll tag you on the PR as soon as it is up for review. If you can then adjust this PR for it, that'd be amazing.


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
Loading