Add WorldObjectService, SpotCheckService, unit tests, Docker CI build pipeline

.github/workflows/spot_ros_docker_build.yml

@@ -0,0 +1,38 @@
+name: Docker image upload
+
+on:
+  push:
+    branches:
+      - main
+      - master
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    defaults:
+      run:
+        shell: bash
+    steps:
+      - name: Checkout
+        uses: actions/checkout@v3
+      - name: Login to Docker Hub
+        uses: docker/login-action@v2
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      - name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v2
+      - name: Extract branch name
+        run: echo "branch=$(echo ${GITHUB_REF##*/})" >> $GITHUB_OUTPUT
+        id: extract_branch
+      - name: Print branch name for Docker Hub
+        run: echo "${{ steps.extract_branch.outputs.branch }}"
+      - name: Build and push
+        uses: docker/build-push-action@v4
+        with:
+          context: .
+          file: ./deploy/Dockerfile
+          push: true
+          tags: ${{ secrets.DOCKERHUB_USERNAME }}/spot_ros:${{ steps.extract_branch.outputs.branch }}
+          secrets: |
+            "SPOTROS_GIT_BRANCH=${{ github.ref_name }}"

.github/workflows/spot_ros_unit_tests.yml

@@ -0,0 +1,79 @@
+name: spot_driver unit tests
+
+# Run on every push
+on: [push]
+
+# Environmental variables
+env:
+  SPOT_ARM: 1
+
+jobs:
+  spot_driver-unit-tests:
+    # All steps inside this job will run inside the ROS Noetic container
+    container: public.ecr.aws/docker/library/ros:noetic
+    runs-on: ubuntu-latest
+    # Let's ensure we use bash, and work within our catkin worksapce
+    defaults:
+      run:
+        shell: bash
+    steps:
+      # Create directory to check out the code to
+      - name: Create catkin_ws directory
+        run: |
+          mkdir -p $GITHUB_WORKSPACE/catkin_ws/src
+
+      # Check out the Spot ROS code
+      - name: Checkout SpotROS
+        uses: actions/checkout@v3
+        with:
+          path: catkin_ws/src/spot_ros
+
+      - name: Checkout Spot Wrapper
+        uses: actions/checkout@v3
+        with:
+          repository: jeremysee2/spot_wrapper #TODO: Change to bdaiinstitute once merged
+          path: catkin_ws/src/spot_ros/spot_wrapper
+
+      # Install the required packages
+      - name: Install dependencies (minus qttools)
+        run: |
+          sudo apt-get update && \
+          sudo apt-get install -y \
+            python3-pip \
+            ros-noetic-tf2-bullet
+          pip install cython \
+            bosdyn-client \
+            bosdyn-mission \
+            bosdyn-api \
+            bosdyn-core \
+            empy
+        working-directory: catkin_ws/
+
+      # Install SpotWrapper
+      - name: Install SpotWrapper
+        run: |
+          cd $GITHUB_WORKSPACE/catkin_ws/src/spot_ros
+          pip install -r spot_wrapper/requirements.txt
+          pip install -e spot_wrapper
+
+      - name: Check that code has been downloaded correctly
+        run: |
+          ls $GITHUB_WORKSPACE/catkin_ws/src
+          ls $GITHUB_WORKSPACE/catkin_ws/src/spot_ros
+
+      # Build our workspace
+      - name: Build workspace for spot_driver
+        run: |
+          source /opt/ros/noetic/setup.bash
+          rosdep update
+          rosdep install --from-paths src --ignore-src -y
+          ls $GITHUB_WORKSPACE/catkin_ws
+          catkin_make --only-pkg-with-deps spot_driver
+        working-directory: catkin_ws/
+
+      # Run our ROS test!
+      - name: Run tests
+        run: |
+          source devel/setup.bash &&
+          rostest spot_driver run_tests.test --text
+        working-directory: catkin_ws/

.gitignore

@@ -1 +1,2 @@
 *.pyc
+*.DS_Store

deploy/Dockerfile

