Merge pull request #1 from MiRoboticsLab/v1.2.0.0

V1.2.0.0

Author: ruheng

cyberdog_manager/CMakeLists.txt

@@ -15,6 +15,7 @@ endif()
 
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rclcpp_action REQUIRED)
 find_package(manager_base REQUIRED)
 find_package(protocol REQUIRED)
 find_package(cyberdog_system REQUIRED)
@@ -35,6 +36,7 @@ add_executable(${PROJECT_NAME} src/cyberdog_manager.cpp src/main.cpp)
 
 ament_target_dependencies( ${PROJECT_NAME}
   rclcpp
+  rclcpp_action
   protocol
   manager_base
   cyberdog_system

cyberdog_manager/include/cyberdog_manager/audio_info.hpp

@@ -94,18 +94,6 @@ class AudioInfoNode final
     auto mcpim_iter = module_code_play_id_map.find(code);
     if (mcpim_iter != module_code_play_id_map.end()) {
       uint16_t play_id = mcpim_iter->second;
-      bool is_self_check = ((code % 10) == 9) ? true : false;
-      if (is_self_check) {
-        std::chrono::seconds timeout(2);
-        auto req = std::make_shared<protocol::srv::SdcardPlayIdQuery::Request>();
-        req->play_id = play_id;
-        auto future_result = sdcard_playid_query_client_->async_send_request(req);
-        std::future_status status = future_result.wait_for(timeout);
-        if (status == std::future_status::ready) {
-        } else {
-          play_id = protocol::msg::AudioPlay::PID_SELF_CHECK_FAILED;
-        }
-      }
       std::chrono::seconds timeout(6);
       auto req = std::make_shared<protocol::srv::AudioTextPlay::Request>();
       req->module_name = audio_info_node_->get_name();
@@ -140,6 +128,8 @@ class AudioInfoNode final
     {1609, 31013},    // Uwb自检失败
     {5109, 31014},    // Audio自检失败
     {3009, 31015},    // Motion自检失败
+    // {4409, 31056},    // Realsense自检失败
+    {5300, 31058},    // algorithm加载完成
     {2403, 31016},    // GPS切换工作模式失败
     {2103, 31017},    // 雷达切换工作模式失败
     {2203, 31018},    // TOF切换工作模式失败

cyberdog_manager/include/cyberdog_manager/battery_capacity_info.hpp

@@ -59,7 +59,7 @@ class BatteryCapacityInfoNode final
   {
     bc_callback_group_ =
       battery_capacity_info_node_->create_callback_group(
-      rclcpp::CallbackGroupType::MutuallyExclusive);
+      rclcpp::CallbackGroupType::Reentrant);
     rclcpp::SubscriptionOptions sub_options;
     sub_options.callback_group = bc_callback_group_;
     bms_status_sub_ = battery_capacity_info_node_->create_subscription<protocol::msg::BmsStatus>(
@@ -94,51 +94,60 @@ class BatteryCapacityInfoNode final
         if (!is_soc_zero_) {
           is_soc_zero_ = true;
           // is_soc_five_ = false;
-          PlayAudioService("电量为0,关机中!");
+          PlayAudioService(protocol::msg::AudioPlay::PID_PERCENT_0);
         }
-      } else if (bms_status_.batt_soc <= 5) {
+      } else if (bms_status_.batt_soc < 5) {
         if (!is_soc_five_) {
           is_soc_five_ = true;
           is_soc_twenty_ = false;
-          PlayAudio("电量低于5%，电池即将耗尽，请尽快充电!");
+          PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_5);
         }
-      } else if (bms_status_.batt_soc <= 20) {
+      } else if (bms_status_.batt_soc < 20) {
         if (!is_soc_twenty_) {
           is_soc_twenty_ = true;
           is_soc_five_ = false;
           is_soc_thirty_ = false;
-          PlayAudio("电量低于20%，部分功能受限!");
+          PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_20);
         }
-      } else if (bms_status_.batt_soc <= 30) {
+      } else if (bms_status_.batt_soc < 30) {
         if (!is_soc_thirty_) {
           is_soc_thirty_ = true;
           is_soc_twenty_ = false;
-          PlayAudio("电量低于30%，请尽快充电!");
+          PlayAudio(protocol::msg::AudioPlay::PID_PERCENT_30);
         }
       }
     } else {
       is_soc_twenty_ = false;
       is_soc_thirty_ = false;
       is_soc_five_ = false;
     }
