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 13 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all 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.

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.


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