Added actions for TrajectoryUntil and ToolContact

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
 )
 

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
 ---

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