Merge pull request #236 from HarvestX/fix/motion-robot-mode-check

Fix robot mode check bug
HarvestX · Sep 19, 2023 · 13ac6f3 · 13ac6f3
2 parents 8a66bcd + 715fd7a
commit 13ac6f3
Show file tree

Hide file tree

Showing 2 changed files with 48 additions and 9 deletions.
diff --git a/mg400_plugin/src/motion_api/mov_j.cpp b/mg400_plugin/src/motion_api/mov_j.cpp
@@ -64,8 +64,10 @@ rclcpp_action::GoalResponse MovJ::handle_goal(
 
   using RobotMode = mg400_msgs::msg::RobotMode;
   if (!this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ENABLE)) {
+    uint64_t mode;
+    this->mg400_interface_->realtime_tcp_interface->getRobotMode(mode);
     RCLCPP_ERROR(
-      this->node_logging_if_->get_logger(), "Robot mode is not enabled");
+      this->node_logging_if_->get_logger(), "Robot mode is not enabled: mode is %ld", mode);
     return rclcpp_action::GoalResponse::REJECT;
   }
 
@@ -146,16 +148,33 @@ void MovJ::execute(const std::shared_ptr<GoalHandle> goal_handle)
   using RobotMode = mg400_msgs::msg::RobotMode;
   using namespace std::chrono_literals;   // NOLINT
   // TODO(anyone): Should calculate timeout with expectation goal time
-  const auto timeout = rclcpp::Duration(5s);
+  const auto timeout = rclcpp::Duration(10s);
   const auto start = this->node_clock_if_->get_clock()->now();
   update_pose(feedback->current_pose);
 
   while (!this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::RUNNING)) {
-    if (this->node_clock_if_->get_clock()->now() - start > timeout) {
-      RCLCPP_ERROR(this->node_logging_if_->get_logger(), "execution timeout");
+    if (this->node_clock_if_->get_clock()->now() - start > rclcpp::Duration(300ms)) {
+      if (is_goal_reached(feedback->current_pose.pose, tf_goal.pose)) {
+        RCLCPP_INFO(
+          this->node_logging_if_->get_logger(),
+          "Arm is already at the goal.");
+        break;
+      }
+
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(),
+        "execution timeout: Robot mode did not become RUNNING.");
+      goal_handle->abort(result);
+      return;
+    }
+
+    if (this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ERROR)) {
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(), "Robot Mode Error while checking becoming RUNNING");
       goal_handle->abort(result);
       return;
     }
+
     control_freq.sleep();
   }
 
@@ -169,7 +188,8 @@ void MovJ::execute(const std::shared_ptr<GoalHandle> goal_handle)
     }
 
     if (this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ERROR)) {
-      RCLCPP_ERROR(this->node_logging_if_->get_logger(), "Robot Mode Error");
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(), "Robot Mode Error while checking goal");
       goal_handle->abort(result);
       return;
     }
@@ -185,6 +205,7 @@ void MovJ::execute(const std::shared_ptr<GoalHandle> goal_handle)
     control_freq.sleep();
   }
 
+  RCLCPP_INFO(this->node_logging_if_->get_logger(), "Execution succeeded");
   result->result = true;
   goal_handle->succeed(result);
 }

diff --git a/mg400_plugin/src/motion_api/mov_l.cpp b/mg400_plugin/src/motion_api/mov_l.cpp
@@ -146,16 +146,33 @@ void MovL::execute(const std::shared_ptr<GoalHandle> goal_handle)
   using RobotMode = mg400_msgs::msg::RobotMode;
   using namespace std::chrono_literals;   // NOLINT
   // TODO(anyone): Should calculate timeout with expectation goal time
-  const auto timeout = rclcpp::Duration(5s);
+  const auto timeout = rclcpp::Duration(10s);
   const auto start = this->node_clock_if_->get_clock()->now();
   update_pose(feedback->current_pose);
 
   while (!this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::RUNNING)) {
-    if (this->node_clock_if_->get_clock()->now() - start > timeout) {
-      RCLCPP_ERROR(this->node_logging_if_->get_logger(), "execution timeout");
+    if (this->node_clock_if_->get_clock()->now() - start > rclcpp::Duration(300ms)) {
+      if (is_goal_reached(feedback->current_pose.pose, tf_goal.pose)) {
+        RCLCPP_INFO(
+          this->node_logging_if_->get_logger(),
+          "Arm is already at the goal.");
+        break;
+      }
+
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(),
+        "execution timeout: Robot mode did not become RUNNING.");
+      goal_handle->abort(result);
+      return;
+    }
+
+    if (this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ERROR)) {
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(), "Robot Mode Error while checking becoming RUNNING");
       goal_handle->abort(result);
       return;
     }
+
     control_freq.sleep();
   }
 
@@ -169,7 +186,8 @@ void MovL::execute(const std::shared_ptr<GoalHandle> goal_handle)
     }
 
     if (this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ERROR)) {
-      RCLCPP_ERROR(this->node_logging_if_->get_logger(), "Robot Mode Error");
+      RCLCPP_ERROR(
+        this->node_logging_if_->get_logger(), "Robot Mode Error while checking reaching goal");
       goal_handle->abort(result);
       return;
     }