diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 1a76a5cab..f564c8c4a 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -137,12 +137,26 @@ target_link_libraries(urscript_interface PUBLIC ur_client_library::urcl ) +# +# trajectory_until_node +# +add_executable(trajectory_until_node + src/trajectory_until_node.cpp +) +target_link_libraries(trajectory_until_node PUBLIC + rclcpp::rclcpp + ${std_msgs_TARGETS} + rclcpp_action::rclcpp_action + ${ur_msgs_TARGETS} +) + install( TARGETS dashboard_client controller_stopper_node urscript_interface robot_state_helper + trajectory_until_node DESTINATION lib/${PROJECT_NAME} ) @@ -211,6 +225,7 @@ install(PROGRAMS scripts/start_ursim.sh examples/examples.py examples/force_mode.py + examples/move_until_example.py DESTINATION lib/${PROJECT_NAME} ) @@ -263,5 +278,9 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_trajectory_until.py + TIMEOUT + 800 + ) endif() endif() diff --git a/ur_robot_driver/doc/index.rst b/ur_robot_driver/doc/index.rst index bc90429e6..60f8d94f4 100644 --- a/ur_robot_driver/doc/index.rst +++ b/ur_robot_driver/doc/index.rst @@ -21,3 +21,4 @@ ur_robot_driver dashboard_client robot_state_helper controller_stopper + trajectory_until_node diff --git a/ur_robot_driver/doc/trajectory_until_node.rst b/ur_robot_driver/doc/trajectory_until_node.rst new file mode 100644 index 000000000..53aa6d63e --- /dev/null +++ b/ur_robot_driver/doc/trajectory_until_node.rst @@ -0,0 +1,23 @@ +.. _trajectory_until_node: + +Trajectory until node +===================== + +The trajectory until node allows a user to execute a trajectory with an "until" condition enabled (currently only tool contact is available) without having to call 2 actions at the same time. This means that the trajectory will execute until either the trajectory is finished or the "until" condition has been triggered. Both scenarios will result in the trajectory being reported as successful. + +Action interface / usage +"""""""""""""""""""""""" +The node provides an action to execute a trajectory with tool contact enabled. For the node to accept action goals, both the motion controller and the tool contact controller need to be in ``active`` state. + +* ``/trajectory_until_node/execute [ur_msgs/action/TrajectoryUntil]`` + +The action contains all the same fields as the ordinary `FollowJointTrajectory `_ action, but has two additional fields. +One in the goal section called ``until_type``, which is used to choose between different conditions that can stop the trajectory. Currently only tool contact is available. +The result section contains the other new field called ``until_condition_result``, which reports whether the chosen condition was triggered or not. + +Implementation details +"""""""""""""""""""""" +Upon instantiation of the node, the internal trajectory action client will connect to an action named ``motion_controller/follow_joint_trajectory``. +This action does not exist, but upon launch of the driver, the node is remapped to connect to the ``initial_joint_controller``, default is ``scaled_joint_trajectory_controller``. +If you wish to use the node with another motion controller use the launch argument ``initial_joint_controller:=`` when launching the driver. +The node is only compatible with motion controllers that use the FollowJointTrajectory action interface. diff --git a/ur_robot_driver/examples/move_until_example.py b/ur_robot_driver/examples/move_until_example.py new file mode 100755 index 000000000..d0f10d894 --- /dev/null +++ b/ur_robot_driver/examples/move_until_example.py @@ -0,0 +1,132 @@ +#!/usr/bin/env python3 +# 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. + +import time + +import rclpy +from rclpy.action import ActionClient + +from builtin_interfaces.msg import Duration +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from ur_msgs.action import FollowJointTrajectoryUntil + +ROBOT_JOINTS = [ + "elbow_joint", + "shoulder_lift_joint", + "shoulder_pan_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +class MoveUntilExample(rclpy.node.Node): + def __init__(self): + super().__init__("move_until_example") + + self._send_goal_future = None + self._get_result_future = None + self._goal_handle = None + self._action_client = ActionClient( + self, FollowJointTrajectoryUntil, "/trajectory_until_node/execute" + ) + self._action_client.wait_for_server() + self.test_traj = { + "waypts": [[1.5, -1.5, 0.0, -1.5, -1.5, -1.5], [2.1, -1.2, 0.0, -2.4, -1.5, -1.5]], + "time_vec": [Duration(sec=3, nanosec=0), Duration(sec=6, nanosec=0)], + } + self.get_logger().info("Initialized") + + def cancel_goal(self): + if self._goal_handle is not None: + self.get_logger().info("Cancelling goal") + future = self._goal_handle.cancel_goal_async() + future.add_done_callback(self.cancel_done) + + def cancel_done(self, future): + try: + future.result() + self.get_logger().info("Goal cancelled successfully") + except Exception as e: + self.get_logger().error(f"Failed to cancel goal: {e}") + + def process(self): + trajectory = JointTrajectory() + trajectory.joint_names = ROBOT_JOINTS + + trajectory.points = [ + JointTrajectoryPoint( + positions=self.test_traj["waypts"][i], time_from_start=self.test_traj["time_vec"][i] + ) + for i in range(len(self.test_traj["waypts"])) + ] + goal = FollowJointTrajectoryUntil.Goal( + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT + ) + self._send_goal_future = self._action_client.send_goal_async(goal) + rclpy.spin_until_future_complete(self, self._send_goal_future) + self._goal_handle = self._send_goal_future.result() + if not self._goal_handle.accepted: + self.get_logger().error("Goal rejected :(") + raise RuntimeError("Goal rejected :(") + + self.get_logger().info( + f"Goal accepted with ID: {bytes(self._goal_handle.goal_id.uuid).hex()}\n" + ) + + result_future = self._goal_handle.get_result_async() + rclpy.spin_until_future_complete(self, result_future) + result = result_future.result().result + print(result) + if result is None: + self.get_logger().error("Result is None") + return + if result.error_code != FollowJointTrajectoryUntil.Result.SUCCESSFUL: + self.get_logger().error(f"Result error code: {result.error_code}") + return + if result.until_condition_result == FollowJointTrajectoryUntil.Result.NOT_TRIGGERED: + self.get_logger().info("Trajectory executed without tool contact trigger") + else: + self.get_logger().info("Trajectory executed with tool contact trigger") + + self.get_logger().info("Trajectory executed successfully with tool contact condition") + + +if __name__ == "__main__": + rclpy.init() + + node = MoveUntilExample() + try: + node.process() + except KeyboardInterrupt: + node.get_logger().info("Interrupted") + node.cancel_goal() + time.sleep(2) + + rclpy.shutdown() diff --git a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp new file mode 100644 index 000000000..cebe3ab74 --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -0,0 +1,137 @@ +// 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 jala@universal-robots.com + * \date 2025-01-17 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_ +#define UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ur_robot_driver +{ +class TrajectoryUntilNode : public rclcpp::Node +{ +public: + explicit TrajectoryUntilNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~TrajectoryUntilNode(); + +private: + using TrajectoryUntilAction = ur_msgs::action::FollowJointTrajectoryUntil; + + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr trajectory_action_client_; + + // Add new until types here, when available + using TCClient = rclcpp_action::Client::SharedPtr; + std::variant until_action_client_variant; + + rclcpp::CallbackGroup::SharedPtr server_callback_group; + rclcpp::CallbackGroup::SharedPtr clients_callback_group; + + std::shared_ptr> server_goal_handle_; + + template + void send_until_goal(std::shared_ptr goal); + + template + void until_response_callback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + + template + void until_result_callback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& result); + + void cancel_until_goal(); + + void trajectory_response_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void trajectory_feedback_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle, + const std::shared_ptr feedback); + void trajectory_result_callback( + const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + bool assign_until_action_client(std::shared_ptr goal); + + rclcpp_action::GoalResponse goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + void + goal_accepted_callback(const std::shared_ptr> goal_handle); + rclcpp_action::CancelResponse + goal_cancelled_callback(const std::shared_ptr> goal_handle); + + void send_trajectory_goal(std::shared_ptr goal); + + void report_goal(rclcpp_action::ClientGoalHandle::WrappedResult result); + + template + void report_goal(UntilResult result); + + void reset_node(); + + rclcpp_action::ClientGoalHandle::SharedPtr + current_trajectory_goal_handle_; + rclcpp_action::ClientGoalHandle::SharedPtr current_until_goal_handle_; + + std::shared_ptr prealloc_res_; + + std::shared_ptr prealloc_fb_; + + std::atomic trajectory_accepted_; + std::atomic until_accepted_; + + std::condition_variable cv_until_; + std::condition_variable cv_trajectory_; + std::mutex mutex_until; + std::mutex mutex_trajectory; +}; + +} // namespace ur_robot_driver + +#endif // UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_ diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 528da2ee7..bdd2131ef 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -158,6 +158,19 @@ def launch_setup(context): arguments=["-d", rviz_config_file], ) + trajectory_until_node = Node( + package="ur_robot_driver", + executable="trajectory_until_node", + name="trajectory_until_node", + output="screen", + remappings=[ + ( + "/motion_controller/follow_joint_trajectory", + f"/{initial_joint_controller.perform(context)}/follow_joint_trajectory", + ), + ], + ) + # Spawn controllers def controller_spawner(controllers, active=True): inactive_flags = ["--inactive"] if not active else [] @@ -221,6 +234,7 @@ def controller_spawner(controllers, active=True): urscript_interface, rsp, rviz_node, + trajectory_until_node, ] + controller_spawners return nodes_to_start diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp new file mode 100644 index 000000000..3e878bd01 --- /dev/null +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -0,0 +1,401 @@ +// 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 jala@universal-robots.com + * \date 2025-01-17 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include + +namespace ur_robot_driver +{ + +using TCAction = ur_msgs::action::ToolContact; +using GoalHandleTrajectoryUntil = rclcpp_action::ServerGoalHandle; +using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; +using TrajectoryResult = rclcpp_action::ClientGoalHandle::WrappedResult; + +using namespace std::chrono_literals; + +TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) + : Node("TrajectoryUntilNode", options) + , server_goal_handle_(nullptr) + , current_trajectory_goal_handle_(nullptr) + , current_until_goal_handle_(nullptr) + , trajectory_accepted_(false) + , until_accepted_(false) +{ + prealloc_res_ = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb_ = std::make_shared(TrajectoryUntilAction::Feedback()); + + // Different callback groups for the server and the clients, so their callbacks can run concurrently. + server_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + clients_callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // Initialize a trajectory action client, to a generic action that does not exist. This is remapped via ros-args when + // launching the node + trajectory_action_client_ = rclcpp_action::create_client(this, + "/motion_controller/" + "follow_joint_" + "trajectory", + clients_callback_group); + + // Create action server to advertise the "/trajectory_until_node/execute" + action_server_ = rclcpp_action::create_server( + this, "~/execute", + std::bind(&TrajectoryUntilNode::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), + std::bind(&TrajectoryUntilNode::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&TrajectoryUntilNode::goal_accepted_callback, this, std::placeholders::_1), + rcl_action_server_get_default_options(), server_callback_group); +} + +TrajectoryUntilNode::~TrajectoryUntilNode() +{ +} + +// Assign the correct type of action client to the variant +bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) +{ + switch (goal->until_type) { + case TrajectoryUntilAction::Goal::TOOL_CONTACT: + until_action_client_variant = rclcpp_action::create_client(this, + "/tool_contact_controller/" + "detect_tool_contact", + clients_callback_group); + break; + + default: + RCLCPP_ERROR(this->get_logger(), "Received unknown until-type."); + return false; + } + + return true; +} + +// Check if the node is capable of accepting a new action goal right now. +rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( + const rclcpp_action::GoalUUID& /* uuid */, std::shared_ptr goal) +{ + if (server_goal_handle_) { + RCLCPP_ERROR(this->get_logger(), "Node is currently busy, rejecting action goal."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!assign_until_action_client(goal)) { + RCLCPP_ERROR(this->get_logger(), "Until type not defined, double check the types in the action " + "definition."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!trajectory_action_client_->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_ERROR(this->get_logger(), "Trajectory action server not available."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (!std::visit([](const auto& client) { return client->wait_for_action_server(std::chrono::seconds(1)); }, + until_action_client_variant)) { + RCLCPP_ERROR(this->get_logger(), "Until action server not available."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check until action server, send action goal to until-controller and wait for it to be accepted. + if (std::holds_alternative(until_action_client_variant)) { + send_until_goal(goal); + } else { + throw std::runtime_error("Until type not implemented. This should not happen."); + } + + // If it is not accepted within 1 seconds, it is + // assumed to be rejected. + std::unique_lock until_lock(mutex_until); + cv_until_.wait_for(until_lock, 1s); + if (!until_accepted_) { + return rclcpp_action::GoalResponse::REJECT; + } + + // Send action goal to trajectory controller and wait for it to be accepted. If it is not accepted within 1 seconds, + // it is assumed to be rejected. + send_trajectory_goal(goal); + std::unique_lock traj_lock(mutex_trajectory); + cv_until_.wait_for(traj_lock, 1s); + if (!trajectory_accepted_) { + reset_node(); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +// Save the server goal handle for later use. +void TrajectoryUntilNode::goal_accepted_callback(const std::shared_ptr goal_handle) +{ + server_goal_handle_ = goal_handle; + RCLCPP_INFO(this->get_logger(), "Accepted goal"); + return; +} + +// If the trajectory_until action is canceled, it should cancel the until and trajectory actions, if any are running. +rclcpp_action::CancelResponse +TrajectoryUntilNode::goal_cancelled_callback(const std::shared_ptr /* goal_handle */) +{ + RCLCPP_INFO(this->get_logger(), "Cancel received. Cancelling trajectory-until action."); + if (current_until_goal_handle_) { + cancel_until_goal(); + } + if (current_trajectory_goal_handle_) { + trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); + } + + return rclcpp_action::CancelResponse::ACCEPT; +} + +// Send the trajectory goal, using the relevant fields supplied from the action server +void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptr goal) +{ + auto goal_msg = FollowJointTrajectory::Goal(); + goal_msg.trajectory = goal->trajectory; + goal_msg.goal_time_tolerance = goal->goal_time_tolerance; + goal_msg.goal_tolerance = goal->goal_tolerance; + goal_msg.path_tolerance = goal->path_tolerance; + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&TrajectoryUntilNode::trajectory_response_callback, this, std::placeholders::_1); + send_goal_options.result_callback = + std::bind(&TrajectoryUntilNode::trajectory_result_callback, this, std::placeholders::_1); + send_goal_options.feedback_callback = + std::bind(&TrajectoryUntilNode::trajectory_feedback_callback, this, std::placeholders::_1, std::placeholders::_2); + trajectory_action_client_->async_send_goal(goal_msg, send_goal_options); +} + +// Send the until goal, using the relevant fields supplied from the action server. +template +void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) +{ + // Setup goal + auto goal_msg = typename ActionType::Goal(); + auto send_goal_options = typename rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = + std::bind(&TrajectoryUntilNode::until_response_callback, this, std::placeholders::_1); + send_goal_options.result_callback = + std::bind(&TrajectoryUntilNode::until_result_callback, this, std::placeholders::_1); + + // Send goal + std::get(until_action_client_variant)->async_send_goal(goal_msg, send_goal_options); +} + +void TrajectoryUntilNode::trajectory_response_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + { + std::lock_guard guard(mutex_trajectory); + if (goal_handle) { + RCLCPP_INFO(this->get_logger(), "Trajectory accepted by server."); + current_trajectory_goal_handle_ = goal_handle; + trajectory_accepted_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Trajectory rejected by server."); + } + } + cv_trajectory_.notify_one(); +} + +template +void TrajectoryUntilNode::until_response_callback( + const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) +{ + { + std::lock_guard guard(mutex_until); + if (goal_handle) { + RCLCPP_INFO(this->get_logger(), "Until condition accepted by server."); + current_until_goal_handle_ = goal_handle; + until_accepted_ = true; + } else { + RCLCPP_ERROR(this->get_logger(), "Until condition was rejected by server."); + } + } + cv_until_.notify_one(); +} + +// Just forward feedback from trajectory controller. No feedback from until controllers so far. +void TrajectoryUntilNode::trajectory_feedback_callback( + const rclcpp_action::ClientGoalHandle::SharedPtr& /* goal_handle */, + const std::shared_ptr feedback) +{ + if (server_goal_handle_) { + prealloc_fb_->actual = feedback->actual; + prealloc_fb_->desired = feedback->desired; + prealloc_fb_->error = feedback->error; + prealloc_fb_->header = feedback->header; + prealloc_fb_->joint_names = feedback->joint_names; + if (server_goal_handle_) { + server_goal_handle_->publish_feedback(prealloc_fb_); + } + } +} + +// When a result is received from either trajectory or until condition, report it back to user +void TrajectoryUntilNode::trajectory_result_callback(const TrajectoryResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Trajectory result received."); + current_trajectory_goal_handle_.reset(); + report_goal(result); +} + +template +void TrajectoryUntilNode::until_result_callback( + const typename rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Until result received."); + current_until_goal_handle_.reset(); + report_goal(result); +} + +void TrajectoryUntilNode::report_goal(TrajectoryResult result) +{ + if (server_goal_handle_) { + prealloc_res_->until_condition_result = TrajectoryUntilAction::Result::NOT_TRIGGERED; + prealloc_res_->error_code = result.result->error_code; + prealloc_res_->error_string = result.result->error_string; + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + server_goal_handle_->succeed(prealloc_res_); + break; + + case rclcpp_action::ResultCode::ABORTED: + prealloc_res_->error_string += " Trajectory action was aborted. Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + + case rclcpp_action::ResultCode::CANCELED: + prealloc_res_->error_string += " Trajectory action was canceled."; + server_goal_handle_->canceled(prealloc_res_); + break; + + default: + prealloc_res_->error_string += " Unknown result code received from trajectory action, this should not happen. " + "Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(this->get_logger(), "%s", prealloc_res_->error_string.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), "Trajectory finished successfully, did not trigger until condition."); + } + } + reset_node(); +} + +template +void TrajectoryUntilNode::report_goal(UntilResult result) +{ + if (server_goal_handle_) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + prealloc_res_->error_code = TrajectoryUntilAction::Result::SUCCESSFUL; + prealloc_res_->until_condition_result = TrajectoryUntilAction::Result::TRIGGERED; + prealloc_res_->error_string += "Trajectory finished successfully by triggering until condition."; + server_goal_handle_->succeed(prealloc_res_); + break; + + case rclcpp_action::ResultCode::ABORTED: + prealloc_res_->error_string += "Until action was aborted. Aborting goal."; + server_goal_handle_->abort(prealloc_res_); + break; + + case rclcpp_action::ResultCode::CANCELED: + prealloc_res_->error_string += "Until action was canceled."; + server_goal_handle_->canceled(prealloc_res_); + break; + + default: + prealloc_res_->error_string += "Unknown result code received from until action, this should not happen. " + "Aborting " + "goal."; + server_goal_handle_->abort(prealloc_res_); + + break; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(this->get_logger(), "%s", prealloc_res_->error_string.c_str()); + } else { + RCLCPP_INFO(this->get_logger(), "Trajectory finished by triggering until condition."); + } + } + reset_node(); +} + +// Reset all variables and cancel running actions, if any. +void TrajectoryUntilNode::reset_node() +{ + if (current_until_goal_handle_) { + cancel_until_goal(); + } + if (current_trajectory_goal_handle_) { + trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); + } + current_trajectory_goal_handle_ = nullptr; + current_until_goal_handle_ = nullptr; + trajectory_accepted_ = false; + until_accepted_ = false; + prealloc_res_ = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb_ = std::make_shared(TrajectoryUntilAction::Feedback()); + + server_goal_handle_ = nullptr; +} + +// Cancel the action contained in the variant, regardless of what type it is. +void TrajectoryUntilNode::cancel_until_goal() +{ + std::visit([this](const auto& until_client) { until_client->async_cancel_goal(current_until_goal_handle_); }, + until_action_client_variant); +} + +} // namespace ur_robot_driver + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + // Use multithreaded executor because we have two callback groups + rclcpp::executors::MultiThreadedExecutor executor; + executor.add_node(node); + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/ur_robot_driver/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py new file mode 100644 index 000000000..7a687acd4 --- /dev/null +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python +# 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. + +import os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController +from ur_msgs.action import FollowJointTrajectoryUntil +from action_msgs.srv import CancelGoal_Response +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from builtin_interfaces.msg import Duration + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + ActionInterface, + ROBOT_JOINTS, + TIMEOUT_EXECUTE_TRAJECTORY, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix, initial_joint_controller", + [("", "scaled_joint_trajectory_controller"), ("my_ur_", "passthrough_trajectory_controller")], +) +def generate_test_description(tf_prefix, initial_joint_controller): + return generate_driver_test_description( + tf_prefix=tf_prefix, initial_joint_controller=initial_joint_controller + ) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + self._trajectory_until_interface = ActionInterface( + self.node, "/trajectory_until_node/execute", FollowJointTrajectoryUntil + ) + self.test_traj = { + "waypts": [[1.5, -1.5, 0.0, -1.5, -1.5, -1.5], [2.1, -1.2, 0.0, -2.4, -1.5, -1.5]], + "time_vec": [Duration(sec=3, nanosec=0), Duration(sec=6, nanosec=0)], + } + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + # + # Tests + # + + def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix): + 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] + + trajectory.points = [ + JointTrajectoryPoint( + positions=self.test_traj["waypts"][i], time_from_start=self.test_traj["time_vec"][i] + ) + for i in range(len(self.test_traj["waypts"])) + ] + goal_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._trajectory_until_interface.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectoryUntil.Result.SUCCESSFUL) + self.assertEqual( + result.until_condition_result, FollowJointTrajectoryUntil.Result.NOT_TRIGGERED + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) + + def test_trajectory_until_can_cancel(self, tf_prefix): + 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] + + trajectory.points = [ + JointTrajectoryPoint( + positions=self.test_traj["waypts"][i], time_from_start=self.test_traj["time_vec"][i] + ) + for i in range(len(self.test_traj["waypts"])) + ] + goal_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT + ) + self.assertTrue(goal_handle.accepted) + result = self._trajectory_until_interface.cancel_goal(goal_handle) + self.assertEqual(result.return_code, CancelGoal_Response.ERROR_NONE) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["tool_contact_controller"], + ).ok + ) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index e07462772..b3db11ece 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -53,10 +53,9 @@ ConfigurationInterface, generate_driver_test_description, ROBOT_JOINTS, + TIMEOUT_EXECUTE_TRAJECTORY, ) -TIMEOUT_EXECUTE_TRAJECTORY = 30 - @pytest.mark.launch_test @launch_testing.parametrize( diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 215f8ff55..d38c0e7ad 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -62,6 +62,7 @@ TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ "elbow_joint",