[feat] 增加关节温度上报的topic: joints_state

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -21,6 +21,7 @@
 #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 {
@@ -45,6 +46,7 @@ class SaturnController {
   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
@@ -138,6 +140,9 @@ class SaturnController {
   // 机器人在充电桩充电返回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_;
@@ -150,6 +155,9 @@ class SaturnController {
   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;
 
@@ -205,5 +213,7 @@ class SaturnController {
   void publishRobotMode();
 
   void publishLeggedOdom();
+
+  void publishJointState();
 };
 }  // namespace saturn_ros2

saturn_controller/src/saturn_controller.cpp

@@ -105,6 +105,8 @@ void SaturnController::init() {
       node_->create_publisher<std_msgs::msg::Int32>("robot_mode", 1);
   charging_state_publisher_ =
       node_->create_publisher<std_msgs::msg::Bool>("charge_state", 1);
+  joints_state_publisher_ =
+      node_->create_publisher<JointsState>("joints_state", 1);
 
   // service
   robot_command_service_cgb_ = node_->create_callback_group(
@@ -372,6 +374,7 @@ void SaturnController::mainTimerCallback() {
   publishChargingState();
   publishRobotMode();
   publishLeggedOdom();
+  publishJointState();
 }
 
 void SaturnController::imuTimerCallback() {
@@ -665,5 +668,28 @@ void SaturnController::publishLeggedOdom() {
   legged_odom_publisher_->publish(odometry_data);
 }
 
-}  // namespace saturn_ros2
+void SaturnController::publishJointState() {
+  common::ROBOT_JOINTS_STATUS status;
+  bool success = rcClientInterfaceGetJointsStatus(&status);
+
+  if (!success) {
+    // RCLCPP_ERROR(this->get_logger(), "Failed to get joint status");
+    return;
+  }
+
+  auto msg = JointsState();
+  msg.joint_temperature.resize(MOTOR_NUM);
+  msg.motor_temperature.resize(MOTOR_NUM);
+  msg.driver_cpu_temperature.resize(MOTOR_NUM);
+
+  for (int i = 0; i < MOTOR_NUM; ++i) {
+    msg.joint_temperature[i] = status.joints[i].joint_temperature;
+    msg.motor_temperature[i] = status.joints[i].motor_temperature;
+    msg.driver_cpu_temperature[i] = status.joints[i].driver_cpu_temperature;
+  }
+
+  joints_state_publisher_->publish(msg);
+}
+
+} // namespace saturn_ros2
 RCLCPP_COMPONENTS_REGISTER_NODE(saturn_ros2::SaturnController)

saturn_msgs/CMakeLists.txt

@@ -27,6 +27,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/Scale.msg"
   "msg/Version.msg"
   "msg/CommonStatus.msg"
+  "msg/JointsStatus.msg"
   "srv/DriverEnable.srv"
   "srv/HeightScale.srv"
   "srv/VelocityScale.srv"

saturn_msgs/msg/JointsStatus.msg

@@ -0,0 +1,3 @@
+float32[] joint_temperature
+float32[] motor_temperature
+float32[] driver_cpu_temperature