saturn_ros2/saturn_controller/include/saturn_controller/saturn_controller.hpp

#include <spdlog/spdlog.h>

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <sensor_msgs/msg/battery_state.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <std_msgs/msg/int32.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/u_int32.hpp>
#include <std_msgs/msg/u_int8.hpp>
#include <std_srvs/srv/set_bool.hpp>

#include "common.h"
#include "config_handler.hpp"
#include "daystar_navigation_msgs/msg/guardian.hpp"
#include "daystar_navigation_msgs/srv/guardian.hpp"
#include "mc_client_interface.h"
#include "saturn_msgs/msg/common_status.hpp"
#include "saturn_msgs/msg/joints_status.hpp"
#include "ysc_robot_msgs/srv/robot_command.hpp"
namespace saturn_ros2 {
enum class Robot_Gait : uint8_t {
  TROT = 0,          // 正常步态
  NORMAL_STAIR = 1,  // 楼梯步态
  SLOPE = 2,         // 斜坡步态
  SENSE_STAIR = 6,   // 感知楼梯步态
};

class SaturnController {
 public:
  //  type alias
  using Twist = geometry_msgs::msg::Twist;
  using Imu = sensor_msgs::msg::Imu;
  using BatteryState = sensor_msgs::msg::BatteryState;
  using Odometry = nav_msgs::msg::Odometry;
  using Bool = std_msgs::msg::Bool;
  using UInt32 = std_msgs::msg::UInt32;
  using Int32 = std_msgs::msg::Int32;
  using UInt8 = std_msgs::msg::UInt8;
  using GuardianMsg = daystar_navigation_msgs::msg::Guardian;
  using SetBool = std_srvs::srv::SetBool;
  using RobotCommand = ysc_robot_msgs::srv::RobotCommand;
  using GuardianSrv = daystar_navigation_msgs::srv::Guardian;
  using JointsState = saturn_msgs::msg::JointsStatus;

  SaturnController(const rclcpp::NodeOptions &options);
  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr
  get_node_base_interface() const;

 private:
  std::shared_ptr<rclcpp::Node> node_;
  std::shared_ptr<ConfigHandler> config_handler_;

  rclcpp::CallbackGroup::SharedPtr main_timer_cbg_;
  rclcpp::TimerBase::SharedPtr main_timer_;

  rclcpp::CallbackGroup::SharedPtr imu_timer_cbg_;
  rclcpp::TimerBase::SharedPtr imu_timer_;

  rclcpp::CallbackGroup::SharedPtr leg_odom_timer_cbg_;
  rclcpp::TimerBase::SharedPtr legged_odom_timer_;

  rclcpp::CallbackGroup::SharedPtr joint_state_timer_cbg_;
  rclcpp::TimerBase::SharedPtr joint_state_timer_;
  // subscription

  // 订阅导航速度指令
  rclcpp::CallbackGroup::SharedPtr cmd_vel_callback_cg_;
  rclcpp::Subscription<Twist>::SharedPtr cmd_vel_sub_;

  // 订阅从navigation发出的停障结果，用于遥控器停障
  rclcpp::CallbackGroup::SharedPtr guardian_callback_cg_;
  rclcpp::Subscription<GuardianMsg>::SharedPtr guardian_sub_;

  // 订阅是否处于sub状态
  rclcpp::CallbackGroup::SharedPtr task_mode_callback_cg_;
  rclcpp::Subscription<Bool>::SharedPtr task_mode_sub_;

  // publisher

  // 六足机器人腿部里程计
  rclcpp::Publisher<Odometry>::SharedPtr legged_odom_publisher_;

  // 六足机器人IMU数据
  rclcpp::Publisher<Imu>::SharedPtr imu_publisher_;

  // 机器人基本状态
  enum class ROBOT_STATE : uint32_t {
    LIE_DOWN = 0,
    STANDING_UP = 1,
    STAND_UP = 2,
    ENABLE_FORCECONTROL = 3,
    STEPPING = 4,
    SITTING_DOWN = 5,
    FALL_DOWN = 6,
  };
  rclcpp::Publisher<UInt32>::SharedPtr robot_state_publisher_;

