From 6bf35ab1f93ed1541a1854c0b245f83026d27e52 Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 14 Jan 2025 15:20:32 +0000 Subject: [PATCH 1/4] Added an action definition for use with the tool contact controller (WIP) --- CMakeLists.txt | 5 +++++ action/ToolContact.action | 8 ++++++++ package.xml | 1 + 3 files changed, 14 insertions(+) create mode 100644 action/ToolContact.action diff --git a/CMakeLists.txt b/CMakeLists.txt index c33e3d1..d28477e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -25,6 +25,10 @@ set(srv_files srv/SetForceMode.srv ) +set(action_files + action/ToolContact.action +) + if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() @@ -33,6 +37,7 @@ endif() rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} + ${action_files} DEPENDENCIES builtin_interfaces geometry_msgs ADD_LINTER_TESTS ) diff --git a/action/ToolContact.action b/action/ToolContact.action new file mode 100644 index 0000000..e1e37b5 --- /dev/null +++ b/action/ToolContact.action @@ -0,0 +1,8 @@ + +--- +uint8 SUCCESS=1 +uint8 CANCELLED_BY_USER=2 +uint8 ABORTED_BY_HARDWARE=3 +uint8 ABORTED_BY_CONTROLLER=4 +uint8 result +--- diff --git a/package.xml b/package.xml index 8e9665a..7798154 100644 --- a/package.xml +++ b/package.xml @@ -16,6 +16,7 @@ builtin_interfaces geometry_msgs + action_msgs rosidl_default_runtime From 4e3127d1d13e86fd33ee3a7094a6bbce8402d002 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 23 Jan 2025 14:05:35 +0000 Subject: [PATCH 2/4] Added actions for TrajectoryUntil and ToolContact --- CMakeLists.txt | 7 +++- action/ToolContact.action | 10 +++--- action/TrajectoryUntil.action | 66 +++++++++++++++++++++++++++++++++++ 3 files changed, 77 insertions(+), 6 deletions(-) create mode 100644 action/TrajectoryUntil.action diff --git a/CMakeLists.txt b/CMakeLists.txt index d28477e..9951976 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,6 +5,10 @@ find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(control_msgs REQUIRED) + set(msg_files msg/Analog.msg @@ -27,6 +31,7 @@ set(srv_files set(action_files action/ToolContact.action + action/TrajectoryUntil.action ) if(BUILD_TESTING) @@ -38,7 +43,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} ${action_files} - DEPENDENCIES builtin_interfaces geometry_msgs + DEPENDENCIES builtin_interfaces geometry_msgs trajectory_msgs std_msgs control_msgs ADD_LINTER_TESTS ) diff --git a/action/ToolContact.action b/action/ToolContact.action index e1e37b5..5ffbff5 100644 --- a/action/ToolContact.action +++ b/action/ToolContact.action @@ -1,8 +1,8 @@ --- -uint8 SUCCESS=1 -uint8 CANCELLED_BY_USER=2 -uint8 ABORTED_BY_HARDWARE=3 -uint8 ABORTED_BY_CONTROLLER=4 -uint8 result +int32 SUCCESS=0 +int32 CANCELLED_BY_USER = -1 +int32 ABORTED_BY_HARDWARE = -2 +int32 ABORTED_BY_CONTROLLER = -3 +int32 result --- diff --git a/action/TrajectoryUntil.action b/action/TrajectoryUntil.action new file mode 100644 index 0000000..30d9eb0 --- /dev/null +++ b/action/TrajectoryUntil.action @@ -0,0 +1,66 @@ +# The joint trajectory to follow +trajectory_msgs/JointTrajectory trajectory + +# Tolerances for the trajectory. If the measured joint values fall +# outside the tolerances the trajectory goal is aborted. Any +# tolerances that are not specified (by being omitted or set to 0) are +# set to the defaults for the action server (often taken from the +# parameter server). + +# Tolerances applied to the joints as the trajectory is executed. If +# violated, the goal aborts with error_code set to +# PATH_TOLERANCE_VIOLATED. +control_msgs/JointTolerance[] path_tolerance + +# To report success, the joints must be within goal_tolerance of the +# final trajectory value. The goal must be achieved by time the +# trajectory ends plus goal_time_tolerance. (goal_time_tolerance +# allows some leeway in time, so that the trajectory goal can still +# succeed even if the joints reach the goal some time after the +# precise end time of the trajectory). +# +# If the joints are not within goal_tolerance after "trajectory finish +# time" + goal_time_tolerance, the goal aborts with error_code set to +# GOAL_TOLERANCE_VIOLATED +control_msgs/JointTolerance[] goal_tolerance +builtin_interfaces/Duration goal_time_tolerance + +# The condition until which the trajectory should run. +uint8 until_type 0 +uint8 TOOL_CONTACT = 0 + +--- +# The trajectory will be reported as succesful if either the trajectory finishes, or the until condition is met. +#The error codes will be used if the trajectory fails and the until condition is not met. +int32 error_code +int32 SUCCESSFUL = 0 +int32 INVALID_GOAL = -1 +int32 INVALID_JOINTS = -2 +int32 OLD_HEADER_TIMESTAMP = -3 +int32 PATH_TOLERANCE_VIOLATED = -4 +int32 GOAL_TOLERANCE_VIOLATED = -5 +int32 ABORTED = -6 + +int32 until_condition_result +int32 TRIGGERED = 0 +int32 NOT_TRIGGERED = 1 +int32 CANCELLED_BY_USER = -1 +int32 ABORTED_BY_HARDWARE = -2 +int32 ABORTED_BY_CONTROLLER = -3 + +# Human readable description of the error code. Contains complementary +# information that is especially useful when execution fails, for instance: +# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested +# trajectory is in the past). +# - INVALID_JOINTS: The mismatch between the expected controller joints +# and those provided in the goal. +# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint +# violated which tolerance, and by how much. +string error_string + +--- +std_msgs/Header header +string[] joint_names +trajectory_msgs/JointTrajectoryPoint desired +trajectory_msgs/JointTrajectoryPoint actual +trajectory_msgs/JointTrajectoryPoint error From bc252947b202a53aa68317af8f42406e0be6ab49 Mon Sep 17 00:00:00 2001 From: URJala Date: Fri, 14 Feb 2025 16:37:20 +0000 Subject: [PATCH 3/4] rename constants and remove trajectory_until definition --- action/ToolContact.action | 5 ++- action/TrajectoryUntil.action | 66 ----------------------------------- 2 files changed, 2 insertions(+), 69 deletions(-) delete mode 100644 action/TrajectoryUntil.action diff --git a/action/ToolContact.action b/action/ToolContact.action index 5ffbff5..71d3192 100644 --- a/action/ToolContact.action +++ b/action/ToolContact.action @@ -1,8 +1,7 @@ --- int32 SUCCESS=0 -int32 CANCELLED_BY_USER = -1 -int32 ABORTED_BY_HARDWARE = -2 -int32 ABORTED_BY_CONTROLLER = -3 +int32 PREEMPTED = -1 +int32 ABORTED = -2 int32 result --- diff --git a/action/TrajectoryUntil.action b/action/TrajectoryUntil.action deleted file mode 100644 index 30d9eb0..0000000 --- a/action/TrajectoryUntil.action +++ /dev/null @@ -1,66 +0,0 @@ -# The joint trajectory to follow -trajectory_msgs/JointTrajectory trajectory - -# Tolerances for the trajectory. If the measured joint values fall -# outside the tolerances the trajectory goal is aborted. Any -# tolerances that are not specified (by being omitted or set to 0) are -# set to the defaults for the action server (often taken from the -# parameter server). - -# Tolerances applied to the joints as the trajectory is executed. If -# violated, the goal aborts with error_code set to -# PATH_TOLERANCE_VIOLATED. -control_msgs/JointTolerance[] path_tolerance - -# To report success, the joints must be within goal_tolerance of the -# final trajectory value. The goal must be achieved by time the -# trajectory ends plus goal_time_tolerance. (goal_time_tolerance -# allows some leeway in time, so that the trajectory goal can still -# succeed even if the joints reach the goal some time after the -# precise end time of the trajectory). -# -# If the joints are not within goal_tolerance after "trajectory finish -# time" + goal_time_tolerance, the goal aborts with error_code set to -# GOAL_TOLERANCE_VIOLATED -control_msgs/JointTolerance[] goal_tolerance -builtin_interfaces/Duration goal_time_tolerance - -# The condition until which the trajectory should run. -uint8 until_type 0 -uint8 TOOL_CONTACT = 0 - ---- -# The trajectory will be reported as succesful if either the trajectory finishes, or the until condition is met. -#The error codes will be used if the trajectory fails and the until condition is not met. -int32 error_code -int32 SUCCESSFUL = 0 -int32 INVALID_GOAL = -1 -int32 INVALID_JOINTS = -2 -int32 OLD_HEADER_TIMESTAMP = -3 -int32 PATH_TOLERANCE_VIOLATED = -4 -int32 GOAL_TOLERANCE_VIOLATED = -5 -int32 ABORTED = -6 - -int32 until_condition_result -int32 TRIGGERED = 0 -int32 NOT_TRIGGERED = 1 -int32 CANCELLED_BY_USER = -1 -int32 ABORTED_BY_HARDWARE = -2 -int32 ABORTED_BY_CONTROLLER = -3 - -# Human readable description of the error code. Contains complementary -# information that is especially useful when execution fails, for instance: -# - INVALID_GOAL: The reason for the invalid goal (e.g., the requested -# trajectory is in the past). -# - INVALID_JOINTS: The mismatch between the expected controller joints -# and those provided in the goal. -# - PATH_TOLERANCE_VIOLATED and GOAL_TOLERANCE_VIOLATED: Which joint -# violated which tolerance, and by how much. -string error_string - ---- -std_msgs/Header header -string[] joint_names -trajectory_msgs/JointTrajectoryPoint desired -trajectory_msgs/JointTrajectoryPoint actual -trajectory_msgs/JointTrajectoryPoint error From d9cd4b2fe64e9bd693963c86096b7ea6deeb648d Mon Sep 17 00:00:00 2001 From: URJala <159417921+URJala@users.noreply.github.com> Date: Fri, 28 Feb 2025 08:36:38 +0100 Subject: [PATCH 4/4] Apply suggestions from code review Co-authored-by: Felix Exner --- CMakeLists.txt | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9951976..d28477e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -5,10 +5,6 @@ find_package(ament_cmake REQUIRED) find_package(builtin_interfaces REQUIRED) find_package(rosidl_default_generators REQUIRED) find_package(geometry_msgs REQUIRED) -find_package(trajectory_msgs REQUIRED) -find_package(std_msgs REQUIRED) -find_package(control_msgs REQUIRED) - set(msg_files msg/Analog.msg @@ -31,7 +27,6 @@ set(srv_files set(action_files action/ToolContact.action - action/TrajectoryUntil.action ) if(BUILD_TESTING) @@ -43,7 +38,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} ${msg_files} ${srv_files} ${action_files} - DEPENDENCIES builtin_interfaces geometry_msgs trajectory_msgs std_msgs control_msgs + DEPENDENCIES builtin_interfaces geometry_msgs ADD_LINTER_TESTS )