Merge pull request #5 from MiRoboticsLab/v1.3.0.0

V1.3.0.0

motion_action/include/motion_action/motion_macros.hpp

@@ -58,6 +58,13 @@ enum class MotionCode : int32_t
   kEstop = 40,
   kStuck = 41,
   // kBusy = 42
+  kMotionSwitchOriErr = 50,
+  kMotionSwitchFootPosErr = 51,
+  kMotionSwitchStandStuck = 52,
+  kMotionSwitchMotorOverHeat = 53,
+  kMotionSwitchMotorOverCurr = 54,
+  kMotionSwitchMotorErr = 55,
+  kMotionSwitchCharging = 56,
 };  // enum class MotionCode
 
 using MotionServoCmdMsg = protocol::msg::MotionServoCmd;

motion_action/preset/motion_id_map.toml

@@ -36,7 +36,7 @@ motion_id = 111
 map  = [12, 0]   
 pre_motion = []
 post_motion = [0, 12, 7, 21, 3, 6, 11, 62, 5, 64, 16, 17, 23, 9, 80, 81, 82]
-min_exec_time = 9000
+min_exec_time = 15000
 
 # 行走后站立， duration必须为0
 [[motion_ids]]

motion_bridge/src/motor_bridge.cpp

@@ -29,7 +29,7 @@ MotorBridge::MotorBridge(const rclcpp::Node::SharedPtr node)
     std::bind(
       &MotorBridge::HandleMotorTempCallback,
       this, std::placeholders::_1, std::placeholders::_2));
-  lcm_subscribe_instance_ = std::make_shared<lcm::LCM>(kLCMActionSubscibeURL);
+  lcm_subscribe_instance_ = std::make_shared<lcm::LCM>(kLCMBirdgeSubscribeURL);
   lcm_subscribe_instance_->subscribe(kLCMBridgeMotorChannel, &MotorBridge::ReadLcm, this);
   lcm_handle_thread_ =
     std::thread(

motion_manager/include/motion_manager/motion_decision.hpp

@@ -138,10 +138,9 @@ class MotionDecision final
     if (std::find(report_error_code_.begin(), report_error_code_.end(), error_code) ==
       report_error_code_.end())
     {
-      INFO("not error code");
       return false;
     } else {
-      INFO("error code");
+      INFO("motion error code %d", error_code);
       return true;
     }
   }

motion_manager/include/motion_manager/motion_manager.hpp

@@ -111,10 +111,10 @@ class MotionManager final : public machine::MachineActuator
       ERROR("Cannot get response from AudioPlay");
     }
   }
-  bool TryGetDownOnFsm()
+  bool TryGetDownOnFsm(bool passive_getdown = false)
   {
     auto request = std::make_shared<MotionResultSrv::Request>();
-    request->motion_id = MotionIDMsg::GETDOWN;
+    request->motion_id = passive_getdown ? 102 : MotionIDMsg::GETDOWN;
     request->cmd_source = MotionResultSrv::Request::FSM;
     auto response = std::make_shared<MotionResultSrv::Response>();
     decision_ptr_->DecideResultCmd(request, response);
@@ -149,6 +149,7 @@ class MotionManager final : public machine::MachineActuator
   std::mutex msg_mutex_;
   int32_t code_;
   int32_t motion_id_;
+  bool isreporting_{false};
 };  // class MotionManager
 }  // namespace motion
 }  // namespace cyberdog

motion_manager/src/motion_decision.cpp

@@ -214,12 +214,18 @@ void MotionDecision::ReportErrorCode(int32_t & error_code, int32_t & motion_id)
   switch (report_response) {
     case 0:
       INFO("save logs and report abnormal events");
+      break;
     case 1:
       INFO("failed to save log");
+      break;
     case 2:
       INFO("failed to report abnormal events");
+      break;
     case 3:
       INFO("failed to save log and report abnormal events");
+      break;
+    default:
+      break;
   }
 }
 

motion_manager/src/motion_handler.cpp

@@ -304,15 +304,81 @@ bool MotionHandler::CheckMotionResult(int32_t & code)
   if (!CheckMotors(code)) {
     return false;
   }
