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}