Remove chaining from controller and add trajectory_until node

This exposes an action to execute a trajectory, either until the trajectory is finished or until the "until" condition is met.

Author: URJala

ur_controllers/CMakeLists.txt

@@ -25,6 +25,7 @@ find_package(generate_parameter_library REQUIRED)
 find_package(trajectory_msgs REQUIRED)
 find_package(control_msgs REQUIRED)
 find_package(action_msgs REQUIRED)
+find_package(rclcpp REQUIRED)
 
 
 set(THIS_PACKAGE_INCLUDE_DEPENDS
@@ -47,6 +48,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   control_msgs
   trajectory_msgs
   action_msgs
+  rclcpp
 )
 
 include_directories(include)
@@ -114,6 +116,15 @@ target_link_libraries(${PROJECT_NAME}
   passthrough_trajectory_controller_parameters
   ur_configuration_controller_parameters
 )
+
+#
+# trajectory_until_node
+#
+add_executable(trajectory_until_node
+  src/trajectory_until_node.cpp
+)
+ament_target_dependencies(trajectory_until_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS})
+
 ament_target_dependencies(${PROJECT_NAME}
   ${THIS_PACKAGE_INCLUDE_DEPENDS}
 )
@@ -124,6 +135,11 @@ target_compile_options(${PROJECT_NAME} PRIVATE -Wpedantic -Werror=return-type)
 target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
 pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml)
 
