Repository: bdaiinstitute/spot_ros2
Pull request #625: [Sw-2050] expose body pose
Author: login: tcappellari-bdai
Created: 2025-03-17T15:04:27Z | Merged: 2025-03-19T21:18:13Z

## Change Overview

Exposes the odom to body and vision to body transforms in the ros2_control hardware interface.
Needs https://github.com/bdaiinstitute/spot_description/pull/10

## Testing Done

Please create a checklist of tests you plan to do and check off the ones that have been completed successfully. Ensure that ROS 2 tests use `domain_coordinator` to prevent port conflicts. Further guidance for testing can be found on the [ros utilities wiki](https://github.com/bdaiinstitute/ros_utilities/wiki/Testing-guidelines).



Diff:
diff --git a/spot_description b/spot_description
index 98267e7f6..820e39c21 160000
--- a/spot_description
+++ b/spot_description
@@ -1 +1 @@
-Subproject commit 98267e7f6c7dba9b287b6bf0869b264d832b93e2
+Subproject commit 820e39c21458563dc699f99faf9bf06a751853ff
diff --git a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
index a61967ddc..952a7f4a0 100644
--- a/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
+++ b/spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
@@ -106,8 +106,11 @@ class StateStreamingHandler {
    *  CONTACT_UNKNOWN	0	Unknown contact. Do not use.
       CONTACT_MADE	1	The foot is currently in contact with the ground.
       CONTACT_LOST	2	The foot is not in contact with the ground.
+   * Save the current transforms from odom to body and vision to body frames
    */
-  void get_states(JointStates& joint_states, ImuStates& imu_states, std::vector<int>& foot_states);
+  void get_states(JointStates& joint_states, ImuStates& imu_states, std::vector<int>& foot_states,
+                  std::vector<double>& odom_pos, std::vector<double>& odom_rot, std::vector<double>& vision_pos,
+                  std::vector<double>& vision_rot);
   /**
    * @brief Reset internal state.
    */
@@ -127,6 +130,11 @@ class StateStreamingHandler {
   // store the current foot contact states
   std::vector<int> current_foot_state_;
   static constexpr size_t nfeet_ = 4;
+  // store current body pose
+  std::vector<double> odom_tform_body_pos_;    // (x, y, z) in m
+  std::vector<double> odom_tform_body_rot_;    // (x, y, z, w) quaternion
+  std::vector<double> vision_tform_body_pos_;  // (x, y, z) in m
+  std::vector<double> vision_tform_body_rot_;  // (x, y, z, w) quaternion
   // responsible for ensuring read/writes of joint states do not happen at the same time.
   std::mutex mutex_;
 };
@@ -179,13 +187,17 @@ class SpotHardware : public hardware_interface::SystemInterface {
   static constexpr size_t nfeet_ = 4;
   // Sensor configuration
   // We have 2 sensors, IMU and feet contact
-  static constexpr size_t n_sensors_ = 2;
+  static constexpr size_t n_sensors_ = 4;
   // index we expect these sensors to be at in info_.sensors
   static constexpr size_t imu_sensor_index_ = 0;
   static constexpr size_t foot_sensor_index_ = 1;
+  static constexpr size_t odom_to_body_sensor_index_ = 2;
+  static constexpr size_t vision_to_body_sensor_index_ = 3;
   // number of state interfaces we expect per sensor
   static constexpr size_t n_imu_sensor_interfaces_ = 10;
   static constexpr size_t n_foot_sensor_interfaces_ = 4;
+  static constexpr size_t n_odom_body_sensor_interfaces_ = 7;
+  static constexpr size_t n_vision_body_sensor_interfaces_ = 7;
 
   // Login info
   std::string hostname_;
@@ -221,6 +233,11 @@ class SpotHardware : public hardware_interface::SystemInterface {
   ImuStates imu_states_;
   // Holds foot states received from the BD SDK
   std::vector<int> foot_states_;
+  // Holds body poses received from the BD SDK
+  std::vector<double> odom_pos_;  // (x, y, z) in m
+  std::vector<double> odom_rot_;  // (x, y, z, w)
+  std::vector<double> vision_pos_;
+  std::vector<double> vision_rot_;
 
   // Thread for reading the state of the robot.
   std::jthread state_thread_;
@@ -309,6 +326,8 @@ class SpotHardware : public hardware_interface::SystemInterface {
 
   std::vector<double> hw_imu_sensor_states_;
   std::vector<double> hw_foot_sensor_states_;
+  std::vector<double> hw_odom_body_sensor_states_;    // Holds the odom to body transforms
+  std::vector<double> hw_vision_body_sensor_states_;  // Holds the vision to body transforms
 };
 
 }  // namespace spot_hardware_interface
diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp
index c3cadb651..90c6924cc 100644
--- a/spot_hardware_interface/src/spot_hardware_interface.cpp
+++ b/spot_hardware_interface/src/spot_hardware_interface.cpp
@@ -61,10 +61,25 @@ void StateStreamingHandler::handle_state_streaming(::bosdyn::api::RobotStateStre
   imu_linear_acceleration_ = {acceleration_msg.x(), acceleration_msg.y(), acceleration_msg.z()};
   imu_angular_velocity_ = {angular_vel_msg.x(), angular_vel_msg.y(), angular_vel_msg.z()};
   imu_odom_rot_quaternion_ = {rot_msg.x(), rot_msg.y(), rot_msg.z(), rot_msg.w()};
+
+  // Get body pose data from the robot
+  const auto& odom_tform_body_pos_msg = robot_state.kinematic_state().odom_tform_body().position();
+  const auto& odom_tform_body_rot_msg = robot_state.kinematic_state().odom_tform_body().rotation();
+  const auto& vision_tform_body_pos_msg = robot_state.kinematic_state().vision_tform_body().position();
+  const auto& vision_tform_body_rot_msg = robot_state.kinematic_state().vision_tform_body().rotation();
+  // Save poses
+  odom_tform_body_pos_ = {odom_tform_body_pos_msg.x(), odom_tform_body_pos_msg.y(), odom_tform_body_pos_msg.z()};
+  odom_tform_body_rot_ = {odom_tform_body_rot_msg.x(), odom_tform_body_rot_msg.y(), odom_tform_body_rot_msg.z(),
+                          odom_tform_body_rot_msg.w()};
+  vision_tform_body_pos_ = {vision_tform_body_pos_msg.x(), vision_tform_body_pos_msg.y(),
+                            vision_tform_body_pos_msg.z()};
+  vision_tform_body_rot_ = {vision_tform_body_rot_msg.x(), vision_tform_body_rot_msg.y(), vision_tform_body_rot_msg.z(),
+                            vision_tform_body_rot_msg.w()};
 }
 
-void StateStreamingHandler::get_states(JointStates& joint_states, ImuStates& imu_states,
-                                       std::vector<int>& foot_states) {
+void StateStreamingHandler::get_states(JointStates& joint_states, ImuStates& imu_states, std::vector<int>& foot_states,
+                                       std::vector<double>& odom_pos, std::vector<double>& odom_rot,
+                                       std::vector<double>& vision_pos, std::vector<double>& vision_rot) {
   // lock so that read/write doesn't happen at the same time
   const std::lock_guard<std::mutex> lock(mutex_);
   // Fill in members of the joint states stuct passed in by reference.
@@ -81,6 +96,12 @@ void StateStreamingHandler::get_states(JointStates& joint_states, ImuStates& imu
 
   // Fill in foot contact states
   foot_states.assign(current_foot_state_.begin(), current_foot_state_.end());
+
+  // Fill in body transforms
+  odom_pos.assign(odom_tform_body_pos_.begin(), odom_tform_body_pos_.end());
+  odom_rot.assign(odom_tform_body_rot_.begin(), odom_tform_body_rot_.end());
+  vision_pos.assign(vision_tform_body_pos_.begin(), vision_tform_body_pos_.end());
+  vision_rot.assign(vision_tform_body_rot_.begin(), vision_tform_body_rot_.end());
 }
 
 void StateStreamingHandler::reset() {
@@ -211,6 +232,18 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
                  info_.sensors[foot_sensor_index_].state_interfaces.size(), n_foot_sensor_interfaces_);
     return hardware_interface::CallbackReturn::ERROR;
   }
+  if (info_.sensors[odom_to_body_sensor_index_].state_interfaces.size() != n_odom_body_sensor_interfaces_) {
+    RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"),
+                 "Odom to body frame sensor state interface has %ld state interfaces found. '%ld' expected.",
+                 info_.sensors[odom_to_body_sensor_index_].state_interfaces.size(), n_odom_body_sensor_interfaces_);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  if (info_.sensors[vision_to_body_sensor_index_].state_interfaces.size() != n_vision_body_sensor_interfaces_) {
+    RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"),
+                 "Vision to body frame state interface has %ld state interfaces found. '%ld' expected.",
+                 info_.sensors[vision_to_body_sensor_index_].state_interfaces.size(), n_vision_body_sensor_interfaces_);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
 
   // resize to fit the expected number of interfaces
   njoints_ = info_.joints.size();
@@ -218,6 +251,8 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
   hw_commands_.resize(njoints_ * command_interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
   hw_imu_sensor_states_.resize((n_imu_sensor_interfaces_), std::numeric_limits<double>::quiet_NaN());
   hw_foot_sensor_states_.resize((n_foot_sensor_interfaces_), std::numeric_limits<double>::quiet_NaN());
+  hw_odom_body_sensor_states_.resize((n_odom_body_sensor_interfaces_), std::numeric_limits<double>::quiet_NaN());
+  hw_vision_body_sensor_states_.resize((n_vision_body_sensor_interfaces_), std::numeric_limits<double>::quiet_NaN());
 
   return hardware_interface::CallbackReturn::SUCCESS;
 }
@@ -313,6 +348,20 @@ std::vector<hardware_interface::StateInterface> SpotHardware::export_state_inter
         foot_sensor.name, foot_sensor.state_interfaces[i].name, &hw_foot_sensor_states_[i]));
   }
 
+  // export odom to body transform sensor states
+  const auto& odom_to_body_sensor = info_.sensors[odom_to_body_sensor_index_];
+  for (size_t i = 0; i < n_odom_body_sensor_interfaces_; i++) {
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+        odom_to_body_sensor.name, odom_to_body_sensor.state_interfaces[i].name, &hw_odom_body_sensor_states_[i]));
+  }
+
+  // export vision to body transform sensor states
+  const auto& vision_to_body_sensor = info_.sensors[vision_to_body_sensor_index_];
+  for (size_t i = 0; i < n_vision_body_sensor_interfaces_; i++) {
+    state_interfaces.emplace_back(hardware_interface::StateInterface(
+        vision_to_body_sensor.name, vision_to_body_sensor.state_interfaces[i].name, &hw_vision_body_sensor_states_[i]));
+  }
+
   return state_interfaces;
 }
 
