Tool contact controller works when used with an action. Chaining is still untested.

I have not done anything with starting and stopping the controller yet.

Author: URJala

ur_controllers/include/ur_controllers/gpio_controller.hpp

@@ -81,10 +81,6 @@ enum CommandInterfaces
   HAND_BACK_CONTROL_CMD = 33,
   HAND_BACK_CONTROL_ASYNC_SUCCESS = 34,
   ANALOG_OUTPUTS_DOMAIN = 35,
-  START_TOOL_CONTACT_CMD = 36,
-  START_TOOL_CONTACT_ASYNC_SUCCESS = 37,
-  END_TOOL_CONTACT_CMD = 38,
-  END_TOOL_CONTACT_ASYNC_SUCCESS = 39,
 };
 
 enum StateInterfaces

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -41,12 +41,18 @@
 #ifndef UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
 #define UR_CONTROLLERS__TOOL_CONTACT_CONTROLLER_HPP_
 
+#include <chrono>
+
 #include <controller_interface/chainable_controller_interface.hpp>
 #include "std_msgs/msg/bool.hpp"
 #include <rclcpp_action/server.hpp>
 #include <rclcpp_action/create_server.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/duration.hpp>
+
+#include <realtime_tools/realtime_buffer.hpp>
+#include <realtime_tools/realtime_server_goal_handle.hpp>
 
 #include <ur_msgs/action/tool_contact.hpp>
 #include "tool_contact_controller_parameters.hpp"
@@ -130,6 +136,14 @@ class ToolContactController : public controller_interface::ChainableControllerIn
                                                               const rclcpp::Duration& period) override;
 
 private:
+  using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
+  using RealtimeGoalHandlePtr = std::shared_ptr<RealtimeGoalHandle>;
+  using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer<RealtimeGoalHandlePtr>;
+
+  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 action_monitor_period_ = rclcpp::Duration(std::chrono::milliseconds(50));
+
   rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& /*uuid*/,
                                                      std::shared_ptr<const ur_msgs::action::ToolContact::Goal> goal);
 
@@ -139,27 +153,33 @@ class ToolContactController : public controller_interface::ChainableControllerIn
   rclcpp_action::CancelResponse goal_cancelled_callback(
       const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::ToolContact>> goal_handle);
 
+  void failed_update();
+
   double tool_contact_enable;
   double tool_contact_active;
 
   std::atomic<bool> tool_contact_active_ = false;
   std::atomic<bool> change_requested_ = false;
+  std::atomic<bool> logged_once_ = false;
 
   double old_reference_val = 0.0;
 
-  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> async_success_interface_;
-  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> enable_command_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> tool_contact_result_interface_;
+  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> tool_contact_status_interface_;
   std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> reference_interface_;
 
   rclcpp_action::Server<ur_msgs::action::ToolContact>::SharedPtr tool_contact_action_server_;
 
   std::shared_ptr<tool_contact_controller::ParamListener> tool_contact_param_listener_;
   tool_contact_controller::Params tool_contact_params_;
 
-  void tool_contact_sub_callback(const std_msgs::msg::Bool::SharedPtr msg);
-
-  static constexpr double ASYNC_WAITING = 2.0;
-  static constexpr double ASYNC_STANDBY_ = std::numeric_limits<double>::quiet_NaN();
+  static constexpr double TOOL_CONTACT_STANDBY = 1.0;
+  static constexpr double TOOL_CONTACT_WAITING_BEGIN = 2.0;
+  static constexpr double TOOL_CONTACT_EXECUTING = 3.0;
+  static constexpr double TOOL_CONTACT_FAILURE_BEGIN = 4.0;
+  static constexpr double TOOL_CONTACT_WAITING_END = 5.0;
+  static constexpr double TOOL_CONTACT_SUCCESS_END = 6.0;
+  static constexpr double TOOL_CONTACT_FAILURE_END = 7.0;
 };
 }  // namespace ur_controllers
 

ur_controllers/src/gpio_controller.cpp

@@ -565,60 +565,6 @@ bool GPIOController::zeroFTSensor(std_srvs::srv::Trigger::Request::SharedPtr /*r
   return true;
 }
 
-// bool GPIOController::startToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
-//                                       std_srvs::srv::Trigger::Response::SharedPtr resp)
-// {
-//   // reset success flag
-//   command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-
-//   // call the service in the hardware
-//   command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_CMD].set_value(1.0);
-
-//   if (!waitForAsyncCommand(
-//           [&]() { return command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
-//     RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was started.");
-//   }
-
-//   resp->success =
-//       static_cast<bool>(command_interfaces_[CommandInterfaces::START_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
-
-//   if (resp->success) {
-//     RCLCPP_INFO(get_node()->get_logger(), "Successfully started tool contact");
-//   } else {
-//     RCLCPP_ERROR(get_node()->get_logger(), "Could not start tool contact");
-//     return false;
-//   }
-
-//   return true;
-// }
-
-// bool GPIOController::endToolContact(std_srvs::srv::Trigger::Request::SharedPtr /*req*/,
-//                                     std_srvs::srv::Trigger::Response::SharedPtr resp)
-// {
-//   // reset success flag
-//   command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].set_value(ASYNC_WAITING);
-
-//   // call the service in the hardware
-//   command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_CMD].set_value(1.0);
-
-//   if (!waitForAsyncCommand(
-//           [&]() { return command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value(); })) {
-//     RCLCPP_WARN(get_node()->get_logger(), "Could not verify that tool contact was stopped.");
-//   }
-
-//   resp->success =
-//   static_cast<bool>(command_interfaces_[CommandInterfaces::END_TOOL_CONTACT_ASYNC_SUCCESS].get_value());
-
-//   if (resp->success) {
-//     RCLCPP_INFO(get_node()->get_logger(), "Successfully stopped tool contact");
-//   } else {
-//     RCLCPP_ERROR(get_node()->get_logger(), "Could not stop tool contact");
-//     return false;
-//   }
-
-//   return true;
-// }
-
 void GPIOController::initMsgs()
 {
   io_msg_.digital_in_states.resize(standard_digital_output_cmd_.size());