From 01cd7f5d098476fd20f8f808102df499d5161b74 Mon Sep 17 00:00:00 2001
From: Mykolas Juraitis <75975027+mykolasjuraitis@users.noreply.github.com>
Date: Wed, 30 Jun 2021 08:21:22 +0000
Subject: [PATCH] SRC-4962 Add command type to JointState & ActuatorCommand

---
 .../include/ros_ethercat_model/hardware_interface.hpp | 11 ++++++++---
 .../include/ros_ethercat_model/joint.hpp              |  6 ++++++
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp
index ce28294..45cf80b 100644
--- a/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp
+++ b/ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp
@@ -39,7 +39,7 @@
 #include <vector>
 
 #include <ros/ros.h>
-
+#include <sr_robot_msgs/ControlType.h>
 
 namespace ros_ethercat_model
 {
@@ -95,12 +95,17 @@ class ActuatorCommand
 public:
   ActuatorCommand() :
     enable_(0),
-    effort_(0)
+    effort_(0),
+    // Initial value is neither FORCE not PWM to indicate that controller didn't set it yet
+    type_(sr_robot_msgs::ControlType::QUERY)
   {
   }
 
   bool enable_;  //!< Enable this actuator
-  double effort_;  //!< Force to apply (in Nm)
+  double effort_;  //!< Torque to apply (in Nm) or PWM value
+
+  /// Indicates if effort_ is torque or PWM
+  sr_robot_msgs::ControlType::_control_type_type type_;
 };
 
 /*!
diff --git a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp
index da5aac3..971f699 100644
--- a/ros_ethercat_model/include/ros_ethercat_model/joint.hpp
+++ b/ros_ethercat_model/include/ros_ethercat_model/joint.hpp
@@ -43,6 +43,7 @@
 #include <limits>
 #include <tinyxml.h>
 #include <urdf_model/joint.h>
+#include <sr_robot_msgs/ControlType.h>
 
 namespace ros_ethercat_model
 {
@@ -167,6 +168,9 @@ class JointState
   /// The effort the joint should apply in Nm or N (write-to variable)
   double commanded_effort_;
 
+  /// Indicates if commanded_effort_ is torque or PWM
+  sr_robot_msgs::ControlType::_control_type_type actuator_command_type_;
+
   /// The position of the optical flag that was used to calibrate this joint
   double reference_position_;
 
@@ -178,6 +182,8 @@ class JointState
     commanded_position_(0.0),
     commanded_velocity_(0.0),
     commanded_effort_(0.0),
+    // Initial value is neither FORCE not PWM to indicate that controller didn't set it yet
+    actuator_command_type_(sr_robot_msgs::ControlType::QUERY),
     reference_position_(0.0)
   {
   }