Add linear interp

usv_gazebo_plugins/CMakeLists.txt

@@ -19,8 +19,8 @@ catkin_package(
   CATKIN_DEPENDS message_runtime gazebo_dev roscpp wave_gazebo_plugins
 )
 
-# Plugins require c++11
-set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
+# Plugins require c++17
+set(CMAKE_CXX_FLAGS "-std=c++17 ${CMAKE_CXX_FLAGS}")
 
 include_directories( include
   ${catkin_INCLUDE_DIRS}

usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh

@@ -51,9 +51,12 @@ namespace gazebo
     /// \param[in] _msg The thrust angle message to process.
     public: void OnThrustAngle(const std_msgs::Float32::ConstPtr &_msg);
 
-    /// \brief Maximum abs val of incoming command.
+    /// \brief Maximum val of incoming command.
     public: double maxCmd;
 
+    /// \brief Minimum val of incoming command.
+    public: double minCmd;
+
     /// \brief Max forward force in Newtons.
     public: double maxForceFwd;
 
@@ -66,9 +69,13 @@ namespace gazebo
     /// \brief Link where thrust force is applied.
     public: physics::LinkPtr link;
 
-    /// \brief Thruster mapping (0=linear; 1=GLF, nonlinear).
+    /// \brief Thruster mapping (0=linear; 1=GLF, nonlinear; 2=linear interp).
     public: int mappingType;
 
+    /// \brief Lookup map of cmd values -> force values
+    ///        for use in mappingType=2,linear interp
+    public: std::map<double, double> cmdForceMap;
+
     /// \brief Topic name for incoming ROS thruster commands.
     public: std::string cmdTopic;
 
@@ -132,14 +139,21 @@ namespace gazebo
   ///   <enableAngle>: If true, thruster will have adjustable angle.
   ///                  If false, thruster will have constant angle.
   ///   Optional elements:
-  ///   <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear),
+  ///   <mappingType>: Thruster mapping (0=linear; 1=GLF, nonlinear;
+  ///                  2=linear interp),
   ///   default is 0
-  ///   <maxCmd>:Maximum (abs val) of thrust commands,
+  ///   <maxCmd>:Maximum of thrust commands,
   ///   defualt is 1.0
+  ///   <minCmd>:Minimum of thrust commands,
+  ///   defualt is -1.0
   ///   <maxForceFwd>: Maximum forward force [N].
   ///   default is 250.0 N
   ///   <maxForceRev>: Maximum reverse force [N].
   ///   default is -100.0 N
+  ///   <cmdValues>: values of thrust commands corresponding to thrust forces
+  ///   default is minCmd maxCmd
+  ///   <forceValues>: values of thrust forces corresponding to thrust commands
+  ///   default is maxForceRev maxForceFwd
   ///   <maxAngle>: Absolute value of maximum thruster angle [radians].
   ///   default is pi/2
   ///
@@ -159,6 +173,7 @@ namespace gazebo
   ///        <enableAngle>false</enableAngle>
   ///        <mappingType>1</mappingType>
   ///        <maxCmd>1.0</maxCmd>
+  ///        <minCmd>-1.0</minCmd>
   ///        <maxForceFwd>250.0</maxForceFwd>
   ///        <maxForceRev>-100.0</maxForceRev>
   ///        <maxAngle>1.57</maxAngle>
@@ -172,8 +187,49 @@ namespace gazebo
   ///        <enableAngle>false</enableAngle>
   ///        <mappingType>1</mappingType>
   ///        <maxCmd>1.0</maxCmd>
+  ///        <minCmd>-1.0</minCmd>
+  ///        <maxForceFwd>250.0</maxForceFwd>
+  ///        <maxForceRev>-100.0</maxForceRev>
+  ///        <maxAngle>1.57</maxAngle>
+  ///      </thruster>
+  ///    </plugin>
+  ///
+  /// Here is an equivalent example but using a mapping type of 2,linear interp:
+  ///
+  ///    <plugin name="example" filename="libusv_gazebo_thrust_plugin.so">
+  ///      <!-- General plugin parameters -->
+  ///      <cmdTimeout>1.0</cmdTimeout>
+  ///
+  ///      <thruster>
+  ///        <linkName>left_propeller_link</linkName>
+  ///        <propJointName>left_engine_propeller_joint</propJointName>
+  ///        <engineJointName>left_chasis_engine_joint</engineJointName>
+  ///        <cmdTopic>left_thrust_cmd</cmdTopic>
+  ///        <angleTopic>left_thrust_angle</angleTopic>
+  ///        <enableAngle>false</enableAngle>
+  ///        <mappingType>2</mappingType>
+  ///        <maxCmd>1.0</maxCmd>
+  ///        <minCmd>-1.0</minCmd>
+  ///        <maxForceFwd>250.0</maxForceFwd>
+  ///        <maxForceRev>-100.0</maxForceRev>
+  ///        <cmdValues>-1.0 0 1.0</cmdValues>
+  ///        <ForceValues>-100.0 0 250.0</cmdValues>
+  ///        <maxAngle>1.57</maxAngle>
+  ///      </thruster>
+  ///      <thruster>
+  ///        <linkName>right_propeller_link</linkName>
+  ///        <propJointName>right_engine_propeller_joint</propJointName>
+  ///        <engineJointName>right_chasis_engine_joint</engineJointName>
+  ///        <cmdTopic>right_thrust_cmd</cmdTopic>
+  ///        <angleTopic>right_thrust_angle</angleTopic>
+  ///        <enableAngle>false</enableAngle>
+  ///        <mappingType>2</mappingType>
+  ///        <maxCmd>1.0</maxCmd>
+  ///        <minCmd>-1.0</minCmd>
   ///        <maxForceFwd>250.0</maxForceFwd>
   ///        <maxForceRev>-100.0</maxForceRev>
+  ///        <cmdValues>-1.0 0 1.0</cmdValues>
+  ///        <ForceValues>-100.0 0 250.0</cmdValues>
   ///        <maxAngle>1.57</maxAngle>
   ///      </thruster>
   ///    </plugin>
@@ -203,11 +259,28 @@ namespace gazebo
                                    const std::string &_paramName,
                                    const double _defaultVal) const;
 
