A plugin that provides common functionality to any scoring plugin. This plugin defines four different task states: More...

#include <scoring_plugin.hh>

Inheritance diagram for ScoringPlugin:

[legend]

Public Member Functions
void	Exit ()
	Shutdown Gazebo and ROS. More...

	ScoringPlugin ()
	Class constructor. More...

Protected Member Functions
gazebo::common::Time	ElapsedTime () const
	Elapsed time in the running state. More...

void	Finish ()
	Finish the current task. This will set the "finished" flag in the task message to true. More...

double	GetRunningStateDuration ()
	Get running duration. More...

double	GetTimeoutScore ()
	Get the timeoutScore. More...

void	Load (gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)

virtual void	ReleaseVehicle ()
	Tries to release the vehicle in case is locked. More...

gazebo::common::Time	RemainingTime () const
	Remaining time in the running state. More...

double	Score () const
	Get the current score. More...

void	SetScore (double _newScore)
	Set the score. More...

void	SetTimeoutScore (double _timeoutScore)
	Set the score in case of timeout. More...

std::string	TaskName () const
	Get the task name. More...

std::string	TaskState () const
	Get the task state. More...

Protected Attributes
gazebo::common::Time	lastCollisionTime
	Last collision time. More...

double	runningStateDuration = 300.0
	Duration (seconds) of the running state (max task time). More...

vrx_gazebo::Task	taskMsg
	The next task message to be published. More...

std::string	taskName = "undefined"
	The name of the task. More...

ros::Publisher	taskPub
	Publisher for the task state. More...

gazebo::physics::ModelPtr	vehicleModel
	Pointer to the vehicle to score. More...

std::string	vehicleName
	The name of the vehicle to score. More...

gazebo::physics::WorldPtr	world
	A world pointer. More...

Private Member Functions
virtual void	OnCollision ()
	Callback executed when a collision is detected for the WAMV. More...

void	OnCollisionMsg (ConstContactsPtr &_contacts)
	Callback function when collision occurs in the world. More...

virtual void	OnFinished ()
	Callback executed when the task state transition into "finished". More...

virtual void	OnReady ()
	Callback executed when the task state transition into "ready". More...

virtual void	OnRunning ()
	Callback executed when the task state transition into "running". More...

bool	ParseJoints ()
	Parse the joints section of the SDF block. More...

bool	ParseSDFParameters ()
	Parse all SDF parameters. More...

void	PublishStats ()
	Publish the task stats over a ROS topic. More...

void	Update ()
	Callback executed at every world update. More...

void	UpdateTaskMessage ()
	Update the task stats message. More...

void	UpdateTaskState ()
	Update the state of the current task. More...

void	UpdateTime ()
	Update all time-related variables. More...

Private Attributes
float	CollisionBuffer = 3.0
	Collision buffer. More...

int	collisionCounter = 0
	Collisions counter. More...

std::vector< std::string >	collisionList
	Collision list. More...

gazebo::transport::SubscriberPtr	collisionSub
	Collision detection node subscriber. More...

std::vector< gazebo::common::Time >	collisionTimestamps
	Collisions timestamps. More...

std::string	contactDebugTopic = "/vrx/debug/contact"
	Topic where debug collision is published. More...

vrx_gazebo::Contact	contactMsg
	ROS Contact Msg. More...

ros::Publisher	contactPub
	Publisher for the collision. More...

gazebo::common::Time	currentTime
	Current time (simulation). More...

bool	debug = true
	Bool flag for debug. More...

gazebo::common::Time	elapsedTime

gazebo::common::Time	finishTime
	Absolute time specifying the start of the finish state. More...

gazebo::transport::NodePtr	gzNode
	gazebo node pointer More...

double	initialStateDuration = 30.0
	Duration (seconds) of the initial state. More...

gazebo::common::Time	lastStatsSent = gazebo::common::Time::Zero
	Time at which the last message was sent. More...

std::vector< std::string >	lockJointNames
	The name of the joints to be dettached during ReleaseVehicle(). More...

double	readyStateDuration = 60.0
	Duration (seconds) of the ready state. More...

gazebo::common::Time	readyTime
	Absolute time specifying the start of the ready state. More...

gazebo::common::Time	remainingTime
	Remaining time since the start of the task (running state). More...

std::unique_ptr< ros::NodeHandle >	rosNode
	ROS node handle. More...

gazebo::common::Time	runningTime
	Absolute time specifying the start of the running state. More...

