[SW-1727] improve spot ros2 readmes

Dockerfile

@@ -28,4 +28,4 @@ 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
+    colcon build --symlink-install

spot_description/README.md

@@ -44,42 +44,6 @@ For the arm, we use the following mass values:
 + `arm_link_wr1` = 0.785 Kg
 + `arm_link_fngr` = 0.200 Kg 
 
-To get these inertial properties, use the following method:
-
-```
-BdaiWorld.import_inertia_tensor = False
-
-sh_mass = 2.596
-fa_mass = 1.450
-hip_mass = 1.68
-uleg_mass = 2.34
-lleg_mass = 0.35
-desired_mass = {
-    "body_inertia": 32.86,
-    "fl_hip": hip_mass,
-    "fr_hip": hip_mass,
-    "hl_hip": hip_mass,
-    "hr_hip": hip_mass,
-    "fl_uleg": uleg_mass,
-    "fr_uleg": uleg_mass,
-    "hl_uleg": uleg_mass,
-    "hr_uleg": uleg_mass,
-    "fl_lleg": lleg_mass,
-    "fr_lleg": lleg_mass,
-    "hl_lleg": lleg_mass,
-    "hr_lleg": lleg_mass,
-    "arm_link_sh0": 0.9 * sh_mass,
-    "arm_link_sh1": 0.1 * sh_mass,
-    "arm_link_hr0": 1e-6,
-    "arm_link_el0": 0.5 * fa_mass,
-    "arm_link_el1": 0.5 * fa_mass,
-    "arm_link_wr0": 0.980,
-    "arm_link_wr1": 0.785,
-    "arm_link_fngr": 0.200
-}
-print(world.get_urdf_inertial(use_diagonal_inertia=True, desired_mass=desired_mass))
-```
-
 ## ROS 2 Control
 The Spot URDF also has the option to be constructed with ROS 2 control tags, which uses the hardware interface plugin from the [`spot_hardware_interface`](../spot_hardware_interface) package. To get the plain URDF file containing these tags, run
 

spot_driver/EyeInHandCalibration.md

@@ -0,0 +1,86 @@
+
+## 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_driver/README.md

@@ -1,17 +1,103 @@
 # spot_driver
 
-This package provides the central ROS 2 interface to interact with Spot.
+The Spot driver contains all of the necessary topics, services, and actions for controlling Spot over ROS 2.
+To launch the driver, run the following command, with the appropriate launch arguments and/or config file that are discussed below.
+```
+ros2 launch spot_driver spot_driver.launch.py [config_file:=<path/to/config.yaml>] [spot_name:=<Spot Name>] [launch_rviz:=<True|False>] [launch_image_publishers:=<True|False>] [publish_point_clouds:=<True|False>] [uncompress_images:=<True|False>] [publish_compressed_images:=<True|False>] [stitch_front_images:=<True|False>]
+```
 
-The primary launchfile in this package is `spot_driver.launch.py`. Once you've built and sourced your workspace, launch it with:
+## Configuration
+The Spot login data hostname, username and password can be specified either as ROS parameters or as environment variables.
+If using ROS parameters, see [`spot_driver/config/spot_ros_example.yaml`](spot_driver/config/spot_ros_example.yaml) for an example of what your file could look like, and pass this to the driver as a launch argument with `config_file:=path/to/config.yaml`.
+If using environment variables, define `BOSDYN_CLIENT_USERNAME`, `BOSDYN_CLIENT_PASSWORD`, and `SPOT_IP`.
 
-`ros2 launch spot_driver spot_driver.launch.py`
+## Namespacing
+By default, the driver is launched in the global namespace.
+To avoid this, it is recommended to launch the driver with the launch argument `spot_name:=<Spot Name>`.
+This will place all of the nodes, topics, services, and actions provided by the driver in the `<Spot Name>` namespace.
+Additionally, it will prefix all of the TF frames and joints of the robot with `<Spot Name>`.
 
