Merge pull request #110 from StarlingUAS/pr-ap-camera

Gazebo Ros2 Ardupilot Camera Support

docs/docker-images/sim-ardupilot-gazebo.md

@@ -3,3 +3,34 @@
 Based on [`starling-sim-base-core`](../sim-base-core)
 
 This image contains the base Gazebo installation and adds the ArduPilot specific plugin.
+
+## Environment Variables
+
+Name                  | Default Value                | Description
+----------------------|------------------------------|------------
+`AP_SYSID`            | 1                            | MAVLink system ID to be used by the SITL. Can also be set to `"ordinal"` or `"ip"`
+`AP_SYSID_BASE`       | 1                            | Base system ID for ordinal-based generation
+`AP_SITL_ADDRESS`     | 127.0.0.1                    | IP address for Gazebo plugin to use to talk to ArduPilot instance
+`AP_SITL_HOST`        | {null}                       | Hostname for Gazebo plugin to use to talk to ArduPilot instance. Set to __override__ IP address.
+`VEHICLE_NAMESPACE`   | `vehicle_${AP_SYSID}`        | ROS2 Namespace in which to place vehicle sensor topics (e.g. gimbal_cmd)
+
+### `AP_SYSID`
+
+Sets the MAVLink system ID to be used by the SITL. This must either be a value between 1 and 255 inclusive or set to
+`"ordinal"` or `"ip"`. Other values will cause the container to exit.
+
+When set to `"ordinal`, the entrypoint script will attempt to retrieve a 0-indexed ordinal from the end of the
+container's hostname. The hostnames should be of the form `${HOSTNAME}-${ORDINAL}`, *e.g.* `sitl-0`, `sitl-1`, `sitl-3`.
+This behaviour supports deployment of this image as part of a Kubernetes StatefulSet.
+
+When set to `"ip"`, the entrypoint script retrieves the last octet of the container's IP address and uses that as the
+system ID. *e.g.* `172.18.0.4` will result in a system ID of `4`.
+
+At present, the system ID is set by appending it to the parameter file. If a custom parameter file is supplied, it
+should not contain the `SYSID_THISMAV` parameter.
+
+### `AP_SYSID_BASE`
+
+This variable only affects the container when using the ordinal-based generation outlined above. The value set here will
+be added to the ordinal from the hostname to derive the MAVLink system ID. If the resultant value is not a valid system
+ID, *i.e.* it is not between 1 and 255 inclusive, the container will exit.

docs/docker-images/sim-base-core.md

@@ -10,4 +10,10 @@ This image acts as a base to build other simulation environments on.
 To ease the expansion of this image, a mechanism has been built to enable expansion by volume mount. As part of the
 entrypoint, the image will look for any folders in `/ros.env.d` that contain a `setup.bash` file. Any such file will be
 sourced as part of the entrypoint. This allows for arbitrary expansion of the entrypoint by adding additional volume
-mounts.
+mounts.
+
+## Environment Variables
+
+Name                  | Default Value                | Description
+----------------------|------------------------------|------------
+`ENABLE_VIRTUAL_FRAMEBUFFER` | true | Enables the `setup_display.sh` script and starts a virtual X server. Use for simulated vehicles which have cameras on them. 

docs/docker-images/sim-iris-ap.md

@@ -1,14 +1,29 @@
 # `starling-sim-iris-ap`
 
 Based on [`staring-sim-ardupilot-gazebo`](../sim-ardupilot-gazebo)
+See for additional environment variables.
 
 ## Overview
 
 This image adds the launch files required to launch an Iris model with the ArduPilot plugin using Gazebo.
 
