Fix humble multirobot launch (#30)

* fix multirobot simulation humble

* Add devcotainer for development

* Add user non root

* Finish dockerfile working with humble

* Fix launch for humble

* Clarify change in humble

.devcontainer/Dockerfile

@@ -0,0 +1,123 @@
+FROM nvidia/cuda:11.7.1-devel-ubuntu22.04
+# FROM nvidia/cuda:11.1.1-cudnn8-devel-ubuntu20.04
+
+ENV DEBIAN_FRONTEND noninteractive
+
+RUN apt-get update && apt-get install --no-install-recommends -y \
+    apt-utils \
+    bzip2 \
+    lbzip2 \ 
+    tar \
+    wget \
+    libzbar0 \
+    unzip \
+    build-essential \
+    zlib1g-dev \
+    libcurl4-gnutls-dev \
+    locales \
+    curl  \
+    gnupg2  \
+    lsb-release \
+    && apt autoremove -y && apt clean -y \
+    && rm -rf /var/lib/apt/lists/*
+
+# https://index.ros.org/doc/ros2/Installation/Crystal/Linux-Install-Debians/
+ENV ROS_DISTRO=humble
+
+ENV LANG=C.UTF-8
+ENV LC_ALL=C.UTF-8
+RUN curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key  -o /usr/share/keyrings/ros-archive-keyring.gpg \
+    && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(lsb_release -cs) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null
+
+RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' \
+    && wget https://packages.osrfoundation.org/gazebo.key -O - | apt-key add - \ 
+    && apt update && apt install --no-install-recommends -y \ 
+    ros-${ROS_DISTRO}-ros-base \
+    ros-${ROS_DISTRO}-rclcpp-cascade-lifecycle \
+    ros-${ROS_DISTRO}-geographic-msgs \
+    ros-${ROS_DISTRO}-camera-info-manager \
+    ros-${ROS_DISTRO}-launch-testing-ament-cmake \
+    ros-${ROS_DISTRO}-diagnostic-updater \ 
+    ros-${ROS_DISTRO}-rviz2 \
+    ros-${ROS_DISTRO}-gazebo-ros \
+    ros-${ROS_DISTRO}-gazebo-ros-pkgs \
+    ros-${ROS_DISTRO}-gazebo-msgs \
+    ros-${ROS_DISTRO}-gazebo-plugins \
+    ros-${ROS_DISTRO}-robot-state-publisher \
+    ros-${ROS_DISTRO}-cv-bridge \
+    ros-${ROS_DISTRO}-message-filters \
+    ros-${ROS_DISTRO}-image-transport \ 
+    ros-${ROS_DISTRO}-rqt* \
+    ros-${ROS_DISTRO}-slam-toolbox \
+    ros-${ROS_DISTRO}-navigation2 \
+    ros-${ROS_DISTRO}-nav2-bringup \
+    ros-${ROS_DISTRO}-behaviortree-cpp-v3 \
+    ros-${ROS_DISTRO}-angles \
+    ros-${ROS_DISTRO}-ompl \ 
+    ros-${ROS_DISTRO}-turtlebot3* \
+    ros-${ROS_DISTRO}-image-geometry \
+    && apt autoremove && apt clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+# Install ROS2 gazebo dependencies
+RUN apt update && apt-get install --no-install-recommends -y \
+    libglvnd0 \
+    libglx0 \
+    libegl1 \
+    libxext6 \
+    libx11-6 \
+    libblkid-dev \
+    e2fslibs-dev \
+    libboost-all-dev \
+    libaudit-dev \
+    git \
+    nano \
+    # ------------------------------
+    && apt autoremove && apt clean -y \ 
+    && rm -rf /var/lib/apt/lists/*
+
+RUN apt update && apt install --no-install-recommends -y \
+    python3-dev \
+    python3-pip \
+    python3-colcon-common-extensions \
+    && pip3 install rosdep \ 
+    && rosdep init \
+    && rosdep update \
+    && apt autoremove && apt clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+# Or your actual UID, GID on Linux if not the default 1000
+ARG USERNAME=dev
+ARG USER_UID=1000
+ARG USER_GID=$USER_UID
+
+# Create a non-root user to use if preferred - see https://aka.ms/vscode-remote/containers/non-root-user.
+RUN groupadd --gid $USER_GID $USERNAME \
+    && useradd -s /bin/bash --uid $USER_UID --gid $USER_GID -m $USERNAME \
+    # Add sudo support for non-root user
+    && apt-get update && apt-get install -y sudo \
+    && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \
+    && chmod 0440 /etc/sudoers.d/$USERNAME \
+    && apt-get autoremove && apt-get clean -y \
+    && rm -rf /var/lib/apt/lists/* 
+
+RUN \
+    mkdir -p /home/${USERNAME}/.ignition/fuel/ \
+    && echo "servers:\n  -\n    name: osrf\n    url: https://api.ignitionrobotics.org" >> /home/${USERNAME}/.ignition/fuel/config.yaml \
+    && chown ${USERNAME} /home/${USERNAME}/.ignition \
+    && GAZEBO_SOURCE="source /usr/share/gazebo/setup.sh" \
+    && echo $GAZEBO_SOURCE >> "/home/${USERNAME}/.bashrc" \
+    && chown ${USERNAME} /home/${USERNAME}/.ignition
+
+# ROS2 source setup
+RUN ROS_SOURCE="source /opt/ros/${ROS_DISTRO}/setup.sh" \
+    && echo $ROS_SOURCE >> "/home/${USERNAME}/.bashrc"
+
+WORKDIR /workspace/ros_ws/src
+# Give permission to non-root user to access the workspace
+RUN chown -R ${USERNAME} /workspace/ros_ws
+RUN git clone https://github.com/charlielito/slam_gmapping.git --branch feature/namespace_launch
+
+# Switch back to dialog for any ad-hoc use of apt-get
+ENV DEBIAN_FRONTEND=
+CMD ["/bin/bash"]

.devcontainer/devcontainer.json

@@ -0,0 +1,24 @@
+{
+	"name": "Dev container",
+	"dockerComposeFile": "docker-compose.yml",
+	"service": "m-explore-ros2-humble",
+	"workspaceFolder": "/workspace/ros_ws",
+	"runArgs": [
+		"--runtime=nvidia"
+	],
+	"settings": {
+		"terminal.integrated.automationShell.linux": "/bin/bash",
+	},
+	"extensions": [
+		"ms-azuretools.vscode-docker",
+		"eamodio.gitlens",
+		"streetsidesoftware.code-spell-checker",
+		"oderwat.indent-rainbow",
+		"ms-vsliveshare.vsliveshare",
+		"pkief.material-icon-theme",
+		"ms-python.python",
+		"ms-vscode.cpptools",
+		"xaver.clang-format",
+		"ms-python.vscode-pylance",
+	],
+}

.devcontainer/docker-compose.yml

@@ -0,0 +1,53 @@
+# version: "3.7"
+version: "2.3"
+services:
+  m-explore-ros2-humble:
+    runtime: nvidia
+    build:
+      context: ../
+      dockerfile: .devcontainer/Dockerfile
+    working_dir: /workspace/ros_ws
+    user: dev
+
+    network_mode: host
+    ports:
+      - "80:80"
+    expose:
+      - 80
+
+    init: true
+    privileged: true
+
+    environment:
+      - DISPLAY=$DISPLAY
+      - QT_X11_NO_MITSHM=1
+      - UDEV=1
+      - NVIDIA_DRIVER_CAPABILITIES=compute,utility,display
+    volumes:
+      # Update this to wherever you want VS Code to mount the folder of your project
+      - ..:/workspace/ros_ws/src/m-explore-ros2
+      # Forwards the local Docker socket to the container.
+      - /var/run/docker.sock:/var/run/docker.sock
+      # Enable GUI environments
+      - /tmp/.X11-unix:/tmp/.X11-unix:rw
+
+    devices:
+      - /dev/bus/usb:/dev/bus/usb
+      # NVIDIA drivers to use OpenGL, etc...
+      - /dev/nvidia0:/dev/nvidia0
+      - /dev/nvidiactl:/dev/nvidiactl
+      - /dev/nvidia-uvm:/dev/nvidia-uvm
+      - /dev/input:/dev/input
+      - /dev/snd:/dev/snd
+
+    # Uncomment the next four lines if you will use a ptrace-based debuggers like C++, Go, and Rust.
+    cap_add:
+      - SYS_PTRACE
+    security_opt:
+      - seccomp:unconfined
+
+    # Overrides default command so things don't shut down after the process ends.
+    stdin_open: true
+    tty: true
+
+    command: "/bin/bash"

README.md

@@ -115,10 +115,12 @@ colcon build --symlink-install --packages-up-to slam_gmapping
 
 **Note**: You could use [slam_toolbox](https://github.com/SteveMacenski/slam_toolbox) instead but you need to use this [experimental branch](https://github.com/robo-friends/m-explore-ros2/tree/feature/slam_toolbox_compat) which is still under development.
 
-#### Nav2 gazebo spawner
+#### Nav2 gazebo spawner (deprecated in humble)
 To spawn multiple robots, you need the `nav2_gazebo_spawner` which does not come up with the `nav2-bringup` installation. For that, install it with `sudo apt install ros-${ROS_DISTRO}-nav2-gazebo-spawner`.
+Note that was the case for release previous to `humble` but since `humble` release, this package is deprecated and a gazebo node is used for this. So, if you are using `humble` or newer, you don't need to install it.
+
 #### Nav2 config files
-This repo has some config examples and launch files for running this package with 2 TB3 robots and a world with nav2. Nonetheless, they are only compatible with the galactic branch and since some breaking changes were introduced in this branch, if you want to try it with another ros2 distro you'll need to tweak those param files for that nav2's distro version (which shouldn't be hard).
+This repo has some config examples and launch files for running this package with 2 TB3 robots and a world with nav2. Nonetheless, they are only compatible with the galactic/humble distros and since some breaking changes were introduced in this distro, if you want to try it with another ros2 distro you'll need to tweak those param files for that nav2's distro version (which shouldn't be hard).
 
 ### Running the demo with TB3
 First, you'll need to launch the whole simulation stack, nav2 stacks and slam stacks per robot. For that just launch::

map_merge/launch/tb3_simulation/bringup_launch.py

@@ -163,7 +163,7 @@ def generate_launch_description():
             ),
         ]
     )
-    # Not in GroupAction because namespace were prepended twice because 
+    # Not in GroupAction because namespace were prepended twice because
     # slam_gmapping.launch.py already accepts a namespace argument
     slam_gmapping_cmd = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(

map_merge/launch/tb3_simulation/multi_tb3_simulation_launch.py

@@ -23,7 +23,6 @@ The robots co-exist on a shared environment and are controlled by independent na
 import os
 
 from ament_index_python.packages import get_package_share_directory
-
 from launch import LaunchDescription, condition
 from launch.actions import (
     DeclareLaunchArgument,
@@ -35,6 +34,7 @@ from launch.actions import (
 from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, TextSubstitution
+from launch_ros.actions import Node
 
 
 def generate_launch_description():
@@ -51,7 +51,7 @@ def generate_launch_description():
         {"name": "robot1", "x_pose": 0.0, "y_pose": 0.5, "z_pose": 0.01},
         {"name": "robot2", "x_pose": -3.0, "y_pose": 1.5, "z_pose": 0.01},
     ]
-    # Names and poses of the robots for unknown poses demo, the must be very close at beggining
+    # Names and poses of the robots for unknown poses demo, the must be very close at beginning
     robots_unknown_poses = [
         {"name": "robot1", "x_pose": -2.0, "y_pose": 0.5, "z_pose": 0.01},
         {"name": "robot2", "x_pose": -3.0, "y_pose": 0.5, "z_pose": 0.01},
@@ -159,39 +159,105 @@ def generate_launch_description():
         output="screen",
     )
 
+    robot_sdf = LaunchConfiguration("robot_sdf")
+    declare_robot_sdf_cmd = DeclareLaunchArgument(
+        "robot_sdf",
+        default_value=os.path.join(bringup_dir, "worlds", "waffle.model"),
+        description="Full path to robot sdf file to spawn the robot in gazebo",
+    )
+
     # Define commands for spawing the robots into Gazebo
     spawn_robots_cmds = []
     for robot_known, robot_unknown in zip(robots_known_poses, robots_unknown_poses):
-        spawn_robots_cmds.append(
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
-                ),
-                launch_arguments={
-                    "x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
-                    "y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
-                    "z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
-                    "robot_name": robot_known["name"],
-                    "turtlebot_type": TextSubstitution(text="waffle"),
-                }.items(),
-                condition=IfCondition(known_init_poses),
+        # after humble release, use spawn_entity.py
+        if os.getenv("ROS_DISTRO") == "humble":
+            spawn_robots_cmds.append(
+                Node(
+                    package="gazebo_ros",
+                    executable="spawn_entity.py",
+                    output="screen",
+                    arguments=[
+                        "-entity",
+                        robot_known["name"],
+                        "-file",
+                        robot_sdf,
+                        "-robot_namespace",
+                        TextSubstitution(text=str(robot_known["name"])),
+                        "-x",
+                        TextSubstitution(text=str(robot_known["x_pose"])),
+                        "-y",
+                        TextSubstitution(text=str(robot_known["y_pose"])),
+                        "-z",
+                        TextSubstitution(text=str(robot_known["z_pose"])),
+                        "-R",
+                        "0.0",
+                        "-P",
+                        "0.0",
+                        "-Y",
+                        "0.0",
+                    ],
+                    condition=IfCondition(known_init_poses),
+                )
             )
-        )
-        spawn_robots_cmds.append(
-            IncludeLaunchDescription(
-                PythonLaunchDescriptionSource(
-                    os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
-                ),
-                launch_arguments={
-                    "x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
-                    "y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
-                    "z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
-                    "robot_name": robot_unknown["name"],
-                    "turtlebot_type": TextSubstitution(text="waffle"),
-                }.items(),
-                condition=UnlessCondition(known_init_poses),
+            spawn_robots_cmds.append(
+                Node(
+                    package="gazebo_ros",
+                    executable="spawn_entity.py",
+                    output="screen",
+                    arguments=[
+                        "-entity",
+                        robot_unknown["name"],
+                        "-file",
+                        robot_sdf,
+                        "-robot_namespace",
+                        TextSubstitution(text=str(robot_unknown["name"])),
+                        "-x",
+                        TextSubstitution(text=str(robot_unknown["x_pose"])),
+                        "-y",
+                        TextSubstitution(text=str(robot_unknown["y_pose"])),
+                        "-z",
+                        TextSubstitution(text=str(robot_unknown["z_pose"])),
+                        "-R",
+                        "0.0",
+                        "-P",
+                        "0.0",
+                        "-Y",
+                        "0.0",
+                    ],
+                    condition=UnlessCondition(known_init_poses),
+                )
+            )
+        else:
+            spawn_robots_cmds.append(
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
+                    ),
+                    launch_arguments={
+                        "x_pose": TextSubstitution(text=str(robot_known["x_pose"])),
+                        "y_pose": TextSubstitution(text=str(robot_known["y_pose"])),
+                        "z_pose": TextSubstitution(text=str(robot_known["z_pose"])),
+                        "robot_name": robot_known["name"],
+                        "turtlebot_type": TextSubstitution(text="waffle"),
+                    }.items(),
+                    condition=IfCondition(known_init_poses),
+                )
+            )
+            spawn_robots_cmds.append(
+                IncludeLaunchDescription(
+                    PythonLaunchDescriptionSource(
+                        os.path.join(bringup_dir, "launch", "spawn_tb3_launch.py")
+                    ),
+                    launch_arguments={
+                        "x_pose": TextSubstitution(text=str(robot_unknown["x_pose"])),
+                        "y_pose": TextSubstitution(text=str(robot_unknown["y_pose"])),
+                        "z_pose": TextSubstitution(text=str(robot_unknown["z_pose"])),
+                        "robot_name": robot_unknown["name"],
+                        "turtlebot_type": TextSubstitution(text="waffle"),
+                    }.items(),
+                    condition=UnlessCondition(known_init_poses),
+                )
             )
-        )
 
     # Define commands for launching the navigation instances
     nav_instances_cmds = []
@@ -280,6 +346,7 @@ def generate_launch_description():
     ld.add_action(declare_slam_toolbox_cmd)
     ld.add_action(declare_slam_gmapping_cmd)
     ld.add_action(declare_known_init_poses_cmd)
+    ld.add_action(declare_robot_sdf_cmd)
 
     # Add the actions to start gazebo, robots and simulations
     ld.add_action(start_gazebo_cmd)