Multi-Robot Support

.devcontainer/nouveau/Dockerfile

@@ -1,4 +1,4 @@
-FROM ghcr.io/robotic-decision-making-lab/blue:rolling-desktop
+FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop
 
 # Install ROS dependencies
 # This is done in a previous stage, but we include it again here in case anyone wants to

.devcontainer/nvidia/Dockerfile

@@ -1,4 +1,5 @@
-FROM ghcr.io/robotic-decision-making-lab/blue:rolling-desktop-nvidia
+FROM ghcr.io/jbvakshaya/blue:iron-nvidia-ompl
+# FROM ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia
 
 # Install ROS dependencies
 # This is done in a previous stage, but we include it again here in case anyone wants to
@@ -16,6 +17,34 @@ RUN sudo apt-get -q update \
     && sudo apt-get clean -y \
     && sudo rm -rf /var/lib/apt/lists/*
 
+# Install OMPL with ROS2
+RUN curl http://repo.ros2.org/repos.key | sudo apt-key add -\
+    && sudo apt-get update \
+    && sudo apt-get install -y nano \
+    && sudo apt-get install -y ros-iron-ompl
+
+# RUN cd ${USER_WORKSPACE}/src/learn_ros2 \
+#     && echo "Check me" \
+#     && echo ls -d -- */
+
+# Install pinocchio
+RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-pinocchio
+
+# Update rqt to find plugins work with ROS2
+RUN  sudo apt update \
+    && sudo apt install -y ros-${ROS_DISTRO}-rqt*
+
+# tf2 depedencies
+# RUN sudo apt-get install ros-${ROS_DISTRO}-rviz2 ros-${ROS_DISTRO}-turtle-tf2-py ros-${ROS_DISTRO}-tf2-ros ros-${ROS_DISTRO}-tf2-tools ros-${ROS_DISTRO}-turtlesim
+
+# Install the learn_ros2 package
+# RUN cd ${USER_WORKSPACE} \
+#     && echo "Check me" \
+#     && pwd \
+#     && colcon build --packages-select learn_ros2 \
+#     && . install/setup.bash
+
+RUN echo 'nvidia' > ${USER_WORKSPACE}/nvidia.txt
 # Install debugging/linting Python packages
 RUN python3 -m pip install \
     pre-commit \

.devcontainer/nvidia/devcontainer.json

@@ -14,6 +14,7 @@
     "--privileged",
     "--volume=/tmp/.X11-unix:/tmp/.X11-unix",
     "--volume=/mnt/wslg:/mnt/wslg",