@@ -0,0 +1,53 @@
+FROM ros:noetic
+
+RUN DEBIAN_FRONTEND=noninteractive apt update
+RUN DEBIAN_FRONTEND=noninteractive apt install -y git
+
+# Download the Spot ROS packages
+RUN mkdir -p /ros/catkin_ws/src
+WORKDIR /ros/catkin_ws/src
+# Get the secrets from Github Actions
+RUN --mount=type=secret,id=SPOTROS_GIT_BRANCH \
+    export SPOTROS_GIT_BRANCH=$(cat /run/secrets/SPOTROS_GIT_BRANCH) && \
+    echo $SPOTROS_GIT_BRANCH && \
+    git clone -b $SPOTROS_GIT_BRANCH https://github.com/jeremysee2/spot_ros.git /ros/catkin_ws/src/spot_ros
+
+RUN DEBIAN_FRONTEND=noninteractive apt install -y python3-pip git qttools5-dev \
+    ros-noetic-tf2-bullet ros-noetic-roslint ros-noetic-pcl-ros ros-noetic-catkin \
+    python3-catkin-pkg nano vim ufw net-tools
+RUN python3 -m pip install cython bosdyn-client bosdyn-mission bosdyn-api bosdyn-core empy numpy
+RUN chmod +x /opt/ros/noetic/setup.sh
+RUN /opt/ros/noetic/setup.sh
+
+# Git clone Spot Wrapper
+WORKDIR /ros/catkin_ws/src/spot_ros
+RUN git clone https://github.com/jeremysee2/spot_wrapper.git /ros/catkin_ws/src/spot_ros/spot_wrapper
+RUN pip install -r /ros/catkin_ws/src/spot_ros/spot_wrapper/requirements.txt
+RUN pip install -e /ros/catkin_ws/src/spot_ros/spot_wrapper
+
+# Install ROS dependencies
+WORKDIR /ros/catkin_ws/
+
+RUN rosdep install --from-paths /ros/catkin_ws/src --ignore-src -y
+
+# Build ROS packages, remember to source the setup.bash file in the same command
+WORKDIR /ros/catkin_ws/
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
+    catkin_make --only-pkg-with-deps spot_driver"
+RUN /bin/bash -c "source /opt/ros/noetic/setup.bash && \
+    catkin_make"
+
+# Source the new packages
+RUN chmod +x /ros/catkin_ws/devel/setup.sh && /ros/catkin_ws/devel/setup.sh
+
+# Setup the environmental variables for including these in the URDF
+ARG SPOT_ARM=1
+ARG SPOT_PACK=1
+ARG SPOT_LIDAR_MOUNT=1
+ARG SPOT_VELODYNE=1
+ARG SPOT_USERNAME=admin
+ARG SPOT_PASSWORD=dummypassword
+
+ARG ROS_MASTER_URI=http://localhost:21311
+
+ENTRYPOINT /bin/bash -c "/ros/catkin_ws/src/spot_ros/deploy/entrypoint.sh"

deploy/entrypoint.sh

@@ -0,0 +1,3 @@
+#!/bin/bash
+source /ros/catkin_ws/devel/setup.sh && \
+    roslaunch spot_driver driver.launch username:=admin password:=$SPOT_PASSWORD

spot_driver/CMakeLists.txt

@@ -15,9 +15,13 @@ include_directories(
 
 catkin_install_python(PROGRAMS
     scripts/spot_ros
+    test/ros_helpers_test.py
+    test/spot_ros_test.py
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY doc DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
 install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+catkin_add_nosetests(test/run_tests.py)

spot_driver/config/spot_ros.yaml

@@ -6,21 +6,22 @@ rates:
   robot_state: 20.0
   metrics: 0.04
   lease: 1.0
-  front_image: 10.0
-  side_image: 10.0
-  rear_image: 10.0
+  camera_images: 10.0
   point_cloud: 10.0
   hand_image: 10.0
   feedback: 10.0
-  mobility_params: 10.0
+  mobility_params: 20.0
   check_subscribers: 10.0
+  world_objects: 10.0
 
 # If true, automatically claim the lease when the driver starts
 auto_claim: False
 # If true, automatically power on when the driver starts (requires auto_claim)
 auto_power_on: False
 # If true, automatically stand when the driver starts (requires auto_power_on)
 auto_stand: False
+depth_in_visual: True
+mode_parent_odom_tf: vision
 
 claim:
   # If true, forcefully take the lease, instead of acquiring it, which can fail if someone else has it

spot_driver/launch/driver.launch

@@ -1,7 +1,7 @@
 <launch>
   <arg name="username" default="dummyusername" />
   <arg name="password" default="dummypassword" />
-  <arg name="hostname" default="192.168.50.3" />
+  <arg name="hostname" default="192.168.80.3" />
   <arg name="estop_timeout" default="9.0"/>
   <arg name="autonomy_enabled" default="true"/>
   <!-- In m/s. 0 applies spot's internal limits -->
@@ -35,5 +35,5 @@
     <rosparam command="load" file="$(find spot_driver)/config/twist_mux.yaml" />
     <remap from="cmd_vel_out" to="spot/cmd_vel"/>
   </node>
-
+  
 </launch>

spot_driver/package.xml

@@ -23,6 +23,9 @@
   <exec_depend>velodyne_pointcloud</exec_depend>
   <exec_depend>python-transforms3d-pip</exec_depend>
 
+  <test_depend>rostest</test_depend>
+  <test_depend>rosunit</test_depend>
+
   <export>
     <rosdoc config="rosdoc.yaml" />
   </export>

spot_driver/setup.py

@@ -2,7 +2,13 @@
 from catkin_pkg.python_setup import generate_distutils_setup
 
 d = generate_distutils_setup(
-    packages=["spot_driver"], scripts=["scripts/spot_ros"], package_dir={"": "src"}
+    packages=["spot_driver"],
+    scripts=[
+        "scripts/spot_ros",
+        "test/ros_helpers_test.py",
+        "test/spot_ros_test.py",
+    ],
+    package_dir={"": "src"},
 )
 
 setup(**d)