初始化项目，包含fast_lio_ws和unitree_sdk2

unitree_sdk2/include/unitree/robot/g1/agv/g1_agv_api.hpp

@@ -0,0 +1,60 @@
+#ifndef __UT_ROBOT_G1_AGV_API_HPP__
+#define __UT_ROBOT_G1_AGV_API_HPP__
+
+#include <unitree/common/json/jsonize.hpp>
+
+using namespace unitree::common;
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+/*service name*/
+const std::string AGV_SERVICE_NAME = "agv";
+
+/*api version*/
+const std::string AGV_API_VERSION = "1.0.0.1";
+
+/*api id*/
+const int32_t ROBOT_API_ID_AGV_MOVE = 1001;
+const int32_t ROBOT_API_ID_AGV_HEIGHT_ADJUST = 1002;
+
+
+class MoveParameter : public Jsonize
+{
+public:
+    MoveParameter() :
+        vx(0.0), vy(0.0), vyaw(0.0)
+    {}
+
+    ~MoveParameter()
+    {}
+
+public:
+    void fromJson(JsonMap& json)
+    {
+        FromJson(json["vx"], vx);
+        FromJson(json["vy"], vy);
+        FromJson(json["vyaw"], vyaw);
+    }
+
+    void toJson(JsonMap& json) const
+    {
+        ToJson(vx, json["vx"]);
+        ToJson(vy, json["vy"]);
+        ToJson(vyaw, json["vyaw"]);
+    }
+
+public:
+    float vx;
+    float vy;
+    float vyaw;
+};
+
+}   //namespace agv
+}   //namespace robot
+}   //namespace unitree
+
+#endif  // __UT_ROBOT_G1_AGV_API_HPP__

unitree_sdk2/include/unitree/robot/g1/agv/g1_agv_client.hpp

@@ -0,0 +1,109 @@
+#ifndef __UT_ROBOT_G1_AGV_CLIENT_HPP__
+#define __UT_ROBOT_G1_AGV_CLIENT_HPP__
+
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+#include "g1_agv_api.hpp"
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+
+
+class AgvClient : public Client {
+public:
+    AgvClient() : Client(AGV_SERVICE_NAME, false) {}
+    ~AgvClient() {}
+
+    /*Init*/
+    void Init() {
+        SetApiVersion(AGV_API_VERSION);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AGV_MOVE);
+        UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AGV_HEIGHT_ADJUST);
+    };
+
+    /*API Call*/
+    /**
+    * @brief Linear and angular velocity control interface
+    * 
+    * Mechanical Parameters:
+    *   - Max Linear Velocity: 1.5 m/s
+    *   - Max Angular Velocity: 0.6 rad/s
+    * 
+    * Control Mapping:
+    *   - vx: linear velocity in x direction
+    *       Range: [-1.5, 1.5] m/s
+    *       Positive value: move forward
+    *       Negative value: move backward
+    *       Zero: stop linear motion
+    *   - vy: lateral velocity in y direction (not supported for AGV, ignored)
+    *   - vyaw: angular velocity around z axis (rotation)
+    *       Range: [-0.6, 0.6] rad/s
+    *       Positive value: rotate counter-clockwise
+    *       Negative value: rotate clockwise
+    *       Zero: stop rotation
+    * 
+    * @param vx Linear velocity in x direction, unit: m/s, range: [-1.5, 1.5]
+    * @param vy Lateral velocity in y direction (not used for AGV, pass 0.0)
+    * @param vyaw Angular velocity around z axis, unit: rad/s, range: [-0.6, 0.6]
+    * @return 0 on success, -1 on failure (not initialized or communication error)
+    * 
+    * @note Call Init() before using this function
+    * @note This function is non-blocking and returns immediately after sending the command
+    * @example
+    *   ac.Move(0.5f, 0.0f, 0.3f);  // Move forward at 0.5 m/s and rotate at 0.3 rad/s
+    */
+    int32_t Move(float vx, float vy, float vyaw) {
+        MoveParameter param;
+
+        param.vx = vx;
+        param.vy = vy;
+        param.vyaw = vyaw;
+
+        std::string parameter = common::ToJsonString(param);
+        std::string data;
+
+        return Call(ROBOT_API_ID_AGV_MOVE, parameter, data);
+    }
+
+    /**
+    * @brief Height column velocity control interface
+    * 
+    * Mechanical Parameters:
+    *   - Max Column Velocity: 76.5 mm/s (0.0765 m/s)
+    * 
+    * Normalized Linear Mapping (vz value → column velocity):
+    *   The input vz is normalized [-1.0, +1.0] and linearly mapped to column velocity.
+    *   Formula: column_velocity(mm/s) = vz × 76.5   
+    *       vz = +1.0  →  +76.5 mm/s  (max rise)
+    *       vz =  0.0  →  0 mm/s      (stop)
+    *       vz = -1.0  →  -76.5 mm/s  (max descent)
+    * 
+     * Linear Mapping Benefits:
+    *   - Intuitive control: proportional input produces proportional output
+    *   - Predictable behavior: 0.5 input always gives exactly 50% of max velocity
+    *   - Hardware independent: easy to adapt when motor parameters change
+    *   - Simple conversion: column_velocity = normalized_input × max_velocity_constant
+    
+    * @param vz Height velocity command (range: [-1.0 , +1.0])
+    * @return 0 on success, -1 on failure
+    * 
+    * @note Only z component is used, transmitted via DDS protocol to motor controller
+    */
+    int32_t HeightAdjust(float vz) {
+        std::string parameter, data;
+
+        go2::JsonizeDataFloat json;
+        json.data = vz;
+
+        parameter = common::ToJsonString(json);
+
+        return Call(ROBOT_API_ID_AGV_HEIGHT_ADJUST, parameter, data);
+    }
+};
+}  // namespace g1
+
+}  // namespace robot
+}  // namespace unitree
+
+#endif // __UT_ROBOT_G1_AGV_CLIENT_HPP__

