[feat]在发现grpc通讯持续失败时，加入断联机制。

.vscode/settings.json

@@ -1,5 +1,50 @@
 {
     "files.associations": {
-        "functional": "cpp"
+        "functional": "cpp",
+        "array": "cpp",
+        "atomic": "cpp",
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstddef": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "deque": "cpp",
+        "unordered_map": "cpp",
+        "vector": "cpp",
+        "exception": "cpp",
+        "algorithm": "cpp",
+        "iterator": "cpp",
+        "memory": "cpp",
+        "memory_resource": "cpp",
+        "numeric": "cpp",
+        "optional": "cpp",
+        "random": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "system_error": "cpp",
+        "tuple": "cpp",
+        "type_traits": "cpp",
+        "utility": "cpp",
+        "fstream": "cpp",
+        "initializer_list": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp",
+        "bit": "cpp"
     }
 }

saturn_controller/CMakeLists.txt

@@ -62,7 +62,6 @@ install(DIRECTORY
 )
 
 install(DIRECTORY
-  config
   launch
   DESTINATION share/${PROJECT_NAME}/)
 

saturn_controller/config/config.yaml

@@ -1,6 +0,0 @@
-saturn_controller:
-  ros__parameters:
-    motion_computer_ip_address: 127.0.0.1
-    main_timer_frequency: 50
-    imu_timer_frequency: 300
-    legged_odom_timer_frequency: 100

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -133,6 +133,8 @@ class SaturnController {
     bool is_getting_imu_success = true;
     bool is_getting_vf_success = true;
 
+    int reconnect_count = 0;
+
     void init();
 
     void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg);

saturn_controller/src/config_handler.cpp

@@ -1,4 +1,9 @@
 #include "saturn_controller/config_handler.hpp"
+#include <spdlog/spdlog.h>
+#include <spdlog/async.h>
+#include <spdlog/sinks/rotating_file_sink.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include <spdlog/spdlog.h>
 
 namespace saturn_ros2 {
 ConfigHandler::ConfigHandler(std::shared_ptr<rclcpp::Node> node)
@@ -10,8 +15,8 @@ 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);                                                     
-    legged_odom_timer_frequency_ = node_->declare_parameter("legged_odom_timer_frequency", 100.0);
+    main_timer_frequency_ = node_->declare_parameter("main_timer_frequency", 1.0);                                                      
+    imu_timer_frequency_ = node_->declare_parameter("imu_timer_frequency", 1.0);                                                     
+    legged_odom_timer_frequency_ = node_->declare_parameter("legged_odom_timer_frequency", 1.0);
 }
 }

saturn_controller/src/saturn_controller.cpp

@@ -208,8 +208,6 @@ void SaturnController::robotCommandCallback(
 
 void SaturnController::guardianCallback(
     const daystar_navigation_msgs::msg::Guardian::SharedPtr guardian_msg) {
-    //SPDLOG_INFO("guardianCallback {} velocity_decay_ratio {}",
-    //            guardian_msg->trigger_source.c_str(), guardian_msg->velocity_decay_ratio);
     float velocity_decay_ratio = guardian_msg->velocity_decay_ratio;
     std::string trigger_source = guardian_msg->trigger_source;
     rcClientInterfaceSetGuardian(robot_control::common::NAV_OR_JOY_MODE::nav_control,
@@ -238,10 +236,26 @@ void SaturnController::mainTimerCallback() {
     if(is_getting_com_success == true && result == false){
         is_getting_com_success = false;
         SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to fail.");
+
+        //重新init
+        if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
+            RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+                        config_handler_->motion_computer_ip_address_.c_str());
+        }
+
     } else if(is_getting_com_success == false && result == true){
         is_getting_com_success = true;
         SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to succeed.");
+    }  else if(is_getting_com_success == false && result == false){
+        reconnect_count ++;
+        if(reconnect_count % 500 == 0) {
+            if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
+                SPDLOG_INFO(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+                            config_handler_->motion_computer_ip_address_.c_str());
+            }
+        }
     }
+
     robot_heartbeat_ = common_status_data_.heartbeat;
     publishRobotState();
     publishDockingState();
@@ -258,10 +272,27 @@ void SaturnController::imuTimerCallback() {
     if(is_getting_imu_success == true && result == false){
         is_getting_imu_success = false;
         SPDLOG_INFO("rcClientInterfaceGetImuData starts to fail.");
+
+        //重新init
+        if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
+            RCLCPP_ERROR(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+                        config_handler_->motion_computer_ip_address_.c_str());
+        }
+
     } else if(is_getting_imu_success == false && result == true){
         is_getting_imu_success = true;
         SPDLOG_INFO("rcClientInterfaceGetImuData starts to succeed.");
+    }  else if(is_getting_imu_success == false && result == false){
+        reconnect_count ++;
+        if(reconnect_count % 500 == 0) {
+            if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
+                SPDLOG_INFO(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+                            config_handler_->motion_computer_ip_address_.c_str());
+            }
+        }
     }
+
+    
     imu_message.header.stamp = rclcpp::Clock().now();
     imu_message.header.frame_id = "imu";
 
@@ -296,10 +327,19 @@ void SaturnController::leggedOodmTimerCallback() {
 
     if(is_getting_vf_success == true && result == false){
         is_getting_vf_success = false;
-        SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to fail.");
+        SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to fail. Tried to reconnect.");
+
     } else if(is_getting_vf_success == false && result == true){
         is_getting_vf_success = true;
         SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to succeed.");
+    } else if(is_getting_vf_success == false && result == false){
+        reconnect_count ++;
+        if(reconnect_count % 500 == 0) {
+            if (rcClientInterfaceInit(config_handler_->motion_computer_ip_address_) == false) {
+                SPDLOG_INFO(node_->get_logger(), "rcClientInterfaceInit failed with %s",
+                            config_handler_->motion_computer_ip_address_.c_str());
+            }
+        }
     }
 
     auto odometry_data = nav_msgs::msg::Odometry();