world objects, spot check, unit tests, Docker CI build pipeline, modu…

…lar wrapper (heuristicus#126) Updates spot_ros to use the modularised wrapper code, adds code to access world objects and spot check with the driver, adds some unit tests and a docker build pipeline. co-authored-by: Michal Staniaszek <[email protected]>
GoldenZephyr · Aug 6, 2023 · 5e0b61c · 5e0b61c
1 parent 2dbdc13
commit 5e0b61c
Show file tree

Hide file tree

Showing 98 changed files with 6,460 additions and 493 deletions.
diff --git a/.github/workflows/spot_ros_docker_build.yml b/.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 }}"
diff --git a/.github/workflows/spot_ros_unit_tests.yml b/.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/
diff --git a/.gitignore b/.gitignore
@@ -1 +1,2 @@
 *.pyc
+*.DS_Store
diff --git a/deploy/Dockerfile b/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"
diff --git a/deploy/entrypoint.sh b/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
diff --git a/docs/README.md b/docs/README.md
@@ -1,5 +1,12 @@
-To generate docs, run
+To install dependencies to generate documentation:
 
+```bash
+sudo apt-get install python3-sphinx
+pip install sphinx sphinx_rtd_theme
 ```
+
+To generate docs, run
+
+```bash
 sphinx-build -b html . html
 ```
diff --git a/docs/arm_usage.rst b/docs/arm_usage.rst
@@ -1,29 +1,60 @@
-Arm control
+Arm Control
 ===========
 