+    /// \brief Convenience function for getting SDF parameters.
+    /// \param[in] _sdfPtr Pointer to an SDF element to parse.
+    /// \param[in] _paramName The name of the vector element to parse.
+    /// \param[in] _defaultVal The default vector value returned if the element
+    /// does not exist.
+    /// \return The vector value parsed.
+    private: std::vector<double> SdfParamVector(
+                                sdf::ElementPtr _sdfPtr,
+                                const std::string &_paramName,
+                                const std::string _defaultValString) const;
+
+    /// \brief Conversion of a string to a double vector
+    /// \param[in] _input The string to convert to a vector of doubles.
+    /// \return The vector converted from the input string.
+    private: std::vector<double> StrToVector(std::string _input) const;
+
     /// \brief Takes ROS Drive commands and scales them by max thrust.
     /// \param[in] _cmd ROS drive command.
     /// \return Value scaled and saturated.
     private: double ScaleThrustCmd(const double _cmd,
                                    const double _max_cmd,
+                                   const double _min_cmd,
                                    const double _max_pos,
                                    const double _max_neg) const;
 
@@ -237,6 +310,14 @@ namespace gazebo
                                  const double _maxPos,
                                  const double _maxNeg) const;
 
+    /// \brief Uses linear interpolatoin between given thrust command
+    /// to thruster force mapping.
+    /// \param[in] _cmd Thrust command.
+    /// \param[in] _cmdForceMap Mapping b/t thrust command and thrust force.
+    /// \return Thrust force [N].
+    private: double LinearInterpThrustCmd(const double _cmd,
+                            const std::map<double, double> _cmdForceMap) const;
+
     /// \brief Rotate engine using engine joint PID
     /// \param[in] _i Index of thruster whose engine will be rotated
     /// \param[in] _stepTime common::Time since last rotation

usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc

@@ -20,6 +20,7 @@
 
 #include <cmath>
 #include <functional>
+#include <vector>
 
 #include "usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh"
 
@@ -82,6 +83,36 @@ double UsvThrust::SdfParamDouble(sdf::ElementPtr _sdfPtr,
   return val;
 }
 
