diff --git a/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh b/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh index 6beb8307c..51a96423e 100644 --- a/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh +++ b/usv_gazebo_plugins/include/usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh @@ -69,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 cmdForceMap; + /// \brief Topic name for incoming ROS thruster commands. public: std::string cmdTopic; @@ -135,7 +139,8 @@ namespace gazebo /// : If true, thruster will have adjustable angle. /// If false, thruster will have constant angle. /// Optional elements: - /// : Thruster mapping (0=linear; 1=GLF, nonlinear), + /// : Thruster mapping (0=linear; 1=GLF, nonlinear; + /// 2=linear interp), /// default is 0 /// :Maximum of thrust commands, /// defualt is 1.0 @@ -145,6 +150,10 @@ namespace gazebo /// default is 250.0 N /// : Maximum reverse force [N]. /// default is -100.0 N + /// : values of thrust commands corresponding to thrust forces + /// default is minCmd maxCmd + /// : values of thrust forces corresponding to thrust commands + /// default is maxForceRev maxForceFwd /// : Absolute value of maximum thruster angle [radians]. /// default is pi/2 /// @@ -184,6 +193,46 @@ namespace gazebo /// 1.57 /// /// + /// + /// Here is an equivalent example but using a mapping type of 2,linear interp: + /// + /// + /// + /// 1.0 + /// + /// + /// left_propeller_link + /// left_engine_propeller_joint + /// left_chasis_engine_joint + /// left_thrust_cmd + /// left_thrust_angle + /// false + /// 2 + /// 1.0 + /// -1.0 + /// 250.0 + /// -100.0 + /// -1.0 0 1.0 + /// -100.0 0 250.0 + /// 1.57 + /// + /// + /// right_propeller_link + /// right_engine_propeller_joint + /// right_chasis_engine_joint + /// right_thrust_cmd + /// right_thrust_angle + /// false + /// 2 + /// 1.0 + /// -1.0 + /// 250.0 + /// -100.0 + /// -1.0 0 1.0 + /// -100.0 0 250.0 + /// 1.57 + /// + /// class UsvThrust : public ModelPlugin { @@ -210,6 +259,22 @@ 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 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 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. @@ -245,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 _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 diff --git a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc index d4d0152eb..a90659bd0 100644 --- a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc +++ b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc @@ -20,6 +20,7 @@ #include #include +#include #include "usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh" @@ -82,6 +83,36 @@ double UsvThrust::SdfParamDouble(sdf::ElementPtr _sdfPtr, return val; } +////////////////////////////////////////////////// +std::vector 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 _defaultVal = StrToVector(_defaultValString); + return _defaultVal; + } + + std::string valString = _sdfPtr->Get(_paramName); + std::vector val = StrToVector(valString); + ROS_DEBUG_STREAM("Parameter found - setting <" << _paramName << + "> to <" << valString << ">."); + return val; +} + +////////////////////////////////////////////////// +std::vector UsvThrust::StrToVector(std::string _input) const +{ + std::vector 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) { @@ -240,6 +271,33 @@ void UsvThrust::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 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 cmdValues = this->SdfParamVector(thrusterSDF, + "cmdValues", + defaultCmdValuesString); + std::string defaultForceValuesString = + std::to_string(thruster.maxForceRev) + + " " + + std::to_string(thruster.maxForceFwd); + std::vector 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"); @@ -342,6 +400,41 @@ double UsvThrust::GlfThrustCmd(const double _cmd, return val; } +////////////////////////////////////////////////// +double UsvThrust::LinearInterpThrustCmd(const double _cmd, + const std::map _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() { @@ -390,6 +483,11 @@ 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);