+    "--volume=/home/akshaya/Research/doc_vol/learn_ros2:/home/blue/ws_blue/src/learn_ros2",
     "--gpus=all"
   ],
   "containerEnv": {

.docker/Dockerfile

@@ -1,10 +1,15 @@
 ARG ROS_DISTRO=rolling
 FROM ros:$ROS_DISTRO-ros-base AS ci
 
+FROM ros:$ROS_DISTRO-ros-base AS ci
+ARG USERNAME
+ARG USER_UID
+ARG USER_GID
 ENV DEBIAN_FRONTEND=noninteractive
 
 WORKDIR /root/ws_blue
 COPY . src/blue
+RUN echo 'ompl' > ompl.txt
 
 # Install apt packages needed for CI
 RUN apt-get -q update \
@@ -53,7 +58,7 @@ FROM ci AS robot
 # jazzy and rolling images, now includes a user "ubuntu" at UID 1000
 ARG USERNAME=ubuntu
 ARG USER_UID=1000
-ARG USER_GID=$USER_UID
+ARG USER_GID=1000
 
 RUN echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
     && chmod 0440 /etc/sudoers.d/$USERNAME \
@@ -89,6 +94,8 @@ COPY --chown=$USER_UID:$USER_GID . src/blue
 # Install the Python requirements that aren't available as rosdeps
 RUN python3 -m pip install -r $(pwd)/src/blue/requirements-build.txt
 
+RUN sudo apt-get -q update && sudo apt-get install nano
+
 # Install gstreamer
 RUN sudo apt-get -q update \
     && sudo apt-get -q -y upgrade \
@@ -206,7 +213,7 @@ RUN sudo apt-get -q update \
     && sudo apt-get -q -y upgrade \
     && vcs import src < src/blue/sim.repos \
     && rosdep update \
-    && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} \
+    && rosdep install -y --from-paths src --ignore-src --rosdistro ${ROS_DISTRO} --skip-keys="gz-transport12 gz-sim7 gz-math7 gz-msgs9 gz-plugin2" \
     && sudo apt-get autoremove -y \
     && sudo apt-get clean -y \
     && sudo rm -rf /var/lib/apt/lists/*
@@ -217,6 +224,12 @@ RUN sudo apt-get -q update \
 RUN . "/opt/ros/${ROS_DISTRO}/setup.sh" \
     && colcon build
 
+# Install OMPL with ROS2
+RUN curl http://repo.ros2.org/repos.key | sudo apt-key add -\
+    && sudo apt-get update \
+    && sudo apt-get install -y nano \
+    && sudo apt-get install -y ros-iron-ompl
+
 # Setup the simulation environment variables
 RUN  <<EOT cat >> /home/$USERNAME/.bashrc
 
@@ -235,6 +248,17 @@ EOT
 
 FROM desktop AS desktop-nvidia
 
+# Add ardupilot_gazebo plugin
+export GZ_SIM_SYSTEM_PLUGIN_PATH=\$HOME/ardupilot_gazebo/build:\$GZ_SIM_SYSTEM_PLUGIN_PATH
+
+# Add ardupilot_gazebo models and worlds
+export GZ_SIM_RESOURCE_PATH=\$HOME/ardupilot_gazebo/models:\$HOME/ardupilot_gazebo/worlds:\$GZ_SIM_RESOURCE_PATH
+EOT
+
+FROM desktop AS desktop-nvidia
+ARG USERNAME
+ARG USER_UID
+ARG USER_GID
 # Install NVIDIA software
 RUN sudo apt-get update \
     && sudo apt-get -q -y upgrade \
@@ -249,6 +273,10 @@ RUN sudo apt-get update \
     && sudo apt-get clean -y \
     && sudo rm -rf /var/lib/apt/lists/*
 
+#Install nano
+RUN sudo apt-get update \
+    && sudo apt-get install -y nano
+
 # Env vars for the nvidia-container-runtime.
 ENV NVIDIA_VISIBLE_DEVICES=all
 ENV NVIDIA_DRIVER_CAPABILITIES=graphics,utility,compute

.docker/compose/nouveau-desktop.yaml

@@ -1,6 +1,6 @@
 services:
   blue:
-    image: ghcr.io/robotic-decision-making-lab/blue:rolling-desktop
+    image: ghcr.io/robotic-decision-making-lab/blue:iron-desktop
     environment:
       - DISPLAY=${DISPLAY}
       - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR}

.docker/compose/nvidia-desktop.yaml

@@ -1,6 +1,6 @@
 services:
   blue:
-    image: ghcr.io/robotic-decision-making-lab/blue:rolling-desktop-nvidia
+    image: ghcr.io/robotic-decision-making-lab/blue:iron-desktop-nvidia
     environment:
       - DISPLAY=${DISPLAY}
       - XDG_RUNTIME_DIR=${XDG_RUNTIME_DIR}

.github/workflows/ci.yaml

@@ -3,7 +3,7 @@ name: Check Implementation
 on:
   push:
     branches:
-      - main
+      - iron
   pull_request:
   workflow_dispatch:
 
@@ -19,8 +19,8 @@ jobs:
       fail-fast: false
       matrix:
         env:
-          - IMAGE: rolling-ci
-            ROS_DISTRO: rolling
+          - IMAGE: iron-ci
+            ROS_DISTRO: iron
     steps:
       - name: Checkout repository
         uses: actions/checkout@v4

.github/workflows/docker.yaml

@@ -5,7 +5,7 @@ on:
     - cron: "0 17 * * 6"
   push:
     branches:
-      - main
+      - iron
   pull_request:
     paths:
       - .docker/**
@@ -22,7 +22,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ROS_DISTRO: [rolling]
+        ROS_DISTRO: [iron]
     runs-on: ubuntu-latest
     permissions:
       packages: write

.github/workflows/format.yaml

@@ -4,7 +4,7 @@ on:
   pull_request:
   push:
     branches:
-      - main
+      - iron
   workflow_dispatch:
 
 jobs:

blue.repos

@@ -8,7 +8,7 @@ repositories:
   auv_controllers:
     type: git
     url: https://github.com/Robotic-Decision-Making-Lab/auv_controllers.git
-    version: main
+    version: iron
 
   ardusub_driver:
     type: git

blue_bringup/launch/bluerov2_heavy/ns_bluerov2_heavy.launch.yaml

@@ -0,0 +1,168 @@
+launch:
+
+  - let:
+      name: model_name
+      value: bluerov2_heavy
+
+  # Arguments
+  - arg:
+      name: use_camera
+      default: "false"
+
+  - arg:
+      name: use_mocap
+      default: "false"
+
+  - arg:
+      name: localization_source
+      default: gazebo
+      choice:
+        - value: gazebo
+        - value: mocap
+        - value: camera
+
+  - arg:
+      name: use_sim
+      default: "false"
+
+  - arg:
+      name: prefix
+      default: ""
+
+  - arg:
+      name: use_rviz
+      default: "false"
+
+  - arg:
+      name: use_manager
+      default: "true"
+
+  - arg:
+      name: rviz_config
+      default: $(find-pkg-share blue_description)/rviz/$(var model_name).rviz
+
+  - arg:
+      name: mavros_file
+      default: $(find-pkg-share blue_description)/config/ardusub/ns_mavros.yaml
+
+  - arg:
+      name: manager_file
+      default: $(find-pkg-share blue_description)/config/ardusub/ns_ardusub_manager.yaml
+
+  - arg:
+      name: ardusub_params_file
+      default: $(find-pkg-share blue_description)/config/$(var model_name)/ardusub.parm
+
+  - arg:
+      name: localization_file
+      default: $(find-pkg-share blue_description)/config/$(var model_name)/ns_localization.yaml
+
+  # Load the description file
+  - let:
+      name: description_file
+      value: $(find-pkg-share blue_description)/description/$(var model_name)/config.xacro
+
+  - arg:
+      name: robot_description
+      default: $(command 'xacro $(var description_file) prefix:=$(var prefix) use_sim:=$(var use_sim)')
+
+  - arg:
+      name: sitl_file
+      default: $(find-pkg-share ardusub_bringup)/launch/sitl_multi_robot.launch.yaml
+
+  - arg:
+      name: ardusub_instance
+      default: "0"
+
+  - arg:
+      name: sysid_thismav
+      default: "1"
+
+  - arg:
+      name: rob_x
+      default: "0.0"
+
+  - arg:
+      name: rob_y
+      default: "0.0"
+
+  - arg:
+      name: rob_z
+      default: "0.0"
+
+  # Nodes
+
+  - node:
+      pkg: robot_state_publisher
+      exec: robot_state_publisher
+      param:
+        - name: robot_description
+          value: $(var robot_description)
+        - name: use_sim_time
+          value: $(var use_sim)
+        - name: tf_prefix
+          value: $(var prefix)
+
+  - node:
+      pkg: rviz2
+      exec: rviz2
+      if: $(var use_rviz)
+      args: -d $(var rviz_config)
+      param:
+        - name: robot_description
+          value: $(var robot_description)
+
+# Includes
+  - include:
+      file: $(find-pkg-share ardusub_bringup)/launch/ns_ardusub.launch.yaml
+      arg:
+        - name: mavros_file
+          value: $(var mavros_file)
+        - name: ardusub_params_file
+          value: $(var ardusub_params_file)
+        - name: manager_file
+          value: $(var manager_file)
+        - name: model_name
+          value: $(var model_name)
+        - name: use_sim
+          value: $(var use_sim)
+        - name: use_manager
+          value: $(var use_manager)
+        - name: sitl_file
+          value: $(var sitl_file)
+        - name: ardusub_instance
+          value: $(var ardusub_instance)
+        - name: sysid_thismav
+          value: $(var sysid_thismav)
+        - name: rob_x
+          value: $(var rob_x)
+        - name: rob_y
+          value: $(var rob_y)
+        - name: rob_z
+          value: $(var rob_z)
+
+  - include:
+      file: $(find-pkg-share blue_localization)/localization.launch.py
+      arg:
+        - name: localization_source
+          value: $(var localization_source)
+        - name: use_camera
+          value: $(var use_camera)
+        - name: use_mocap
+          value: $(var use_mocap)
+        - name: use_sim_time
+          value: $(var use_sim)
+        - name: config_filepath
+          value: $(var localization_file)
+
+  - include:
+      file: $(find-pkg-share blue_bringup)/launch/$(var model_name)/thrusters.launch.yaml
+      arg:
+        - name: prefix
+          value: $(var prefix)
+
+  - include:
+      file: $(find-pkg-share message_transforms)/launch/tf.launch.yaml
+      arg:
+        - name: prefix
+          value: $(var prefix)