unitree_sdk2/include/unitree/robot/g1/agv/g1_agv_error.hpp

@@ -0,0 +1,21 @@
+#ifndef __UT_ROBOT_G1_AGV_ERROR_HPP__
+#define __UT_ROBOT_G1_AGV_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree
+{
+namespace robot
+{
+namespace g1
+{
+
+UT_DECL_ERR(UT_ROBOT_G1_AGV_ERR_NOT_INIT,   9101,   "Module not initialized.")
+UT_DECL_ERR(UT_ROBOT_G1_AGV_ERR_EXEC_MOVE,   9102,   "Failed to execute move command.")
+UT_DECL_ERR(UT_ROBOT_G1_AGV_ERR_EXEC_HEIGHT_ADJUST,   9103,   "Failed to execute height adjust command.")
+
+}
+}
+}
+
+#endif // __UT_ROBOT_G1_AGV_ERROR_HPP__

unitree_sdk2/include/unitree/robot/g1/arm/g1_arm_action_api.hpp

@@ -0,0 +1,23 @@
+#pragma once
+
+#include <unitree/common/json/jsonize.hpp>
+#include <variant>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+/*service name*/
+const std::string ARM_ACTION_SERVICE_NAME = "arm";
+
+/*api version*/
+const std::string ARM_ACTION_API_VERSION = "1.0.0.14";
+
+/*api id*/
+const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION = 7106;
+const int32_t ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST = 7107;
+const int32_t ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION = 7108;
+const int32_t ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION = 7113;
+
+}  // namespace g1
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/g1/arm/g1_arm_action_client.hpp