@@ -366,7 +415,8 @@ hardware_interface::CallbackReturn SpotHardware::on_cleanup(const rclcpp_lifecyc
 }
 
 hardware_interface::return_type SpotHardware::read(const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
-  state_streaming_handler_.get_states(joint_states_, imu_states_, foot_states_);
+  state_streaming_handler_.get_states(joint_states_, imu_states_, foot_states_, odom_pos_, odom_rot_, vision_pos_,
+                                      vision_rot_);
   const auto& joint_pos = joint_states_.position;
   const auto& joint_vel = joint_states_.velocity;
   const auto& joint_load = joint_states_.load;
@@ -422,10 +472,25 @@ hardware_interface::return_type SpotHardware::read(const rclcpp::Time& /*time*/,
   hw_imu_sensor_states_.at(9) = imu_states_.linear_acceleration.at(2);
 
   // Load foot contact states
-  hw_foot_sensor_states_.at(0) = foot_states_.at(0);
-  hw_foot_sensor_states_.at(1) = foot_states_.at(1);
-  hw_foot_sensor_states_.at(2) = foot_states_.at(2);
-  hw_foot_sensor_states_.at(3) = foot_states_.at(3);
+  hw_foot_sensor_states_.assign(foot_states_.begin(), foot_states_.end());
+
+  // Load odom to body transform
+  hw_odom_body_sensor_states_.at(0) = odom_pos_.at(0);
+  hw_odom_body_sensor_states_.at(1) = odom_pos_.at(1);
+  hw_odom_body_sensor_states_.at(2) = odom_pos_.at(2);
+  hw_odom_body_sensor_states_.at(3) = odom_rot_.at(0);
+  hw_odom_body_sensor_states_.at(4) = odom_rot_.at(1);
+  hw_odom_body_sensor_states_.at(5) = odom_rot_.at(2);
+  hw_odom_body_sensor_states_.at(6) = odom_rot_.at(3);
+
+  // Load vision to body transform
+  hw_vision_body_sensor_states_.at(0) = vision_pos_.at(0);
+  hw_vision_body_sensor_states_.at(1) = vision_pos_.at(1);
+  hw_vision_body_sensor_states_.at(2) = vision_pos_.at(2);
+  hw_vision_body_sensor_states_.at(3) = vision_rot_.at(0);
+  hw_vision_body_sensor_states_.at(4) = vision_rot_.at(1);
+  hw_vision_body_sensor_states_.at(5) = vision_rot_.at(2);
+  hw_vision_body_sensor_states_.at(6) = vision_rot_.at(3);
 
   return hardware_interface::return_type::OK;
 }


Reviews:
user: login: amessing-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T15:06:34Z
user: login: khughes-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T15:26:43Z
user: login: khughes-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T15:28:37Z
user: login: khughes-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T15:29:23Z
user: login: khughes-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T15:32:42Z
user: login: khughes-bdai
state: COMMENTED
author_association: CONTRIBUTOR
submitted_at: 2025-03-17T19:46:59Z
user: login: khughes-bdai
body: LGTM 
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-03-19T21:17:50Z

Review comments:
diff_hunk: @@ -107,7 +107,8 @@ class StateStreamingHandler {
       CONTACT_MADE	1	The foot is currently in contact with the ground.
path: spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
user: login: amessing-bdai
body: <!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'><i>[Re: line +102]</i></p><!--__GRAPHITE_HTML_TAG_END__-->
Can you update this comment block?

```
@param[out] joint_states ...
@param[out] imu_states ...
....
```
<!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'>See this comment inline on <a href="https://app.graphite.dev/github/pr/bdaiinstitute/spot_ros2/625?utm_source=unchanged-line-comment">Graphite</a>.</p><!--__GRAPHITE_HTML_TAG_END__-->
created_at: 2025-03-17T15:06:33Z
author_association: CONTRIBUTOR
diff_hunk: @@ -186,6 +192,7 @@ class SpotHardware : public hardware_interface::SystemInterface {
   // number of state interfaces we expect per sensor
path: spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
user: login: khughes-bdai
body: <!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'><i>[Re: line +188]</i></p><!--__GRAPHITE_HTML_TAG_END__-->
Can you update this to 4? Since I think you are adding 2 new sensors from the xacro 
<!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'>See this comment inline on <a href="https://app.graphite.dev/github/pr/bdaiinstitute/spot_ros2/625?utm_source=unchanged-line-comment">Graphite</a>.</p><!--__GRAPHITE_HTML_TAG_END__-->
created_at: 2025-03-17T15:26:42Z
author_association: CONTRIBUTOR
diff_hunk: @@ -216,7 +237,7 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
   njoints_ = info_.joints.size();
path: spot_hardware_interface/src/spot_hardware_interface.cpp
user: login: khughes-bdai
body: <!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'><i>[Re: lines +216 to +220]</i></p><!--__GRAPHITE_HTML_TAG_END__-->
I think you shouldn't need to change this after n_sensors is changed to 4, but this is how it's used to check that the number of sensors in the xacro matches what we expect 
<!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'>See this comment inline on <a href="https://app.graphite.dev/github/pr/bdaiinstitute/spot_ros2/625?utm_source=unchanged-line-comment">Graphite</a>.</p><!--__GRAPHITE_HTML_TAG_END__-->
created_at: 2025-03-17T15:28:34Z
author_association: CONTRIBUTOR
diff_hunk: @@ -216,7 +237,7 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
   njoints_ = info_.joints.size();
path: spot_hardware_interface/src/spot_hardware_interface.cpp
user: login: khughes-bdai
body: <!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'><i>[Re: lines +229 to +234]</i></p><!--__GRAPHITE_HTML_TAG_END__-->
Can you add a similar check for the two new sensors, to check that the number of interfaces in each of them is correct? 
<!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'>See this comment inline on <a href="https://app.graphite.dev/github/pr/bdaiinstitute/spot_ros2/625?utm_source=unchanged-line-comment">Graphite</a>.</p><!--__GRAPHITE_HTML_TAG_END__-->
created_at: 2025-03-17T15:29:22Z
author_association: CONTRIBUTOR
diff_hunk: @@ -186,6 +192,7 @@ class SpotHardware : public hardware_interface::SystemInterface {
   // number of state interfaces we expect per sensor
path: spot_hardware_interface/include/spot_hardware_interface/spot_hardware_interface.hpp
user: login: khughes-bdai
body: <!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'><i>[Re: lines +190 to +191]</i></p><!--__GRAPHITE_HTML_TAG_END__-->
it might be useful to add the indices of the new sensors you're adding
```gt_suggestion
  static constexpr size_t imu_sensor_index_ = 0;
  static constexpr size_t foot_sensor_index_ = 1;
  static constexpr size_t odom_sensor_index_ = 2;
  static constexpr size_t vision_sensor_index_ = 3;
```
then you can use these in the checks to make sure that there are the right number of sensors 
<!--__GRAPHITE_HTML_TAG_START__--><p class='graphite__hidden'>See this comment inline on <a href="https://app.graphite.dev/github/pr/bdaiinstitute/spot_ros2/625?utm_source=unchanged-line-comment">Graphite</a>.</p><!--__GRAPHITE_HTML_TAG_END__-->
created_at: 2025-03-17T15:32:41Z
author_association: CONTRIBUTOR
diff_hunk: @@ -211,12 +232,25 @@ hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interfac
                  info_.sensors[foot_sensor_index_].state_interfaces.size(), n_foot_sensor_interfaces_);
     return hardware_interface::CallbackReturn::ERROR;
   }
+  if (info_.sensors[odom_to_body_sensor_index_].state_interfaces.size() != n_odom_body_sensor_interfaces_) {
+    RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"),
+                 "Odom to body frame sensor state interface has %ld state interfaces found. '%ld' expected.",
+                 info_.sensors[odom_to_body_sensor_index_].state_interfaces.size(), n_odom_body_sensor_interfaces_);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
+  if (info_.sensors[vision_to_body_sensor_index_].state_interfaces.size() != n_vision_body_sensor_interfaces_) {
+    RCLCPP_FATAL(rclcpp::get_logger("SpotHardware"),
+                 "Vision to body frame state interface has %ld state interfaces found. '%ld' expected.",
+                 info_.sensors[vision_to_body_sensor_index_].state_interfaces.size(), n_vision_body_sensor_interfaces_);
+    return hardware_interface::CallbackReturn::ERROR;
+  }
 
   // resize to fit the expected number of interfaces
   njoints_ = info_.joints.size();
   hw_states_.resize(njoints_ * state_interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
   hw_commands_.resize(njoints_ * command_interfaces_per_joint_, std::numeric_limits<double>::quiet_NaN());
-  hw_sensor_states_.resize((n_imu_sensor_interfaces_ + n_foot_sensor_interfaces_),
+  hw_sensor_states_.resize((n_imu_sensor_interfaces_ + n_foot_sensor_interfaces_ + n_odom_body_sensor_interfaces_ +
+                            n_odom_body_sensor_interfaces_),
path: spot_hardware_interface/src/spot_hardware_interface.cpp
user: login: khughes-bdai
body: nit
```suggestion
  hw_sensor_states_.resize((n_imu_sensor_interfaces_ + n_foot_sensor_interfaces_ + n_odom_body_sensor_interfaces_ +
                            n_vision_body_sensor_interfaces_),
```
created_at: 2025-03-17T19:46:59Z
author_association: CONTRIBUTOR

PR comments:
user: login: coveralls
created_at: 2025-03-17T19:33:34Z
body: ## Pull Request Test Coverage Report for [Build 13954979121](https://coveralls.io/builds/72819681)


### Details

* **0** of **0**   changed or added relevant lines in **0** files are covered.
* No unchanged relevant lines lost coverage.
* Overall coverage remained the same at **51.709%**

---



|  Totals | [![Coverage Status](https://coveralls.io/builds/72819681/badge)](https://coveralls.io/builds/72819681) |
| :-- | --: |
| Change from base [Build 13953064393](https://coveralls.io/builds/72817547): |  0.0% |
| Covered Lines: | 2088 |
| Relevant Lines: | 4038 |

---
##### 💛  - [Coveralls](https://coveralls.io)

author_association: NONE
user: login: khughes-bdai
created_at: 2025-03-17T20:00:42Z
body: Looks good! would just want to test on the robot before merging to make sure the existing examples still work 
author_association: CONTRIBUTOR