Wrote more tests and a little refactoring

Author: URJala

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -90,9 +90,9 @@ class ToolContactController : public controller_interface::ControllerInterface
 
   RealtimeGoalHandleBuffer rt_active_goal_;         ///< Currently active action goal, if any.
   rclcpp::TimerBase::SharedPtr goal_handle_timer_;  ///< Timer to frequently check on the running goal
-  rclcpp::duration<std::chrono::nanoseconds> action_monitor_period_;
+  rclcpp::Duration action_monitor_period_ = rclcpp::Duration::from_seconds(0.05);
 
-  void action_handler(RealtimeGoalHandlePtr rt_goal);
+  void action_handler();
 
   rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/,
                                                      std::shared_ptr<const ur_msgs::action::ToolContact::Goal> goal);
@@ -110,6 +110,7 @@ class ToolContactController : public controller_interface::ControllerInterface
   std::atomic<bool> tool_contact_abort_ = false;
   std::atomic<bool> change_requested_ = false;
   std::atomic<bool> logged_once_ = false;
+  std::atomic<bool> should_reset_goal = false;
 
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_result_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> major_version_state_interface_;

ur_controllers/src/tool_contact_controller.cpp

@@ -74,7 +74,7 @@ controller_interface::InterfaceConfiguration ToolContactController::command_inte
   const std::string tf_prefix = tool_contact_params_.tf_prefix;
 
   // Start or stop tool contact functionality
-  config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_status");
+  config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_set_state");
   return config;
 }
 
@@ -84,28 +84,29 @@ controller_interface::InterfaceConfiguration ToolContactController::state_interf
   config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
 
   const std::string tf_prefix = tool_contact_params_.tf_prefix;
-  config.names.push_back(tf_prefix + "tool_contact/tool_contact_result");
-  config.names.push_back(tf_prefix + "get_robot_software_version/get_version_major");
+  config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_result");
+  config.names.emplace_back(tf_prefix + "get_robot_software_version/get_version_major");
+  config.names.emplace_back(tf_prefix + "tool_contact/tool_contact_state");
   return config;
 }
 
 controller_interface::CallbackReturn
 ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_state */)
 {
+  double val;
   {
     const std::string interface_name = tool_contact_params_.tf_prefix + "tool_contact/tool_contact_state";
     auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(),
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != state_interfaces_.end()) {
       tool_contact_state_interface_ = *it;
-      if (!tool_contact_state_interface_->get().get_value()) {
+      if (!tool_contact_state_interface_->get().get_value(val)) {
         RCLCPP_ERROR(get_node()->get_logger(),
-                     "Failed to set '%s' command interface, aborting activation of controller.",
-                     interface_name.c_str());
+                     "Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
         return controller_interface::CallbackReturn::ERROR;
       }
     } else {
-      RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str());
+      RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str());
       return controller_interface::CallbackReturn::ERROR;
     }
   }
