From 5f93d25d90f6c3e01e659729619f3c5b8eabe14b Mon Sep 17 00:00:00 2001
From: Tiffany Cappellari <tcappellari@theaiinstitute.com>
Date: Thu, 27 Feb 2025 21:43:10 +0000
Subject: [PATCH] changed to vec of ints instead of footstate_contact

---
 .../spot_hardware_interface.hpp                       |  8 +++++---
 .../src/spot_hardware_interface.cpp                   | 11 +++++++----
 2 files changed, 12 insertions(+), 7 deletions(-)

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 23fb6485a..8593c20b7 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
@@ -93,7 +93,7 @@ class StateStreamingHandler {
       CONTACT_MADE	1	The foot is currently in contact with the ground.
       CONTACT_LOST	2	The foot is not in contact with the ground.
    */
-  void get_foot_states(::bosdyn::api::FootState::Contact& foot_states);
+  void get_foot_states(std::vector<int>& foot_states);
 
  private:
   // Stores the current position, velocity, and load of the robot's joints.
@@ -101,7 +101,7 @@ class StateStreamingHandler {
   std::vector<float> current_velocity_;
   std::vector<float> current_load_;
   // store the current foot contact states
-  ::bosdyn::api::FootState::Contact current_foot_state_;
+  std::vector<int> current_foot_state_;
   // responsible for ensuring read/writes of joint states do not happen at the same time.
   std::mutex mutex_;
 };
@@ -149,6 +149,7 @@ class SpotHardware : public hardware_interface::SystemInterface {
   // The 3 state interfaces are position, velocity, and effort.
   static constexpr size_t state_interfaces_per_joint_ = 3;
   size_t njoints_;
+  size_t nfeet = 4;
 
   // Login info
   std::string hostname_;
@@ -174,7 +175,8 @@ class SpotHardware : public hardware_interface::SystemInterface {
   // Holds joint commands for the robot to send to BD SDK
   JointCommands joint_commands_;
   // Holds foot states received from the BD SDK
-  ::bosdyn::api::FootState::Contact foot_states_;
+  // ::bosdyn::api::FootState::Contact foot_states_;
+  std::vector<int> foot_states_;
 
   // Thread for reading the state of the robot.
   std::jthread state_thread_;
diff --git a/spot_hardware_interface/src/spot_hardware_interface.cpp b/spot_hardware_interface/src/spot_hardware_interface.cpp
index e7ab0b15f..73a87b1e0 100644
--- a/spot_hardware_interface/src/spot_hardware_interface.cpp
+++ b/spot_hardware_interface/src/spot_hardware_interface.cpp
@@ -37,11 +37,13 @@ void StateStreamingHandler::handle_state_streaming(::bosdyn::api::RobotStateStre
   const auto& position_msg = robot_state.joint_states().position();
   const auto& velocity_msg = robot_state.joint_states().velocity();
   const auto& load_msg = robot_state.joint_states().load();
-  // const auto& foot_state_msg = robot_state.contact_states(0);
   current_position_.assign(position_msg.begin(), position_msg.end());
   current_velocity_.assign(velocity_msg.begin(), velocity_msg.end());
   current_load_.assign(load_msg.begin(), load_msg.end());
-  current_foot_state_ = robot_state.contact_states(0);
+  // Save current foot contact states
+  for (size_t i = 0; i < 4; i++) {
+    current_foot_state_.push_back(robot_state.contact_states(i));
+  }
 }
 
 void StateStreamingHandler::get_joint_states(JointStates& joint_states) {
@@ -53,10 +55,10 @@ void StateStreamingHandler::get_joint_states(JointStates& joint_states) {
   joint_states.load.assign(current_load_.begin(), current_load_.end());
 }
 
-void StateStreamingHandler::get_foot_states(::bosdyn::api::FootState::Contact& foot_states) {
+void StateStreamingHandler::get_foot_states(std::vector<int>& foot_states) {
   // lock so that read/write doesn't happen at the same time
   const std::lock_guard<std::mutex> lock(mutex_);
-  foot_states = current_foot_state_;
+  foot_states.assign(current_foot_state_.begin(), current_foot_state_.end());
 }
 
 hardware_interface::CallbackReturn SpotHardware::on_init(const hardware_interface::HardwareInfo& info) {
@@ -146,6 +148,7 @@ hardware_interface::CallbackReturn SpotHardware::on_configure(const rclcpp_lifec
   joint_states_.position.assign(njoints_, 0);
   joint_states_.velocity.assign(njoints_, 0);
   joint_states_.load.assign(njoints_, 0);
+  foot_states_.assign(nfeet, 0);
 
   // Set up the robot using the BD SDK and start command streaming.
   if (!authenticate_robot(hostname_, username_, password_)) {