A plugin to simulate a propulsion system under water. This plugin accepts the following SDF parameters. See https://github.com/bsb808/robotx_docs/blob/master/theoryofoperation/theory_of_operation.pdf for more information. More...

#include <usv_gazebo_thrust_plugin.hh>

Inheritance diagram for gazebo::UsvThrust:

[legend]

Public Member Functions
virtual void	Load (physics::ModelPtr _parent, sdf::ElementPtr _sdf)

	UsvThrust ()=default
	Constructor. More...

virtual	~UsvThrust ()=default
	Destructor. More...

Public Attributes
std::mutex	mutex
	A mutex to protect member variables accessed during OnThustCmd() and Update(). More...

physics::WorldPtr	world
	Pointer to the Gazebo world, retrieved when the model is loaded. More...

Protected Member Functions
virtual void	Update ()
	Callback executed at every physics update. More...

Private Member Functions
double	Glf (const double _x, const float _A, const float _K, const float _B, const float _v, const float _C, const float _M) const
	Generalized logistic function (GLF) used for non-linear thruster model. More...

double	GlfThrustCmd (const double _cmd, const double _maxPos, const double _maxNeg) const
	Uses GLF function to map thrust command to thruster force in Newtons. More...

void	RotateEngine (size_t _i, common::Time _stepTime)
	Rotate engine using engine joint PID. More...

double	ScaleThrustCmd (const double _cmd, const double _max_cmd, const double _max_pos, const double _max_neg) const
	Takes ROS Drive commands and scales them by max thrust. More...

double	SdfParamDouble (sdf::ElementPtr _sdfPtr, const std::string &_paramName, const double _defaultVal) const
	Convenience function for getting SDF parameters. More...

void	SpinPropeller (size_t _i)
	Spin a propeller based on its current command. More...

Private Attributes
double	cmdTimeout
	Timeout for receiving Drive commands [s]. More...

sensor_msgs::JointState	jointStateMsg
	The propeller message state. More...

ros::Publisher	jointStatePub
	For publishing to /joint_state with propeller state. More...

physics::ModelPtr	model
	Pointer to Gazebo parent model, retrieved when the model is loaded. More...

std::unique_ptr< ros::NodeHandle >	rosnode
	The ROS node handler used for communications. More...

std::vector< Thruster >	thrusters
	Vector of thruster instances. More...

event::ConnectionPtr	updateConnection
	Pointer to the update event connection. More...

Detailed Description

A plugin to simulate a propulsion system under water. This plugin accepts the following SDF parameters. See https://github.com/bsb808/robotx_docs/blob/master/theoryofoperation/theory_of_operation.pdf for more information.

<robotNamespace>: Namespace prefix for USV. <cmdTimeout>: Timeout, after which thrust is set to zero [s].

Multiple thrusters are declared by including <thruster> SDF elements, where each thruster includes the following SDF parameters specific to the individual thruster Required elements: <linkName>: Name of the link on which to apply thrust forces. <propJointName>: The name of the propeller joint. <engineJointName>: The name of the engine joint. <cmdTopic>: The ROS topic to control this thruster, typically within [-1.0 , 1.0] <angleTopic>: The ROS topic to control the angle of this thruster, will be clipped to stay within [-maxAngle, maxAngle] <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), default is 0 <maxCmd>:Maximum (abs val) 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 <maxAngle>: Absolute value of maximum thruster angle [radians]. default is pi/2

Here is an example:

 <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>1</mappingType>
   <maxCmd>1.0</maxCmd>
   <maxForceFwd>250.0</maxForceFwd>
   <maxForceRev>-100.0</maxForceRev>
   <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>1</mappingType>
   <maxCmd>1.0</maxCmd>
   <maxForceFwd>250.0</maxForceFwd>
   <maxForceRev>-100.0</maxForceRev>
   <maxAngle>1.57</maxAngle>
 </thruster>

</plugin>

Definition at line 181 of file usv_gazebo_thrust_plugin.hh.

Constructor & Destructor Documentation

gazebo::UsvThrust::UsvThrust ( )

default

Constructor.

virtual gazebo::UsvThrust::~UsvThrust ( )

virtualdefault

Destructor.

Member Function Documentation

double UsvThrust::Glf	(	const double	_x,
		const float	_A,
		const float	_K,
		const float	_B,
		const float	_v,
		const float	_C,
		const float	_M
	)		const

private

Generalized logistic function (GLF) used for non-linear thruster model.

Parameters

[in]	_x	Independent variable (input) of GLF.
[in]	_A	Lower asymptote.
[in]	_K	Upper asymptote.
[in]	_B	Growth rate
[in]	_v	Affects near which asymptote max. growth occurs.
[in]	_C	Typically near 1.0.
[in]	_M	Offset to input.

Returns

Definition at line 309 of file usv_gazebo_thrust_plugin.cc.

double UsvThrust::GlfThrustCmd	(	const double	_cmd,
		const double	_maxPos,
		const double	_maxNeg
	)		const

private

Uses GLF function to map thrust command to thruster force in Newtons.

Parameters

[in] _cmd Thrust command {-1.0,1.0}.

Returns: Thrust force [N].

Definition at line 316 of file usv_gazebo_thrust_plugin.cc.

void UsvThrust::Load	(	physics::ModelPtr	_parent,
		sdf::ElementPtr	_sdf
	)

virtual

Definition at line 86 of file usv_gazebo_thrust_plugin.cc.

void UsvThrust::RotateEngine	(	size_t	_i,
		common::Time	_stepTime
	)

private

Rotate engine using engine joint PID.

Parameters

[in]	_i	Index of thruster whose engine will be rotated
[in]	_stepTime	common::Time since last rotation

Definition at line 395 of file usv_gazebo_thrust_plugin.cc.

double UsvThrust::ScaleThrustCmd	(	const double	_cmd,
		const double	_max_cmd,
		const double	_max_pos,
		const double	_max_neg
	)		const

private

Takes ROS Drive commands and scales them by max thrust.

Parameters

[in] _cmd ROS drive command.

Returns: Value scaled and saturated.

Definition at line 290 of file usv_gazebo_thrust_plugin.cc.

double UsvThrust::SdfParamDouble	(	sdf::ElementPtr	_sdfPtr,
		const std::string &	_paramName,
		const double	_defaultVal
	)		const

private

Convenience function for getting SDF parameters.

Parameters

[in]	_sdfPtr	Pointer to an SDF element to parse.
[in]	_paramName	The name of the element to parse.
[in]	_defaultVal	The default value returned if the element does not exist.