From ffbf31e54e514d0ea14778a942b423501ee32a93 Mon Sep 17 00:00:00 2001
From: Henry Moore <henry.moore@picknik.ai>
Date: Thu, 9 May 2024 19:30:12 +0000
Subject: [PATCH] add msg and srv

---
 control_msgs/CMakeLists.txt                       |  2 ++
 .../msg/CartesianTrajectoryGeneratorState.msg     | 15 +++++++++++++++
 control_msgs/srv/ResetDofs.srv                    |  8 ++++++++
 3 files changed, 25 insertions(+)
 create mode 100644 control_msgs/msg/CartesianTrajectoryGeneratorState.msg
 create mode 100644 control_msgs/srv/ResetDofs.srv

diff --git a/control_msgs/CMakeLists.txt b/control_msgs/CMakeLists.txt
index 30ca2ab..5875975 100644
--- a/control_msgs/CMakeLists.txt
+++ b/control_msgs/CMakeLists.txt
@@ -21,6 +21,7 @@ find_package(trajectory_msgs REQUIRED)
 
 set(msg_files
   msg/AdmittanceControllerState.msg
+  msg/CartesianTrajectoryGeneratorState.msg
   msg/DynamicJointState.msg
   msg/GripperCommand.msg
   msg/InterfaceValue.msg
@@ -50,6 +51,7 @@ set(action_files
 set(srv_files
   srv/QueryCalibrationState.srv
   srv/QueryTrajectoryState.srv
+  srv/ResetDofs.srv
 )
 
 rosidl_generate_interfaces(${PROJECT_NAME}
diff --git a/control_msgs/msg/CartesianTrajectoryGeneratorState.msg b/control_msgs/msg/CartesianTrajectoryGeneratorState.msg
new file mode 100644
index 0000000..a83fbf5
--- /dev/null
+++ b/control_msgs/msg/CartesianTrajectoryGeneratorState.msg
@@ -0,0 +1,15 @@
+std_msgs/Header header
+
+string[] dof_names
+# The set point, that is, the reference.
+trajectory_msgs/MultiDOFJointTrajectoryPoint reference_world
+trajectory_msgs/MultiDOFJointTrajectoryPoint reference_local
+# Current value of the process (ie: latest sensor measurement or estimation on the controlled value).
+trajectory_msgs/MultiDOFJointTrajectoryPoint feedback
+# Adjusted feedback message for the interpolation in the local frame.
+trajectory_msgs/MultiDOFJointTrajectoryPoint feedback_local
+# The error of the controlled value, essentially reference - feedback (for a regular PID implementation).
+trajectory_msgs/MultiDOFJointTrajectoryPoint error
+# Current output of the controller.
+trajectory_msgs/MultiDOFJointTrajectoryPoint output_world
+trajectory_msgs/MultiDOFJointTrajectoryPoint output_local
diff --git a/control_msgs/srv/ResetDofs.srv b/control_msgs/srv/ResetDofs.srv
new file mode 100644
index 0000000..a6a5e9b
--- /dev/null
+++ b/control_msgs/srv/ResetDofs.srv
@@ -0,0 +1,8 @@
+# This service enables resetting internal value of a controller with multiple degrees of freedom.
+
+string[] names
+float64[] positions # if present, positions should match size of dof names and also size of velocities/accelerations
+float64[] velocities
+float64[] accelerations
+---
+bool ok