double	score = 0.0
	The score. More...

sdf::ElementPtr	sdf
	Pointer to the SDF plugin element. More...

gazebo::transport::PublisherPtr	serverControlPub
	gazebo server control publisher More...

std::string	taskInfoTopic = "/vrx/task/info"
	Topic where the task stats are published. More...

std::string	taskState = "initial"
	The task state. More...

bool	timedOut = false
	Whether the current task has timed out or not. More...

double	timeoutScore = -1.0
	Score in case of timeout - added for Navigation task. More...

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

Detailed Description

A plugin that provides common functionality to any scoring plugin. This plugin defines four different task states:

Initial: The vehicle might be locked to the world via some joints with different constraints. This state is used to stabilize the vehicle and let it settle for a while.
Ready: If the vehicle was locked, it will be released in this state. The task hasn't started yet, but the vehicle can be controlled and prepared for the start of the task.
Running: The task has started. The clockwatch task starts too.
Finished: The maximum allowed task time has been reached (time out) or the task has been completed. Other plugins derived from the ScoringPlugin class can call the Finish() method to trigger the completion of the task.

The plugin also exposes a pair of methods [Set]Score() for setting and getting a score.

Derived plugins can also override the OnReady(), OnRunning(), and OnFinished() to be notified when the task transitions into the "ready". "running" or "finished" states respectively.

The plugin publishes task information on a ROS topic at 1Hz.

This plugin uses the following SDF parameters:

<vehicle>: Required parameter (string type) with the name of the main vehicle to be under control during the task.

<task_name>: Required parameter specifying the task name (string type).

<task_info_topic>: Optional parameter (string type) containing the ROS topic name to publish the task stats. The default topic name is /vrx/task/info .

<contact_debug_topic>: Optional parameter (string type) containing the ROS topic name to publish every instant a collision with the wamv is happening. Default is /vrx/debug/contact.

<initial_state_duration>: Optional parameter (double type) specifying the amount of seconds that the plugin will be in the "initial" state.

<ready_state_duration>: Optional parameter (double type) specifying the amount of seconds that the plugin will be in the "ready" state.

<running_state_duration>: Optional parameter (double type) specifying the amount of maximum seconds that the plugin will be in the "running" state. Note that this parameter specifies the maximum task time.

<collision_buffer>: Optional parameter (double type) specifying the minimum amount of seconds between two collisions. If N collisions happen within this time frame, only one will be counted.

<release_joints>: Optional element specifying the collection of joints that should be dettached when transitioning to the "ready" state.

This block should contain at least one of these blocks: <joint>: This block should contain a <name> element with the name of the joint to release.

Here's an example: <plugin name="scoring_plugin" filename="libscoring_plugin.so"> <vehicle>wamv</vehicle> <task_name>navigation_course</task_name> <initial_state_duration>10</initial_state_duration> <ready_state_duration>10</ready_state_duration> <running_state_duration>30</running_state_duration> <release_joints> <joint> <name>wamv_external_pivot_joint</name> </joint> <joint> <name>wamv_external_riser</name> </joint> </release_joints> </plugin>

Definition at line 113 of file scoring_plugin.hh.

Constructor & Destructor Documentation

ScoringPlugin::ScoringPlugin ( )

Class constructor.

Definition at line 25 of file scoring_plugin.cc.

Member Function Documentation

gazebo::common::Time ScoringPlugin::ElapsedTime ( ) const

protected

Elapsed time in the running state.

Returns: The elapsed time in the running state.

Definition at line 117 of file scoring_plugin.cc.

void ScoringPlugin::Exit ( )

Shutdown Gazebo and ROS.

Definition at line 438 of file scoring_plugin.cc.

void ScoringPlugin::Finish ( )

protected

Finish the current task. This will set the "finished" flag in the task message to true.

Definition at line 129 of file scoring_plugin.cc.

double ScoringPlugin::GetRunningStateDuration ( )

protected

Get running duration.

Definition at line 474 of file scoring_plugin.cc.

double ScoringPlugin::GetTimeoutScore ( )

protected

Get the timeoutScore.

Definition at line 469 of file scoring_plugin.cc.

void ScoringPlugin::Load	(	gazebo::physics::WorldPtr	_world,
		sdf::ElementPtr	_sdf
	)

protected

Definition at line 29 of file scoring_plugin.cc.

void ScoringPlugin::OnCollision ( )

privatevirtual