+//////////////////////////////////////////////////
+std::vector<double> UsvThrust::SdfParamVector(sdf::ElementPtr _sdfPtr,
+  const std::string &_paramName, const std::string _defaultValString) const
+{
+  if (!_sdfPtr->HasElement(_paramName))
+  {
+    ROS_INFO_STREAM("Parameter <" << _paramName << "> not found: "
+                    "Using default value of <" << _defaultValString << ">.");
+    std::vector<double> _defaultVal = StrToVector(_defaultValString);
+    return _defaultVal;
+  }
+
+  std::string valString = _sdfPtr->Get<std::string>(_paramName);
+  std::vector<double> val = StrToVector(valString);
+  ROS_DEBUG_STREAM("Parameter found - setting <" << _paramName <<
+                   "> to <" << valString << ">.");
+  return val;
+}
+
+//////////////////////////////////////////////////
+std::vector<double> UsvThrust::StrToVector(std::string _input) const
+{
+  std::vector<double> output;
+  std::string buf;
+  std::stringstream ss(_input);
+  while (ss >> buf)
+    output.push_back(std::stod(buf));
+  return output;
+}
+
 //////////////////////////////////////////////////
 void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
 {
@@ -232,13 +263,41 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
       }
 
       thruster.maxCmd = this->SdfParamDouble(thrusterSDF, "maxCmd", 1.0);
+      thruster.minCmd = this->SdfParamDouble(thrusterSDF, "minCmd", -1.0);
       thruster.maxForceFwd =
         this->SdfParamDouble(thrusterSDF, "maxForceFwd", 250.0);
       thruster.maxForceRev =
         this->SdfParamDouble(thrusterSDF, "maxForceRev", -100.0);
       thruster.maxAngle = this->SdfParamDouble(thrusterSDF, "maxAngle",
                                                M_PI / 2);
 
+      if (thruster.mappingType == 2)
+      {
+        std::string defaultCmdValuesString = std::to_string(thruster.minCmd)
+                                             + " "
+                                             + std::to_string(thruster.maxCmd);
+        std::vector<double> cmdValues = this->SdfParamVector(thrusterSDF,
+                                                    "cmdValues",
+                                                    defaultCmdValuesString);
+        std::string defaultForceValuesString =
+                                        std::to_string(thruster.maxForceRev)
+                                        + " "
+                                        + std::to_string(thruster.maxForceFwd);
+        std::vector<double> forceValues = this->SdfParamVector(thrusterSDF,
+                                                    "forceValues",
+                                                    defaultForceValuesString);
+        if (cmdValues.size() != forceValues.size())
+          ROS_FATAL_STREAM("cmdValues and forceValues size must match!");
+
+        if (cmdValues.size() < 1)
+          ROS_FATAL_STREAM("need at least one cmdValues/forceValues pair!");
+
+        for (size_t i = 0; i < cmdValues.size(); ++i)
+        {
+          thruster.cmdForceMap[cmdValues[i]] = forceValues[i];
+        }
+      }
+
       // Push to vector and increment
       this->thrusters.push_back(thruster);
       thrusterSDF = thrusterSDF->GetNextElement("thruster");
