[refactor] 将joint_state上报频率调整至5hz

saturn_controller/include/saturn_controller/config_handler.hpp

@@ -15,7 +15,8 @@ class ConfigHandler {
     float main_timer_frequency_;
     float imu_timer_frequency_;
     float legged_odom_timer_frequency_;
-    
+    float joints_state_timer_frequency_;
+
    private:
     std::shared_ptr<rclcpp::Node> node_;
 

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -65,6 +65,8 @@ class SaturnController {
   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
 
   // 订阅导航速度指令

saturn_controller/src/config_handler.cpp

@@ -12,11 +12,12 @@ ConfigHandler::ConfigHandler(std::shared_ptr<rclcpp::Node> node)
 }
 
 void ConfigHandler::loadConfig() {
-    // 声明参数                                                                
+    // 声明参数
     motion_computer_ip_address_ = node_->declare_parameter("motion_computer_ip_address", "127.0.0.1");
-                                                                               
-    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 50.0);                                                      
-    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 300.0);                                                     
+
+    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 50.0);
+    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 300.0);
     legged_odom_timer_frequency_ = node_->declare_parameter("legged_odom_timer_frequency", 100.0);
+    joints_state_timer_frequency_ = node_->declare_parameter("joints_state_timer_frequency", 5.0);
 }
 }

saturn_controller/src/saturn_controller.cpp

@@ -47,6 +47,13 @@ void SaturnController::init() {
           (int)(1e9 / config_handler_->imu_timer_frequency_)),
       std::bind(&SaturnController::imuTimerCallback, this), imu_timer_cbg_);
 
+  joint_state_timer_cbg_ = node_->create_callback_group(
+      rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  joint_state_timer_ = node_->create_wall_timer(
+      std::chrono::nanoseconds(
+          (int)(1e9 / config_handler_->joints_state_timer_frequency_)),
+      std::bind(&SaturnController::publishJointState, this), joint_state_timer_cbg_);
   // subscription
   cmd_vel_callback_cg_ = node_->create_callback_group(
       rclcpp::CallbackGroupType::MutuallyExclusive);
@@ -374,7 +381,6 @@ void SaturnController::mainTimerCallback() {
   publishChargingState();
   publishRobotMode();
   publishLeggedOdom();
-  publishJointState();
 }
 
 void SaturnController::imuTimerCallback() {