-The driver can also send commands to the robot's arm, if it has one. The following services allow control of various
-parts of the arm.
-
-+------------------------+------------------------+
-|Service                 |Description             |
-+========================+========================+
-|/spot/arm_carry         |                        |
-+------------------------+------------------------+
-|/spot/arm_joint_move    |                        |
-+------------------------+------------------------+
-|/spot/arm_stow          |                        |
-+------------------------+------------------------+
-|/spot/arm_unstow        |                        |
-+------------------------+------------------------+
-|/spot/force_trajectory  |                        |
-+------------------------+------------------------+
-|/spot/grasp_3d          |                        |
-+------------------------+------------------------+
-|/spot/gripper_angle_open|                        |
-+------------------------+------------------------+
-|/spot/gripper_close     |                        |
-+------------------------+------------------------+
-|/spot/gripper_open      |                        |
-+------------------------+------------------------+
-|/spot/gripper_pose      |                        |
-+------------------------+------------------------+
+The driver can also send commands to the robot's arm, if it has one. The following services
+allow control of various parts of the arm. Note that the arm does not have collision avoidance,
+so it is important to be careful when using these services.
+
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+|         Service          |                                                                                                                               Description                                                                                                                                |
++==========================+==========================================================================================================================================================================================================================================================================+
+| /spot/arm_carry          | Move the arm into the `carry` position.                                                                                                                                                                                                                                  |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/arm_joint_move     | Move each joint into a position specified by a specific angle, corresponding to the `6 degrees of freedom <https://dev.bostondynamics.com/python/bosdyn-client/src/bosdyn/client/robot_command#bosdyn.client.robot_command.RobotCommandBuilder.arm_joint_move_helper>`_. |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/arm_stow           | Move the arm into the `stow` position.                                                                                                                                                                                                                                   |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/arm_unstow         | Move the arm into the `unstow` position.                                                                                                                                                                                                                                 |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/force_trajectory   | Move the arm by specifying forces in (x,y,z) linear forces and (rx,ry,rz) rotational forces, acting as a virtual wrench.                                                                                                                                                 |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/grasp_3d           | Use the `Pick` autonomy feature of Spot to pick an object specified in Cartesian (x,y,z) coordinates in a chosen reference frame, usually ``body``.                                                                                                                      |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/gripper_angle_open | Open the gripper to a specific angle, between 0 and 90 degrees.                                                                                                                                                                                                          |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/gripper_close      | Close the gripper to 0 degrees.                                                                                                                                                                                                                                          |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/gripper_open       | Open the gripper to 90 degrees.                                                                                                                                                                                                                                          |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+| /spot/gripper_pose       | Move the gripper to a point specified in Cartesian (x,y,z) coordinates of the robot's ``body`` frame.                                                                                                                                                                    |
++--------------------------+--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
+
+Arm Carry, Stow, Unstow
+-----------------------
+
+The arm can be moved into the predefined `carry`, `stow`, and `unstow` positions.
+
+Carry:
+
+.. image:: images/spot-arm-carry.gif
+
+Stow:
+
+.. image:: images/spot-arm-stow.gif
+
+
+Pick Service (Grasp 3D)
+-----------------------
+
+Note that it is important to perform the Spot Check calibration before using the Pick service of the arm.
+If any cameras are in an error state, the Spot robot will refuse to execute any Autonomy features, including picking.
+The Picking process is shown below:
+
+1. Walking to the object
+2. Unstowing the arm
+3. Surveying the object with the gripper camera
+4. Walking closer to the object
+5. Picking the object
+6. Closing the gripper
+
+.. image:: images/spot-grasp-3d.gif
diff --git a/docs/docker_deploy.rst b/docs/docker_deploy.rst
@@ -0,0 +1,25 @@
+Docker Deployment
+=================
+
+It is recommended to deploy the ROS driver in a Docker container to the Spot CORE, or any computer carried by Spot.
+The Spot CORE utilises Portainer to handle Docker containers, with a deployment guide `here <https://dev.bostondynamics.com/docs/payload/spot_core_portainer>`_.
+
+The Dockerfile for the ROS driver is located in the ``deploy`` directory of the repository, building on the ROS
+Noetic image. You can modify this to build your own image, or use the pre-built image from Docker Hub.
+
+Deployment Steps
+----------------
+
+1. ``docker pull`` the image from Docker Hub
+2. ``docker save`` the image to a tar file
+3. Transfer the tar file to the Spot CORE, or any computer carried by Spot
+4. Create a container from the image, using the ``docker run`` command or the Portainer interface
+
+Portainer
+---------
+
+Portainer is a web interface for managing Docker containers. It is installed on the Spot CORE by default, and can be accessed
+by navigating to ``192.168.80.3:21900`` in a web browser. Portainer has a `guide <https://docs.portainer.io/user/docker/dashboard>`_
+on how to manage images and containers through its interface.
+
+.. image:: images/spot-core-portainer.png
diff --git a/docs/eap_usage.rst b/docs/eap_usage.rst
@@ -0,0 +1,52 @@
+Spot EAP Usage
+==========================================
+
+The `Spot EAP package <https://support.bostondynamics.com/s/article/Spot-Enhanced-Autonomy-Package-EAP>`_ is a payload that
+includes a Velodyne VLP-16 lidar. This allows it to generate point clouds of the surrounding environment. This ROS driver
+allows you to retrieve the lidar data from the Spot SDK and publish it to ROS in the
+`PointCloud2 <http://docs.ros.org/en/melodic/api/sensor_msgs/html/msg/PointCloud2.html>`_ format.
+
+Hardware Setup
+----------------
+
+Follow `Boston Dynamics' instructions <https://dev.bostondynamics.com/docs/payload/readme>`_ for installing the Spot EAP package.
+You will need to use one of the payload ports on the top of the robot (usually the rear one) to connect the lidar to the robot,
+and register the payload with the controller or from the admin console web interface of the robot at ``192.168.80.3``.
+
+Accessing the Point Cloud data
+--------------------------------
+
+Once the spot_driver has been started, you can access the point cloud data by subscribing to the ``/spot/lidar/points`` topic.
+The point cloud data publishing rate can be configured using the ``spot_ros.yaml`` file in ``spot_driver`` package. The default
+publishing rate is 10 Hz. This can be verified by running the following command.
+
+.. code-block:: bash
+
+    rostopic echo /spot/lidar/points
+    rostopic hz /spot/lidar/points
+
+The data can also be viewed in RViz. It is published in the ``sensor_msgs/PointCloud2`` format, in the ``odom`` frame of the robot.
+An example of it in RViz is shown below, where the white dots represent points output from the lidar.
+
+.. image:: images/lidar_on_spot.png
+
+Note that this data has been filtered by the Spot SDK for localization usage, hence it is provided in the ``odom`` frame.
+However, if you are using the point cloud data for other purposes, you may want to use the ``tf2`` package to transform it
+into the ``base_link`` frame instead, using the ``tf2_sensor_msgs.do_transform_cloud()`` function.
+
+Directly accessing the raw data
+-----------------------------------
+
+The data provided through the ``/spot/lidar/points`` topic is filtered by the Spot SDK for localization usage. If you want to
+access the raw data, you can use a separate ROS package such as `velodyne_driver <http://wiki.ros.org/velodyne_driver>`_ to
+access the data directly from the lidar.
+
+Note that Boston Dynamics has claimed that this may `interfere <https://support.bostondynamics.com/s/article/Spot-Enhanced-Autonomy-Package-EAP>`_
+with the Autowalk feature of Spot. The officially listed limitations of the EAP are:
+
+::
+    
+    - Changing networking settings or otherwise interfering with the ``velodyne_service`` program on the EAP's Core will cause GraphNav not to function. 
+    - Although point cloud data is published via the ``velodyne_service`` ``RemotePointCloud`` service, this data is heavily filtered for use with GraphNav. 
+    - Users are free to access the VLP-16 sensor directly using its communication protocols; however, this likely interferes with its function in the ``velodyne_service`` which breaks Autowalk.
+    - The EAP is not intended for reality capture applications as it currently designed. We recommend working with laser scanner companies such as Trimble for laser scanning and/or digital twin applications.