+The [iris ardupilot model](https://github.com/khancyr/ardupilot_gazebo/tree/master/models/iris_with_ardupilot) depends on the iris with standoffs and a *gimbal_small_2d*. 
+
+The original gimbal in the gazebo model library is out of date, therefore an updated one is included in this image. It also includes an updated plugin which allows control of the gimbal and streams the camera image. 
 ## Environment Variables
 
 Name                  | Default Value                | Description
 ----------------------|------------------------------|------------
-`AP_SITL_ADDRESS`     | 127.0.0.1                    | IP address for Gazebo plugin to use to talk to ArduPilot instance
-`AP_SITL_HOST`        | {null}                       | Hostname for Gazebo plugin to use to talk to ArduPilot instance. Set to __override__ IP address.
+`CAMERA_NAME`         | camera                       | ROS2 Name of the camera, camera topic is `$VEHICLE_NAMESPACE/$CAMERA_NAME/image_raw`. 
+`CAMERA_HEIGHT`       | 480                          | Height resolution of camera image
+`CAMERA_WIDTH`        | 640                          | Width resolution of camera image
+`GIMBAL_INITIAL_ANGLE`| 0.785                        | Initial angle (radians) of the gimbal. 0.0 Angle is forwards, pi/2 is down. 
+
+## Exposed Topics
+
+Name                  | Topic                | Description
+----------------------|------------------------------|------------
+`$VEHICLE_NAMESPACE/$CAMERA_NAME/image_raw` | `sensor_msgs/msg/Image` | Image topic from the camera attached to the gimbal
+`$VEHICLE_NAMESPACE/$CAMERA_NAME/camera_info` | `sensor_msgs/msg/CameraInfo` | Camera Info topic from the camera attached to the gimbal
+`$VEHICLE_NAMESPACE/gimbal_tilt_cmd` | `std_msgs/msg/Float32` | The target angle (radians) of the gimbal camera tilt [0.0, 3.14]. 
+`$VEHICLE_NAMESPACE/gimbal_tilt_status` | `std_msgs/msg/Float32` | The current angle (radians) of the gimbal camera tilt [0.0, 3.14]. 

simulator/base/ardupilot/Dockerfile

@@ -16,6 +16,12 @@ RUN git clone https://github.com/khancyr/ardupilot_gazebo \
     && make install
 
 # Setup a ros.env.d folder for AP to add to the resource paths for Gazebo
-RUN mkdir -p /ros.env.d/ardupilot \
-    && echo 'export GAZEBO_MODEL_PATH=/ardupilot_gazebo/models:${GAZEBO_MODEL_PATH}' >> /ros.env.d/ardupilot/setup.bash \
-    && echo 'export GAZEBO_RESOURCE_PATH=/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> /ros.env.d/ardupilot/setup.bash
+RUN mkdir -p /ros.env.d/00_ardupilot \
+    && echo 'export GAZEBO_MODEL_PATH=/ardupilot_gazebo/models:${GAZEBO_MODEL_PATH}' >> /ros.env.d/00_ardupilot/setup.bash \
+    && echo 'export GAZEBO_RESOURCE_PATH=/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> /ros.env.d/00_ardupilot/setup.bash
+
+COPY sim_setup.sh /ros.env.d/00_setup/setup.bash
+
+ENV AP_SYSID 1
+ENV AP_SITL_ADDRESS 127.0.0.1
+# AP_SITL_HOST

simulator/base/ardupilot/sim_setup.sh

@@ -0,0 +1,46 @@
+#!/bin/bash
+
+# If AP_SYSID is "ordinal", then set AP_SYSID id to take AP_SYSID_BASE + ORDINAL from StatefulSet hostname
+# Hostname is of the form '<stateful set name>-<ordinal>'
+if [ "$AP_SYSID" == "ordinal" ]; then
+    ORDINAL="${HOSTNAME##*-}"
+    AP_SYSID=$((AP_SYSID_BASE + ORDINAL));
+    if (($AP_SYSID > 0 && $AP_SYSID <= 255 )); then
+        echo "AP_SYSID was 'ordinal' therefore set to $AP_SYSID (from base: $AP_SYSID_BASE and hostname: $HOSTNAME)"
+        export AP_SYSID
+    else
+        echo "AP_SYSID was 'ordinal' but result ($AP_SYSID) was not valid (from base: $AP_SYSID_BASE and hostname: $HOSTNAME)"
+        exit 1
+    fi
+elif [ "$AP_SYSID" == "ip" ]; then
+    AP_SYSID=$(hostname -i | cut -d'.' -f4)
+    echo "AP_SYSID was 'ip' therefore set to $AP_SYSID (from IP: $(hostname -i))"
+    export AP_SYSID
+elif (($AP_SYSID > 0 && $AP_SYSID <= 255 )); then
+    echo "AP_SYSID setting as specified: $AP_SYSID"
+    export AP_SYSID
+else
+    echo "AP_SYSID ($AP_SYSID) is invalid. Must either be set to 'ordinal', 'ip' or number between 1 and 255 inclusive"
+    exit 1
+fi
+echo "AP_SYSID is set to $AP_SYSID"
+
+if [ ! -z "${AP_SITL_HOST}" ]; then
+    # AP_SITL_HOST has been set, use it to set AP_SITL_ADDRESS
+    AP_SITL_ADDRESS="$(getent hosts ${AP_SITL_HOST} | cut -d ' ' -f1)"
+    if [ -z "${AP_SITL_ADDRESS}" ]; then
+        # Address lookup failed
+        echo "Error: Failed to lookup IP address for host '${AP_SITL_HOST}'"
+        exit 1
+    fi
+fi
+
+if [ ! -v $VEHICLE_NAMESPACE ]; then
+    # If set Ensure VEHICLE_NAMESPACE is a valid topic name
+    # Replace all '-' with '_'
+    export VEHICLE_NAMESPACE=${VEHICLE_NAMESPACE//-/_}
+    echo "VEHICLE_NAMESPACE setting to $VEHICLE_NAMESPACE"
+else
+    export VEHICLE_NAMESPACE="vehicle_$AP_SYSID"
+    echo "VEHICLE_NAMESPACE not set, default to auto generated default based on ardupilot sysid target ($AP_SYSID) of $VEHICLE_NAMESPACE"
+fi

simulator/base/core/Dockerfile

@@ -38,11 +38,13 @@ EXPOSE 11345
 # Add gazebo environemnt setup to /ros.env
 RUN echo "source /usr/share/gazebo/setup.sh" >> /ros.env
 
-# Install gazebo_ros and xacro
+# Install gazebo_ros, gazebo ros extra packages (e.g. plugins) and xacro
 RUN apt-get update \
     && apt-get install --no-install-recommends -y \
       ros-foxy-gazebo-ros \
+      ros-foxy-gazebo-ros-pkgs \ 
       ros-foxy-xacro \
+      ros-foxy-image-transport-plugins \
     && rm -rf /var/lib/apt/lists/*
 
 # Install gzweb dependencies
@@ -97,5 +99,9 @@ COPY tools /ros_ws/tools
 COPY fastrtps_profiles.xml /ros_ws/
 ENV FASTRTPS_DEFAULT_PROFILES_FILE /ros_ws/fastrtps_profiles.xml
 
+# Setup any display components for virtual cameras
+COPY setup_display.sh /
+ENV ENABLE_VIRTUAL_FRAMEBUFFER true
+
 # Need to wait until models are installed before running the deploy step
 CMD [ "gzweb_entrypoint.sh" ]

simulator/base/core/gzweb.launch.xml

@@ -1,7 +1,9 @@
 <?xml version="1.0"?>
 <launch>
 
-    <include file="$(find-pkg-share gazebo_ros)/launch/gzserver.launch.py" />
+    <include file="$(find-pkg-share gazebo_ros)/launch/gzserver.launch.py">
+        <arg name="verbose" value="true"/>
+    </include>
 
     <executable cwd="/root/gzweb" cmd="/gzweb_entrypoint.sh" output="screen"/>
 

simulator/base/core/ros_entrypoint.sh

@@ -4,12 +4,17 @@ set -e
 # setup ros2 environment
 source "/opt/ros/$ROS_DISTRO/setup.bash"
 
+# setup display environment
+source /setup_display.sh
+
 # Setup additional environment (e.g. gazebo paths)
 source /ros.env
 
 # Attempt to source additional env files (possibly volume mounted)
-for folder in $(find /ros.env.d -mindepth 1 -maxdepth 1 -type d -name "[!.]*"); do
+ROSENVD_LIST=$(find /ros.env.d -mindepth 1 -maxdepth 1 -type d -name "[!.]*")
+for folder in $(echo $ROSENVD_LIST | xargs -n1 | sort | xargs ); do
     if [ -f ${folder}/setup.bash ]; then
+        echo "---- ros_entrypoint.sh: sourcing ${folder}/setup.bash -----"
         source ${folder}/setup.bash
     fi
 done

simulator/base/core/setup_display.sh

@@ -0,0 +1,15 @@
+set -e 
+echo "--------------- Start setup_display.sh ----------------------"
+
+if [ "$ENABLE_VIRTUAL_FRAMEBUFFER" = true ]; then
+    echo "Creating virtual display using virtual framebuffer"
+    export DISPLAY=:1
+    rm -f /tmp/.X1-lock
+    Xvfb $DISPLAY -screen 0 1600x900x24 &
+
+    echo "Waiting for display to become available"
+    while [ ! -e /tmp/.X11-unix/X1 ]; do echo "Wait for X server.."; sleep 0.1; done
+else
+    echo "Virtual Framebuffer Disabled"
+fi
+echo "--------------- End setup_display.sh ----------------------"

simulator/vehicles/iris-ap/Dockerfile

@@ -6,13 +6,19 @@ FROM ${REGISTRY}uobflightlabstarling/starling-sim-ardupilot-gazebo:${VERSION}
 COPY iris.launch.xml /ros_ws/launch/
 COPY spawn_iris.sh /ros_ws/
 
-ENV AP_SITL_ADDRESS 127.0.0.1
-# AP_SITL_HOST
+# Copy in the xacro & bash file to setup the model
+# Using ros.env.d automatic sourcing on entrypoint (see /simulator/base/core/Dockerfile)
+COPY models/gimbal_small_2d /ros.env.d/02_gimbal_small_2d
+COPY models/iris_ap /ros.env.d/03_iris_ap
 
-# Copy in the xacro & bash file to setup the model with a specific address
-RUN mkdir /ros.env.d/iris_ap
-COPY model.sdf.xacro /ros.env.d/iris_ap/model.sdf.xacro
-COPY iris_setup.sh /ros.env.d/iris_ap/setup.bash
+# Build gimbal plugin with ROS2 on path and add plugin to path
+COPY gimbal_plugin /ros_ws/src/gimbal_plugin
+RUN cd /ros_ws \
+    && . /opt/ros/foxy/setup.sh \
+    && export CMAKE_PREFIX_PATH=$AMENT_PREFIX_PATH:$CMAKE_PREFIX_PATH \
+    && colcon build --cmake-force-configure \
+    && rm -r build \
+    && echo 'export GAZEBO_PLUGIN_PATH=/ros_ws/install/gimbal_plugin/lib:${GAZEBO_PLUGIN_PATH}' >> /ros.env.d/02_gimbal_small_2d/setup.bash
 
 WORKDIR /ros_ws
 

simulator/vehicles/iris-ap/gimbal_plugin/CMakeLists.txt

@@ -0,0 +1,83 @@
+cmake_minimum_required(VERSION 3.5)
+project(gimbal_plugin)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  # we dont use add_compile_options with pedantic in message packages
+  # because the Python C extensions dont comply with it
+  set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic -Wno-inconsistent-missing-override")
+endif()
+
+if(WIN32)
+  add_compile_definitions(
+    # For math constants
+    _USE_MATH_DEFINES
+    # Minimize Windows namespace collision
+    NOMINMAX
+    WIN32_LEAN_AND_MEAN
+  )
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(gazebo_dev REQUIRED)
+find_package(gazebo_msgs REQUIRED)
+find_package(gazebo_ros REQUIRED)
+find_package(geometry_msgs REQUIRED)
+# find_package(nav_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
+# find_package(sensor_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+# find_package(tf2 REQUIRED)
+# find_package(tf2_geometry_msgs REQUIRED)
+# find_package(tf2_ros REQUIRED)
+# find_package(trajectory_msgs REQUIRED)
+
+link_directories(${gazebo_dev_LIBRARY_DIRS})
+
+# Gimbal2dPlugin
+add_library(Gimbal2dPlugin SHARED
+  src/Gimbal2dPlugin.cpp
+)
+target_include_directories(Gimbal2dPlugin PUBLIC include)
+ament_target_dependencies(Gimbal2dPlugin
+  "gazebo_dev"
+  "gazebo_ros"
+  "rclcpp"
+  "geometry_msgs"
+)
+ament_export_libraries(Gimbal2dPlugin)
+
+# ament_export_include_directories(include)
+ament_export_dependencies(rclcpp)
+ament_export_dependencies(gazebo_dev)
+ament_export_dependencies(gazebo_msgs)
+ament_export_dependencies(gazebo_ros)
+
+if(NOT WIN32)
+  if(NOT APPLE)
+    set(
+      AMENT_CMAKE_ENVIRONMENT_HOOKS_DESC_gazebo_plugins
+      "prepend-non-duplicate;LD_LIBRARY_PATH;${GAZEBO_PLUGIN_PATH}")
+  else()
+    set(
+      AMENT_CMAKE_ENVIRONMENT_HOOKS_DESC_gazebo_plugins
+      "prepend-non-duplicate;DYLD_LIBRARY_PATH;${GAZEBO_PLUGIN_PATH}")
+  endif()
+endif()
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/gazebo_plugins.sh.in")
+
+ament_package()
+
+install(DIRECTORY include/
+        DESTINATION include)
+
+install(TARGETS
+    Gimbal2dPlugin
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin)
+

simulator/vehicles/iris-ap/gimbal_plugin/colcon.pkg

@@ -0,0 +1,8 @@
+# Configuration file for colcon (https://colcon.readthedocs.io).
+#
+# Please see the doc for the details of the spec:
+#   - https://colcon.readthedocs.io/en/released/user/configuration.html#colcon-pkg-files
+
+{
+  "dependencies": ["Gazebo"],
+}

simulator/vehicles/iris-ap/gimbal_plugin/env-hooks/gazebo_plugins.sh.in

@@ -0,0 +1,19 @@
+# generated from gazebo_plugins/env-hooks/gazebo_plugins.sh.in
+
+# detect if running on Darwin platform
+_UNAME=`uname -s`
+_IS_DARWIN=0
+if [ "$_UNAME" = "Darwin" ]; then
+  _IS_DARWIN=1
+fi
+unset _UNAME
+
+if [ $_IS_DARWIN -eq 0 ]; then
+  ament_prepend_unique_value LD_LIBRARY_PATH @GAZEBO_PLUGIN_PATH@
+else
+  ament_prepend_unique_value DYLD_LIBRARY_PATH @GAZEBO_PLUGIN_PATH@
+fi
+unset _IS_DARWIN
+
+ament_prepend_unique_value GAZEBO_PLUGIN_PATH "$AMENT_CURRENT_PREFIX/lib"
+