From c3e421a243c5d87c85c4ebaf3b88019bc8a3546e Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 22 Apr 2025 08:15:59 +0000 Subject: [PATCH 01/18] add trajectory until node --- .../ur_robot_driver/trajectory_until_node.hpp | 127 ++++++ ur_robot_driver/src/trajectory_until_node.cpp | 379 ++++++++++++++++++ 2 files changed, 506 insertions(+) create mode 100644 ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp create mode 100644 ur_robot_driver/src/trajectory_until_node.cpp 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..2c80a749a --- /dev/null +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -0,0 +1,127 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/bool.hpp" +#include +#include +#include +#include +#include +#include +#include +#include + +namespace ur_robot_driver +{ +class TrajectoryUntilNode : public rclcpp::Node +{ +private: + rclcpp_action::Server::SharedPtr action_server_; + rclcpp_action::Client::SharedPtr trajectory_action_client_; + rclcpp_action::Client::SharedPtr until_action_client_; + + rclcpp::CallbackGroup::SharedPtr server_callback_group; + rclcpp::CallbackGroup::SharedPtr clients_callback_group; + + std::shared_ptr> server_goal_handle_; + + void + until_response_callback(const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + void + until_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& result); + + 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); + + void 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 send_until_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; + +public: + explicit TrajectoryUntilNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~TrajectoryUntilNode(); +}; + +} // namespace ur_robot_driver + +#endif // UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_ 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..77697ab3c --- /dev/null +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -0,0 +1,379 @@ +// 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 +#include + +namespace ur_robot_driver +{ + +using UntilType = ur_msgs::action::ToolContact; +using TrajectoryUntil = ur_msgs::action::TrajectoryUntil; +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) + , until_action_client_(nullptr) + , 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(TrajectoryUntil::Result()); + prealloc_fb = std::make_shared(TrajectoryUntil::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/execute" + action_server_ = rclcpp_action::create_server( + this, std::string(this->get_name()) + "/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 until condition action client (This is not as extensible as I thought) +void TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) +{ + int type = goal->until_type; + switch (type) { + case TrajectoryUntil::Goal::TOOL_CONTACT: + until_action_client_ = rclcpp_action::create_client(this, + "/tool_contact_controller/" + "enable_tool_contact", + clients_callback_group); + break; + + default: + RCLCPP_ERROR(this->get_logger(), "Received unknown until-type."); + break; + } +} + +// 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) +{ + assign_until_action_client(goal); + if (!until_action_client_) { + RCLCPP_ERROR(this->get_logger(), "Until action server not defined, double check the types in the action " + "definition."); + return rclcpp_action::GoalResponse::REJECT; + } + if (!until_action_client_->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_ERROR(this->get_logger(), "Until action server not available."); + 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 (server_goal_handle_) { + RCLCPP_ERROR(this->get_logger(), "Node is currently busy, rejecting action goal."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Send action goal to until-controller and wait for it to be accepted. If it is not accepted within 1 seconds, it is + // assumed to be rejected. + send_until_goal(goal); + 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; + 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 */) +{ + if (current_until_goal_handle_) { + until_action_client_->async_cancel_goal(current_until_goal_handle_); + } + if (current_trajectory_goal_handle_) { + trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); + } + server_goal_handle_ = nullptr; + + 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. This should be extensible to other +// conditions, but isn't right now +void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) +{ + auto goal_msg = UntilType::Goal(); + auto send_goal_options = 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); + until_action_client_->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(); +} + +void TrajectoryUntilNode::until_response_callback( + const 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 */ +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; + server_goal_handle_->publish_feedback(prealloc_fb); + } +} + +void TrajectoryUntilNode::trajectory_result_callback( + const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Trajectory result received."); + current_trajectory_goal_handle_ = nullptr; + report_goal(result); +} + +void TrajectoryUntilNode::until_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& result) +{ + RCLCPP_INFO(this->get_logger(), "Until result received."); + current_until_goal_handle_ = nullptr; + report_goal(result); +} + +void TrajectoryUntilNode::report_goal(TrajectoryResult result) +{ + if (server_goal_handle_) { + prealloc_res->until_condition_result = TrajectoryUntil::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, this should not happen. Aborting goal."; + server_goal_handle_->abort(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(), 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_) { + prealloc_res->until_condition_result = result.result->result; + + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + prealloc_res->error_code = TrajectoryUntil::Result::SUCCESSFUL; + server_goal_handle_->succeed(prealloc_res); + break; + + case rclcpp_action::ResultCode::ABORTED: + prealloc_res->error_string = "Until action was aborted. Aborting goal."; + prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + server_goal_handle_->abort(prealloc_res); + break; + + case rclcpp_action::ResultCode::CANCELED: + prealloc_res->error_string = "Until action was canceled, this should not happen. Aborting goal."; + prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + server_goal_handle_->abort(prealloc_res); + break; + + default: + prealloc_res->error_string = "Unknown result code received from until action, this should not happen. Aborting " + "goal."; + prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + server_goal_handle_->abort(prealloc_res); + + break; + } + if (result.code != rclcpp_action::ResultCode::SUCCEEDED) { + RCLCPP_ERROR(this->get_logger(), 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_) { + until_action_client_->async_cancel_goal(current_until_goal_handle_); + } + 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(TrajectoryUntil::Result()); + prealloc_fb = std::make_shared(TrajectoryUntil::Feedback()); + until_action_client_ = nullptr; + server_goal_handle_ = nullptr; +} + +} // 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; +} From e20749fd74524dc30d7380011a3a48019505907a Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 17 Jun 2025 09:04:52 +0000 Subject: [PATCH 02/18] Work done so far. --- ur_robot_driver/CMakeLists.txt | 8 ++++++++ .../ur_robot_driver/trajectory_until_node.hpp | 3 +++ ur_robot_driver/src/trajectory_until_node.cpp | 15 ++++++++++++--- 3 files changed, 23 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 1a76a5cab..6e6ac418f 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -137,6 +137,14 @@ target_link_libraries(urscript_interface PUBLIC ur_client_library::urcl ) +# +# 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}) + install( TARGETS dashboard_client 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 index 2c80a749a..9be77f56f 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -42,6 +42,7 @@ #define UR_ROBOT_DRIVER__TRAJECTORY_UNTIL_NODE_HPP_ #include +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/bool.hpp" @@ -62,6 +63,8 @@ class TrajectoryUntilNode : public rclcpp::Node rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::Client::SharedPtr trajectory_action_client_; rclcpp_action::Client::SharedPtr until_action_client_; + // Add new until types here, when available + std::variant::SharedPtr> until_action_client_variant; rclcpp::CallbackGroup::SharedPtr server_callback_group; rclcpp::CallbackGroup::SharedPtr clients_callback_group; diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index 77697ab3c..47de2ca79 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -42,6 +42,12 @@ #include #include +template +struct until_container +{ + T until_client_type; +}; + namespace ur_robot_driver { @@ -113,6 +119,7 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( const rclcpp_action::GoalUUID& /* uuid */, std::shared_ptr goal) { assign_until_action_client(goal); + if (!until_action_client_) { RCLCPP_ERROR(this->get_logger(), "Until action server not defined, double check the types in the action " "definition."); @@ -308,23 +315,24 @@ template void TrajectoryUntilNode::report_goal(UntilResult result) { if (server_goal_handle_) { - prealloc_res->until_condition_result = result.result->result; - switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: prealloc_res->error_code = TrajectoryUntil::Result::SUCCESSFUL; + prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::TRIGGERED; server_goal_handle_->succeed(prealloc_res); break; case rclcpp_action::ResultCode::ABORTED: prealloc_res->error_string = "Until action was aborted. Aborting goal."; prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::ABORTED; server_goal_handle_->abort(prealloc_res); break; case rclcpp_action::ResultCode::CANCELED: prealloc_res->error_string = "Until action was canceled, this should not happen. Aborting goal."; prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::PREEMPTED; server_goal_handle_->abort(prealloc_res); break; @@ -332,6 +340,7 @@ void TrajectoryUntilNode::report_goal(UntilResult result) prealloc_res->error_string = "Unknown result code received from until action, this should not happen. Aborting " "goal."; prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; + prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::ABORTED; server_goal_handle_->abort(prealloc_res); break; @@ -369,7 +378,7 @@ void TrajectoryUntilNode::reset_node() int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); // Use multithreaded executor because we have two callback groups rclcpp::executors::MultiThreadedExecutor executor; executor.add_node(node); From c013b77fc4fb6a1b566a344c3cccef3ca07283c4 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 30 Jun 2025 11:52:24 +0000 Subject: [PATCH 03/18] FIx until node CMAKE --- ur_robot_driver/CMakeLists.txt | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 6e6ac418f..eb459e9e6 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -143,7 +143,12 @@ target_link_libraries(urscript_interface PUBLIC add_executable(trajectory_until_node src/trajectory_until_node.cpp ) -ament_target_dependencies(trajectory_until_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(trajectory_until_node PUBLIC + rclcpp::rclcpp + ${std_msgs_TARGETS} + rclcpp_action::rclcpp_action + ${ur_msgs_TARGETS} + ) install( TARGETS @@ -151,6 +156,7 @@ install( controller_stopper_node urscript_interface robot_state_helper + trajectory_until_node DESTINATION lib/${PROJECT_NAME} ) From 03052debe7ab8d20a94e2fdd8d542fdebbe15336 Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 1 Jul 2025 13:03:21 +0000 Subject: [PATCH 04/18] Rewrite to use a variant for the until type And add node to launch script Not quite done yet --- .../ur_robot_driver/trajectory_until_node.hpp | 35 +++++--- ur_robot_driver/launch/ur_control.launch.py | 11 +++ ur_robot_driver/src/trajectory_until_node.cpp | 89 ++++++++++++------- 3 files changed, 91 insertions(+), 44 deletions(-) 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 index 9be77f56f..308341618 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -44,8 +44,8 @@ #include #include -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/bool.hpp" +#include +#include #include #include #include @@ -55,26 +55,41 @@ #include #include + namespace ur_robot_driver { class TrajectoryUntilNode : public rclcpp::Node { +public: + explicit TrajectoryUntilNode(const rclcpp::NodeOptions& options = rclcpp::NodeOptions()); + ~TrajectoryUntilNode(); + private: rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::Client::SharedPtr trajectory_action_client_; - rclcpp_action::Client::SharedPtr until_action_client_; // Add new until types here, when available - std::variant::SharedPtr> until_action_client_variant; + // Append only, no changing the sequence + using tc_client = 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 rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + until_response_callback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); + + template void - until_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& result); + 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); @@ -84,7 +99,7 @@ class TrajectoryUntilNode : public rclcpp::Node void trajectory_result_callback( const rclcpp_action::ClientGoalHandle::WrappedResult& result); - void assign_until_action_client(std::shared_ptr goal); + bool assign_until_action_client(std::shared_ptr goal); rclcpp_action::GoalResponse goal_received_callback( const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); @@ -95,7 +110,7 @@ class TrajectoryUntilNode : public rclcpp::Node void send_trajectory_goal(std::shared_ptr goal); - void send_until_goal(std::shared_ptr goal); + void report_goal(rclcpp_action::ClientGoalHandle::WrappedResult result); @@ -119,10 +134,6 @@ class TrajectoryUntilNode : public rclcpp::Node 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_robot_driver diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 528da2ee7..dff02f4a5 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -158,6 +158,16 @@ 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 +231,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 index 47de2ca79..b47a2ace9 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -51,7 +51,7 @@ struct until_container namespace ur_robot_driver { -using UntilType = ur_msgs::action::ToolContact; +using TCAction = ur_msgs::action::ToolContact; using TrajectoryUntil = ur_msgs::action::TrajectoryUntil; using GoalHandleTrajectoryUntil = rclcpp_action::ServerGoalHandle; using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; @@ -61,7 +61,6 @@ using namespace std::chrono_literals; TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) : Node("TrajectoryUntilNode", options) - , until_action_client_(nullptr) , server_goal_handle_(nullptr) , current_trajectory_goal_handle_(nullptr) , current_until_goal_handle_(nullptr) @@ -97,38 +96,34 @@ TrajectoryUntilNode::~TrajectoryUntilNode() } // Assign the until condition action client (This is not as extensible as I thought) -void TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) +bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) { int type = goal->until_type; switch (type) { case TrajectoryUntil::Goal::TOOL_CONTACT: - until_action_client_ = rclcpp_action::create_client(this, + until_action_client_variant = rclcpp_action::create_client(this, "/tool_contact_controller/" - "enable_tool_contact", + "detect_tool_contact", clients_callback_group); break; default: RCLCPP_ERROR(this->get_logger(), "Received unknown until-type."); - break; + 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) { - assign_until_action_client(goal); - - if (!until_action_client_) { - RCLCPP_ERROR(this->get_logger(), "Until action server not defined, double check the types in the action " + 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 (!until_action_client_->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_ERROR(this->get_logger(), "Until action server not available."); - 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; @@ -138,9 +133,20 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( return rclcpp_action::GoalResponse::REJECT; } - // Send action goal to until-controller and wait for it to be accepted. If it is not accepted within 1 seconds, it is + // 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)){ + if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { + RCLCPP_ERROR(this->get_logger(), "Until action server not available."); + return rclcpp_action::GoalResponse::REJECT; + } + 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. - send_until_goal(goal); std::unique_lock until_lock(mutex_until); cv_until_.wait_for(until_lock, 1s); if (!until_accepted_) { @@ -164,6 +170,7 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( void TrajectoryUntilNode::goal_accepted_callback(const std::shared_ptr goal_handle) { server_goal_handle_ = goal_handle; + RCLCPP_INFO(this->get_logger(), "Accepted goal"); return; } @@ -172,12 +179,13 @@ rclcpp_action::CancelResponse TrajectoryUntilNode::goal_cancelled_callback(const std::shared_ptr /* goal_handle */) { if (current_until_goal_handle_) { - until_action_client_->async_cancel_goal(current_until_goal_handle_); + cancel_until_goal(); } if (current_trajectory_goal_handle_) { trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); } - server_goal_handle_ = nullptr; + + //server_goal_handle_ = nullptr; return rclcpp_action::CancelResponse::ACCEPT; } @@ -201,17 +209,20 @@ void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptrasync_send_goal(goal_msg, send_goal_options); } -// Send the until goal, using the relevant fields supplied from the action server. This should be extensible to other -// conditions, but isn't right now +// Send the until goal, using the relevant fields supplied from the action server. +template void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) { - auto goal_msg = UntilType::Goal(); - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + // 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); + 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); - until_action_client_->async_send_goal(goal_msg, send_goal_options); + 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( @@ -230,8 +241,9 @@ void TrajectoryUntilNode::trajectory_response_callback( cv_trajectory_.notify_one(); } +template void TrajectoryUntilNode::until_response_callback( - const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) + const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { { std::lock_guard guard(mutex_until); @@ -257,7 +269,9 @@ void TrajectoryUntilNode::trajectory_feedback_callback( prealloc_fb->error = feedback->error; prealloc_fb->header = feedback->header; prealloc_fb->joint_names = feedback->joint_names; - server_goal_handle_->publish_feedback(prealloc_fb); + if(server_goal_handle_){ + server_goal_handle_->publish_feedback(prealloc_fb); + } } } @@ -265,19 +279,21 @@ void TrajectoryUntilNode::trajectory_result_callback( const rclcpp_action::ClientGoalHandle::WrappedResult& result) { RCLCPP_INFO(this->get_logger(), "Trajectory result received."); - current_trajectory_goal_handle_ = nullptr; + current_trajectory_goal_handle_.reset(); report_goal(result); } -void TrajectoryUntilNode::until_result_callback(const rclcpp_action::ClientGoalHandle::WrappedResult& 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_ = nullptr; + current_until_goal_handle_.reset(); report_goal(result); } void TrajectoryUntilNode::report_goal(TrajectoryResult result) { + std::cout << "----------------------- Reporting trajectory goal ----------------------------------" << std::endl; if (server_goal_handle_) { prealloc_res->until_condition_result = TrajectoryUntil::Result::NOT_TRIGGERED; prealloc_res->error_code = result.result->error_code; @@ -314,6 +330,7 @@ void TrajectoryUntilNode::report_goal(TrajectoryResult result) template void TrajectoryUntilNode::report_goal(UntilResult result) { + std::cout << "----------------------- Reporting until goal ----------------------------------" << std::endl; if (server_goal_handle_) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: @@ -358,7 +375,7 @@ void TrajectoryUntilNode::report_goal(UntilResult result) void TrajectoryUntilNode::reset_node() { if (current_until_goal_handle_) { - until_action_client_->async_cancel_goal(current_until_goal_handle_); + cancel_until_goal(); } if (current_trajectory_goal_handle_) { trajectory_action_client_->async_cancel_goal(current_trajectory_goal_handle_); @@ -369,15 +386,23 @@ void TrajectoryUntilNode::reset_node() until_accepted_ = false; prealloc_res = std::make_shared(TrajectoryUntil::Result()); prealloc_fb = std::make_shared(TrajectoryUntil::Feedback()); - until_action_client_ = nullptr; + + //until_action_client_variant = std::monostate(); server_goal_handle_ = nullptr; } +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 = rclcpp::Node::make_shared("trajectory_until_node"); auto node = std::make_shared(); // Use multithreaded executor because we have two callback groups rclcpp::executors::MultiThreadedExecutor executor; From 89149c660acb4c05ef89dc92e836570f60710df4 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 11:20:56 +0000 Subject: [PATCH 05/18] Trajectory until node functionality complete --- ur_robot_driver/src/trajectory_until_node.cpp | 37 ++++++++----------- 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index b47a2ace9..59fd47b36 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -95,7 +95,6 @@ TrajectoryUntilNode::~TrajectoryUntilNode() { } -// Assign the until condition action client (This is not as extensible as I thought) bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) { int type = goal->until_type; @@ -119,6 +118,11 @@ bool TrajectoryUntilNode::assign_until_action_client(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."); @@ -128,12 +132,8 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( RCLCPP_ERROR(this->get_logger(), "Trajectory action server not available."); return rclcpp_action::GoalResponse::REJECT; } - if (server_goal_handle_) { - RCLCPP_ERROR(this->get_logger(), "Node is currently busy, rejecting action goal."); - return rclcpp_action::GoalResponse::REJECT; - } - // Check until action server, send action goal to until-controller and wait for it to be accepted. + // 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)){ if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_ERROR(this->get_logger(), "Until action server not available."); @@ -178,6 +178,7 @@ void TrajectoryUntilNode::goal_accepted_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(); } @@ -293,7 +294,6 @@ void TrajectoryUntilNode::until_result_callback(const typename rclcpp_action::Cl void TrajectoryUntilNode::report_goal(TrajectoryResult result) { - std::cout << "----------------------- Reporting trajectory goal ----------------------------------" << std::endl; if (server_goal_handle_) { prealloc_res->until_condition_result = TrajectoryUntil::Result::NOT_TRIGGERED; prealloc_res->error_code = result.result->error_code; @@ -309,8 +309,8 @@ void TrajectoryUntilNode::report_goal(TrajectoryResult result) break; case rclcpp_action::ResultCode::CANCELED: - prealloc_res->error_string += " Trajectory action was canceled, this should not happen. Aborting goal."; - server_goal_handle_->abort(prealloc_res); + 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. " @@ -330,34 +330,28 @@ void TrajectoryUntilNode::report_goal(TrajectoryResult result) template void TrajectoryUntilNode::report_goal(UntilResult result) { - std::cout << "----------------------- Reporting until goal ----------------------------------" << std::endl; if (server_goal_handle_) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: prealloc_res->error_code = TrajectoryUntil::Result::SUCCESSFUL; prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::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."; - prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; - prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::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, this should not happen. Aborting goal."; - prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; - prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::PREEMPTED; - server_goal_handle_->abort(prealloc_res); + 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 " + prealloc_res->error_string += "Unknown result code received from until action, this should not happen. Aborting " "goal."; - prealloc_res->error_code = TrajectoryUntil::Result::ABORTED; - prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::ABORTED; server_goal_handle_->abort(prealloc_res); break; @@ -387,10 +381,10 @@ void TrajectoryUntilNode::reset_node() prealloc_res = std::make_shared(TrajectoryUntil::Result()); prealloc_fb = std::make_shared(TrajectoryUntil::Feedback()); - //until_action_client_variant = std::monostate(); 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_); @@ -402,7 +396,6 @@ void TrajectoryUntilNode::cancel_until_goal(){ int main(int argc, char** argv) { rclcpp::init(argc, argv); - //auto node = rclcpp::Node::make_shared("trajectory_until_node"); auto node = std::make_shared(); // Use multithreaded executor because we have two callback groups rclcpp::executors::MultiThreadedExecutor executor; From b0f52d91570483f9690faece1332bfaa7441fb37 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 13:01:34 +0000 Subject: [PATCH 06/18] Write tests for trajectory until node --- ur_robot_driver/src/trajectory_until_node.cpp | 1 + .../test/integration_test_trajectory_until.py | 165 ++++++++++++++++++ 2 files changed, 166 insertions(+) create mode 100644 ur_robot_driver/test/integration_test_trajectory_until.py diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index 59fd47b36..cbf6bda47 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -312,6 +312,7 @@ void TrajectoryUntilNode::report_goal(TrajectoryResult result) 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."; 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..24ab4f363 --- /dev/null +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -0,0 +1,165 @@ +#!/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 logging + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController +from ur_msgs.action import TrajectoryUntil +from action_msgs.msg import GoalStatus +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", TrajectoryUntil + ) + 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=TrajectoryUntil.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, TrajectoryUntil.Result.SUCCESSFUL) + self.assertEqual(result.until_condition_result, TrajectoryUntil.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=TrajectoryUntil.Goal.TOOL_CONTACT) + self.assertTrue(goal_handle.accepted) + time.sleep(2) + result = self._trajectory_until_interface.cancel_goal(goal_handle) + time.sleep(2) + 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 + ) + From e7d67680182f8c6db590908e1f8484f10940e0c6 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 13:02:54 +0000 Subject: [PATCH 07/18] Move TIMEOUT_EXECUTE_TRAJECTORY to test_common file So it can be reused elsewhere --- ur_robot_driver/test/robot_driver.py | 3 +-- ur_robot_driver/test/test_common.py | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) 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", From ddfdaee1603849442c14be5923cc74913262ef7e Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 13:17:01 +0000 Subject: [PATCH 08/18] Add documentation --- ur_robot_driver/doc/index.rst | 1 + ur_robot_driver/doc/trajectory_until_node.rst | 22 +++++++++++++++++++ 2 files changed, 23 insertions(+) create mode 100644 ur_robot_driver/doc/trajectory_until_node.rst 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..8e59198c3 --- /dev/null +++ b/ur_robot_driver/doc/trajectory_until_node.rst @@ -0,0 +1,22 @@ +.. _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. \ No newline at end of file From e32a8f2fc864f046c4a27df46271abaae45905b6 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 13:28:36 +0000 Subject: [PATCH 09/18] Clean up and comments --- .../ur_robot_driver/trajectory_until_node.hpp | 9 +++----- ur_robot_driver/src/trajectory_until_node.cpp | 22 +++++++------------ 2 files changed, 11 insertions(+), 20 deletions(-) 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 index 308341618..c82c6b27a 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -67,11 +67,10 @@ class TrajectoryUntilNode : public rclcpp::Node private: rclcpp_action::Server::SharedPtr action_server_; rclcpp_action::Client::SharedPtr trajectory_action_client_; - // Add new until types here, when available - // Append only, no changing the sequence - using tc_client = rclcpp_action::Client::SharedPtr; - std::variant until_action_client_variant; + // 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; @@ -110,8 +109,6 @@ class TrajectoryUntilNode : public rclcpp::Node void send_trajectory_goal(std::shared_ptr goal); - - void report_goal(rclcpp_action::ClientGoalHandle::WrappedResult result); template diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index cbf6bda47..862bd0848 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -42,12 +42,6 @@ #include #include -template -struct until_container -{ - T until_client_type; -}; - namespace ur_robot_driver { @@ -82,7 +76,7 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) "trajectory", clients_callback_group); - // Create action server to advertise the "/trajectory_until/execute" + // Create action server to advertise the "/trajectory_until_node/execute" action_server_ = rclcpp_action::create_server( this, std::string(this->get_name()) + "/execute", std::bind(&TrajectoryUntilNode::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), @@ -95,6 +89,7 @@ TrajectoryUntilNode::~TrajectoryUntilNode() { } +// Assign the correct type of action client to the variant bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) { int type = goal->until_type; @@ -134,12 +129,12 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( } // 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)){ - if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { + if(std::holds_alternative(until_action_client_variant)){ + if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_ERROR(this->get_logger(), "Until action server not available."); return rclcpp_action::GoalResponse::REJECT; } - send_until_goal(goal); + send_until_goal(goal); } else{ throw std::runtime_error("Until type not implemented. This should not happen."); @@ -186,8 +181,6 @@ TrajectoryUntilNode::goal_cancelled_callback(const std::shared_ptrasync_cancel_goal(current_trajectory_goal_handle_); } - //server_goal_handle_ = nullptr; - return rclcpp_action::CancelResponse::ACCEPT; } @@ -259,7 +252,7 @@ void TrajectoryUntilNode::until_response_callback( cv_until_.notify_one(); } -/* Just forward feedback from trajectory controller */ +// 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) @@ -276,6 +269,7 @@ void TrajectoryUntilNode::trajectory_feedback_callback( } } +// When a result is received from either trajectory or until condition, report it back to user void TrajectoryUntilNode::trajectory_result_callback( const rclcpp_action::ClientGoalHandle::WrappedResult& result) { @@ -312,7 +306,7 @@ void TrajectoryUntilNode::report_goal(TrajectoryResult result) 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."; From 2603a1e0420dce9020dedd4e044071aa45a68903 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 18 Jul 2025 13:44:59 +0000 Subject: [PATCH 10/18] pre-commit changes --- ur_robot_driver/CMakeLists.txt | 2 +- ur_robot_driver/doc/trajectory_until_node.rst | 2 +- .../ur_robot_driver/trajectory_until_node.hpp | 13 ++--- ur_robot_driver/launch/ur_control.launch.py | 7 ++- ur_robot_driver/src/trajectory_until_node.cpp | 47 ++++++++++--------- .../test/integration_test_trajectory_until.py | 17 ++++--- 6 files changed, 46 insertions(+), 42 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index eb459e9e6..94b65c702 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -148,7 +148,7 @@ target_link_libraries(trajectory_until_node PUBLIC ${std_msgs_TARGETS} rclcpp_action::rclcpp_action ${ur_msgs_TARGETS} - ) +) install( TARGETS diff --git a/ur_robot_driver/doc/trajectory_until_node.rst b/ur_robot_driver/doc/trajectory_until_node.rst index 8e59198c3..0d003f0ac 100644 --- a/ur_robot_driver/doc/trajectory_until_node.rst +++ b/ur_robot_driver/doc/trajectory_until_node.rst @@ -19,4 +19,4 @@ 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. \ No newline at end of file +If you wish to use the node with another motion controller use the launch argument ``initial_joint_controller:=`` when launching the driver. 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 index c82c6b27a..25deff7f3 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -55,7 +55,6 @@ #include #include - namespace ur_robot_driver { class TrajectoryUntilNode : public rclcpp::Node @@ -77,16 +76,14 @@ class TrajectoryUntilNode : public rclcpp::Node std::shared_ptr> server_goal_handle_; - template + 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_response_callback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); - template - void - until_result_callback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& result); + template + void until_result_callback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& result); void cancel_until_goal(); diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index dff02f4a5..bdd2131ef 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -164,8 +164,11 @@ def launch_setup(context): name="trajectory_until_node", output="screen", remappings=[ - ('/motion_controller/follow_joint_trajectory', f'/{initial_joint_controller.perform(context)}/follow_joint_trajectory'), - ] + ( + "/motion_controller/follow_joint_trajectory", + f"/{initial_joint_controller.perform(context)}/follow_joint_trajectory", + ), + ], ) # Spawn controllers diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index 862bd0848..a5779dc5e 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -96,9 +96,9 @@ bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr(this, - "/tool_contact_controller/" - "detect_tool_contact", - clients_callback_group); + "/tool_contact_controller/" + "detect_tool_contact", + clients_callback_group); break; default: @@ -118,7 +118,7 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( return rclcpp_action::GoalResponse::REJECT; } - if(!assign_until_action_client(goal)){ + 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; @@ -129,17 +129,16 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( } // 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)){ + if (std::holds_alternative(until_action_client_variant)) { if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { RCLCPP_ERROR(this->get_logger(), "Until action server not available."); return rclcpp_action::GoalResponse::REJECT; } send_until_goal(goal); - } - else{ + } 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); @@ -204,19 +203,19 @@ void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptr +template void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) { - // Setup 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); + 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); + 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); + std::get(until_action_client_variant)->async_send_goal(goal_msg, send_goal_options); } void TrajectoryUntilNode::trajectory_response_callback( @@ -235,7 +234,7 @@ void TrajectoryUntilNode::trajectory_response_callback( cv_trajectory_.notify_one(); } -template +template void TrajectoryUntilNode::until_response_callback( const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { @@ -263,7 +262,7 @@ void TrajectoryUntilNode::trajectory_feedback_callback( prealloc_fb->error = feedback->error; prealloc_fb->header = feedback->header; prealloc_fb->joint_names = feedback->joint_names; - if(server_goal_handle_){ + if (server_goal_handle_) { server_goal_handle_->publish_feedback(prealloc_fb); } } @@ -278,8 +277,9 @@ void TrajectoryUntilNode::trajectory_result_callback( report_goal(result); } -template -void TrajectoryUntilNode::until_result_callback(const typename rclcpp_action::ClientGoalHandle::WrappedResult& 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(); @@ -345,8 +345,9 @@ void TrajectoryUntilNode::report_goal(UntilResult result) break; default: - prealloc_res->error_string += "Unknown result code received from until action, this should not happen. Aborting " - "goal."; + prealloc_res->error_string += "Unknown result code received from until action, this should not happen. " + "Aborting " + "goal."; server_goal_handle_->abort(prealloc_res); break; @@ -380,10 +381,10 @@ void TrajectoryUntilNode::reset_node() } // 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); +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 diff --git a/ur_robot_driver/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py index 24ab4f363..b19867000 100644 --- a/ur_robot_driver/test/integration_test_trajectory_until.py +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -31,7 +31,6 @@ import sys import time import unittest -import logging import pytest @@ -41,7 +40,6 @@ from controller_manager_msgs.srv import SwitchController from ur_msgs.action import TrajectoryUntil -from action_msgs.msg import GoalStatus from action_msgs.srv import CancelGoal_Response from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from builtin_interfaces.msg import Duration @@ -64,7 +62,9 @@ [("", "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) + return generate_driver_test_description( + tf_prefix=tf_prefix, initial_joint_controller=initial_joint_controller + ) class RobotDriverTest(unittest.TestCase): @@ -119,7 +119,9 @@ def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix): ) for i in range(len(self.test_traj["waypts"])) ] - goal_handle = self._trajectory_until_interface.send_goal(trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT) + goal_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT + ) self.assertTrue(goal_handle.accepted) if goal_handle.accepted: result = self._trajectory_until_interface.get_result( @@ -133,7 +135,7 @@ def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix): deactivate_controllers=["tool_contact_controller"], ).ok ) - + def test_trajectory_until_can_cancel(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller( @@ -150,7 +152,9 @@ def test_trajectory_until_can_cancel(self, tf_prefix): ) for i in range(len(self.test_traj["waypts"])) ] - goal_handle = self._trajectory_until_interface.send_goal(trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT) + goal_handle = self._trajectory_until_interface.send_goal( + trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT + ) self.assertTrue(goal_handle.accepted) time.sleep(2) result = self._trajectory_until_interface.cancel_goal(goal_handle) @@ -162,4 +166,3 @@ def test_trajectory_until_can_cancel(self, tf_prefix): deactivate_controllers=["tool_contact_controller"], ).ok ) - From 73508578d7672c435d863570e220914d13fc49b9 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 30 Jul 2025 09:17:30 +0000 Subject: [PATCH 11/18] Change includes and rename action --- .../ur_robot_driver/trajectory_until_node.hpp | 24 ++++++----- ur_robot_driver/src/trajectory_until_node.cpp | 40 +++++++++---------- 2 files changed, 32 insertions(+), 32 deletions(-) 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 index 25deff7f3..2bcbbe472 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -52,7 +52,7 @@ #include #include #include -#include +#include #include namespace ur_robot_driver @@ -64,7 +64,9 @@ class TrajectoryUntilNode : public rclcpp::Node ~TrajectoryUntilNode(); private: - rclcpp_action::Server::SharedPtr action_server_; + 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 @@ -74,10 +76,10 @@ class TrajectoryUntilNode : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr server_callback_group; rclcpp::CallbackGroup::SharedPtr clients_callback_group; - std::shared_ptr> server_goal_handle_; + std::shared_ptr> server_goal_handle_; template - void send_until_goal(std::shared_ptr goal); + void send_until_goal(std::shared_ptr goal); template void until_response_callback(const typename rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle); @@ -95,16 +97,16 @@ class TrajectoryUntilNode : public rclcpp::Node void trajectory_result_callback( const rclcpp_action::ClientGoalHandle::WrappedResult& result); - bool assign_until_action_client(std::shared_ptr goal); + bool assign_until_action_client(std::shared_ptr goal); rclcpp_action::GoalResponse goal_received_callback( - const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); + const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal); void goal_accepted_callback( - const std::shared_ptr> goal_handle); + const std::shared_ptr> goal_handle); rclcpp_action::CancelResponse goal_cancelled_callback( - const std::shared_ptr> goal_handle); + const std::shared_ptr> goal_handle); - void send_trajectory_goal(std::shared_ptr goal); + void send_trajectory_goal(std::shared_ptr goal); void report_goal(rclcpp_action::ClientGoalHandle::WrappedResult result); @@ -117,9 +119,9 @@ class TrajectoryUntilNode : public rclcpp::Node current_trajectory_goal_handle_; rclcpp_action::ClientGoalHandle::SharedPtr current_until_goal_handle_; - std::shared_ptr prealloc_res; + std::shared_ptr prealloc_res; - std::shared_ptr prealloc_fb; + std::shared_ptr prealloc_fb; std::atomic trajectory_accepted_; std::atomic until_accepted_; diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index a5779dc5e..226f1d17b 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -46,8 +46,7 @@ namespace ur_robot_driver { using TCAction = ur_msgs::action::ToolContact; -using TrajectoryUntil = ur_msgs::action::TrajectoryUntil; -using GoalHandleTrajectoryUntil = rclcpp_action::ServerGoalHandle; +using GoalHandleTrajectoryUntil = rclcpp_action::ServerGoalHandle; using FollowJointTrajectory = control_msgs::action::FollowJointTrajectory; using TrajectoryResult = rclcpp_action::ClientGoalHandle::WrappedResult; @@ -61,8 +60,8 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) , trajectory_accepted_(false) , until_accepted_(false) { - prealloc_res = std::make_shared(TrajectoryUntil::Result()); - prealloc_fb = std::make_shared(TrajectoryUntil::Feedback()); + 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); @@ -77,7 +76,7 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) clients_callback_group); // Create action server to advertise the "/trajectory_until_node/execute" - action_server_ = rclcpp_action::create_server( + action_server_ = rclcpp_action::create_server( this, std::string(this->get_name()) + "/execute", std::bind(&TrajectoryUntilNode::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), std::bind(&TrajectoryUntilNode::goal_cancelled_callback, this, std::placeholders::_1), @@ -90,11 +89,10 @@ TrajectoryUntilNode::~TrajectoryUntilNode() } // Assign the correct type of action client to the variant -bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) +bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) { - int type = goal->until_type; - switch (type) { - case TrajectoryUntil::Goal::TOOL_CONTACT: + switch (goal->until_type) { + case TrajectoryUntilAction::Goal::TOOL_CONTACT: until_action_client_variant = rclcpp_action::create_client(this, "/tool_contact_controller/" "detect_tool_contact", @@ -111,7 +109,7 @@ bool TrajectoryUntilNode::assign_until_action_client(std::shared_ptr goal) + 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."); @@ -184,7 +182,7 @@ TrajectoryUntilNode::goal_cancelled_callback(const std::shared_ptr goal) +void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptr goal) { auto goal_msg = FollowJointTrajectory::Goal(); goal_msg.trajectory = goal->trajectory; @@ -204,7 +202,7 @@ void TrajectoryUntilNode::send_trajectory_goal(std::shared_ptr -void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) +void TrajectoryUntilNode::send_until_goal(std::shared_ptr /* goal */) { // Setup goal auto goal_msg = typename ActionType::Goal(); @@ -219,7 +217,7 @@ void TrajectoryUntilNode::send_until_goal(std::shared_ptr::SharedPtr& goal_handle) + const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { { std::lock_guard guard(mutex_trajectory); @@ -253,8 +251,8 @@ void TrajectoryUntilNode::until_response_callback( // 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) + const rclcpp_action::ClientGoalHandle::SharedPtr& /* goal_handle */, + const std::shared_ptr feedback) { if (server_goal_handle_) { prealloc_fb->actual = feedback->actual; @@ -270,7 +268,7 @@ void TrajectoryUntilNode::trajectory_feedback_callback( // When a result is received from either trajectory or until condition, report it back to user void TrajectoryUntilNode::trajectory_result_callback( - const rclcpp_action::ClientGoalHandle::WrappedResult& result) + const TrajectoryResult& result) { RCLCPP_INFO(this->get_logger(), "Trajectory result received."); current_trajectory_goal_handle_.reset(); @@ -289,7 +287,7 @@ void TrajectoryUntilNode::until_result_callback( void TrajectoryUntilNode::report_goal(TrajectoryResult result) { if (server_goal_handle_) { - prealloc_res->until_condition_result = TrajectoryUntil::Result::NOT_TRIGGERED; + 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) { @@ -328,8 +326,8 @@ void TrajectoryUntilNode::report_goal(UntilResult result) if (server_goal_handle_) { switch (result.code) { case rclcpp_action::ResultCode::SUCCEEDED: - prealloc_res->error_code = TrajectoryUntil::Result::SUCCESSFUL; - prealloc_res->until_condition_result = ur_msgs::action::TrajectoryUntil::Result::TRIGGERED; + 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; @@ -374,8 +372,8 @@ void TrajectoryUntilNode::reset_node() current_until_goal_handle_ = nullptr; trajectory_accepted_ = false; until_accepted_ = false; - prealloc_res = std::make_shared(TrajectoryUntil::Result()); - prealloc_fb = std::make_shared(TrajectoryUntil::Feedback()); + prealloc_res = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb = std::make_shared(TrajectoryUntilAction::Feedback()); server_goal_handle_ = nullptr; } From f1640c5627d37e05a40aaada4ffe1d7d54d00932 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 30 Jul 2025 09:23:35 +0000 Subject: [PATCH 12/18] formatting --- .../ur_robot_driver/trajectory_until_node.hpp | 12 ++++++------ ur_robot_driver/src/trajectory_until_node.cpp | 3 +-- 2 files changed, 7 insertions(+), 8 deletions(-) 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 index 2bcbbe472..83dfffd29 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -99,12 +99,12 @@ class TrajectoryUntilNode : public rclcpp::Node 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); + 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); diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index 226f1d17b..d70472823 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -267,8 +267,7 @@ void TrajectoryUntilNode::trajectory_feedback_callback( } // When a result is received from either trajectory or until condition, report it back to user -void TrajectoryUntilNode::trajectory_result_callback( - const TrajectoryResult& result) +void TrajectoryUntilNode::trajectory_result_callback(const TrajectoryResult& result) { RCLCPP_INFO(this->get_logger(), "Trajectory result received."); current_trajectory_goal_handle_.reset(); From 461fe06357e28c27b9b5fe7d4b838246419b5e61 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 30 Jul 2025 14:30:57 +0200 Subject: [PATCH 13/18] Add an example using the move_until_node --- ur_robot_driver/CMakeLists.txt | 1 + .../examples/move_until_example.py | 132 ++++++++++++++++++ 2 files changed, 133 insertions(+) create mode 100755 ur_robot_driver/examples/move_until_example.py diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 94b65c702..3eb4a8e0c 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -225,6 +225,7 @@ install(PROGRAMS scripts/start_ursim.sh examples/examples.py examples/force_mode.py + examples/trajectory_until.py DESTINATION lib/${PROJECT_NAME} ) 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() From 6cb3d58bb940dbad3947ad94943de7909b46f786 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 30 Jul 2025 14:31:23 +0200 Subject: [PATCH 14/18] Fixed and added move_until integration test --- ur_robot_driver/CMakeLists.txt | 4 ++++ .../test/integration_test_trajectory_until.py | 16 ++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 3eb4a8e0c..48a9c3ad7 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -278,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/test/integration_test_trajectory_until.py b/ur_robot_driver/test/integration_test_trajectory_until.py index b19867000..7a687acd4 100644 --- a/ur_robot_driver/test/integration_test_trajectory_until.py +++ b/ur_robot_driver/test/integration_test_trajectory_until.py @@ -39,7 +39,7 @@ from rclpy.node import Node from controller_manager_msgs.srv import SwitchController -from ur_msgs.action import TrajectoryUntil +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 @@ -87,7 +87,7 @@ def init_robot(self): 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", TrajectoryUntil + 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]], @@ -120,15 +120,17 @@ def test_trajectory_with_tool_contact_no_trigger_succeeds(self, tf_prefix): for i in range(len(self.test_traj["waypts"])) ] goal_handle = self._trajectory_until_interface.send_goal( - trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT + 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, TrajectoryUntil.Result.SUCCESSFUL) - self.assertEqual(result.until_condition_result, TrajectoryUntil.Result.NOT_TRIGGERED) + 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, @@ -153,12 +155,10 @@ def test_trajectory_until_can_cancel(self, tf_prefix): for i in range(len(self.test_traj["waypts"])) ] goal_handle = self._trajectory_until_interface.send_goal( - trajectory=trajectory, until_type=TrajectoryUntil.Goal.TOOL_CONTACT + trajectory=trajectory, until_type=FollowJointTrajectoryUntil.Goal.TOOL_CONTACT ) self.assertTrue(goal_handle.accepted) - time.sleep(2) result = self._trajectory_until_interface.cancel_goal(goal_handle) - time.sleep(2) self.assertEqual(result.return_code, CancelGoal_Response.ERROR_NONE) self.assertTrue( self._controller_manager_interface.switch_controller( From e8e2f94fb19b17dfd995e95c79109f9fe2de4779 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 30 Jul 2025 14:32:04 +0200 Subject: [PATCH 15/18] Fix member naming --- .../ur_robot_driver/trajectory_until_node.hpp | 4 +- ur_robot_driver/src/trajectory_until_node.cpp | 73 +++++++++---------- 2 files changed, 38 insertions(+), 39 deletions(-) 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 index 83dfffd29..cebe3ab74 100644 --- a/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp +++ b/ur_robot_driver/include/ur_robot_driver/trajectory_until_node.hpp @@ -119,9 +119,9 @@ class TrajectoryUntilNode : public rclcpp::Node current_trajectory_goal_handle_; rclcpp_action::ClientGoalHandle::SharedPtr current_until_goal_handle_; - std::shared_ptr prealloc_res; + std::shared_ptr prealloc_res_; - std::shared_ptr prealloc_fb; + std::shared_ptr prealloc_fb_; std::atomic trajectory_accepted_; std::atomic until_accepted_; diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index d70472823..ab79bd75a 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -40,7 +40,6 @@ #include #include -#include namespace ur_robot_driver { @@ -60,8 +59,8 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) , trajectory_accepted_(false) , until_accepted_(false) { - prealloc_res = std::make_shared(TrajectoryUntilAction::Result()); - prealloc_fb = std::make_shared(TrajectoryUntilAction::Feedback()); + 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); @@ -77,7 +76,7 @@ TrajectoryUntilNode::TrajectoryUntilNode(const rclcpp::NodeOptions& options) // Create action server to advertise the "/trajectory_until_node/execute" action_server_ = rclcpp_action::create_server( - this, std::string(this->get_name()) + "/execute", + 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), @@ -255,13 +254,13 @@ void TrajectoryUntilNode::trajectory_feedback_callback( 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; + 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); + server_goal_handle_->publish_feedback(prealloc_fb_); } } } @@ -286,32 +285,32 @@ void TrajectoryUntilNode::until_result_callback( 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; + 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); + 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); + 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); + 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); + 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(), prealloc_res->error_string.c_str()); + 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."); } @@ -325,32 +324,32 @@ 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); + 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); + 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); + 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); + 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(), prealloc_res->error_string.c_str()); + RCLCPP_ERROR(this->get_logger(), "%s", prealloc_res_->error_string.c_str()); } else { RCLCPP_INFO(this->get_logger(), "Trajectory finished by triggering until condition."); } @@ -371,8 +370,8 @@ void TrajectoryUntilNode::reset_node() 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()); + prealloc_res_ = std::make_shared(TrajectoryUntilAction::Result()); + prealloc_fb_ = std::make_shared(TrajectoryUntilAction::Feedback()); server_goal_handle_ = nullptr; } From a1df413d3d88867ea378e440e427a6f778ec7311 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 30 Jul 2025 14:33:03 +0000 Subject: [PATCH 16/18] Use std::visit to wait for action server --- ur_robot_driver/src/trajectory_until_node.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/ur_robot_driver/src/trajectory_until_node.cpp b/ur_robot_driver/src/trajectory_until_node.cpp index ab79bd75a..3e878bd01 100644 --- a/ur_robot_driver/src/trajectory_until_node.cpp +++ b/ur_robot_driver/src/trajectory_until_node.cpp @@ -120,17 +120,20 @@ rclcpp_action::GoalResponse TrajectoryUntilNode::goal_received_callback( "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)) { - if (!std::get(until_action_client_variant)->wait_for_action_server(std::chrono::seconds(1))) { - RCLCPP_ERROR(this->get_logger(), "Until action server not available."); - return rclcpp_action::GoalResponse::REJECT; - } send_until_goal(goal); } else { throw std::runtime_error("Until type not implemented. This should not happen."); From 518400721ea615862106e944cc588518c5ab33eb Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 30 Jul 2025 14:33:45 +0000 Subject: [PATCH 17/18] Put correct example file name in Cmake --- ur_robot_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 48a9c3ad7..f564c8c4a 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -225,7 +225,7 @@ install(PROGRAMS scripts/start_ursim.sh examples/examples.py examples/force_mode.py - examples/trajectory_until.py + examples/move_until_example.py DESTINATION lib/${PROJECT_NAME} ) From b8ee86dbdd0534d21efca3dad970c87c6477ce75 Mon Sep 17 00:00:00 2001 From: Jacob Larsen Date: Wed, 30 Jul 2025 14:36:01 +0000 Subject: [PATCH 18/18] Mention which motion controllers can be used in the docs --- ur_robot_driver/doc/trajectory_until_node.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/ur_robot_driver/doc/trajectory_until_node.rst b/ur_robot_driver/doc/trajectory_until_node.rst index 0d003f0ac..53aa6d63e 100644 --- a/ur_robot_driver/doc/trajectory_until_node.rst +++ b/ur_robot_driver/doc/trajectory_until_node.rst @@ -20,3 +20,4 @@ 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.