  // dock_state(std_msgs::msg::UInt32): 机器人上桩状态
  enum class DOCK_STATE : uint32_t {
    IDLE = 0,
    GOING_TO_DOCK = 1,
    STND_ABOVE_DOCK = 2,
    ADJUST_POSE = 3,
    LIE_ON_DOCKER = 4,
    STANDING_AFTER_CHARGING = 5,
    LEAVING_DOCK = 6,
    LEAVE_DOCK = 7,
  };
  rclcpp::Publisher<UInt32>::SharedPtr docking_state_publisher_;

  enum class CONTROL_MODE : uint8_t {
    JOY_CONTROL = 0,   //  0 -> 手持遥控
    NAVI_CONTROL = 1,  //  1 -> 导航
    NULL_MODE = 2,
  };
  rclcpp::Publisher<UInt8>::SharedPtr control_mode_publisher_;

  // 设置六足机器人场景
  //  0 -> null
  //  1 -> 趴下
  //  2 -> 行走步态
  //  3 -> 楼梯步态
  //  4 -> 充电
  rclcpp::Publisher<UInt8>::SharedPtr gait_publisher_;

  // 电池电量信息,目前只有percentage字段有意义
  rclcpp::Publisher<BatteryState>::SharedPtr battery_state_publisher_;

  // 机器人模式，用于向巡检平台上报
  //  0 ->空闲（默认）
  //  1 ->手持遥控
  //  2 ->任务
  //  3 ->充电中
  rclcpp::Publisher<Int32>::SharedPtr robot_mode_publisher_;

  // 机器人在充电桩充电返回true，否则返回false
  rclcpp::Publisher<Bool>::SharedPtr charging_state_publisher_;

  // Joint statue publisher
  rclcpp::Publisher<JointsState>::SharedPtr joints_state_publisher_;

  // services
  rclcpp::CallbackGroup::SharedPtr robot_command_service_cgb_;
  rclcpp::Service<RobotCommand>::SharedPtr robot_command_service_;

  // 用于导航停障中的服务接口
  rclcpp::CallbackGroup::SharedPtr guardian_service_cgb_;
  rclcpp::Service<GuardianSrv>::SharedPtr guardian_service_;

  robot_control::common::MC_MOTION_IMU_DATA imu_data_;
  robot_control::common::JETSON_VISUAL_FOOTHOLD_STATE foothold_state_data_;
  robot_control::common::ROBOT_COMMON_STATUS common_status_data_;

  // Joint state data
  robot_control::common::ROBOT_JOINTS_STATUS  joints_state_data_;

  bool is_robot_in_idle_state_ = true;
  long int robot_heartbeat_ = 0;

  // 确保线程安全
  std::atomic<float> guardian_velocity_decay_ratio_;

  bool is_getting_com_success_ = true;
  bool is_getting_imu_success_ = true;
  bool is_getting_vf_success_ = true;

  int reconnect_count_ = 0;

  bool common_status_result_ = false;

  void init();

  void cmdVelCallback(const Twist::ConstSharedPtr vel_msg);

  void robotCommandCallback(const RobotCommand::Request::SharedPtr request,
                            RobotCommand::Response::SharedPtr response);

  bool goToDock();

  bool leaveDock();

  bool waitForDockFinishing(bool is_on_dock, size_t time_out_s);

  void guardianCallback(const GuardianMsg::ConstSharedPtr guardian_msg);

  void guardianServiceCallback(const GuardianSrv::Request::SharedPtr request,
                               GuardianSrv::Response::SharedPtr response);

  void taskModeCallback(const Bool::SharedPtr task_mode);

  void mainTimerCallback();

  void imuTimerCallback();

  void leggedOodmTimerCallback();

  void publishRobotState();

  void publishDockingState();

  void publishControlMode();

  void publishGait();

  void publishBatteryState();

  void publishChargingState();

  void publishRobotMode();

  void publishLeggedOdom();

  void publishJointState();
};
}  // namespace saturn_ros2