[feat] grpc通讯错误时增加SPDLOG记录。

saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -129,6 +129,10 @@ class SaturnController {
     long int robot_heartbeat_ = 0;
     float guardian_velocity_decay_ratio_ = 1.0;
 
+    bool is_getting_com_success = true;
+    bool is_getting_imu_success = true;
+    bool is_getting_vf_success = true;
+
     void init();
 
     void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg);

saturn_controller/src/saturn_controller.cpp

@@ -234,7 +234,14 @@ void SaturnController::taskModeCallback(const std_msgs::msg::Bool::SharedPtr tas
 }
 
 void SaturnController::mainTimerCallback() {
-    rcClientInterfaceGetCommonStatus(&common_status_data_);
+    bool result = rcClientInterfaceGetCommonStatus(&common_status_data_);
+    if(is_getting_com_success == true && result == false){
+        is_getting_com_success = false;
+        SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to fail.");
+    } else if(is_getting_com_success == false && result == true){
+        is_getting_com_success = true;
+        SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to succeed.");
+    }
     robot_heartbeat_ = common_status_data_.heartbeat;
     publishRobotState();
     publishDockingState();
@@ -247,8 +254,14 @@ void SaturnController::mainTimerCallback() {
 
 void SaturnController::imuTimerCallback() {
     auto imu_message = sensor_msgs::msg::Imu();
-    rcClientInterfaceGetImuData(&imu_data_);
-
+    bool result = rcClientInterfaceGetImuData(&imu_data_);
+    if(is_getting_imu_success == true && result == false){
+        is_getting_imu_success = false;
+        SPDLOG_INFO("rcClientInterfaceGetImuData starts to fail.");
+    } else if(is_getting_imu_success == false && result == true){
+        is_getting_imu_success = true;
+        SPDLOG_INFO("rcClientInterfaceGetImuData starts to succeed.");
+    }
     imu_message.header.stamp = rclcpp::Clock().now();
     imu_message.header.frame_id = "imu";
 
@@ -277,10 +290,17 @@ void SaturnController::leggedOodmTimerCallback() {
 
     int64_t leg_odom_start_time = std::chrono::time_point_cast<std::chrono::nanoseconds> \
             (std::chrono::high_resolution_clock::now()).time_since_epoch().count();
-    rcClientInterfaceGetVisualFootholdState(&foothold_state_data_);
+    bool result = rcClientInterfaceGetVisualFootholdState(&foothold_state_data_);
     int64_t leg_odom_end_time = std::chrono::time_point_cast<std::chrono::nanoseconds> \
             (std::chrono::high_resolution_clock::now()).time_since_epoch().count();
-    //SPDLOG_INFO("time diff {} ",leg_odom_end_time-leg_odom_start_time);
+
+    if(is_getting_vf_success == true && result == false){
+        is_getting_vf_success = false;
+        SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to fail.");
+    } else if(is_getting_vf_success == false && result == true){
+        is_getting_vf_success = true;
+        SPDLOG_INFO("rcClientInterfaceGetVisualFootholdState starts to succeed.");
+    }
 
     auto odometry_data = nav_msgs::msg::Odometry();
     // odometry_data.header.stamp = rclcpp::Clock().now();