@@ -0,0 +1,108 @@
+#pragma once
+
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+#include "g1_arm_action_api.hpp"
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+
+  /**
+   * @brief Arm action client
+   * 
+   * The arm action server provides some upper body actions.
+   * The controller is based on the `rt/arm_sdk` interface.
+   * 
+   * The current state of the arm will be published on the `rt/arm/action/state` topic:
+   * {
+   *   "holding": false,  # Whether to hold the position after the action ends; will release after a maximum of 20 seconds
+   *   "id": 99,          # Current action ID
+   *   "name": "release_arm" # Current action name
+   * }
+   */
+class G1ArmActionClient : public Client {
+  public:
+    G1ArmActionClient() : Client(ARM_ACTION_SERVICE_NAME, false) {}
+    ~G1ArmActionClient() {}
+  
+    /*Init*/
+    void Init() {
+      SetApiVersion(ARM_ACTION_API_VERSION);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION);
+      UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION);
+  }
+  
+  /*API Call*/
+  /**
+   * @brief Execute Unitree's internal preset actions
+   * Indexed by ID, where 99 is to release the action.
+   * Some actions will hold the position at the last keyframe after completion; 
+   * you can send id = 99 or the same id to release.
+   * 
+   * Attention:
+   *   Some actions will not be displayed on the APP, 
+   *   but can be executed by the program.
+   *   These actions may cause the robot to fall,
+   *   so please execute them with caution.
+   */
+  int32_t ExecuteAction(int32_t action_id) {
+    std::string parameter, data;
+    parameter = R"({"action_id":)" + std::to_string(action_id) + R"(})";
+    return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_ACTION, parameter, data);
+  }
+
+  /**
+   * @brief Execute custom teach actions, indexed by name
+   * 
+   * # rt/arm/action/state
+   * {
+   *   "holding": false,  # teach actions do not include holding
+   *   "id": 100,         # Always 100, the action is determined by the name
+   *   "name": "sth"      # Current teach action name
+   * }
+   */
+  int32_t ExecuteAction(const std::string &action_name) {
+    std::string parameter, data;
+    parameter = R"({"action_name":")" + action_name + R"("})";
+    return Call(ROBOT_API_ID_ARM_ACTION_EXECUTE_CUSTOM_ACTION, parameter, data);
+  }
+
+  /* Stop Custom Action */
+  int32_t StopCustomAction() {
+    std::string parameter, data;
+    return Call(ROBOT_API_ID_ARM_ACTION_STOP_CUSTOM_ACTION, parameter, data);
+  }
+
+  int32_t GetActionList(std::string &data) {
+    std::string parameter;
+    return Call(ROBOT_API_ID_ARM_ACTION_GET_ACTION_LIST, parameter, data);
+  }
+
+  /*Action List*/
+  std::map<std::string, int32_t> action_map = {
+    {"release arm", 99},
+    {"two-hand kiss", 11},
+    {"left kiss", 12},
+    {"right kiss", 12},
+    {"hands up", 15},
+    {"clap", 17},
+    {"high five", 18},
+    {"hug", 19},
+    {"heart", 20},
+    {"right heart", 21},
+    {"reject", 22},
+    {"right hand up", 23},
+    {"x-ray", 24},
+    {"face wave", 25},
+    {"high wave", 26},
+    {"shake hand", 27},
+  };
+};
+
+
+} // namespace g1
+} // namespace robot
+} // namespace unitree 

unitree_sdk2/include/unitree/robot/g1/arm/g1_arm_action_error.hpp

@@ -0,0 +1,20 @@
+#pragma once
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_ARMSDK, 7400, "The topic rt/armsdk is occupied.")
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_HOLDING, 7401, "The arm is holding. Expecting release action(99) or the same last action id.")
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_INVALID_ACTION_ID, 7402, "Invalid action id.")
+// The actions are only supported in fsm id {500, 501, 801};
+// You can subscribe the topic rt/sportmodestate to check the fsm id.
+// And in the state 801, the actions are only supported in the fsm mode {0, 3}.
+// See https://support.unitree.com/home/en/G1_developer/sport_services_interface#Expert%20interface
+UT_DECL_ERR(UT_ROBOT_ARM_ACTION_ERR_INVALID_FSM_ID, 7404, "Invalid fsm id.")
+
+}  // namespace g1
+}  // namespace robot
+}  // namespace unitree

unitree_sdk2/include/unitree/robot/g1/audio/g1_audio_api.hpp

