Browse Source

[fix]增加如果grpc通信不成功,则不发电量topic的逻辑

rundong 8 tháng trước cách đây
mục cha
commit
7b42404e17

+ 2 - 0
saturn_controller/include/saturn_controller/saturn_controller.hpp

@@ -135,6 +135,8 @@ class SaturnController {
 
     int reconnect_count_ = 0;
 
+    bool common_status_result_ = false;
+
     void init();
 
     void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr vel_msg);

+ 4 - 2
saturn_controller/src/saturn_controller.cpp

@@ -265,7 +265,7 @@ void SaturnController::taskModeCallback(const std_msgs::msg::Bool::SharedPtr tas
 }
 
 void SaturnController::mainTimerCallback() {
-    bool result = rcClientInterfaceGetCommonStatus(&common_status_data_);
+    common_status_result_ = rcClientInterfaceGetCommonStatus(&common_status_data_);
     if(is_getting_com_success_ == true && result == false){
         is_getting_com_success_ = false;
         SPDLOG_INFO("rcClientInterfaceGetCommonStatus starts to fail.");
@@ -426,7 +426,9 @@ void SaturnController::publishBatteryState() {  // todo
     }
     auto battery_state_state = sensor_msgs::msg::BatteryState();
     battery_state_state.percentage = battery_power;
-    battery_state_publisher_->publish(battery_state_state);
+    if(common_status_result_ == true){
+        battery_state_publisher_->publish(battery_state_state);
+    }
 }
 
 void SaturnController::publishChargingState() {