-  if (motion_status_ptr_->ori_error != 0 ||
-    // TODO(harvey): footpos_error需要等到运控组确定策略后再加进来
-    //  motion_status_ptr_->footpos_error == 0 &&
-    (motion_status_ptr_->switch_status != MotionStatusMsg::NORMAL &&
-    motion_status_ptr_->switch_status != MotionStatusMsg::TRANSITIONING))
-  {
-    ERROR("Motion ori error or switch error");
-    code = code_ptr_->GetCode(MotionCode::kMotionExecuteError);
-    return false;
+  // if (motion_status_ptr_->ori_error != 0 ||
+  //   // TODO(harvey): footpos_error需要等到运控组确定策略后再加进来
+  //   //  motion_status_ptr_->footpos_error == 0 &&
+  //   (motion_status_ptr_->switch_status != MotionStatusMsg::NORMAL &&
+  //   motion_status_ptr_->switch_status != MotionStatusMsg::TRANSITIONING))
+  // {
+  //   ERROR("Motion ori error or switch error");
+  //   code = code_ptr_->GetCode(MotionCode::kMotionExecuteError);
+  //   return false;
+  // }
+  switch (motion_status_ptr_->switch_status) {
+    case MotionStatusMsg::BAN_TRANS:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchBantrans);
+      ERROR("Motion result BAN_TRANS");
+      return false;
+
+    case MotionStatusMsg::EDAMP:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchEdamp);
+      ERROR("Motion result EDAMP");
+      return false;
+
+    case MotionStatusMsg::ESTOP:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchEstop);
+      ERROR("Motion result ESTOP");
+      return false;
+
+    case MotionStatusMsg::LIFTED:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchLifted);
+      ERROR("Motion result LIFTED");
+      return false;
+
+    case MotionStatusMsg::OVER_HEAT:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchOverHeat);
+      ERROR("Motion result OVER_HEAT");
+      return false;
+
+    case MotionStatusMsg::LOW_BAT:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchLowBat);
+      ERROR("Motion result LOW_BAT");
+      return false;
+
+    case MotionStatusMsg::ORI_ERR:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchOriErr);
+      ERROR("Motion result ORI_ERR");
+      return false;
+
+    case MotionStatusMsg::FOOTPOS_ERR:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchFootPosErr);
+      ERROR("Motion result FOOTPOS_ERR");
+      return false;
+
+    case MotionStatusMsg::STAND_STUCK:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchStandStuck);
+      ERROR("Motion result STAND_STUCK");
+      return false;
+
+    case MotionStatusMsg::MOTOR_OVER_HEAT:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorOverHeat);
+      ERROR("Motion result MOTOR_OVER_HEAT");
+      return false;
+
+    case MotionStatusMsg::MOTOR_OVER_CURR:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorOverCurr);
+      ERROR("Motion result MOTOR_OVER_CURR");
+      return false;
+
+    case MotionStatusMsg::MOTOR_ERR:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorErr);
+      ERROR("Motion result MOTOR_ERR");
+      return false;
+
+    case MotionStatusMsg::CHARGING:
+      code = code_ptr_->GetCode(MotionCode::kMotionSwitchCharging);
+      ERROR("Motion result CHARGING");
+      return false;
   }
   return true;
 }
@@ -420,6 +486,55 @@ void MotionHandler::ExecuteResultCmd(const CmdRequestT request, CmdResponseT res
       ERROR("Motion switch LOW_BAT");
       return;
 
+    case MotionStatusMsg::ORI_ERR:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchOriErr);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch ORI_ERR");
+      return;
+
+    case MotionStatusMsg::FOOTPOS_ERR:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchFootPosErr);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch FOOTPOS_ERR");
+      return;
+
+    case MotionStatusMsg::STAND_STUCK:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchStandStuck);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch STAND_STUCK");
+      return;
+
+    case MotionStatusMsg::MOTOR_OVER_HEAT:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorOverHeat);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch MOTOR_OVER_HEAT");
+      return;
+
+    case MotionStatusMsg::MOTOR_OVER_CURR:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorOverCurr);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch MOTOR_OVER_CURR");
+      return;
+
+    case MotionStatusMsg::MOTOR_ERR:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchMotorErr);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch MOTOR_ERR");
+      return;
+
+    case MotionStatusMsg::CHARGING:
+      response->code = code_ptr_->GetCode(MotionCode::kMotionSwitchCharging);
+      response->result = false;
+      response->motion_id = motion_status_ptr_->motion_id;
+      ERROR("Motion switch CHARGING");
+      return;
+
     case MotionStatusMsg::TRANSITIONING:
       is_transitioning_wait_ = true;
       WARN("Transitioning waiting");