@@ -0,0 +1,94 @@
+#ifndef __UT_ROBOT_G1_AUDIO_API_HPP__
+#define __UT_ROBOT_G1_AUDIO_API_HPP__
+
+#include <unitree/common/json/jsonize.hpp>
+// #include <variant>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+/*service name*/
+const std::string AUDIO_SERVICE_NAME = "voice";
+
+/*api version*/
+const std::string AUDIO_API_VERSION = "1.0.0.0";
+
+/*api id*/
+const int32_t ROBOT_API_ID_AUDIO_TTS = 1001;
+const int32_t ROBOT_API_ID_AUDIO_ASR = 1002;
+const int32_t ROBOT_API_ID_AUDIO_START_PLAY = 1003;
+const int32_t ROBOT_API_ID_AUDIO_STOP_PLAY = 1004;
+const int32_t ROBOT_API_ID_AUDIO_GET_VOLUME = 1005;
+const int32_t ROBOT_API_ID_AUDIO_SET_VOLUME = 1006;
+const int32_t ROBOT_API_ID_AUDIO_SET_RGB_LED = 1010;
+
+class TtsMakerParameter : public common::Jsonize {
+ public:
+  TtsMakerParameter() {}
+  ~TtsMakerParameter() {}
+
+  void fromJson(common::JsonMap &json) {}
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(index, json["index"]);
+    common::ToJson(speaker_id, json["speaker_id"]);
+    common::ToJson(text, json["text"]);
+  }
+
+  int32_t index = 0;
+  uint16_t speaker_id = 0;
+  std::string text;
+};
+
+class PlayStreamParameter : public common::Jsonize {
+ public:
+  PlayStreamParameter() {}
+  ~PlayStreamParameter() {}
+
+  void fromJson(common::JsonMap &json) {}
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(app_name, json["app_name"]);
+    common::ToJson(stream_id, json["stream_id"]);
+  }
+
+  std::string app_name;
+  std::string stream_id;
+};
+
+class PlayStopParameter : public common::Jsonize {
+ public:
+  PlayStopParameter() {}
+  ~PlayStopParameter() {}
+
+  void fromJson(common::JsonMap &json) {}
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(app_name, json["app_name"]);
+  }
+
+  std::string app_name;
+};
+
+class LedControlParameter : public common::Jsonize {
+ public:
+  LedControlParameter() {}
+  ~LedControlParameter() {}
+
+  void fromJson(common::JsonMap &json) {}
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(R, json["R"]);
+    common::ToJson(G, json["G"]);
+    common::ToJson(B, json["B"]);
+  }
+
+  uint8_t R;
+  uint8_t G;
+  uint8_t B;
+};
+}  // namespace g1
+}  // namespace robot
+}  // namespace unitree
+
+#endif  // __UT_ROBOT_G1_AUDIO_API_HPP__

unitree_sdk2/include/unitree/robot/g1/audio/g1_audio_client.hpp