@@ -132,7 +133,7 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
                            [&](auto& interface) { return (interface.get_name() == interface_name); });
     if (it != state_interfaces_.end()) {
       tool_contact_result_interface_ = *it;
-      if (!tool_contact_result_interface_->get().get_value()) {
+      if (!tool_contact_result_interface_->get().get_value(val)) {
         RCLCPP_ERROR(get_node()->get_logger(),
                      "Failed to read '%s' state interface, aborting activation of controller.", interface_name.c_str());
         return controller_interface::CallbackReturn::ERROR;
@@ -165,8 +166,7 @@ ToolContactController::on_activate(const rclcpp_lifecycle::State& /* previous_st
     }
   }
 
-  action_monitor_period_ =
-      rclcpp::duration<std::chrono::nanoseconds>(rclcpp::Rate(tool_contact_params_.action_monitor_rate).period());
+  action_monitor_period_ = rclcpp::Duration(rclcpp::Rate(tool_contact_params_.action_monitor_rate).period());
   return controller_interface::CallbackReturn::SUCCESS;
 }
 
@@ -179,9 +179,9 @@ ToolContactController::on_deactivate(const rclcpp_lifecycle::State& /* previous_
     RCLCPP_INFO(get_node()->get_logger(), "Aborting tool contact, as controller has been deactivated.");
     // Mark the current goal as abort
     auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-    result->result = ur_msgs::action::ToolContact::Result::ABORTED_BY_CONTROLLER;
+    result->result = ur_msgs::action::ToolContact::Result::ABORTED;
     active_goal->setAborted(result);
-    rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+    should_reset_goal = true;
   }
   if (tool_contact_active_) {
     tool_contact_active_ = false;
@@ -212,7 +212,8 @@ rclcpp_action::GoalResponse ToolContactController::goal_received_callback(
     return rclcpp_action::GoalResponse::REJECT;
   }
 
-  if (tool_contact_active_) {
+  const auto active_goal = *rt_active_goal_.readFromNonRT();
+  if (active_goal) {
     RCLCPP_ERROR(get_node()->get_logger(), "Tool contact already active, rejecting goal.");
     return rclcpp_action::GoalResponse::REJECT;
   }
@@ -229,22 +230,22 @@ void ToolContactController::goal_accepted_callback(
   rt_goal->execute();
   rt_active_goal_.writeFromNonRT(rt_goal);
   goal_handle_timer_.reset();
-  goal_handle_timer_ =
-      get_node()->create_wall_timer(action_monitor_period_, std::bind(&ToolContactController::action_handler, rt_goal));
+  auto period = std::chrono::duration_cast<std::chrono::nanoseconds>(
+      std::chrono::nanoseconds(action_monitor_period_.nanoseconds()));
+  goal_handle_timer_ = get_node()->create_wall_timer(period, std::bind(&ToolContactController::action_handler, this));
   return;
 }
 
-void ToolContactController::action_handler(RealtimeGoalHandlePtr rt_goal)
+void ToolContactController::action_handler()
 {
   const auto active_goal = *rt_active_goal_.readFromNonRT();
   if (active_goal) {
-    // Save the value if any of the goal ending conditions have been activated
-    static bool reset_goal = active_goal->req_abort_ | active_goal->req_cancel_ | active_goal->req_succeed_;
     // Allow the goal to handle any actions it needs to perform
-    rt_goal->runNonRealtime();
+    active_goal->runNonRealtime();
     // If one of the goal ending conditions were met, reset our active goal pointer
-    if (reset_goal) {
+    if (should_reset_goal) {
       rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
+      should_reset_goal = false;
     }
   }
 }
@@ -259,7 +260,7 @@ rclcpp_action::CancelResponse ToolContactController::goal_cancelled_callback(
 
     // Mark the current goal as canceled
     auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-    result->result = ur_msgs::action::ToolContact::Result::CANCELLED_BY_USER;
+    result->result = ur_msgs::action::ToolContact::Result::PREEMPTED;
     active_goal->setCanceled(result);
     rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
     tool_contact_abort_ = true;
@@ -305,6 +306,7 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
           auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
           result->result = ur_msgs::action::ToolContact::Result::SUCCESS;
           active_goal->setSucceeded(result);
+          should_reset_goal = true;
         }
       } else if (result == 1.0) {
         tool_contact_active_ = false;
@@ -313,8 +315,9 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
         write_success &= tool_contact_set_state_interface_->get().set_value(TOOL_CONTACT_STANDBY);
         if (active_goal) {
           auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-          result->result = ur_msgs::action::ToolContact::Result::ABORTED_BY_HARDWARE;
+          result->result = ur_msgs::action::ToolContact::Result::ABORTED;
           active_goal->setAborted(result);
+          should_reset_goal = true;
         }
       }
     } break;
@@ -327,8 +330,9 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
 
       if (active_goal) {
         auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-        result->result = ur_msgs::action::ToolContact::Result::ABORTED_BY_HARDWARE;
+        result->result = ur_msgs::action::ToolContact::Result::ABORTED;
         active_goal->setAborted(result);
+        should_reset_goal = true;
       }
     } break;
 
@@ -348,8 +352,9 @@ controller_interface::return_type ToolContactController::update(const rclcpp::Ti
 
       if (active_goal) {
         auto result = std::make_shared<ur_msgs::action::ToolContact::Result>();
-        result->result = ur_msgs::action::ToolContact::Result::ABORTED_BY_HARDWARE;
+        result->result = ur_msgs::action::ToolContact::Result::ABORTED;
         active_goal->setAborted(result);
+        should_reset_goal = true;
       }
     } break;
     case static_cast<int>(TOOL_CONTACT_STANDBY):

ur_robot_driver/src/hardware_interface.cpp

@@ -745,8 +745,6 @@ hardware_interface::return_type URPositionHardwareInterface::read(const rclcpp::
       force_mode_disable_cmd_ = NO_NEW_CMD_;
       freedrive_mode_abort_ = NO_NEW_CMD_;
       freedrive_mode_enable_ = NO_NEW_CMD_;
-      tool_contact_result_ = NO_NEW_CMD_;
-      tool_contact_status_ = NO_NEW_CMD_;
       initialized_ = true;
     }
 

ur_robot_driver/test/integration_test_tool_contact.py

@@ -39,9 +39,7 @@
 from rclpy.node import Node
 
 from controller_manager_msgs.srv import SwitchController
-from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
-from builtin_interfaces.msg import Duration
-from ur_msgs.action import ToolContact, TrajectoryUntil
+from ur_msgs.action import ToolContact
 
 sys.path.append(os.path.dirname(__file__))
 from test_common import (  # noqa: E402
@@ -50,21 +48,16 @@
     IoStatusInterface,
     ActionInterface,
     generate_driver_test_description,
-    ROBOT_JOINTS,
 )
 
-TIMEOUT_EXECUTE_TRAJECTORY = 30
-
 
 @pytest.mark.launch_test
 @launch_testing.parametrize(
-    "tf_prefix, initial_joint_controller",
-    [("", "scaled_joint_trajectory_controller"), ("my_ur_", "passthrough_trajectory_controller")],
+    "tf_prefix",
+    [(""), ("my_ur_")],
 )
-def generate_test_description(tf_prefix, initial_joint_controller):
-    return generate_driver_test_description(
-        tf_prefix=tf_prefix, initial_joint_controller=initial_joint_controller
-    )
+def generate_test_description(tf_prefix):
+    return generate_driver_test_description(tf_prefix=tf_prefix)
 
 
 class RobotDriverTest(unittest.TestCase):
@@ -87,10 +80,7 @@ def init_robot(self):
         self._controller_manager_interface = ControllerManagerInterface(self.node)
         self._io_status_controller_interface = IoStatusInterface(self.node)
         self._tool_contact_interface = ActionInterface(
-            self.node, "/tool_contact_controller/enable_tool_contact", ToolContact
-        )
-        self._trajectory_until_interface = ActionInterface(
-            self.node, "/trajectory_until_node/execute", TrajectoryUntil
+            self.node, "/tool_contact_controller/detect_tool_contact", ToolContact
         )
 
     def setUp(self):
@@ -110,37 +100,61 @@ def test_start_tool_contact_controller(self):
             ).ok
         )
 
-    def test_tool_contact(self):
+    def test_goal_can_be_cancelled(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["tool_contact_controller"],
+            ).ok
+        )
         goal_handle = self._tool_contact_interface.send_goal()
         self.assertTrue(goal_handle.accepted)
 
         cancel_res = self._tool_contact_interface.cancel_goal(goal_handle)
         self.assertEqual(cancel_res.return_code, 0)
 
-    def test_trajectory_until(self, tf_prefix):
-        L_pose_to_down = {
-            "waypts": [[1.5, -1.5, -0.45, -1.5, -1.5, -1.5], [2.1, -1.2, -0.45, -2.4, -1.5, -1.5]],
-            "time_vec": [Duration(sec=3, nanosec=0), Duration(sec=6, nanosec=0)],
-        }
+    def test_deactivate_controller_aborts_action(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["tool_contact_controller"],
+            ).ok
+        )
 
-        trajectory = JointTrajectory()
-        trajectory.joint_names = [tf_prefix + joint for joint in ROBOT_JOINTS]
+        goal_handle = self._tool_contact_interface.send_goal()
+        self.assertTrue(goal_handle.accepted)
+
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["tool_contact_controller"],
+            ).ok
+        )
 
-        trajectory.points = [
-            JointTrajectoryPoint(
-                positions=L_pose_to_down["waypts"][i], time_from_start=L_pose_to_down["time_vec"][i]
-            )
-            for i in range(len(L_pose_to_down["waypts"]))
-        ]
+        result = self._tool_contact_interface.get_result(goal_handle, 5)
+        self.assertEqual(result.result, ToolContact.Result.ABORTED)
 
-        goal_handle = self._trajectory_until_interface.send_goal(
-            trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT
+    def test_inactive_controller_rejects_actions(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                deactivate_controllers=["tool_contact_controller"],
+            ).ok
         )
-        self.assertTrue(goal_handle.accepted)
 
-        result = self._trajectory_until_interface.get_result(
-            goal_handle=goal_handle, timeout=TIMEOUT_EXECUTE_TRAJECTORY
+        goal_handle = self._tool_contact_interface.send_goal()
+        self.assertFalse(goal_handle.accepted)
+
+    def test_busy_controller_rejects_actions(self):
+        self.assertTrue(
+            self._controller_manager_interface.switch_controller(
+                strictness=SwitchController.Request.BEST_EFFORT,
+                activate_controllers=["tool_contact_controller"],
+            ).ok
         )
 
-        self.assertEqual(result.error_code, TrajectoryUntil.Result.SUCCESSFUL)
-        self.assertGreaterEqual(result.until_condition_result, TrajectoryUntil.Result.NOT_TRIGGERED)
+        goal_handle = self._tool_contact_interface.send_goal()
+        self.assertTrue(goal_handle.accepted)
+
+        goal_handle = self._tool_contact_interface.send_goal()
+        self.assertFalse(goal_handle.accepted)

ur_robot_driver/urdf/ur.ros2_control.xacro

@@ -233,9 +233,6 @@
 
       <gpio name="${tf_prefix}tool_contact">
         <command_interface name="tool_contact_set_state"/>
-      </gpio>
-
-      <gpio name="${tf_prefix}tool_contact">
         <state_interface name="tool_contact_result"/>
         <state_interface name="tool_contact_state"/>
       </gpio>