Callback executed when a collision is detected for the WAMV.

Reimplemented in NavigationScoringPlugin.

Definition at line 270 of file scoring_plugin.cc.

void ScoringPlugin::OnCollisionMsg ( ConstContactsPtr & _contacts )

private

Callback function when collision occurs in the world.

Parameters

[in] _contacts List of all collisions from last simulation iteration

Definition at line 275 of file scoring_plugin.cc.

void ScoringPlugin::OnFinished ( )

privatevirtual

Callback executed when the task state transition into "finished".

Definition at line 256 of file scoring_plugin.cc.

void ScoringPlugin::OnReady ( )

privatevirtual

Callback executed when the task state transition into "ready".

Reimplemented in ScanDockScoringPlugin, WayfindingScoringPlugin, and StationkeepingScoringPlugin.

Definition at line 244 of file scoring_plugin.cc.

void ScoringPlugin::OnRunning ( )

privatevirtual

Callback executed when the task state transition into "running".

Reimplemented in ScanDockScoringPlugin, PerceptionScoringPlugin, WayfindingScoringPlugin, and StationkeepingScoringPlugin.

Definition at line 250 of file scoring_plugin.cc.

bool ScoringPlugin::ParseJoints ( )

private

Parse the joints section of the SDF block.

Returns: True when all parameters were successfully parsed or false otherwise.

Definition at line 401 of file scoring_plugin.cc.

bool ScoringPlugin::ParseSDFParameters ( )

private

Parse all SDF parameters.

Returns: True when all parameters were successfully parsed or false otherwise.

Definition at line 324 of file scoring_plugin.cc.

void ScoringPlugin::PublishStats ( )

private

Publish the task stats over a ROS topic.

Definition at line 211 of file scoring_plugin.cc.

void ScoringPlugin::ReleaseVehicle ( )

protectedvirtual

Tries to release the vehicle in case is locked.

Reimplemented in PerceptionScoringPlugin.

Definition at line 224 of file scoring_plugin.cc.

gazebo::common::Time ScoringPlugin::RemainingTime ( ) const

protected

Remaining time in the running state.

Returns: The remaining time in the running state.

Definition at line 123 of file scoring_plugin.cc.

double ScoringPlugin::Score ( ) const

protected

Get the current score.

Returns: The current score.

Definition at line 92 of file scoring_plugin.cc.

void ScoringPlugin::SetScore ( double _newScore )

protected

Set the score.

Parameters

[in] _newScore The new score.

Definition at line 98 of file scoring_plugin.cc.

void ScoringPlugin::SetTimeoutScore ( double _timeoutScore )

protected

Set the score in case of timeout.

Definition at line 464 of file scoring_plugin.cc.

std::string ScoringPlugin::TaskName ( ) const

protected

Get the task name.

Returns: Task name.

Definition at line 105 of file scoring_plugin.cc.

std::string ScoringPlugin::TaskState ( ) const

protected

Get the task state.

Returns: Task state.

Definition at line 111 of file scoring_plugin.cc.

void ScoringPlugin::Update ( )

private

Callback executed at every world update.

Definition at line 139 of file scoring_plugin.cc.

void ScoringPlugin::UpdateTaskMessage ( )

private

Update the task stats message.

Definition at line 201 of file scoring_plugin.cc.

void ScoringPlugin::UpdateTaskState ( )

private

Update the state of the current task.

Definition at line 173 of file scoring_plugin.cc.

void ScoringPlugin::UpdateTime ( )

private

Update all time-related variables.

Definition at line 156 of file scoring_plugin.cc.

Member Data Documentation

float ScoringPlugin::CollisionBuffer = 3.0

private

Collision buffer.

Definition at line 276 of file scoring_plugin.hh.

int ScoringPlugin::collisionCounter = 0

private

Collisions counter.

Definition at line 279 of file scoring_plugin.hh.

std::vector<std::string> ScoringPlugin::collisionList

private

Collision list.

Definition at line 282 of file scoring_plugin.hh.

gazebo::transport::SubscriberPtr ScoringPlugin::collisionSub

private

Collision detection node subscriber.

Definition at line 225 of file scoring_plugin.hh.

std::vector<gazebo::common::Time> ScoringPlugin::collisionTimestamps

private

Collisions timestamps.

Definition at line 285 of file scoring_plugin.hh.

std::string ScoringPlugin::contactDebugTopic = "/vrx/debug/contact"

private