@@ -0,0 +1,110 @@
+#ifndef __UT_ROBOT_G1_AUDIO_CLIENT_HPP__
+#define __UT_ROBOT_G1_AUDIO_CLIENT_HPP__
+
+#include <limits>
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+
+#include "g1_audio_api.hpp"
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+class AudioClient : public Client {
+ public:
+  AudioClient() : Client(AUDIO_SERVICE_NAME, false) {}
+  ~AudioClient() {}
+
+  /*Init*/
+  void Init() {
+    SetApiVersion(AUDIO_API_VERSION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_TTS);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_ASR);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_START_PLAY);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_STOP_PLAY);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_GET_VOLUME);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_SET_VOLUME);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_AUDIO_SET_RGB_LED);
+  };
+
+  /*API Call*/
+  int32_t TtsMaker(const std::string& text, int32_t speaker_id) {
+    std::string parameter, data;
+    TtsMakerParameter json;
+
+    json.index = tts_index++;
+    json.text = text;
+    json.speaker_id = speaker_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_AUDIO_TTS, parameter, data);
+  }
+
+  int32_t GetVolume(uint8_t& volume) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_AUDIO_GET_VOLUME, parameter, data);
+    if (ret == 0) {
+      unitree::robot::go2::JsonizeCommObjInt json;
+      json.name = "volume";
+      common::FromJsonString(data, json);
+      volume = json.value;
+    }
+
+    return ret;
+  }
+
+  int32_t SetVolume(uint8_t volume) {
+    std::string parameter, data;
+    unitree::robot::go2::JsonizeCommObjInt json;
+
+    json.value = volume;
+    json.name = "volume";
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_AUDIO_SET_VOLUME, parameter, data);
+  }
+
+  int32_t PlayStream(std::string app_name, std::string stream_id,
+                     std::vector<uint8_t> pcm_data) {
+    std::string parameter;
+    PlayStreamParameter json;
+
+    json.app_name = app_name;
+    json.stream_id = stream_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_AUDIO_START_PLAY, parameter, pcm_data);
+  }
+
+  int32_t PlayStop(std::string app_name) {
+    std::string parameter, data;
+    PlayStopParameter json;
+
+    json.app_name = app_name;
+    parameter = common::ToJsonString(json);
+
+    Call(ROBOT_API_ID_AUDIO_STOP_PLAY, parameter, data);
+    return 0;
+  }
+
+  int32_t LedControl(uint8_t R, uint8_t G, uint8_t B) {
+    std::string parameter, data;
+    LedControlParameter json;
+
+    json.R = R;
+    json.G = G;
+    json.B = B;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_AUDIO_SET_RGB_LED, parameter, data);
+  }
+
+ private:
+  uint32_t tts_index = 0;
+};
+}  // namespace g1
+
+}  // namespace robot
+}  // namespace unitree
+#endif  // __UT_ROBOT_G1_AUDIO_CLIENT_HPP__

unitree_sdk2/include/unitree/robot/g1/audio/g1_audio_error.hpp

@@ -0,0 +1,14 @@
+#ifndef __UT_ROBOT_G1_AUDIO_ERROR_HPP__
+#define __UT_ROBOT_G1_AUDIO_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+UT_DECL_ERR(UT_ROBOT_AUDIO_ERR_COMM, 100, "Invalid parameter ")
+}  // namespace g1
+}  // namespace robot
+}  // namespace unitree
+
+#endif  // __UT_ROBOT_G1_AUDIO_ERROR_HPP__

unitree_sdk2/include/unitree/robot/g1/common/terminations.hpp

@@ -0,0 +1,90 @@
+#pragma once
+
+/**
+ * This file implements some functions used to protect the G1 during runtime.
+ * When the function returns true, it is recommended to set the motor to passive mode in the lower-level control.
+ */
+
+#include <eigen3/Eigen/Dense>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/idl/hg/BmsState_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {  
+
+// robot orientation is too far from the desired orientation limits.
+inline bool bad_orientation(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_angle = 1.0)
+{
+    auto & imu = lowstate.imu_state();
+    Eigen::Quaternionf quat(
+        imu.quaternion()[0],
+        imu.quaternion()[1],
+        imu.quaternion()[2],
+        imu.quaternion()[3]
+    );
+    Eigen::Vector3f projected_gravity_b = quat.conjugate() * Eigen::Vector3f(0, 0, -1);
+    return std::fabs(std::acos(-projected_gravity_b[2])) > limit_angle;
+}
+
+// Joint velocities are outside of the soft joint limits
+inline bool joint_vel_out_of_limit(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_vel = 10.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_vel](const auto & motor) {
+        return std::fabs(motor.dq()) > limit_vel;
+    });
+}
+
+// Agnular velocities are outside of the soft joint limits
+inline bool ang_vel_out_of_limit(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_vel = 6.0)
+{
+    auto & gyroscope = lowstate.imu_state().gyroscope();
+    return std::any_of(gyroscope.begin(), gyroscope.end(), [limit_vel](const auto & ang_vel) {
+        return std::fabs(ang_vel) > limit_vel;
+    });
+}
+
+// Motor winding temperature is above the limit
+inline bool motor_winding_overheat(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_temp = 120.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto & motor) {
+        return motor.temperature()[1] > limit_temp;
+    });
+}
+
+// Motor casing temperature is above the limit
+inline bool motor_casing_overheat(const unitree_hg::msg::dds_::LowState_ & lowstate, float limit_temp = 85.0)
+{
+    auto & motors = lowstate.motor_state();
+    return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto & motor) {
+        return motor.temperature()[0] > limit_temp;
+    });
+}
+
+// State of charge (SOC) is below the limit 
+inline bool low_battery(const  unitree_hg::msg::dds_::BmsState_ & bms_state, float limit_soc = 20.0)
+{
+    return bms_state.soc() < limit_soc;
+}
+
+/**
+ * @brief Lost connection to the robot
+ * This function checks if the last data available time is older than the specified timeout.
+ * If the timeout is reached, it indicates a lost connection.
+ * 
+ * When using a wired connection to the robot, a loose network cable may cause the connection to be interrupted.
+ * If the program continues to run at this time, it will send a step signal to the motors, causing violent movement.
+ */
+inline bool lost_connection(unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_> & subscriber, int64_t timeout_ms = 1000)
+{
+    auto now = unitree::common::GetCurrentMonotonicTimeNanosecond();
+    auto elasped_ms = (now - subscriber->GetLastDataAvailableTime()) / 1e6;
+    return elasped_ms > timeout_ms;
+}
+
+}
+}
+}

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_api.hpp