-* Various driver parameters can be customized using a configuration file -- see [spot_ros_example.yaml](config/spot_ros_example.yaml) for reference. To launch the driver with a configuration file, add the launch argument `config_file:=path/to/config.yaml`. Note that you can specify the hostname, username, and password for login to the robot in this file, or they can alternatively be set in the environment variables `SPOT_IP`, `BOSDYN_CLIENT_USERNAME`, and `BOSDYN_CLIENT_PASSWORD`, respectively. 
-* To launch the process within a namespace, add the launch argument `spot_name:={name}`
-* To visualize Spot in RViz, add the launch argument `launch_rviz:=True`. This will automatically generate the appropriate RViz config file for your robot's name using `rviz.launch.py`.
-* To publish point clouds, add the launch argument `publish_point_clouds:=True`. This is disabled by default.
+## Frames
+Background information about Spot's frames from Boston Dynamics can be found [here](https://dev.bostondynamics.com/docs/concepts/geometry_and_frames). 
+By default, the Spot driver will place the "odom" frame as the root of the TF tree.
+This can be changed by setting the `tf_root` parameter in your config file to either "vision" or "body".
+The Spot driver will also publish odometry topics with respect to the "odom" frame by default.
+If you wish to change this to "vision", update the `preferred_odom_frame` parameter in your config file.
+
+## Simple Robot Commands
+Many simple robot commands can be called as services from the command line once the driver is running. For example:
+
+* `ros2 service call /<Robot Name>/sit std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/stand std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/undock std_srvs/srv/Trigger`
+
+If your Spot has an arm, some additional helpful services are exposed:
+* `ros2 service call /<Robot Name>/arm_stow std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/arm_unstow std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/arm_carry std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/open_gripper std_srvs/srv/Trigger`
+* `ros2 service call /<Robot Name>/close_gripper std_srvs/srv/Trigger`
+
+## More Complex Robot Commands
+The full list of interfaces provided by the driver can be explored via `ros2 topic list`, `ros2 service list`, and `ros2 action list`. 
+For more information about the custom message types used in this package, run `ros2 interface show <interface_type>`. 
+More details can also be found on the [`spot_ros2` wiki](https://github.com/bdaiinstitute/spot_ros2/wiki/Spot-Driver-Available-Interfaces). 
+
+
+## Images
+Perception data from Spot is handled through the `spot_image_publishers.launch.py` launchfile, which is launched by default from the driver.
+If you want to only view images from Spot, without bringing up any of the nodes to control the robot, you can also choose to run this launchfile independently.
+
+By default, the driver will publish RGB images as well as depth maps from the `frontleft`, `frontright`, `left`, `right`, and `back` cameras on Spot (plus `hand` if your Spot has an arm).
+You can customize the cameras that are streamed from by adding the `cameras_used` parameter to your config yaml. (For example, to stream from only the front left and front right cameras, you can add `cameras_used: ["frontleft", "frontright"]`).
+Additionally, if your Spot has greyscale cameras, you will need to set `rgb_cameras: False` in your configuration YAML file, or you will not receive any image data.
+
+By default, the driver does not publish point clouds.
+To enable this, launch the driver with `publish_point_clouds:=True`.
+
+The driver can publish both compressed images (under `/<Robot Name>/camera/<camera location>/compressed`) and uncompressed images (under `/<Robot Name>/camera/<camera location>/image`).
+By default, it will only publish the uncompressed images.
+You can turn (un)compressed images on/off by launching the driver with the flags `uncompress_images:=<True|False>` and `publish_compressed_images:=<True|False>`.
+
+The driver also has the option to publish a stitched image created from Spot's front left and front right cameras (similar to what is seen on the tablet).
+If you wish to enable this, launch the driver with `stitch_front_images:=True`, and the image will be published under `/<Robot Name>/camera/frontmiddle_virtual/image`.
+In order to receive meaningful stitched images, you will have to specify the parameters `virtual_camera_intrinsics`, `virtual_camera_projection_plane`, `virtual_camera_plane_distance`, and `stitched_image_row_padding` (see [`spot_driver/config/spot_ros_example.yaml`](spot_driver/config/spot_ros_example.yaml) for some default values). 
+
+> **_NOTE:_**  
+If your image publishing rate is very slow, you can try 
+> - connecting to your robot via ethernet cable 
+> - exporting a custom DDS profile we have provided by running the following in the same terminal your driver will run in, or adding to your `.bashrc`:
+> ```
+> export=FASTRTPS_DEFAULT_PROFILES_FILE=<path_to_file>/custom_dds_profile.xml
+> ```
+
+## Calibration
+A calibration procedure for the hand camera is provided by this package. For more information on how to run this, refer to [EyeInHandCalibration.md](EyeInHandCalibration.md)
+
+
+<details>
+<summary><h2>Spot CAM</h2></summary>
+<br>
+
+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`
+
+The Spot CAM payload has known issues with the SSL certification process in https. If you get the following errors:
+
+```
+non-existing PPS 0 referenced
+decode_slice_header error
+no frame!
+```
+
+Then you want to log into the Spot CAM over the browser. In your browser, type in:
+
+    https://<ip_address_of_spot>:<sdp_port>/h264.sdp.html
+
+The default port for SDP is 31102 for the Spot CAM. Once inside, you will be prompted to log in using your username and password. Do so and the WebRTC frames should begin to properly stream.
+
+</details>
 
-The Spot driver contains both Python and C++ nodes. Spot's Python SDK is used for many operations. For example, `spot_ros2` is the primary node that connects with Spot and creates the ROS 2 action servers and services. Spot's C++ SDK is used in nodes like `spot_image_publisher_node` to retrieve images from Spot's RGB and depth cameras at close to their native refresh rate of 15 Hz -- something that is not possible using the Python SDK. 
 
 ## Examples
 For some examples of using the Spot ROS 2 driver, check out [`spot_examples`](../spot_examples/).

spot_msgs/README.md

@@ -1,123 +1,5 @@
 # spot_msgs
 
-## How to build `.deb`
+`spot_msgs` is a ROS 2 package containing interfaces useful for interacting with the `spot_driver`.
 
-(Largely borrowed from [here](https://gist.github.com/awesomebytes/196eab972a94dd8fcdd69adfe3bd1152))
-
-### Get dependencies
-You may need the latest pip, follow the [official instructions](https://pip.pypa.io/en/stable/installing/).
-
-Install [bloom](http://ros-infrastructure.github.io/bloom/):
-
-```bash
-sudo pip install -U bloom
-```
-
-Install fakeroot:
-
-```bash
-sudo apt-get install fakeroot
-```
-
-### Deal with rosdep
-Bloom won't create a package that depends on something `rosdep` can't find.  Until `bosdyn_msgs` is distributed, we need to trick `rosdep` into thinking it exists:
-1.  Clone `https://github.com/ros/rosdistro`
-2.  Change `rosdep/base.yaml` to just list an empty target for `bosdyn_msgs`:
-
-```bash
-bosdyn_msgs:
-  ubuntu: []
-```
-3.  Add a file `/etc/ros/rosdep/sources.list.d/8-local.list` with the entry:
-
-```bash
-yaml file:///<absolute path to base.yaml>
-```
-
-4.  Run `rosdep update` and make sure it can resolve the `bosdyn_msgs` key:
-
-```bash
-rosdep update
-rosdep resolve bosdyn_msgs
-```
-
-### Get ready
-
-To make a debian folder structure from the ROS package you must cd into
-the package to be in the same folder where `package.xml` file is.
-
-### Create debian structure
-
-```bash
-bloom-generate rosdebian --os-name ubuntu --os-version jammy --ros-distro humble
-```
-
-You can also let the tool guess some stuff:
-```bash
-bloom-generate rosdebian --ros-distro humble
-```
-
-You'll get something like this:
-
-```bash
-$ bloom-generate rosdebian --os-name ubuntu --os-version jammy --ros-distro humble
-/usr/lib/python3/dist-packages/pkg_resources/__init__.py:116: PkgResourcesDeprecationWarning:  is an invalid version and will not be supported in a future release
-  warnings.warn(
-/usr/lib/python3/dist-packages/pkg_resources/__init__.py:116: PkgResourcesDeprecationWarning:  is an invalid version and will not be supported in a future release
-  warnings.warn(
-ROS Distro index file associate with commit 'e7ba7d4d4fa7ad70260b34fb0e03f4b96e6f62bb'
-New ROS Distro index url: 'https://raw.githubusercontent.com/ros/rosdistro/e7ba7d4d4fa7ad70260b34fb0e03f4b96e6f62bb/index-v4.yaml'
-==> Generating debs for ubuntu:jammy for package(s) ['spot_msgs']
-No homepage set, defaulting to ''
-No historical releaser history, using current maintainer name and email for each versioned changelog entry.
-No CHANGELOG.rst found for package 'spot_msgs'
-Package 'spot-msgs' has dependencies:
-Run Dependencies:
-  rosdep key           => jammy key
-  rosidl_default_runtime => ['ros-humble-rosidl-default-runtime']
-Build and Build Tool Dependencies:
-  rosdep key           => jammy key
-  builtin_interfaces   => ['ros-humble-builtin-interfaces']
-  common_interfaces    => ['ros-humble-common-interfaces']
-  std_msgs             => ['ros-humble-std-msgs']
-  sensor_msgs          => ['ros-humble-sensor-msgs']
-  ament_cmake          => ['ros-humble-ament-cmake']
-  rosidl_default_generators => ['ros-humble-rosidl-default-generators']
-==> Placing templates files in the 'debian' folder.
-==> In place processing templates in 'debian' folder.
-Expanding 'debian/rules.em' -> 'debian/rules'
-Expanding 'debian/compat.em' -> 'debian/compat'
-Expanding 'debian/changelog.em' -> 'debian/changelog'
-Expanding 'debian/copyright.em' -> 'debian/copyright'
-Expanding 'debian/control.em' -> 'debian/control'
-Expanding 'debian/source/format.em' -> 'debian/source/format'
-Expanding 'debian/source/options.em' -> 'debian/source/options'
-
-```
-
-### Create binary debian
-
-Having sourced the necessary dependencies (most probably `source /opt/ros/humble/setup.bash`) execute:
-
-```bash
-fakeroot debian/rules binary
-```
-
-If you get the error:
-```bash
-dh: Command not found
-```
-You need to install:
-```bash
-sudo apt-get install dpkg-dev debhelper
-```
-
-In the end you'll get a line like:
-
-```
-dpkg-deb: building package 'ros-humble-spot-msgs' in '../ros-humble-spot-msgs_0.0.0-0jammy_amd64.deb'.
-```
-
-### Upload debian to GitHub
-
-You should now have a `.deb` file containing the `spot_msgs` package. You need to upload that as a release to GitHub [here](https://docs.github.com/en/repositories/releasing-projects-on-github/managing-releases-in-a-repository). Please increment the version of the release. At some point we may consider automating this process but we don't have a formalized way of doing these precompilations yet.
+It builds on top of the [`bosdyn_msgs`](https://github.com/bdaiinstitute/bosdyn_msgs) package, which contains autogenerated ROS messages from Spot SDK Protobuf messages.