+install(
+  TARGETS trajectory_until_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
 install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib

ur_controllers/controller_plugins.xml

@@ -34,7 +34,7 @@
       Controller used to get and change the configuration of the robot
     </description>
   </class>
-  <class name="ur_controllers/ToolContactController" type="ur_controllers::ToolContactController" base_class_type="controller_interface::ChainableControllerInterface">
+  <class name="ur_controllers/ToolContactController" type="ur_controllers::ToolContactController" base_class_type="controller_interface::ControllerInterface">
     <description>
       Chainable controller to use the tool contact functionality of the robot.
     </description>

ur_controllers/include/ur_controllers/tool_contact_controller.hpp

@@ -45,7 +45,7 @@
 #include <vector>
 #include <memory>
 
-#include <controller_interface/chainable_controller_interface.hpp>
+#include <controller_interface/controller_interface.hpp>
 #include "std_msgs/msg/bool.hpp"
 #include <rclcpp_action/server.hpp>
 #include <rclcpp_action/create_server.hpp>
@@ -57,13 +57,16 @@
 #include <realtime_tools/realtime_server_goal_handle.hpp>
 
 #include <ur_msgs/action/tool_contact.hpp>
-#include "tool_contact_controller_parameters.hpp"
+#include "ur_controllers/tool_contact_controller_parameters.hpp"
 
 namespace ur_controllers
 {
-class ToolContactController : public controller_interface::ChainableControllerInterface
+class ToolContactController : public controller_interface::ControllerInterface
 {
 public:
+  ToolContactController() = default;
+  ~ToolContactController() override = default;
+
   controller_interface::CallbackReturn on_init() override;
 
   controller_interface::InterfaceConfiguration command_interface_configuration() const override;
@@ -76,66 +79,9 @@ class ToolContactController : public controller_interface::ChainableControllerIn
 
   controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
 
-protected:
-  /// Virtual method that each chainable controller should implement to export its read-only
-  /// chainable interfaces.
-  /**
-   * Each chainable controller implements this methods where all its state(read only) interfaces are
-   * exported. The method has the same meaning as `export_state_interfaces` method from
-   * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
-   *
-   * \returns list of StateInterfaces that other controller can use as their inputs.
-   */
-  std::vector<hardware_interface::StateInterface> on_export_state_interfaces() override;
-
-  /// Virtual method that each chainable controller should implement to export its read/write
-  /// chainable interfaces.
-  /**
-   * Each chainable controller implements this methods where all input (command) interfaces are
-   * exported. The method has the same meaning as `export_command_interface` method from
-   * hardware_interface::SystemInterface or hardware_interface::ActuatorInterface.
-   *
-   * \returns list of CommandInterfaces that other controller can use as their outputs.
-   */
-  std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
-
-  /// Virtual method that each chainable controller should implement to switch chained mode.
-  /**
-   * Each chainable controller implements this methods to switch between "chained" and "external"
-   * mode. In "chained" mode all external interfaces like subscriber and service servers are
-   * disabled to avoid potential concurrency in input commands.
-   *
-   * \param[in] flag marking a switch to or from chained mode.
-   *
-   * \returns true if controller successfully switched between "chained" and "external" mode.
-   * \default returns true so the method don't have to be overridden if controller can always switch
-   * chained mode.
-   */
-  bool on_set_chained_mode(bool chained_mode) override;
-
-  /// Update reference from input topics when not in chained mode.
-  /**
-   * Each chainable controller implements this method to update reference from subscribers when not
-   * in chained mode.
-   *
-   * \returns return_type::OK if update is successfully, otherwise return_type::ERROR.
-   */
-  controller_interface::return_type update_reference_from_subscribers(const rclcpp::Time& time,
-                                                                      const rclcpp::Duration& period) override;
-
-  /// Execute calculations of the controller and update command interfaces.
-  /**
-   * Update method for chainable controllers.
-   * In this method is valid to assume that \reference_interfaces_ hold the values for calculation
-   * of the commands in the current control step.
-   * This means that this method is called after \update_reference_from_subscribers if controller is
-   * not in chained mode.
-   *
-   * \returns return_type::OK if calculation and writing of interface is successfully, otherwise
-   * return_type::ERROR.
-   */
-  controller_interface::return_type update_and_write_commands(const rclcpp::Time& time,
-                                                              const rclcpp::Duration& period) override;
+  controller_interface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& previous_state) override;
+
+  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
 
 private:
   using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle<ur_msgs::action::ToolContact>;
@@ -155,12 +101,14 @@ 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();
+  controller_interface::return_type failed_update();
 
-  double tool_contact_enable;
-  double tool_contact_active;
+  double tool_contact_enable_ref_interface;
+  double tool_contact_active_state_interface;
 
+  std::atomic<bool> tool_contact_enable_ = false;
   std::atomic<bool> tool_contact_active_ = false;
+  std::atomic<bool> tool_contact_abort_ = false;
   std::atomic<bool> change_requested_ = false;
   std::atomic<bool> logged_once_ = false;
 

ur_controllers/include/ur_controllers/trajectory_until_node.hpp

@@ -0,0 +1,129 @@
+// Copyright 2025, Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Jacob Larsen [email protected]
+ * \date    2025-01-17
+ *
+ *
+ *
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef UR_CONTROLLERS__TRAJECTORY_UNTIL_NODE_HPP_
+#define UR_CONTROLLERS__TRAJECTORY_UNTIL_NODE_HPP_
+
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/bool.hpp"
+#include <rclcpp_action/server.hpp>
+#include <rclcpp_action/rclcpp_action.hpp>
+#include <rclcpp_action/create_server.hpp>
+#include <rclcpp_action/server_goal_handle.hpp>
+#include <rclcpp/duration.hpp>
+#include <ur_msgs/action/tool_contact.hpp>
+#include <ur_msgs/action/trajectory_until.hpp>
+#include <control_msgs/action/follow_joint_trajectory.hpp>
+
+namespace ur_controllers
+{
+
+// template <typename action_type>
+class TrajectoryUntilNode : public rclcpp::Node
+{
+private:
+  rclcpp_action::Server<ur_msgs::action::TrajectoryUntil>::SharedPtr action_server_;
+  rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr trajectory_action_client_;
+  rclcpp_action::Client<ur_msgs::action::ToolContact>::SharedPtr until_action_client_;
+
+  rclcpp::CallbackGroup::SharedPtr server_callback_group =
+      create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+  rclcpp::CallbackGroup::SharedPtr clients_callback_group =
+      create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
+
+  std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::TrajectoryUntil>> server_goal_handle_;
+
+  void
+  until_response_callback(const rclcpp_action::ClientGoalHandle<ur_msgs::action::ToolContact>::SharedPtr& goal_handle);
+  void
+  until_result_callback(const rclcpp_action::ClientGoalHandle<ur_msgs::action::ToolContact>::WrappedResult& result);
+
+  void trajectory_response_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle);
+  void trajectory_feedback_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr& goal_handle,
+      const std::shared_ptr<const control_msgs::action::FollowJointTrajectory::Feedback> feedback);
+  void trajectory_result_callback(
+      const rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::WrappedResult& result);
+
+  void assign_until_action_client(std::shared_ptr<const ur_msgs::action::TrajectoryUntil::Goal> goal);
+
+  rclcpp_action::GoalResponse goal_received_callback(
+      const rclcpp_action::GoalUUID& uuid, std::shared_ptr<const ur_msgs::action::TrajectoryUntil::Goal> goal);
+  void goal_accepted_callback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::TrajectoryUntil>> goal_handle);
+  rclcpp_action::CancelResponse goal_cancelled_callback(
+      const std::shared_ptr<rclcpp_action::ServerGoalHandle<ur_msgs::action::TrajectoryUntil>> goal_handle);
+
+  void send_trajectory_goal(std::shared_ptr<const ur_msgs::action::TrajectoryUntil::Goal> goal);
+  void send_until_goal(std::shared_ptr<const ur_msgs::action::TrajectoryUntil::Goal> goal);
+
+  void report_goal(auto result, bool until_triggered);
+
+  void reset_vars();
+
+  std::atomic<bool> trajectory_accepted_;
+  std::atomic<bool> until_accepted_;
+
+  rclcpp_action::ClientGoalHandle<control_msgs::action::FollowJointTrajectory>::SharedPtr
+      current_trajectory_goal_handle_;
+  rclcpp_action::ClientGoalHandle<ur_msgs::action::ToolContact>::SharedPtr current_until_goal_handle_;
+
+  std::shared_ptr<ur_msgs::action::TrajectoryUntil::Result> prealloc_res =
+      std::make_shared<ur_msgs::action::TrajectoryUntil::Result>(ur_msgs::action::TrajectoryUntil::Result());
+
+  std::shared_ptr<ur_msgs::action::TrajectoryUntil::Feedback> prealloc_fb =
+      std::make_shared<ur_msgs::action::TrajectoryUntil::Feedback>(ur_msgs::action::TrajectoryUntil::Feedback());
+
+  std::condition_variable cv_until_;
+  std::condition_variable cv_trajectory_;
+  std::mutex mutex_until;
+  std::mutex mutex_trajectory;
+
+public:
+  explicit TrajectoryUntilNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
+  ~TrajectoryUntilNode();
+};
+
+}  // namespace ur_controllers
+
+#endif  // UR_CONTROLLERS__TRAJECTORY_UNTIL_NODE_HPP_

ur_controllers/package.xml

@@ -42,6 +42,7 @@
   <depend>control_msgs</depend>
   <depend>trajectory_msgs</depend>
   <depend>action_msgs</depend>
+  <depend>rclcpp_components</depend>
 
   <test_depend>controller_manager</test_depend>
   <test_depend>ros2_control_test_assets</test_depend>