+
+    // 状态机切换过程中，不再重复请求切换
+    if (!ms_switch_mutex_.try_lock()) {
+      WARN_MILLSECONDS(
+        5000, "The machine_state is switching..."
+      );
+      return;
+    }
     batsoc_notify_handler(bms_status_.batt_soc, bms_status_.power_wired_charging);
+    ms_switch_mutex_.unlock();
   }
 
-  void PlayAudio(const std::string & text)
+  void PlayAudio(const uint16_t play_id)
   {
     protocol::msg::AudioPlayExtend msg;
-    msg.is_online = true;
+    msg.is_online = false;
     msg.module_name = battery_capacity_info_node_->get_name();
-    msg.text = text;
+    msg.speech.play_id = play_id;
     audio_play_extend_pub->publish(msg);
   }
 
-  void PlayAudioService(const std::string & text)
+  void PlayAudioService(const uint16_t play_id)
   {
     auto request_audio = std::make_shared<protocol::srv::AudioTextPlay::Request>();
     request_audio->module_name = battery_capacity_info_node_->get_name();
-    request_audio->is_online = true;
-    request_audio->text = text;
+    request_audio->is_online = false;
+    request_audio->speech.play_id = play_id;
     auto callback = [](rclcpp::Client<protocol::srv::AudioTextPlay>::SharedFuture future) {
         INFO("Audio play result: %s", future.get()->status == 0 ? "success" : "failed");
       };
@@ -168,6 +177,7 @@ class BatteryCapacityInfoNode final
   protocol::msg::BmsStatus bms_status_;
   BCINSOC_CALLBACK batsoc_notify_handler;
   BCINBMS_CALLBACK bms_notify_handler;
+  std::mutex ms_switch_mutex_;
   bool is_soc_zero_;
   bool is_soc_five_;
   bool is_soc_twenty_;

cyberdog_manager/include/cyberdog_manager/error_context.hpp

@@ -88,7 +88,7 @@ class ErrorContext final
   }
   void Init()
   {
-    INFO("arror context thread started.");
+    INFO("error context thread started.");
     thread_ = std::thread(
       [this]() {
         while (rclcpp::ok() && !exit_) {

cyberdog_manager/include/cyberdog_manager/led_info.hpp

@@ -57,65 +57,112 @@ class LedInfoNode final
 
   void BmsStatus(const protocol::msg::BmsStatus::SharedPtr msg)
   {
+    power_charging_ = msg->power_wired_charging;
     battery_soc_ = msg->batt_soc;
-    bool power_wired_charging = msg->power_wired_charging;
-    static bool is_set_led_zero = false;
-    static bool is_set_led_twenty = false;
-    static bool is_set_led_eigthy = false;
-    static bool is_set_led_more_eigthy = false;
-
-    if (battery_soc_ <= 0) {
-      if (!power_wired_charging && !is_set_led_zero) {
-        is_set_led_zero = true;
+    static int start_battery_soc = battery_soc_;
+    int charging_status_switch = false;
+
+    // 充电状态切换flag
+    if (pre_charging_status_ != power_charging_) {
+      charging_status_switch = true;
+    }
+    // 非充电状态，Bms释放灯效
+    if (battery_soc_ > 20) {
+      if (!power_charging_ && !bms_occupied_led_) {
+        bms_occupied_led_ = true;
+        LedMode head{false, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32};
+        LedMode tail{false, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32};
+        LedMode mini{false, "bms", 3, 0x02, 0x30, 0xFF, 0x32, 0x32};
+        bool result = ReqLedService(head, tail, mini);
+        INFO("Bms %s to release led occupation ", result ? "successed" : "failed");
+      }
+      if (power_charging_) {
+        bms_occupied_led_ = false;
+      }
+    }
+
+    if ((pre_battery_soc_ == 1 && battery_soc_ == 0) || start_battery_soc == 0) {
+      if (!power_charging_) {
         LedMode poweroff_head{true, "bms", 1, 0x01, 0xA3, 0x00, 0x00, 0x00};
         LedMode poweroff_tail{true, "bms", 2, 0x01, 0xA3, 0x00, 0x00, 0x00};
-        LedMode poweroff_mini{true, "bms", 3, 0x02, 0x31, 0xFF, 0x32, 0x32};
+        LedMode poweroff_mini{true, "bms", 3, 0x02, 0x31, 0xFF, 0x00, 0x00};
         bool result = ReqLedService(poweroff_head, poweroff_tail, poweroff_mini);
         INFO("%s set led when the soc is 0", result ? "successed" : "failed");
       }
-    } else if (battery_soc_ <= 20) {
-      if (!is_set_led_twenty) {
-        is_set_led_twenty = true;
-        is_set_led_eigthy = false;
-        LedMode bringup_head{true, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32};
-        LedMode bringup_tail{true, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32};
-        LedMode bringup_mini{true, "bms", 3, 0x02, 0x30, 0xFF, 0x32, 0x32};
-        bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini);
+    }
+
+    if ((pre_battery_soc_ == 21 && battery_soc_ == 20) ||
+      (start_battery_soc > 0 && start_battery_soc <= 20) ||
+      (charging_status_switch && battery_soc_ <= 20))
+    {
+      if (power_charging_) {
+        LedMode head{true, "bms", 1, 0x02, 0x06, 0xFF, 0x32, 0x32};
+        LedMode tail{true, "bms", 2, 0x02, 0x06, 0xFF, 0x32, 0x32};
+        LedMode mini{true, "bms", 3, 0x02, 0x30, 0xFF, 0x00, 0x00};
+        bool result = ReqLedService(head, tail, mini);
+        INFO("%s set the charging led when the soc less than 20", result ? "successed" : "failed");
+      } else {
+        LedMode head{true, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32};
+        LedMode tail{true, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32};
+        LedMode mini{true, "bms", 3, 0x02, 0x30, 0xFF, 0x00, 0x00};
+        bool result = ReqLedService(head, tail, mini);
         INFO("%s set led when the soc less than 20", result ? "successed" : "failed");
       }
-    } else if (battery_soc_ <= 80) {
-      if (!is_set_led_eigthy) {
-        is_set_led_eigthy = true;
-        is_set_led_twenty = false;
-        is_set_led_more_eigthy = false;
+    }
+
+    if ((pre_battery_soc_ == 20 && battery_soc_ == 21) ||
+      (pre_battery_soc_ == 80 && battery_soc_ == 79) ||
+      (start_battery_soc > 20 && start_battery_soc < 80) ||
+      (charging_status_switch && battery_soc_ > 20 && battery_soc_ < 80))
+    {
+      if (power_charging_) {
+        LedMode head{true, "bms", 1, 0x02, 0x06, 0x75, 0xFC, 0xF6};
+        LedMode tail{true, "bms", 2, 0x02, 0x06, 0x75, 0xFC, 0xF6};
+        LedMode mini{true, "bms", 3, 0x02, 0x30, 0x75, 0xFC, 0xF6};
+        bool result = ReqLedService(head, tail, mini);
+        INFO(
+          "%s set the charging less, the soc more than 20 and less than 80 with charing",
+          result ? "successed" : "failed");
+      } else {
         // 释放Bms灯效
         LedMode bringup_head{false, "bms", 1, 0x02, 0x09, 0xFF, 0x32, 0x32};
         LedMode bringup_tail{false, "bms", 2, 0x02, 0x09, 0xFF, 0x32, 0x32};
         LedMode bringup_mini{false, "bms", 3, 0x02, 0x30, 0xFF, 0x32, 0x32};
         bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini);
-        INFO("%s set led when the soc less than 20", result ? "successed" : "failed");
-
-        // 更改系统灯效
-        LedMode sys_bringup_head{true, "system", 1, 0x02, 0x09, 0x75, 0xFC, 0xF6};
-        LedMode sys_bringup_tail{true, "system", 2, 0x02, 0x09, 0x75, 0xFC, 0xF6};
-        LedMode sys_bringup_mini{true, "system", 3, 0x02, 0x30, 0x75, 0xFC, 0xF6};
-        bool result2 = ReqLedService(sys_bringup_head, sys_bringup_tail, sys_bringup_mini);
-        INFO(
-          "%s set sys led, the soc more than 20 an less than 80",
-          result2 ? "successed" : "failed");
+        INFO("bms %s release led when the soc less than 80", result ? "successed" : "failed");
       }
-    } else {
-      if (!is_set_led_more_eigthy) {
-        is_set_led_more_eigthy = true;
-        is_set_led_eigthy = false;
-        // 更改系统灯效
-        LedMode bringup_head{true, "system", 1, 0x02, 0x09, 0x06, 0x21, 0xE2};
-        LedMode bringup_tail{true, "system", 2, 0x02, 0x09, 0x06, 0x21, 0xE2};
-        LedMode bringup_mini{true, "system", 3, 0x02, 0x30, 0x06, 0x21, 0xE2};
+      // 更改系统灯效
+      LedMode sys_bringup_head{true, "system", 1, 0x02, 0x09, 0x75, 0xFC, 0xF6};
+      LedMode sys_bringup_tail{true, "system", 2, 0x02, 0x09, 0x75, 0xFC, 0xF6};
+      LedMode sys_bringup_mini{true, "system", 3, 0x02, 0x30, 0x75, 0xFC, 0xF6};
+      bool result2 = ReqLedService(sys_bringup_head, sys_bringup_tail, sys_bringup_mini);
+      INFO(
+        "%s set sys led, the soc more than 20 an less than 80",
+        result2 ? "successed" : "failed");
+    }
+
+    if ((pre_battery_soc_ == 79 && battery_soc_ == 80) ||
+      start_battery_soc >= 80 || (charging_status_switch && battery_soc_ >= 80))
+    {
+      // 更改系统灯效
+      LedMode bringup_head{true, "system", 1, 0x02, 0x09, 0x06, 0x21, 0xE2};
+      LedMode bringup_tail{true, "system", 2, 0x02, 0x09, 0x06, 0x21, 0xE2};
+      LedMode bringup_mini{true, "system", 3, 0x02, 0x30, 0x06, 0x21, 0xE2};
+      bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini);
+      INFO("%s set sys led, the soc more than 80", result ? "successed" : "failed");
+      if (power_charging_) {
+        LedMode bringup_head{true, "bms", 1, 0x02, 0x06, 0x06, 0x21, 0xE2};
+        LedMode bringup_tail{true, "bms", 2, 0x02, 0x06, 0x06, 0x21, 0xE2};
+        LedMode bringup_mini{true, "bms", 3, 0x02, 0x30, 0x06, 0x21, 0xE2};
         bool result = ReqLedService(bringup_head, bringup_tail, bringup_mini);
-        INFO("%s set sys led, the soc more than 80", result ? "successed" : "failed");
+        INFO(
+          "%s set the charging, the soc more than 80 with charing",
+          result ? "successed" : "failed");
       }
     }
+    start_battery_soc = -1;
+    pre_battery_soc_ = battery_soc_;
+    pre_charging_status_ = power_charging_;
   }
 
 private:
@@ -208,7 +255,10 @@ class LedInfoNode final
   rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr wake_up_sub_ {nullptr};
   rclcpp::Client<protocol::srv::LedExecute>::SharedPtr led_excute_client_ {nullptr};
   uint8_t battery_soc_ {100};
-  bool is_lowpower_ {false};
+  uint8_t pre_battery_soc_ {0};
+  bool power_charging_ {false};
+  bool pre_charging_status_ {false};
+  bool bms_occupied_led_ {false};
 };
 }  // namespace manager
 }  // namespace cyberdog