@@ -0,0 +1,80 @@
+#ifndef __UT_ROBOT_G1_LOCO_API_HPP__
+#define __UT_ROBOT_G1_LOCO_API_HPP__
+
+#include <unitree/common/json/jsonize.hpp>
+#include <variant>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+/*service name*/
+const std::string LOCO_SERVICE_NAME = "sport";
+
+/*api version*/
+const std::string LOCO_API_VERSION = "1.0.0.0";
+
+/*api id*/
+const int32_t ROBOT_API_ID_LOCO_GET_FSM_ID = 7001;
+const int32_t ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002;
+const int32_t ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003;
+const int32_t ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004;
+const int32_t ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005;
+const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 7006; // deprecated
+
+const int32_t ROBOT_API_ID_LOCO_FSM_API = 7100;
+const int32_t ROBOT_API_ID_LOCO_SET_FSM_ID = 7101;
+const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102;
+const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103;
+const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104;
+const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 7105;
+const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106;
+const int32_t ROBOT_API_ID_LOCO_SET_SPEED_MODE = 7107;
+const int32_t ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL = 7110;
+const int32_t ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL = 7111;
+
+
+using LocoCmd =
+    std::map<std::string, std::variant<int, float, std::vector<float>>>;
+
+enum class InternalFsmMode{
+  LAST, PASSIVE, WALKRUN
+};
+
+class JsonizeDataVecFloat : public common::Jsonize {
+public:
+  JsonizeDataVecFloat() {}
+  ~JsonizeDataVecFloat() {}
+
+  void fromJson(common::JsonMap &json) { common::FromJson(json["data"], data); }
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(data, json["data"]);
+  }
+
+  std::vector<float> data;
+};
+
+class JsonizeVelocityCommand : public common::Jsonize {
+public:
+  JsonizeVelocityCommand() {}
+  ~JsonizeVelocityCommand() {}
+
+  void fromJson(common::JsonMap &json) {
+    common::FromJson(json["velocity"], velocity);
+    common::FromJson(json["duration"], duration);
+  }
+
+  void toJson(common::JsonMap &json) const {
+    common::ToJson(velocity, json["velocity"]);
+    common::ToJson(duration, json["duration"]);
+  }
+
+  std::vector<float> velocity;
+  float duration;
+};
+
+} // namespace g1
+} // namespace robot
+} // namespace unitree
+
+#endif // __UT_ROBOT_G1_LOCO_API_HPP__

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_client.hpp

