From f236282738ea3fa2fcf65c8f14ad358c5ac62391 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 13 Jul 2022 16:47:01 -0400 Subject: [PATCH 1/5] require wave_gazebo_plugins to use c++17 --- wave_gazebo_plugins/CMakeLists.txt | 16 ++++------------ 1 file changed, 4 insertions(+), 12 deletions(-) diff --git a/wave_gazebo_plugins/CMakeLists.txt b/wave_gazebo_plugins/CMakeLists.txt index 998674a3e..c9e04e0e1 100644 --- a/wave_gazebo_plugins/CMakeLists.txt +++ b/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 From 8d52ad072cdc23922ac3a46b9bb55f2eb1c531b4 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Wed, 13 Jul 2022 17:09:28 -0400 Subject: [PATCH 2/5] require usv_gazebo_plugins to use c++17 --- usv_gazebo_plugins/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/usv_gazebo_plugins/CMakeLists.txt b/usv_gazebo_plugins/CMakeLists.txt index 3bed368d1..911f76675 100644 --- a/usv_gazebo_plugins/CMakeLists.txt +++ b/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} From 44deacebef6b5ac5666dbd1979272190bf2242d8 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 18 Jul 2022 16:42:42 -0400 Subject: [PATCH 3/5] separate functionality of maxCmd into max/minCmd use a separate minCmd variable to scale negative input commands --- .../usv_gazebo_plugins/usv_gazebo_thrust_plugin.hh | 12 ++++++++++-- usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc | 7 +++++-- .../wamv_gazebo_thruster_config.xacro | 1 + 3 files changed, 16 insertions(+), 4 deletions(-) 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 d924c22db..6beb8307c 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 @@ -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; @@ -134,8 +137,10 @@ namespace gazebo /// Optional elements: /// : Thruster mapping (0=linear; 1=GLF, nonlinear), /// default is 0 - /// :Maximum (abs val) of thrust commands, + /// :Maximum of thrust commands, /// defualt is 1.0 + /// :Minimum of thrust commands, + /// defualt is -1.0 /// : Maximum forward force [N]. /// default is 250.0 N /// : Maximum reverse force [N]. @@ -159,6 +164,7 @@ namespace gazebo /// false /// 1 /// 1.0 + /// -1.0 /// 250.0 /// -100.0 /// 1.57 @@ -172,6 +178,7 @@ namespace gazebo /// false /// 1 /// 1.0 + /// -1.0 /// 250.0 /// -100.0 /// 1.57 @@ -208,6 +215,7 @@ namespace gazebo /// \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; diff --git a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc index 2527bcb75..98b09de09 100644 --- a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc +++ b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc @@ -232,6 +232,7 @@ 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 = @@ -297,7 +298,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 +309,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; @@ -372,6 +374,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; diff --git a/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro b/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro index 712404953..65e771fe5 100644 --- a/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro +++ b/wamv_gazebo/urdf/thruster_layouts/wamv_gazebo_thruster_config.xacro @@ -13,6 +13,7 @@ 1 1.0 + -1.0 250.0 -100.0 ${pi/2} From 13fed235f83da44623e59f470ce939a103b019a7 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Mon, 18 Jul 2022 18:00:36 -0400 Subject: [PATCH 4/5] add input command and output thrust clamping to thruster plugin --- usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc index 98b09de09..d4d0152eb 100644 --- a/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc +++ b/usv_gazebo_plugins/src/usv_gazebo_thrust_plugin.cc @@ -366,6 +366,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) @@ -390,6 +396,10 @@ void UsvThrust::Update() 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); From d3cf6b7063a901358387b7700a933841fef83167 Mon Sep 17 00:00:00 2001 From: acxz <17132214+acxz@users.noreply.github.com> Date: Tue, 19 Jul 2022 00:34:29 -0400 Subject: [PATCH 5/5] add mappingType=2, linear interp this mappingType allows users to specify a mapping of thrust commands and thrust forces as seen typically in thrust profile datasheets; linear interpolation using the two nearest points in the mapping is done to compute the actual thrust force from a thrust command --- .../usv_gazebo_thrust_plugin.hh | 77 ++++++++++++++- .../src/usv_gazebo_thrust_plugin.cc | 98 +++++++++++++++++++ 2 files changed, 173 insertions(+), 2 deletions(-) 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);