Merge branch 'main' into khughes/odom-velocity-frame-fix

.devcontainer/Dockerfile

@@ -22,10 +22,11 @@ RUN DEBIAN_FRONTEND=noninteractive apt-get update -q && \
     python3-rosdep \
     libpython3-dev \
     python3-vcstool \
-    ros-humble-tl-expected && \
+    ros-${ROS_DISTRO}-ros2-control \
+    ros-${ROS_DISTRO}-ros2-controllers \
+    ros-${ROS_DISTRO}-tl-expected && \
     rm -rf /var/lib/apt/lists/*
 
-# I added this
 RUN source "/opt/ros/${ROS_DISTRO}/setup.bash"
 
 # Install bosdyn_msgs package

.github/workflows/ci.yml

@@ -108,7 +108,7 @@ jobs:
       - name: Build packages
         run: |
           source /opt/ros/$ROS_DISTRO/setup.bash
-          colcon build --symlink-install --packages-up-to spot_driver spot_description spot_msgs spot_examples --cmake-args -DCMAKE_CXX_FLAGS="--coverage"
+          colcon build --symlink-install --packages-up-to spot_driver spot_description spot_msgs spot_examples spot_ros2_control --cmake-args -DCMAKE_CXX_FLAGS="--coverage"
         working-directory: ${{ github.workspace }}/../../
 
       - name: Test non-spot-driver packages

Dockerfile

@@ -0,0 +1,31 @@
+FROM osrf/ros:humble-desktop-full
+
+# Install dependencies (taken from devcontainer dockerfile)
+RUN DEBIAN_FRONTEND=noninteractive apt-get update -q && \
+    apt-get update -q && \
+    apt-get install -yq --no-install-recommends \
+    curl \
+    wget \
+    python3-pip \
+    python-is-python3 \
+    python3-argcomplete \
+    python3-colcon-common-extensions \
+    python3-colcon-mixin \
+    python3-rosdep \
+    libpython3-dev && \
+    rm -rf /var/lib/apt/lists/*
+
+# Create ROS workspace
+WORKDIR /ros_ws/src
+
+# Clone driver code
+RUN git clone https://github.com/bdaiinstitute/spot_ros2.git .
+RUN git submodule update --init --recursive
+
+# Run install script
+RUN /ros_ws/src/install_spot_ros2.sh
+
+# Build packages with Colcon
+WORKDIR /ros_ws/
+RUN . /opt/ros/humble/setup.sh && \
+    colcon build --symlink-install --packages-ignore proto2ros proto2ros_tests

README.md

@@ -57,6 +57,22 @@ source install/local_setup.bash
 
 We suggest ignoring the `proto2ros_tests` package in the build as it is not necessary for running the driver. If you choose to build it, you will see a number of error messages from testing the failure paths. 
 
+### Alternative - Docker Image
+
+Alternatively, a Dockerfile is available that prepares a ready-to-run ROS2 Humble install with the Spot driver installed.
+
+No special precautions or actions need to be taken to build the image. Just clone the repository and run `docker build` in the root of the repo to build the image.
+
+No special actions need to be taken to run the image either. However, depending on what you intend to _do_ with the driver in your project, the following flags may be useful:
+
+| Flag     | Use             |
+| -------- | --------------- |
+| `--runtime nvidia` + `--gpus all`  | Use the [NVIDIA Container Runtime](https://developer.nvidia.com/container-runtime) to run the container with GPU acceleration |
+| `-e DISPLAY`  | Bind your display to the container in order to run GUI apps. Note that you will need to allow the Docker container to connect to your X11 server, which can be done in a number of ways ranging from disabling X11 authentication entirely, or by allowing the Docker daemon specifically to access your display server.  |
+| `--network host` | Use the host network directly. May help resolve issues connecting to Spot Wifi |
+
+The image does not have the `proto2ros_tests` package built. You'll need to build it yourself inside the container if you need to use it.
+
 # Spot ROS 2 Driver
 
 The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2. To launch the driver, run:
@@ -107,6 +123,94 @@ If your image publishing rate is very slow, you can try
 > export=FASTRTPS_DEFAULT_PROFILES_FILE=<path_to_file>/custom_dds_profile.xml
 > ```
 
+## Optional Automatic Eye-in-Hand Stereo Calibration Routine for Manipulator (Arm) Payload
+#### Collect Calibration
+An optional custom Automatic Eye-in-Hand Stereo Calibration Routine for the arm is available for use in the ```spot_wrapper``` submodule, where the
+output results can be used with ROS 2 for improved Depth to RGB correspondence for the hand cameras.
+See the readme at [```/spot_wrapper/spot_wrapper/calibration/README.md```](https://github.com/bdaiinstitute/spot_wrapper/tree/main/spot_wrapper/calibration/README.md) for 
+target setup and relevant information.
+
+First, collect a calibration with ```spot_wrapper/spot_wrapper/calibrate_spot_hand_camera_cli.py```.
+Make sure to use the default ```--stereo_pairs``` configuration, and the default tag configuration (```--tag default```).
+
+For the robot and target setup described in [```/spot_wrapper/spot_wrapper/calibration/README.md```](https://github.com/bdaiinstitute/spot_wrapper/tree/main/spot_wrapper/calibration/README.md), the default viewpoint ranges should suffice.
+
+```
+python3 spot_wrapper/spot_wrapper/calibrate_spot_hand_camera_cli.py --ip <IP> -u user -pw <SECRET> --data_path ~/my_collection/ \
+--save_data --result_path ~/my_collection/calibrated.yaml --photo_utilization_ratio 1 --stereo_pairs "[(1,0)]" \
+--spot_rgb_photo_width=640 --spot_rgb_photo_height=480 --tag default --legacy_charuco_pattern True
+```
+
+Then, you can run a publisher to transform the depth image into the rgb images frame with the same image
+dimensions, so that finding the 3D location of a feature found in rgb can be as easy as passing
+the image feature pixel coordinates to the registered depth image, and extracting the 3D location.
+For all possible command line arguments, run ```ros2 run spot_driver calibated_reregistered_hand_camera_depth_publisher.py -h```
+
+#### Run the Calibrated Re-Publisher
+```
+ros2 run spot_driver calibrated_reregistered_hand_camera_depth_publisher.py --tag=default --calibration_path <SAVED_CAL> --robot_name <ROBOT_NAMESPACE> --topic_name /depth_registered/hand_custom_cal/image
+```
+
+You can treat the reregistered topic, (in the above example, ```<ROBOT_NAME>/depth_registered/hand_custom_cal/image```)
+as a drop in replacement by the registered image published by the default spot driver
+(```<ROBOT_NAME>/depth_registered/hand/image```). The registered depth can be easily used in tools 
+like downstream, like Open3d, (see [creating RGBD Images](https://www.open3d.org/docs/release/python_api/open3d.geometry.RGBDImage.html) and [creating color point clouds from RGBD Images](https://www.open3d.org/docs/release/python_api/open3d.geometry.PointCloud.html#open3d.geometry.PointCloud.create_from_rgbd_image)), due to matching image dimensions and registration
+to a shared frame.
+
+#### Comparing Calibration Results Quick Sanity Check
+You can compare the new calibration to the old calibration through comparing visualizing 
+the colored point cloud from a bag in RViz. See RViz setup below the bagging instructions.
+
+
+First, collect a bag where there is a an object of a clearly different color in the foreground then
+that of the background.
+
+```
+ROBOT_NAME=<ROBOT_NAME> && \ 
+ros2 bag record --output drop_in_test --topics /tf /tf_static \
+/${ROBOT_NAME}/depth/hand/image /${ROBOT_NAME}/camera/hand/camera_info \
+/${ROBOT_NAME}/joint_states /${ROBOT_NAME}/camera/hand/image \
+/${ROBOT_NAME}/depth_registered/hand/image 
+```
+
+To see what the default calibration looks like:
+```
+# In seperate terminals
+
+ros2 bag play drop_in_test --loop
+ROBOT_NAME=<ROBOT_NAME> && \
+ros2 launch spot_driver point_cloud_xyzrgb.launch.py spot_name:=${ROBOT_NAME} camera:=hand
+```
+
+To see what the new calibration looks like:
+```
+# In seperate terminals
+ROBOT_NAME=<ROBOT_NAME> && \
+ros2 bag play drop_in_test --loop --topics /${ROBOT_NAME}/depth/hand/image \
+/${ROBOT_NAME}/camera/hand/camera_info /${ROBOT_NAME}/joint_states \
+/${ROBOT_NAME}/camera/hand/image /tf /tf_static
+
+ROBOT_NAME=<ROBOT_NAME> && \
+CALIBRATION_PATH=<CALIBRATION_PATH> && \
+ros2 run spot_driver calibrated_reregistered_hand_camera_depth_publisher.py --robot_name ${ROBOT_NAME} \
+--calibration_path ${CALIBRATION_PATH} --topic depth_registered/hand/image
+
+ROBOT_NAME=<ROBOT_NAME> && \
+ros2 launch spot_driver point_cloud_xyzrgb.launch.py spot_name:=${ROBOT_NAME} camera:=hand
+```
+
+#### RVIZ Setup for Sanity Check:
+Set global frame to be ```/<ROBOT_NAME>/hand```
+
+Add (bottom left) -> by topic ->
+```/<ROBOT_NAME>/depth_registered/hand/points``` -> ok
+
+On the left pane, expand the PointCloud2 message. Expand Topic. Set History
+Policy to be Keep Last, Reliability Policy to be Best Effort, and Durability policy to be
+Volatile (select these from the dropdowns).
+
+
+
 ## Spot CAM
 Due to known issues with the Spot CAM, it is disabled by default. To enable publishing and usage over the driver, add the following command in your configuration YAML file:
     `initialize_spot_cam: True`

install_spot_ros2.sh

@@ -25,7 +25,7 @@ sudo apt-get update
 
 # Install ROS dependencies
 # TODO(jschornak-bdai): use rosdep to install these packages by parsing dependencies listed in package.xml
-sudo apt install -y ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-tf-transformations ros-$ROS_DISTRO-xacro ros-$ROS_DISTRO-depth-image-proc ros-$ROS_DISTRO-tl-expected
+sudo apt install -y ros-$ROS_DISTRO-joint-state-publisher-gui ros-$ROS_DISTRO-tf-transformations ros-$ROS_DISTRO-xacro ros-$ROS_DISTRO-depth-image-proc ros-$ROS_DISTRO-tl-expected ros-$ROS_DISTRO-ros2-control ros-$ROS_DISTRO-ros2-controllers
 # Install the dist-utils
 sudo apt-get install -y python3-distutils
 sudo apt-get install -y python3-apt

spot_description/launch/description.launch.py

@@ -16,15 +16,29 @@ def generate_launch_description() -> launch.LaunchDescription:
     return launch.LaunchDescription(
         [
             DeclareLaunchArgument(
-                name="gui", default_value="True", description="Flag to enable joint_state_publisher_gui"
+                name="gui",
+                default_value="True",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable joint_state_publisher_gui",
             ),
             DeclareLaunchArgument(
                 name="model", default_value=default_model_path, description="Absolute path to robot urdf file"
             ),
             DeclareLaunchArgument(
                 name="rvizconfig", default_value=default_rviz2_path, description="Absolute path to rviz config file"
             ),
-            DeclareLaunchArgument(name="arm", default_value="False", description="Flag to enable arm"),
+            DeclareLaunchArgument(
+                name="arm",
+                default_value="False",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable arm",
+            ),
+            DeclareLaunchArgument(
+                name="feet",
+                default_value="False",
+                choices=["True", "true", "False", "false"],
+                description="Flag to enable putting frames at the feet",
+            ),
             DeclareLaunchArgument(
                 "tf_prefix", default_value='""', description="Apply namespace prefix to robot links and joints"
             ),
@@ -40,6 +54,8 @@ def generate_launch_description() -> launch.LaunchDescription:
                                 LaunchConfiguration("model"),
                                 " arm:=",
                                 LaunchConfiguration("arm"),
+                                " feet:=",
+                                LaunchConfiguration("feet"),
                                 " tf_prefix:=",
                                 LaunchConfiguration("tf_prefix"),
                             ]

spot_description/urdf/spot.urdf.xacro

@@ -5,11 +5,13 @@
 
   <!-- Parameters -->
   <xacro:arg name="arm" default="false" />
+  <xacro:arg name="feet" default="false" />
   <xacro:arg name="tf_prefix" default="" />
 
   <!-- Load Spot -->
   <xacro:load_spot
     arm="$(arg arm)"
+    feet="$(arg feet)"
     tf_prefix="$(arg tf_prefix)" />
 
 </robot>

spot_description/urdf/spot_arm_macro.urdf

@@ -41,7 +41,7 @@
       <axis xyz="0 0 1" />
       <parent link="${tf_prefix}body" />
       <child link="${tf_prefix}arm_link_sh0" />
-      <limit effort="1000" velocity="1000.00" lower="-2.61799387799149441136"
+      <limit effort="90.9" velocity="10.0" lower="-2.61799387799149441136"
         upper="3.14159265358979311599" />
     </joint>
 
@@ -68,9 +68,7 @@
       <axis xyz="0 1 0" />
       <parent link="${tf_prefix}arm_link_sh0" />
       <child link="${tf_prefix}arm_link_sh1" />
-      <!-- <limit effort="1000" velocity="1000.00" lower="-0.52359877559829881565"
-      upper="3.14159265358979311599"/> -->
-      <limit effort="1000" velocity="1000.00" lower="-3.14159265358979311599"
+      <limit effort="181.8" velocity="10.0" lower="-3.14159265358979311599"
         upper="0.52359877559829881565" />
     </joint>
 
@@ -99,10 +97,8 @@
     </link>
     <joint name="${tf_prefix}arm_hr0" type="fixed">
       <origin xyz="0.0 0.0 0.0" rpy="0 0 0" />
-      <axis xyz="1 0 0" />
       <parent link="${tf_prefix}arm_link_sh1" />
       <child link="${tf_prefix}arm_link_hr0" />
-      <limit effort="1000" velocity="1000.00" lower="-1e6" upper="1e6" />
     </joint>
 
     <link name="${tf_prefix}arm_link_el0">
@@ -133,7 +129,7 @@
       <axis xyz="0 1 0" />
       <parent link="${tf_prefix}arm_link_hr0" />
       <child link="${tf_prefix}arm_link_el0" />
-      <limit effort="1000" velocity="1000.00" lower="0" upper="3.14159265358979311599" />
+      <limit effort="90.9" velocity="10.0" lower="0" upper="3.14159265358979311599" />
     </joint>
 
     <link name="${tf_prefix}arm_link_el1">
@@ -169,7 +165,7 @@
       <axis xyz="1 0 0" />
       <parent link="${tf_prefix}arm_link_el0" />
       <child link="${tf_prefix}arm_link_el1" />
-      <limit effort="1000" velocity="1000.00" lower="-2.79252680319092716487"
+      <limit effort="30.3" velocity="10.0" lower="-2.79252680319092716487"
         upper="2.79252680319092716487" />
     </joint>
 
@@ -201,7 +197,7 @@
       <axis xyz="0 1 0" />
       <parent link="${tf_prefix}arm_link_el1" />
       <child link="${tf_prefix}arm_link_wr0" />
-      <limit effort="1000" velocity="1000.00" lower="-1.83259571459404613236"
+      <limit effort="30.3" velocity="10.0" lower="-1.83259571459404613236"
         upper="1.83259571459404613236" />
     </joint>
 
@@ -248,7 +244,7 @@
       <axis xyz="1 0 0" />
       <parent link="${tf_prefix}arm_link_wr0" />
       <child link="${tf_prefix}arm_link_wr1" />
-      <limit effort="1000" velocity="1000.00" lower="-2.87979326579064354163"
+      <limit effort="30.3" velocity="10.0" lower="-2.87979326579064354163"
         upper="2.87979326579064354163" />
     </joint>
 
@@ -305,7 +301,7 @@
       <axis xyz="0.0 1.0 0.0" />
       <parent link="${tf_prefix}arm_link_wr1" />
       <child link="${tf_prefix}arm_link_fngr" />
-      <limit effort="1000" velocity="1000.00" lower="-1.57" upper="0.0" />
+      <limit effort="15.32" velocity="10.0" lower="-1.57" upper="0.0" />
     </joint>
   </xacro:macro>
 </robot>

spot_description/urdf/spot_feet_macro.urdf

@@ -0,0 +1,44 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="load_feet" params="tf_prefix">
+
+    <!-- This is the distance from foot to knee in m -->
+    <xacro:property name="foot_to_knee" value="0.35"/>
+
+    <link name="${tf_prefix}front_left_foot"/>
+    <joint name="${tf_prefix}front_left_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}front_left_lower_leg"/>
+      <child link="${tf_prefix}front_left_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}front_right_foot"/>
+    <joint name="${tf_prefix}front_right_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}front_right_lower_leg"/>
+      <child link="${tf_prefix}front_right_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}rear_left_foot"/>
+    <joint name="${tf_prefix}rear_left_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}rear_left_lower_leg"/>
+      <child link="${tf_prefix}rear_left_foot"/>
+    </joint>
+
+    <link name="${tf_prefix}rear_right_foot"/>
+    <joint name="${tf_prefix}rear_right_ankle" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 ${-foot_to_knee}"/>
+      <axis xyz="0 0 0"/>
+      <parent link="${tf_prefix}rear_right_lower_leg"/>
+      <child link="${tf_prefix}rear_right_foot"/>
+    </joint>
+
+  </xacro:macro>
+
+</robot>