@@ -477,7 +592,7 @@ void MotionHandler::ExecuteResultCmd(const CmdRequestT request, CmdResponseT res
   if (min_exec_time > 0) {
     wait_timeout = min_exec_time;
   } else if (min_exec_time < 0) {  // 增量力控、增量位控、绝对位控、行走duration必须大于0
-    wait_timeout = request->duration;
+    wait_timeout = request->duration + 500;
   } else {                         // 自定义动作按照设定的参数计算
     wait_timeout = request->duration == 0 ? 500 : request->duration * 2;
   }
@@ -524,7 +639,7 @@ void MotionHandler::HandleResultCmd(const CmdRequestT request, CmdResponseT resp
     request->motion_id == MotionIDMsg::GETDOWN);
   estop_ = request->motion_id == MotionIDMsg::ESTOP;
   INFO("estop: %d", estop_);
-  if (DanceInteruption && is_execute_wait_) {
+  if ((DanceInteruption || estop_) && is_execute_wait_) {
     execute_cv_.notify_all();
   }
   if (GetWorkStatus() != HandlerStatus::kIdle &&

motion_manager/src/motion_manager.cpp

@@ -154,7 +154,7 @@ int32_t MotionManager::OnTearDown()
   if (decision_ptr_->GetMotionID() != MotionIDMsg::ESTOP &&
     decision_ptr_->GetMotionID() != MotionIDMsg::GETDOWN)
   {
-    while (!TryGetDownOnFsm() && rclcpp::ok()) {
+    while (!TryGetDownOnFsm(true) && rclcpp::ok()) {
       INFO("Error when GetDown on TearDown, Will retry");
       std::this_thread::sleep_for(std::chrono::milliseconds(500));
     }
@@ -205,9 +205,17 @@ int32_t MotionManager::OnLowPower()
   if (decision_ptr_->GetMotionID() != MotionIDMsg::ESTOP &&
     decision_ptr_->GetMotionID() != MotionIDMsg::GETDOWN)
   {
+    uint8_t retry_num = 3;
+    uint8_t retry_count = 0;
     while (!TryGetDownOnFsm() && rclcpp::ok()) {
-      INFO("Error when GetDown on LowPower, Will retry");
-      std::this_thread::sleep_for(std::chrono::milliseconds(500));
+      retry_count++;
+      if (retry_count < retry_num) {
+        INFO("Error when GetDown on LowPower, Will retry");
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+      } else {
+        ERROR("Error 3 times when GetDown on LowPower, Will Abort");
+        break;
+      }
     }
   } else {
     INFO("Estop or Getdown, will do nothing when into LowPower");
@@ -262,6 +270,7 @@ void MotionManager::MotionResultCmdCallback(
   bool protected_cmd =
     (request->motion_id != MotionIDMsg::RECOVERYSTAND &&
     request->motion_id != MotionIDMsg::GETDOWN &&
+    request->motion_id != 102 &&
     request->motion_id != MotionIDMsg::ESTOP &&
     request->motion_id != MotionIDMsg::POSECONTROL_RELATIVEYLY);
   if (!IsStateValid(code, protected_cmd)) {
@@ -275,12 +284,17 @@ void MotionManager::MotionResultCmdCallback(
   }
 
   decision_ptr_->DecideResultCmd(request, response);
-  if (decision_ptr_->IsErrorCode(response->code)) {
+  if (decision_ptr_->IsErrorCode(response->code) && !isreporting_) {
     std::unique_lock<std::mutex> lock(msg_mutex_);
     code_ = response->code;
     motion_id_ = response->motion_id;
     msg_condition_.notify_one();
   }
+  if (request->motion_id == MotionIDMsg::ESTOP) {
+    code_ = 3088;
+    motion_id_ = request->motion_id;
+    msg_condition_.notify_one();
+  }
   INFO("Will return MotionResultCmdCallback");
 }
 
@@ -341,7 +355,7 @@ void MotionManager::MotionSequenceShowCmdCallback(
   // }
   // decision_ptr_->SetSequnceTotalDuration(total_duration);
   decision_ptr_->DecideResultCmd(request, response);
-  if (decision_ptr_->IsErrorCode(response->code)) {
+  if (decision_ptr_->IsErrorCode(response->code) && !isreporting_) {
     std::unique_lock<std::mutex> lock(msg_mutex_);
     code_ = response->code;
     motion_id_ = response->motion_id;
@@ -398,10 +412,11 @@ void MotionManager::MotionQueueCmdCallback(
 void MotionManager::Report()
 {
   while (true) {
+    isreporting_ = false;
     std::unique_lock<std::mutex> lock(msg_mutex_);
     msg_condition_.wait(lock);
+    isreporting_ = true;
     decision_ptr_->ReportErrorCode(code_, motion_id_);
-
     std::this_thread::sleep_for(std::chrono::microseconds(20));
   }
 }