SRC-4962 Add command effort type

ros_ethercat_model/include/ros_ethercat_model/command.hpp

@@ -0,0 +1,50 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2021, Shadow Robot Company Ltd.
+*  All rights reserved.
+*
+*  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 Willow Garage 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 OWNER 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.
+*********************************************************************/
+
+#ifndef ROS_ETHERCAT_MODEL_COMMAND_H
+#define ROS_ETHERCAT_MODEL_COMMAND_H
+
+namespace ros_ethercat_model
+{
+
+enum class EffortCommandType
+{
+  UNSPECIFIED,
+  TORQUE,
+  PWM
+};
+
+}  // namespace ros_ethercat_model
+
+#endif  // ROS_ETHERCAT_MODEL_COMMAND_H

ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp

@@ -40,6 +40,7 @@
 
 #include <ros/ros.h>
 
+#include "ros_ethercat_model/command.hpp"
 
 namespace ros_ethercat_model
 {
@@ -95,12 +96,20 @@ class ActuatorCommand
 public:
   ActuatorCommand() :
     enable_(0),
-    effort_(0)
+    effort_(0),
+    effort_command_type_(EffortCommandType::UNSPECIFIED)
   {
   }
 
   bool enable_;  //!< Enable this actuator
-  double effort_;  //!< Force to apply (in Nm)
+  double effort_;  //!< Effor to apply, unit depends on effort_command_type_ field
+  EffortCommandType effort_command_type_;  //!< Type of effort in effort_ field
+
+  void set_effort_command(double effort, EffortCommandType effort_command_type)
+  {
+    effort_ = effort;
+    effort_command_type_ = effort_command_type;
+  }
 };
 
 /*!

ros_ethercat_model/include/ros_ethercat_model/joint.hpp

@@ -44,6 +44,8 @@
 #include <tinyxml.h>
 #include <urdf_model/joint.h>
 
+#include "ros_ethercat_model/command.hpp"
+
 namespace ros_ethercat_model
 {
 
@@ -170,6 +172,9 @@ class JointState
   /// The position of the optical flag that was used to calibrate this joint
   double reference_position_;
 
+  /// Type of effort in commanded_effort_ field
+  EffortCommandType effort_command_type_;
+
   /// Constructor
   JointState() :
     position_(0.0),
@@ -178,8 +183,15 @@ class JointState
     commanded_position_(0.0),
     commanded_velocity_(0.0),
     commanded_effort_(0.0),
-    reference_position_(0.0)
+    reference_position_(0.0),
+    effort_command_type_(EffortCommandType::UNSPECIFIED)
+  {
+  }
+
+  void set_effort_command(double commanded_effort, EffortCommandType effort_command_type)
   {
+    commanded_effort_ = commanded_effort;
+    effort_command_type_ = effort_command_type;
   }
 };
 inline void JointStatistics::update(JointState *jnt)