初始化项目，包含fast_lio_ws和unitree_sdk2

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

@@ -0,0 +1,79 @@
+#pragma once
+
+#include <algorithm>
+#include <cmath>
+#include <cstdint>
+
+#include <eigen3/Eigen/Dense>
+
+#include <unitree/idl/hg/BmsState_.hpp>
+#include <unitree/idl/hg/LowState_.hpp>
+#include <unitree/robot/channel/channel_subscriber.hpp>
+
+namespace unitree {
+namespace robot {
+namespace h2 {
+
+inline bool bad_orientation(const unitree_hg::msg::dds_::LowState_& lowstate,
+                            float limit_angle = 1.0f) {
+  const auto& imu = lowstate.imu_state();
+  Eigen::Quaternionf quat(imu.quaternion()[0], imu.quaternion()[1],
+                          imu.quaternion()[2], imu.quaternion()[3]);
+  const Eigen::Vector3f projected_gravity_b =
+      quat.conjugate() * Eigen::Vector3f(0, 0, -1);
+  return std::fabs(std::acos(-projected_gravity_b[2])) > limit_angle;
+}
+
+inline bool joint_vel_out_of_limit(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_vel = 10.0f) {
+  const 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;
+  });
+}
+
+inline bool ang_vel_out_of_limit(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_vel = 6.0f) {
+  const auto& gyroscope = lowstate.imu_state().gyroscope();
+  return std::any_of(gyroscope.begin(), gyroscope.end(), [limit_vel](float value) {
+    return std::fabs(value) > limit_vel;
+  });
+}
+
+inline bool motor_winding_overheat(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_temp = 120.0f) {
+  const auto& motors = lowstate.motor_state();
+  return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
+    return motor.temperature()[1] > limit_temp;
+  });
+}
+
+inline bool motor_casing_overheat(
+    const unitree_hg::msg::dds_::LowState_& lowstate,
+    float limit_temp = 85.0f) {
+  const auto& motors = lowstate.motor_state();
+  return std::any_of(motors.begin(), motors.end(), [limit_temp](const auto& motor) {
+    return motor.temperature()[0] > limit_temp;
+  });
+}
+
+inline bool low_battery(const unitree_hg::msg::dds_::BmsState_& bms_state,
+                        float limit_soc = 20.0f) {
+  return bms_state.soc() < limit_soc;
+}
+
+inline bool lost_connection(
+    unitree::robot::ChannelSubscriberPtr<unitree_hg::msg::dds_::LowState_>& subscriber,
+    int64_t timeout_ms = 1000) {
+  const auto now = unitree::common::GetCurrentMonotonicTimeNanosecond();
+  const auto elapsed_ms =
+      (now - subscriber->GetLastDataAvailableTime()) / 1e6;
+  return elapsed_ms > timeout_ms;
+}
+
+}  // namespace h2
+}  // namespace robot
+}  // namespace unitree