@@ -297,7 +356,7 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
 
 //////////////////////////////////////////////////
 double UsvThrust::ScaleThrustCmd(const double _cmd, const double _maxCmd,
-  const double _maxPos, const double _maxNeg) const
+  const double _minCmd, const double _maxPos, const double _maxNeg) const
 {
   double val = 0.0;
   if (_cmd >= 0.0)
@@ -308,7 +367,8 @@ double UsvThrust::ScaleThrustCmd(const double _cmd, const double _maxCmd,
   else
   {
     double absMaxNeg = std::abs(_maxNeg);
-    val = _cmd / _maxCmd * absMaxNeg;
+    double absMinCmd = std::abs(_minCmd);
+    val = _cmd / absMinCmd * absMaxNeg;
     val = std::max(val, -1.0 * absMaxNeg);
   }
   return val;
@@ -340,6 +400,41 @@ double UsvThrust::GlfThrustCmd(const double _cmd,
   return val;
 }
 
+//////////////////////////////////////////////////
+double UsvThrust::LinearInterpThrustCmd(const double _cmd,
+  const std::map<double, double> _cmdForceMap) const
+{
+  double val = 0.0;
+
+  // first element whose key is NOT considered to go before _cmd
+  auto iter = _cmdForceMap.lower_bound(_cmd);
+
+  if (iter == _cmdForceMap.end())
+  {
+    // all keys are considered to go before
+    // last element is closest
+    return _cmdForceMap.rbegin()->second;
+  }
+
+  double i1 = iter->first;
+  double o1 = iter->second;
+
+  if (iter == _cmdForceMap.begin())
+    return o1;
+
+  iter--;
+
+  double i0 = iter->first;
+  double o0 = iter->second;
+
+  double w1 = _cmd - i0;
+  double w0 = i1 - _cmd;
+
+  val = (o0*w0 + o1*w1)/(w0 + w1);
+
+  return val;
+}
+
 //////////////////////////////////////////////////
 void UsvThrust::Update()
 {
@@ -364,6 +459,12 @@ void UsvThrust::Update()
       // Adjust thruster engine joint angle with PID
       this->RotateEngine(i, now - this->thrusters[i].lastAngleUpdateTime);
 
+      // Apply input command clamping
+      this->thrusters[i].currCmd = std::min(this->thrusters[i].currCmd,
+                                            this->thrusters[i].maxCmd);
+      this->thrusters[i].currCmd = std::max(this->thrusters[i].currCmd,
+                                            this->thrusters[i].minCmd);
+
       // Apply the thrust mapping
       ignition::math::Vector3d tforcev(0, 0, 0);
       switch (this->thrusters[i].mappingType)
@@ -372,6 +473,7 @@ void UsvThrust::Update()
           tforcev.X() = this->ScaleThrustCmd(this->thrusters[i].currCmd/
                                             this->thrusters[i].maxCmd,
                                             this->thrusters[i].maxCmd,
+                                            this->thrusters[i].minCmd,
                                             this->thrusters[i].maxForceFwd,
                                             this->thrusters[i].maxForceRev);
           break;
@@ -381,12 +483,21 @@ void UsvThrust::Update()
                                           this->thrusters[i].maxForceFwd,
                                           this->thrusters[i].maxForceRev);
           break;
+        case 2:
+          tforcev.X() = this->LinearInterpThrustCmd(
+                            this->thrusters[i].currCmd,
+                            this->thrusters[i].cmdForceMap);
+          break;
         default:
             ROS_FATAL_STREAM("Cannot use mappingType=" <<
               this->thrusters[i].mappingType);
             break;
       }
 
+      // Apply thrust clamping
+      tforcev.X() = std::max(tforcev.X(), this->thrusters[i].maxForceFwd);
+      tforcev.X() = std::min(tforcev.X(), this->thrusters[i].maxForceRev);
+
       // Apply force for each thruster
       this->thrusters[i].link->AddLinkForce(tforcev);
 

wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro

@@ -13,6 +13,7 @@
       <!-- Optional Parameters -->
       <mappingType>1</mappingType>
       <maxCmd>1.0</maxCmd>
+      <minCmd>-1.0</minCmd>
       <maxForceFwd>250.0</maxForceFwd>
       <maxForceRev>-100.0</maxForceRev>
       <maxAngle>${pi/2}</maxAngle>

wave_gazebo_plugins/CMakeLists.txt

@@ -2,21 +2,13 @@ cmake_minimum_required(VERSION 2.8.3)
 project(wave_gazebo_plugins)
 
 ###############################################################################
-# Compile as C++11, supported in ROS Kinetic and newer
 
-#set(CMAKE_CXX_STANDARD 11)
-#set(CMAKE_CXX_STANDARD_REQUIRED ON)
-
-# For this package set as C++14
 include(CheckCXXCompilerFlag)
-CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
-CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
-    set(CMAKE_CXX_FLAGS "-std=c++14")
-elseif(COMPILER_SUPPORTS_CXX0X)
-    set(CMAKE_CXX_FLAGS "-std=c++0x")
+CHECK_CXX_COMPILER_FLAG("-std=c++17" COMPILER_SUPPORTS_CXX17)
+if(COMPILER_SUPPORTS_CXX17)
+    add_compile_options(-std=c++17)
 else()
-    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
+    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++17 support. Please use a different C++ compiler.")
 endif()
 
 # Set policy for CMake 3.1+. Use OLD policy to let FindBoost.cmake, dependency