Topic where debug collision is published.

Definition at line 240 of file scoring_plugin.hh.

vrx_gazebo::Contact ScoringPlugin::contactMsg

private

ROS Contact Msg.

Definition at line 300 of file scoring_plugin.hh.

ros::Publisher ScoringPlugin::contactPub

private

Publisher for the collision.

Definition at line 312 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::currentTime

private

Current time (simulation).

Definition at line 267 of file scoring_plugin.hh.

bool ScoringPlugin::debug = true

private

Bool flag for debug.

Definition at line 237 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::elapsedTime

private

Definition at line 270 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::finishTime

private

Absolute time specifying the start of the finish state.

Definition at line 264 of file scoring_plugin.hh.

gazebo::transport::NodePtr ScoringPlugin::gzNode

private

gazebo node pointer

Definition at line 222 of file scoring_plugin.hh.

double ScoringPlugin::initialStateDuration = 30.0

private

Duration (seconds) of the initial state.

Definition at line 249 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::lastCollisionTime

protected

Last collision time.

Definition at line 219 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::lastStatsSent = gazebo::common::Time::Zero

private

Time at which the last message was sent.

Definition at line 291 of file scoring_plugin.hh.

std::vector<std::string> ScoringPlugin::lockJointNames

private

The name of the joints to be dettached during ReleaseVehicle().

Definition at line 303 of file scoring_plugin.hh.

double ScoringPlugin::readyStateDuration = 60.0

private

Duration (seconds) of the ready state.

Definition at line 252 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::readyTime

private

Absolute time specifying the start of the ready state.

Definition at line 258 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::remainingTime

private

Remaining time since the start of the task (running state).

Definition at line 273 of file scoring_plugin.hh.

std::unique_ptr<ros::NodeHandle> ScoringPlugin::rosNode

private

ROS node handle.

Definition at line 306 of file scoring_plugin.hh.

double ScoringPlugin::runningStateDuration = 300.0

protected

Duration (seconds) of the running state (max task time).

Definition at line 255 of file scoring_plugin.hh.

gazebo::common::Time ScoringPlugin::runningTime

private

Absolute time specifying the start of the running state.

Definition at line 261 of file scoring_plugin.hh.

double ScoringPlugin::score = 0.0

private

The score.

Definition at line 243 of file scoring_plugin.hh.

sdf::ElementPtr ScoringPlugin::sdf

private

Pointer to the SDF plugin element.

Definition at line 246 of file scoring_plugin.hh.

gazebo::transport::PublisherPtr ScoringPlugin::serverControlPub

private

gazebo server control publisher

Definition at line 228 of file scoring_plugin.hh.

std::string ScoringPlugin::taskInfoTopic = "/vrx/task/info"

private

Topic where the task stats are published.

Definition at line 234 of file scoring_plugin.hh.

vrx_gazebo::Task ScoringPlugin::taskMsg

protected

The next task message to be published.

Definition at line 297 of file scoring_plugin.hh.

std::string ScoringPlugin::taskName = "undefined"

protected

The name of the task.

Definition at line 210 of file scoring_plugin.hh.

ros::Publisher ScoringPlugin::taskPub

protected

Publisher for the task state.

Definition at line 309 of file scoring_plugin.hh.

std::string ScoringPlugin::taskState = "initial"

private

The task state.

Definition at line 294 of file scoring_plugin.hh.

bool ScoringPlugin::timedOut = false

private

Whether the current task has timed out or not.

Definition at line 288 of file scoring_plugin.hh.

double ScoringPlugin::timeoutScore = -1.0

private

Score in case of timeout - added for Navigation task.

Definition at line 315 of file scoring_plugin.hh.

gazebo::event::ConnectionPtr ScoringPlugin::updateConnection

private

Pointer to the update event connection.

Definition at line 231 of file scoring_plugin.hh.

gazebo::physics::ModelPtr ScoringPlugin::vehicleModel

protected

Pointer to the vehicle to score.

Definition at line 216 of file scoring_plugin.hh.

std::string ScoringPlugin::vehicleName

protected

The name of the vehicle to score.

Definition at line 213 of file scoring_plugin.hh.

gazebo::physics::WorldPtr ScoringPlugin::world

protected

A world pointer.

Definition at line 207 of file scoring_plugin.hh.

The documentation for this class was generated from the following files:

Public Member Functions

Protected Member Functions

Protected Attributes

Private Member Functions

Private Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation