Merge pull request #234 from HarvestX/fix/without-sleep

Update MovJ and MovL to check the robot mode during execution
HarvestX · Sep 8, 2023 · 8a66bcd · 8a66bcd
2 parents 4534659 + bb83c9e
commit 8a66bcd
Show file tree

Hide file tree

Showing 2 changed files with 24 additions and 2 deletions.
diff --git a/mg400_plugin/src/motion_api/mov_j.cpp b/mg400_plugin/src/motion_api/mov_j.cpp
@@ -150,7 +150,18 @@ void MovJ::execute(const std::shared_ptr<GoalHandle> goal_handle)
   const auto start = this->node_clock_if_->get_clock()->now();
   update_pose(feedback->current_pose);
 
-  while (!is_goal_reached(feedback->current_pose.pose, tf_goal.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");
+      goal_handle->abort(result);
+      return;
+    }
+    control_freq.sleep();
+  }
+
+  while (!is_goal_reached(feedback->current_pose.pose, tf_goal.pose) ||
+    !this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ENABLE))
+  {
     if (!this->mg400_interface_->ok()) {
       RCLCPP_ERROR(this->node_logging_if_->get_logger(), "MG400 Connection Error");
       goal_handle->abort(result);

diff --git a/mg400_plugin/src/motion_api/mov_l.cpp b/mg400_plugin/src/motion_api/mov_l.cpp
@@ -150,7 +150,18 @@ void MovL::execute(const std::shared_ptr<GoalHandle> goal_handle)
   const auto start = this->node_clock_if_->get_clock()->now();
   update_pose(feedback->current_pose);
 
-  while (!is_goal_reached(feedback->current_pose.pose, tf_goal.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");
+      goal_handle->abort(result);
+      return;
+    }
+    control_freq.sleep();
+  }
+
+  while (!is_goal_reached(feedback->current_pose.pose, tf_goal.pose) ||
+    !this->mg400_interface_->realtime_tcp_interface->isRobotMode(RobotMode::ENABLE))
+  {
     if (!this->mg400_interface_->ok()) {
       RCLCPP_ERROR(this->node_logging_if_->get_logger(), "MG400 Connection Error");
       goal_handle->abort(result);