@@ -0,0 +1,315 @@
+#ifndef __UT_ROBOT_G1_LOCO_CLIENT_HPP__
+#define __UT_ROBOT_G1_LOCO_CLIENT_HPP__
+
+#include "g1_loco_api.hpp"
+#include <limits>
+#include <unitree/robot/client/client.hpp>
+#include <unitree/robot/go2/public/jsonize_type.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+class LocoClient : public Client {
+ public:
+  LocoClient() : Client(LOCO_SERVICE_NAME, false) {}
+  ~LocoClient() {}
+
+  /*Init*/
+  void Init() {
+    SetApiVersion(LOCO_API_VERSION);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_FSM_ID);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_FSM_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_BALANCE_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_GET_PHASE);  // deprecated
+
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_FSM_API);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_FSM_ID);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_BALANCE_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_VELOCITY);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_ARM_TASK);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SET_SPEED_MODE);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL);
+    UT_ROBOT_CLIENT_REG_API_NO_PROI(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL);
+  };
+
+  /*Low Level API Call*/
+  int32_t GetFsmId(int& fsm_id) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_FSM_ID, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      fsm_id = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetFsmMode(int& fsm_mode) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_FSM_MODE, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      fsm_mode = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetBalanceMode(int& balance_mode) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_BALANCE_MODE, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataInt json;
+      common::FromJsonString(data, json);
+      balance_mode = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetSwingHeight(float& swing_height) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_SWING_HEIGHT, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataFloat json;
+      common::FromJsonString(data, json);
+      swing_height = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetStandHeight(float& stand_height) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_STAND_HEIGHT, parameter, data);
+
+    if (ret == 0) {
+      go2::JsonizeDataFloat json;
+      common::FromJsonString(data, json);
+      stand_height = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t GetPhase(std::vector<float>& phase) {
+    std::string parameter, data;
+
+    int32_t ret = Call(ROBOT_API_ID_LOCO_GET_PHASE, parameter, data);
+
+    if (ret == 0) {
+      JsonizeDataVecFloat json;
+      common::FromJsonString(data, json);
+      phase = json.data;
+    }
+
+    return ret;
+  }
+
+  int32_t SetFsmId(int fsm_id) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = fsm_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_FSM_ID, parameter, data);
+  }
+
+  int32_t SetBalanceMode(int balance_mode) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = balance_mode;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_BALANCE_MODE, parameter, data);
+  }
+
+  int32_t SetSwingHeight(float swing_height) {
+    std::string parameter, data;
+
+    go2::JsonizeDataFloat json;
+    json.data = swing_height;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_SWING_HEIGHT, parameter, data);
+  }
+
+  int32_t SetStandHeight(float stand_height) {
+    std::string parameter, data;
+
+    go2::JsonizeDataFloat json;
+    json.data = stand_height;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_STAND_HEIGHT, parameter, data);
+  }
+
+  int32_t SetVelocity(float vx, float vy, float omega, float duration = 1.f) {
+    std::string parameter, data;
+
+    JsonizeVelocityCommand json;
+    std::vector<float> velocity = {vx, vy, omega};
+    json.velocity = velocity;
+    json.duration = duration;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_VELOCITY, parameter, data);
+  }
+
+  int32_t SetTaskId(int task_id) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = task_id;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_ARM_TASK, parameter, data);
+  }
+
+  int32_t SwitchToUserCtrl() {
+    std::string parameter, data;
+
+    go2::JsonizeDataBool json;
+    parameter = common::ToJsonString(json);
+    return Call(ROBOT_API_ID_LOCO_SWITCH_TO_USER_CTRL, parameter, data);
+  }
+
+  int32_t SwitchToInternalCtrl(InternalFsmMode mode) {
+    std::string parameter, data;
+    go2::JsonizeDataInt json;
+    switch (mode)
+    {
+    case InternalFsmMode::LAST:
+      json.data = 0;
+      break;
+    case InternalFsmMode::PASSIVE:
+      json.data = 1;
+      break;
+    case InternalFsmMode::WALKRUN:
+      json.data = 2;
+      break;
+    }
+    parameter = common::ToJsonString(json);
+    return Call(ROBOT_API_ID_LOCO_SWITCH_TO_INTERNAL_CTRL, parameter, data);
+  }
+
+  /*High Level API Call*/
+  int32_t Damp() { return SetFsmId(1); }
+
+  int32_t Start() { return SetFsmId(500); }
+
+  int32_t Squat() { return SetFsmId(2); }
+
+  int32_t Sit() { return SetFsmId(3); }
+
+  int32_t StandUp() { return SetFsmId(4); }
+
+  int32_t ZeroTorque() { return SetFsmId(0); }
+
+  int32_t StopMove() { return SetVelocity(0.f, 0.f, 0.f); }
+
+  int32_t HighStand() { return SetStandHeight(std::numeric_limits<uint32_t>::max()); }
+
+  int32_t LowStand() { return SetStandHeight(std::numeric_limits<uint32_t>::min()); }
+
+  int32_t Move(float vx, float vy, float vyaw, bool continous_move) {
+    return SetVelocity(vx, vy, vyaw, continous_move ? 864000.f : 1.f);
+  }
+
+  int32_t Move(float vx, float vy, float vyaw) { return Move(vx, vy, vyaw, continous_move_); }
+
+  int32_t BalanceStand() { return SetBalanceMode(0); }
+
+  int32_t ContinuousGait(bool flag) { return SetBalanceMode(flag ? 1 : 0); }
+
+  int32_t SwitchMoveMode(bool flag) {
+    continous_move_ = flag;
+    return 0;
+  }
+
+  int32_t WaveHand(bool turn_flag = false) { return SetTaskId(turn_flag ? 1 : 0); }
+
+  int32_t ShakeHand(int stage = -1) {
+    switch (stage) {
+      case 0:
+        first_shake_hand_stage_ = false;
+        return SetTaskId(2);
+
+      case 1:
+        first_shake_hand_stage_ = true;
+        return SetTaskId(3);
+
+      default:
+        first_shake_hand_stage_ = !first_shake_hand_stage_;
+        return SetTaskId(first_shake_hand_stage_ ? 3 : 2);
+    }
+  }
+
+  int32_t SetSpeedMode(int speed_mode) {
+    std::string parameter, data;
+
+    go2::JsonizeDataInt json;
+    json.data = speed_mode;
+    parameter = common::ToJsonString(json);
+
+    return Call(ROBOT_API_ID_LOCO_SET_SPEED_MODE, parameter, data);
+  }
+
+  /**
+   * @brief Get available mimic motions from the robot.
+   *
+   * @param[out] data Response data containing available motions.
+   *
+   * @return Response data format:
+   * {
+   *   "fsm_id": 550,
+   *   "api_id": 1,
+   *   "motion": {
+   *     "507": { # motion_api_id
+   *       "duration": float,
+   *       "name": string
+   *     },
+   *     ...
+   *   }
+   * }
+   *
+   * @note Use set_fsm_id to invoke specific motion_api_id with prefix 550.
+   *       Example: --set_fsm_id 550507
+   * 
+   * @attention The Robot must be in a stable state (e.g., standing);
+   */
+  int32_t GetMimicMotion(std::string& data) {
+    std::string parameter = "{\"fsm_id\": 550, \"api_id\": 1}";
+    return _fsm_api(parameter, data);
+  }
+
+  int32_t _fsm_api(std::string parameter, std::string& data) {
+    return Call(ROBOT_API_ID_LOCO_FSM_API, parameter, data);
+  }
+
+private:
+  bool continous_move_ = false;
+  bool first_shake_hand_stage_ = true;
+};
+} // namespace g1
+
+} // namespace robot
+} // namespace unitree
+#endif // __UT_ROBOT_G1_LOCO_CLIENT_HPP__

unitree_sdk2/include/unitree/robot/g1/loco/g1_loco_error.hpp

@@ -0,0 +1,17 @@
+#ifndef __UT_ROBOT_G1_LOCO_ERROR_HPP__
+#define __UT_ROBOT_G1_LOCO_ERROR_HPP__
+
+#include <unitree/common/decl.hpp>
+
+namespace unitree {
+namespace robot {
+namespace g1 {
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_LOCOSTATE_NOT_AVAILABLE, 7301,
+            "LocoState not available.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_FSM_ID, 7302, "Invalid fsm id.")
+UT_DECL_ERR(UT_ROBOT_LOCO_ERR_INVALID_TASK_ID, 7303, "Invalid task id.")
+} // namespace g1
+} // namespace robot
+} // namespace unitree
+
+#endif // __UT_ROBOT_G1_LOCO_ERROR_HPP__