add support for ground truth estimator

controllers/ocs2_quadruped_controller/CMakeLists.txt

@@ -12,7 +12,6 @@ set(CONTROLLER_INCLUDE_DEPENDS
         pluginlib
         rcpputils
         controller_interface
-        realtime_tools
 
         ocs2_legged_robot_ros
         ocs2_self_collision
@@ -31,7 +30,7 @@ endforeach ()
 add_library(${PROJECT_NAME} SHARED
         src/Ocs2QuadrupedController.cpp
 
-        #        src/estimator/FromTopicEstimate.cpp
+        src/estimator/GroundTruth.cpp
         src/estimator/LinearKalmanFilter.cpp
         src/estimator/StateEstimateBase.cpp
 

controllers/ocs2_quadruped_controller/README.md

@@ -10,6 +10,9 @@ Tested environment:
 * Ubuntu 22.04
     * ROS2 Humble
 
+*[x] **[2025-01-16]** Add support for ground truth estimator.
+
+
 [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/)
 
 ## 1. Interfaces

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/control/CtrlComponent.h

@@ -41,9 +41,13 @@ struct CtrlComponent {
     std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
     foot_force_state_interface_;
 
+    std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface> >
+    odom_state_interface_;
+
     control_input_msgs::msg::Inputs control_inputs_;
     ocs2::SystemObservation observation_;
     int frequency_{};
+    bool reset = true;
 
     std::shared_ptr<ocs2::legged_robot::StateEstimateBase> estimator_;
     std::shared_ptr<ocs2::legged_robot::TargetManager> target_manager_;

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/FromTopiceEstimate.h

@@ -1,33 +0,0 @@
-//
-// Created by qiayuan on 2022/7/24.
-//
-#pragma once
-
-#include "StateEstimateBase.h"
-
-#include <realtime_tools/realtime_tools/realtime_buffer.h>
-
-#pragma once
-namespace ocs2::legged_robot {
-    using namespace ocs2;
-
-    class FromTopicStateEstimate : public StateEstimateBase {
-    public:
-        FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
-                               const PinocchioEndEffectorKinematics &eeKinematics);
-
-        void updateImu(const Eigen::Quaternion<scalar_t> &quat, const vector3_t &angularVelLocal,
-                       const vector3_t &linearAccelLocal,
-                       const matrix3_t &orientationCovariance, const matrix3_t &angularVelCovariance,
-                       const matrix3_t &linearAccelCovariance) override {
-        };
-
-        vector_t update(const ros::Time &time, const ros::Duration &period) override;
-
-    private:
-        void callback(const nav_msgs::Odometry::ConstPtr &msg);
-
-        ros::Subscriber sub_;
-        realtime_tools::RealtimeBuffer<nav_msgs::Odometry> buffer_;
-    };
-} // namespace legged

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/GroundTruth.h

@@ -0,0 +1,24 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+#pragma once
+
+#include "StateEstimateBase.h"
+#include <ocs2_centroidal_model/CentroidalModelPinocchioMapping.h>
+
+#include <tf2_ros/transform_broadcaster.h>
+
+namespace ocs2::legged_robot {
+    class GroundTruth final : public StateEstimateBase {
+    public:
+        GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
+                    const rclcpp_lifecycle::LifecycleNode::SharedPtr &node);
+
+        vector_t update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+
+    protected:
+        nav_msgs::msg::Odometry getOdomMsg();
+        vector3_t position_;
+        vector3_t linear_velocity_;
+    };
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/LinearKalmanFilter.h

@@ -26,6 +26,9 @@ namespace ocs2::legged_robot {
     protected:
         nav_msgs::msg::Odometry getOdomMsg();
 
+        PinocchioInterface pinocchio_interface_;
+        std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
+
         vector_t feet_heights_;
 
         // Config
@@ -43,4 +46,4 @@ namespace ocs2::legged_robot {
         matrix_t a_, b_, c_, q_, p_, r_;
         vector_t xHat_, ps_, vs_;
     };
-} // namespace legged
+}

controllers/ocs2_quadruped_controller/include/ocs2_quadruped_controller/estimator/StateEstimateBase.h

@@ -22,9 +22,7 @@ namespace ocs2::legged_robot {
     public:
         virtual ~StateEstimateBase() = default;
 
-        StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
+        StateEstimateBase(CentroidalModelInfo info, CtrlComponent &ctrl_component,
-                          const PinocchioEndEffectorKinematics &ee_kinematics,
-                          CtrlComponent &ctrl_component,
                           rclcpp_lifecycle::LifecycleNode::SharedPtr node);
 
         virtual void updateJointStates();
@@ -45,10 +43,7 @@ namespace ocs2::legged_robot {
         void publishMsgs(const nav_msgs::msg::Odometry &odom) const;
 
         CtrlComponent &ctrl_component_;
-
-        PinocchioInterface pinocchio_interface_;
         CentroidalModelInfo info_;
-        std::unique_ptr<PinocchioEndEffectorKinematics> ee_kinematics_;
 
         vector3_t zyx_offset_ = vector3_t::Zero();
         vector_t rbd_state_;

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.cpp

@@ -15,6 +15,7 @@
 #include <ocs2_sqp/SqpMpc.h>
 #include <angles/angles.h>
 #include <ocs2_quadruped_controller/control/GaitManager.h>
+#include <ocs2_quadruped_controller/estimator/GroundTruth.h>
 
 namespace ocs2::legged_robot {
     using config_type = controller_interface::interface_configuration_type;
@@ -46,9 +47,15 @@ namespace ocs2::legged_robot {
             conf.names.push_back(imu_name_ + "/" += interface_type);
         }
 
+        if (use_odom_) {
+            for (const auto &interface_type: odom_interface_types_) {
+                conf.names.push_back(odom_name_ + "/" += interface_type);
+            }
+        } else {
             for (const auto &interface_type: foot_force_interface_types_) {
                 conf.names.push_back(foot_force_name_ + "/" += interface_type);
             }
+        }
 
         return conf;
     }
@@ -139,11 +146,22 @@ namespace ocs2::legged_robot {
                 auto_declare<std::vector<std::string> >("command_interfaces", command_interface_types_);
         state_interface_types_ =
                 auto_declare<std::vector<std::string> >("state_interfaces", state_interface_types_);
+
+        // IMU Sensor
         imu_name_ = auto_declare<std::string>("imu_name", imu_name_);
         imu_interface_types_ = auto_declare<std::vector<std::string> >("imu_interfaces", state_interface_types_);
+
+        // Odometer Sensor (Ground Truth)
+        use_odom_ = auto_declare<bool>("use_odom", use_odom_);
+        if (use_odom_) {
+            odom_name_ = auto_declare<std::string>("odom_name", odom_name_);
+            odom_interface_types_ = auto_declare<std::vector<std::string> >("odom_interfaces", state_interface_types_);
+        } else {
+            // Foot Force Sensor
             foot_force_name_ = auto_declare<std::string>("foot_force_name", foot_force_name_);
             foot_force_interface_types_ =
                     auto_declare<std::vector<std::string> >("foot_force_interfaces", state_interface_types_);
+        }
 
         // PD gains
         default_kp_ = auto_declare<double>("default_kp", default_kp_);
@@ -220,6 +238,8 @@ namespace ocs2::legged_robot {
                 ctrl_comp_.imu_state_interface_.emplace_back(interface);
             } else if (interface.get_prefix_name() == foot_force_name_) {
                 ctrl_comp_.foot_force_state_interface_.emplace_back(interface);
+            } else if (interface.get_prefix_name() == odom_name_) {
+                ctrl_comp_.odom_state_interface_.emplace_back(interface);
             } else {
                 state_interface_map_[interface.get_interface_name()]->push_back(interface);
             }
@@ -330,10 +350,19 @@ namespace ocs2::legged_robot {
     }
 
     void Ocs2QuadrupedController::setupStateEstimate() {
+        if (use_odom_) {
+            ctrl_comp_.estimator_ = std::make_shared<GroundTruth>(legged_interface_->getCentroidalModelInfo(),
+                                                                  ctrl_comp_,
+                                                                  get_node());
+            RCLCPP_INFO(get_node()->get_logger(), "Using Ground Truth Estimator");
+        } else {
             ctrl_comp_.estimator_ = std::make_shared<KalmanFilterEstimate>(legged_interface_->getPinocchioInterface(),
                                                                            legged_interface_->getCentroidalModelInfo(),
-                                                                       *eeKinematicsPtr_, ctrl_comp_, this->get_node());
+                                                                           *eeKinematicsPtr_, ctrl_comp_,
+                                                                           get_node());
             dynamic_cast<KalmanFilterEstimate &>(*ctrl_comp_.estimator_).loadSettings(task_file_, verbose_);
+            RCLCPP_INFO(get_node()->get_logger(), "Using Kalman Filter Estimator");
+        }
         ctrl_comp_.observation_.time = 0;
     }
 
@@ -344,7 +373,7 @@ namespace ocs2::legged_robot {
         ctrl_comp_.observation_.state = rbd_conversions_->computeCentroidalStateFromRbdModel(measured_rbd_state_);
         ctrl_comp_.observation_.state(9) = yaw_last + angles::shortest_angular_distance(
                                                yaw_last, ctrl_comp_.observation_.state(9));
-        ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
+        // ctrl_comp_.observation_.mode = ctrl_comp_.estimator_->getMode();
     }
 }
 

controllers/ocs2_quadruped_controller/src/Ocs2QuadrupedController.h

@@ -97,10 +97,16 @@ namespace ocs2::legged_robot {
         // IMU Sensor
         std::string imu_name_;
         std::vector<std::string> imu_interface_types_;
+
         // Foot Force Sensor
         std::string foot_force_name_;
         std::vector<std::string> foot_force_interface_types_;
 
+        // Odometer Sensor (Ground Truth)
+        bool use_odom_ = false;
+        std::string odom_name_;
+        std::vector<std::string> odom_interface_types_;
+
         double default_kp_ = 0;
         double default_kd_ = 6;
 

controllers/ocs2_quadruped_controller/src/control/GaitManager.cpp

@@ -48,5 +48,6 @@ namespace ocs2::legged_robot {
         RCLCPP_INFO(rclcpp::get_logger("GaitManager"), "Switch to gait: %s",
                     gait_name_list_[ctrl_component_.control_inputs_.command - 1].c_str());
         gait_updated_ = true;
+        ctrl_component_.reset = false;
     }
 }

controllers/ocs2_quadruped_controller/src/control/TargetManager.cpp

@@ -25,6 +25,7 @@ namespace ocs2::legged_robot {
     }
 
     void TargetManager::update() {
+        if (ctrl_component_.reset) return;
         vector_t cmdGoal = vector_t::Zero(6);
         cmdGoal[0] = ctrl_component_.control_inputs_.ly * target_displacement_velocity_;
         cmdGoal[1] = -ctrl_component_.control_inputs_.lx * target_displacement_velocity_;

controllers/ocs2_quadruped_controller/src/estimator/FromTopicEstimate.cpp

@@ -1,36 +0,0 @@
-//
-// Created by qiayuan on 2022/7/24.
-//
-
-#include "ocs2_quadruped_controller/estimator/FromTopiceEstimate.h"
-
-namespace ocs2::legged_robot {
-    FromTopicStateEstimate::FromTopicStateEstimate(PinocchioInterface pinocchioInterface, CentroidalModelInfo info,
-                                                   const PinocchioEndEffectorKinematics &eeKinematics)
-        : StateEstimateBase(std::move(pinocchioInterface), std::move(info), eeKinematics) {
-        ros::NodeHandle nh;
-        sub_ = nh.subscribe<nav_msgs::Odometry>("/ground_truth/state", 10, &FromTopicStateEstimate::callback, this);
-    }
-
-    void FromTopicStateEstimate::callback(const nav_msgs::Odometry::ConstPtr &msg) {
-        buffer_.writeFromNonRT(*msg);
-    }
-
-    vector_t FromTopicStateEstimate::update(const ros::Time & /*time*/, const ros::Duration & /*period*/) {
-        nav_msgs::Odometry odom = *buffer_.readFromRT();
-
-        updateAngular(quatToZyx(Eigen::Quaternion<scalar_t>(odom.pose.pose.orientation.w, odom.pose.pose.orientation.x,
-                                                            odom.pose.pose.orientation.y,
-                                                            odom.pose.pose.orientation.z)),
-                      Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.angular.x, odom.twist.twist.angular.y,
-                                                    odom.twist.twist.angular.z));
-        updateLinear(Eigen::Matrix<scalar_t, 3, 1>(odom.pose.pose.position.x, odom.pose.pose.position.y,
-                                                   odom.pose.pose.position.z),
-                     Eigen::Matrix<scalar_t, 3, 1>(odom.twist.twist.linear.x, odom.twist.twist.linear.y,
-                                                   odom.twist.twist.linear.z));
-
-        publishMsgs(odom);
-
-        return rbdState_;
-    }
-} // namespace legged

controllers/ocs2_quadruped_controller/src/estimator/GroundTruth.cpp

@@ -0,0 +1,64 @@
+//
+// Created by qiayuan on 2022/7/24.
+//
+
+#include "ocs2_quadruped_controller/estimator/GroundTruth.h"
+
+#include <ocs2_quadruped_controller/control/CtrlComponent.h>
+
+namespace ocs2::legged_robot {
+    GroundTruth::GroundTruth(CentroidalModelInfo info, CtrlComponent &ctrl_component,
+                             const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
+        : StateEstimateBase(
+            std::move(info), ctrl_component,
+            node) {
+    }
+
+    vector_t GroundTruth::update(const rclcpp::Time &time, const rclcpp::Duration &period) {
+        updateJointStates();
+        updateContact();
+        updateImu();
+
+        position_ = {
+            ctrl_component_.odom_state_interface_[0].get().get_value(),
+            ctrl_component_.odom_state_interface_[1].get().get_value(),
+            ctrl_component_.odom_state_interface_[2].get().get_value()
+        };
+
+        linear_velocity_ = {
+            ctrl_component_.odom_state_interface_[3].get().get_value(),
+            ctrl_component_.odom_state_interface_[4].get().get_value(),
+            ctrl_component_.odom_state_interface_[5].get().get_value()
+        };
+
+        updateLinear(position_, linear_velocity_);
+
+        auto odom = getOdomMsg();
+        odom.header.stamp = time;
+        odom.header.frame_id = "odom";
+        odom.child_frame_id = "base";
+        publishMsgs(odom);
+
+        return rbd_state_;
+    }
+
+    nav_msgs::msg::Odometry GroundTruth::getOdomMsg() {
+        nav_msgs::msg::Odometry odom;
+        odom.pose.pose.position.x = position_(0);
+        odom.pose.pose.position.y = position_(1);
+        odom.pose.pose.position.z = position_(2);
+        odom.pose.pose.orientation.x = quat_.x();
+        odom.pose.pose.orientation.y = quat_.y();
+        odom.pose.pose.orientation.z = quat_.z();
+        odom.pose.pose.orientation.w = quat_.w();
+
+        odom.twist.twist.linear.x = linear_velocity_(0);
+        odom.twist.twist.linear.y = linear_velocity_(1);
+        odom.twist.twist.linear.z = linear_velocity_(2);
+        odom.twist.twist.angular.x = angular_vel_local_.x();
+        odom.twist.twist.angular.y = angular_vel_local_.y();
+        odom.twist.twist.angular.z = angular_vel_local_.z();
+
+        return odom;
+    }
+} // namespace legged

controllers/ocs2_quadruped_controller/src/estimator/LinearKalmanFilter.cpp

@@ -18,8 +18,10 @@ namespace ocs2::legged_robot {
                                                const PinocchioEndEffectorKinematics &ee_kinematics,
                                                CtrlComponent &ctrl_component,
                                                const rclcpp_lifecycle::LifecycleNode::SharedPtr &node)
-        : StateEstimateBase(std::move(pinocchio_interface), std::move(info), ee_kinematics, ctrl_component,
+        : StateEstimateBase(std::move(info), ctrl_component,
                             node),
+          pinocchio_interface_(std::move(pinocchio_interface)),
+          ee_kinematics_(ee_kinematics.clone()),
           numContacts_(info_.numThreeDofContacts + info_.numSixDofContacts),
           dimContacts_(3 * numContacts_),
           numState_(6 + dimContacts_),
@@ -167,7 +169,6 @@ namespace ocs2::legged_robot {
         odom.pose.pose.orientation.y = quat_.y();
         odom.pose.pose.orientation.z = quat_.z();
         odom.pose.pose.orientation.w = quat_.w();
-        odom.pose.pose.orientation.x = quat_.x();
         for (int i = 0; i < 3; ++i) {
             for (int j = 0; j < 3; ++j) {
                 odom.pose.covariance[i * 6 + j] = p_(i, j);

controllers/ocs2_quadruped_controller/src/estimator/StateEstimateBase.cpp

@@ -12,15 +12,11 @@
 #include "ocs2_quadruped_controller/control/CtrlComponent.h"
 
 namespace ocs2::legged_robot {
-
+    StateEstimateBase::StateEstimateBase(CentroidalModelInfo info,
-    StateEstimateBase::StateEstimateBase(PinocchioInterface pinocchio_interface, CentroidalModelInfo info,
-                                         const PinocchioEndEffectorKinematics &ee_kinematics,
                                          CtrlComponent &ctrl_component,
                                          rclcpp_lifecycle::LifecycleNode::SharedPtr node)
         : ctrl_component_(ctrl_component),
-          pinocchio_interface_(std::move(pinocchio_interface)),
           info_(std::move(info)),
-          ee_kinematics_(ee_kinematics.clone()),
           rbd_state_(vector_t::Zero(2 * info_.generalizedCoordinatesNum)), node_(std::move(node)) {
         odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 10);
         pose_pub_ = node_->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("pose", 10);

descriptions/unitree/go1_description/xacro/ft_sensor.xacro

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:macro name="ft_sensor" params="name">
+        <gazebo reference="${name}_foot_fixed">
+            <!-- Enable feedback for this joint -->
+            <provideFeedback>true</provideFeedback>
+            <!-- Prevent ros2_control from lumping this fixed joint with others -->
+            <disableFixedJointLumping>true</disableFixedJointLumping>
+            <sensor name="${name}_ft_sensor" type="force_torque">
+                <always_on>1</always_on>
+                <update_rate>500</update_rate>
+                <visualize>true</visualize>
+                <force_torque>
+                    <frame>child</frame>
+                    <!--                <measure_direction>child_to_parent</measure_direction>-->
+                </force_torque>
+            </sensor>
+        </gazebo>
+    </xacro:macro>
+</robot>

descriptions/unitree/go1_description/xacro/gazebo.xacro

@@ -1,5 +1,6 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+	<xacro:include filename="$(find go1_description)/xacro/ft_sensor.xacro"/>
 
 	<ros2_control name="GazeboSystem" type="system">
 		<hardware>
@@ -185,4 +186,9 @@
 		<disableFixedJointLumping>true</disableFixedJointLumping>
 	</gazebo>
 
+	<xacro:ft_sensor name="FR"/>
+	<xacro:ft_sensor name="FL"/>
+	<xacro:ft_sensor name="RR"/>
+	<xacro:ft_sensor name="RL"/>
+
 </robot>

descriptions/unitree/go2_description/config/robot_control.yaml

@@ -125,6 +125,16 @@ ocs2_quadruped_controller:
       - linear_acceleration.y
       - linear_acceleration.z
 
+    use_odom: True
+    odom_name: "odometer"
+    odom_interfaces:
+      - position.x
+      - position.y
+      - position.z
+      - velocity.x
+      - velocity.y
+      - velocity.z
+
     foot_force_name: "foot_force"
     foot_force_interfaces:
       - FL

descriptions/unitree/go2_description/xacro/ros2_control.xacro

@@ -171,6 +171,15 @@
 			<state_interface name="RL"/>
 		</sensor>
 
+		<sensor name="odometer">
+			<state_interface name="position.x"/>
+			<state_interface name="position.y"/>
+			<state_interface name="position.z"/>
+			<state_interface name="velocity.x"/>
+			<state_interface name="velocity.y"/>
+			<state_interface name="velocity.z"/>
+		</sensor>
+
 	</ros2_control>
 
 </robot>

hardwares/hardware_unitree_mujoco/README.md

@@ -4,6 +4,8 @@ This package contains the hardware interface based on [unitree_sdk2](https://git
 
 In theory, it also can communicate with real robot, but it is not tested yet. You can use go2 simulation in [unitree_mujoco](https://github.com/legubiao/unitree_mujoco). In this simulation, I add foot force sensor support.
 
+*[x] **[2025-01-16]** Add odometer states for simulation. 
+
 ## 1. Interfaces
 
 Required hardware interfaces:

hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h

@@ -10,6 +10,7 @@
 
 #include <unitree/idl/go2/LowState_.hpp>
 #include <unitree/idl/go2/LowCmd_.hpp>
+#include <unitree/idl/go2/SportModeState_.hpp>
 #include <unitree/robot/channel/channel_publisher.hpp>
 #include <unitree/robot/channel/channel_subscriber.hpp>
 
@@ -38,6 +39,7 @@ protected:
 
     std::vector<double> imu_states_;
     std::vector<double> foot_force_;
+    std::vector<double> high_states_;
 
     std::unordered_map<std::string, std::vector<std::string> > joint_interfaces = {
         {"position", {}},
@@ -50,13 +52,17 @@ protected:
 
     void lowStateMessageHandle(const void *messages);
 
+    void highStateMessageHandle(const void *messages);
+
     unitree_go::msg::dds_::LowCmd_ low_cmd_{}; // default init
     unitree_go::msg::dds_::LowState_ low_state_{}; // default init
+    unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init
 
     /*publisher*/
     unitree::robot::ChannelPublisherPtr<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
     /*subscriber*/
     unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::LowState_> lows_tate_subscriber_;
+    unitree::robot::ChannelSubscriberPtr<unitree_go::msg::dds_::SportModeState_> high_state_subscriber_;
 };
 
 

hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp

@@ -8,6 +8,7 @@
 
 #define TOPIC_LOWCMD "rt/lowcmd"
 #define TOPIC_LOWSTATE "rt/lowstate"
+#define TOPIC_HIGHSTATE "rt/sportmodestate"
 
 using namespace unitree::robot;
 using hardware_interface::return_type;
@@ -30,6 +31,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
 
     imu_states_.assign(10, 0);
     foot_force_.assign(4, 0);
+    high_states_.assign(6, 0);
 
     for (const auto &joint: info_.joints) {
         for (const auto &interface: joint.state_interfaces) {
@@ -38,12 +40,12 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
     }
 
     ChannelFactory::Instance()->Init(1, "lo");
+
     low_cmd_publisher_ =
             std::make_shared<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(
                 TOPIC_LOWCMD);
     low_cmd_publisher_->InitChannel();
 
-    /*create subscriber*/
     lows_tate_subscriber_ =
             std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::LowState_> >(
                 TOPIC_LOWSTATE);
@@ -54,6 +56,15 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa
         1);
     initLowCmd();
 
+    high_state_subscriber_ =
+            std::make_shared<ChannelSubscriber<unitree_go::msg::dds_::SportModeState_> >(
+                TOPIC_HIGHSTATE);
+    high_state_subscriber_->InitChannel(
+        [this](auto &&PH1) {
+            highStateMessageHandle(std::forward<decltype(PH1)>(PH1));
+        },
+        1);
+
 
     return SystemInterface::on_init(info);
 }
@@ -83,13 +94,25 @@ std::vector<hardware_interface::StateInterface> HardwareUnitree::export_state_in
     }
 
     // export foot force sensor state interface
+    if (info_.sensors.size() > 1) {
         for (uint i = 0; i < info_.sensors[1].state_interfaces.size(); i++) {
             state_interfaces.emplace_back(
                 info_.sensors[1].name, info_.sensors[1].state_interfaces[i].name, &foot_force_[i]);
         }
+    }
+
+    // export odometer state interface
+    if (info_.sensors.size() > 2) {
+        // export high state interface
+        for (uint i = 0; i < info_.sensors[2].state_interfaces.size(); i++) {
+            state_interfaces.emplace_back(
+                info_.sensors[2].name, info_.sensors[2].state_interfaces[i].name, &high_states_[i]);
+        }
+    }
 
 
-    return state_interfaces;
+    return
+            state_interfaces;
 }
 
 std::vector<hardware_interface::CommandInterface> HardwareUnitree::export_command_interfaces() {
@@ -141,6 +164,17 @@ return_type HardwareUnitree::read(const rclcpp::Time & /*time*/, const rclcpp::D
     foot_force_[2] = low_state_.foot_force()[2];
     foot_force_[3] = low_state_.foot_force()[3];
 
+    // high states
+    high_states_[0] = high_state_.position()[0];
+    high_states_[1] = high_state_.position()[1];
+    high_states_[2] = high_state_.position()[2];
+    high_states_[3] = high_state_.velocity()[0];
+    high_states_[4] = high_state_.velocity()[1];
+    high_states_[5] = high_state_.velocity()[2];
+
+    // RCLCPP_INFO(get_logger(), "high state: %f %f %f %f %f %f", high_states_[0], high_states_[1], high_states_[2],
+    //             high_states_[3], high_states_[4], high_states_[5]);
+
     return return_type::OK;
 }
 
@@ -182,6 +216,10 @@ void HardwareUnitree::lowStateMessageHandle(const void *messages) {
     low_state_ = *static_cast<const unitree_go::msg::dds_::LowState_ *>(messages);
 }
 
+void HardwareUnitree::highStateMessageHandle(const void *messages) {
+    high_state_ = *static_cast<const unitree_go::msg::dds_::SportModeState_ *>(messages);
+}
+
 #include "pluginlib/class_list_macros.hpp"
 
 PLUGINLIB_EXPORT_CLASS(