A plugin for computing the score of the wayfinding navigation task. This plugin derives from the generic ScoringPlugin class. Refer to that plugin for an explanation of the four states defined (Initial, Ready, Running and Finished) as well as other required SDF elements. More...

#include <wayfinding_scoring_plugin.hh>

Inheritance diagram for WayfindingScoringPlugin:

[legend]

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

	WayfindingScoringPlugin ()
	Constructor. More...

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

	ScoringPlugin ()
	Class constructor. More...

Private Member Functions
void	OnReady () override
	Callback executed when the task state transition into "ready". More...

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

void	PublishWaypoints ()
	Publish the waypoints through which the vehicle must navigate. More...

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

Private Attributes
std::vector< ignition::math::Vector3d >	localWaypoints
	Vector containing waypoints as 3D vectors of doubles representing X Y yaw, where X and Y are local (Gazebo) coordinates. More...

double	meanError
	Current average minimum error for all waypoints. More...

ros::Publisher	meanErrorPub
	Publisher for the current rms error. More...

std::string	meanErrorTopic = "/vrx/wayfinding/mean_error"
	Topic where the current average minimum error is published. More...

std::vector< double >	minErrors
	Vector containing current minimum 2D pose error achieved for each waypoint so far. More...

ros::Publisher	minErrorsPub
	Publisher for the combined 2D pose error. More...

std::string	minErrorsTopic = "/vrx/wayfinding/min_errors"
	Topic where the current minimum pose error distance for each waypoint is published. More...

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

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

std::vector< ignition::math::Vector3d >	sphericalWaypoints
	Vector containing waypoints as 3D vectors of doubles representing Lattitude Longitude yaw, where lattitude and longitude are given in spherical (WGS84) coordinates. More...

gazebo::common::Timer	timer
	Timer used to calculate the elapsed time docked in the bay. More...

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

WaypointMarkers	waypointMarkers
	Waypoint visualization markers. More...

ros::Publisher	waypointsPub
	Publisher for the goal. More...

std::string	waypointsTopic = "/vrx/wayfinding/waypoints"
	Topic where the list of waypoints is published. More...

Additional Inherited Members
Protected Member Functions inherited from ScoringPlugin
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 inherited from ScoringPlugin
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...

Detailed Description

A plugin for computing the score of the wayfinding navigation task. This plugin derives from the generic ScoringPlugin class. Refer to that plugin for an explanation of the four states defined (Initial, Ready, Running and Finished) as well as other required SDF elements.

This plugin publishes a series of poses to a topic when it enters the Ready states.

In the running state it calculates a 2D pose error distance between the vehicle and each of the waypoints and stores the minimum error distance achieved for each waypoint so far. The current recorded minimums are published to a topic as an array of doubles. The average of all minimums is also updated. This value is published to a separate topic for mean error, and set as the current score using the SetScore() method inherited from the parent. This causes it to also appear in the task information topic.

This plugin requires the following SDF parameters:

<waypoints>: Required element specifying the set of waypoints that the the vehicle should navigate through.

This block should contain at least one of these blocks: <waypoint>: This block should contain a <pose> element specifying the lattitude, longitude and yaw of a waypoint. <markers>: Optional parameter to enable visualization markers. Check the WaypointMarkers class for SDF documentation.

Definition at line 58 of file wayfinding_scoring_plugin.hh.

Constructor & Destructor Documentation

WayfindingScoringPlugin::WayfindingScoringPlugin ( )

Constructor.

Definition at line 34 of file wayfinding_scoring_plugin.cc.

Member Function Documentation

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

Definition at line 43 of file wayfinding_scoring_plugin.cc.

void WayfindingScoringPlugin::OnReady ( )

overrideprivatevirtual

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

Reimplemented from ScoringPlugin.

Definition at line 249 of file wayfinding_scoring_plugin.cc.

void WayfindingScoringPlugin::OnRunning ( )

overrideprivatevirtual

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

Reimplemented from ScoringPlugin.

Definition at line 257 of file wayfinding_scoring_plugin.cc.

void WayfindingScoringPlugin::PublishWaypoints ( )

private

Publish the waypoints through which the vehicle must navigate.

Definition at line 221 of file wayfinding_scoring_plugin.cc.

void WayfindingScoringPlugin::Update ( )

private

Callback executed at every world update.

Definition at line 141 of file wayfinding_scoring_plugin.cc.

Member Data Documentation

std::vector<ignition::math::Vector3d> WayfindingScoringPlugin::localWaypoints

private

Vector containing waypoints as 3D vectors of doubles representing X Y yaw, where X and Y are local (Gazebo) coordinates.

Definition at line 109 of file wayfinding_scoring_plugin.hh.

double WayfindingScoringPlugin::meanError

private

Current average minimum error for all waypoints.

Definition at line 121 of file wayfinding_scoring_plugin.hh.

ros::Publisher WayfindingScoringPlugin::meanErrorPub

private

Publisher for the current rms error.

Definition at line 105 of file wayfinding_scoring_plugin.hh.

std::string WayfindingScoringPlugin::meanErrorTopic = "/vrx/wayfinding/mean_error"

private

Topic where the current average minimum error is published.

Definition at line 93 of file wayfinding_scoring_plugin.hh.

std::vector<double> WayfindingScoringPlugin::minErrors

private

Vector containing current minimum 2D pose error achieved for each waypoint so far.

Definition at line 118 of file wayfinding_scoring_plugin.hh.

ros::Publisher WayfindingScoringPlugin::minErrorsPub

private

Publisher for the combined 2D pose error.

Definition at line 102 of file wayfinding_scoring_plugin.hh.

std::string WayfindingScoringPlugin::minErrorsTopic = "/vrx/wayfinding/min_errors"

private

Topic where the current minimum pose error distance for each waypoint is published.

Definition at line 90 of file wayfinding_scoring_plugin.hh.

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

private

ROS node handle.

Definition at line 96 of file wayfinding_scoring_plugin.hh.

sdf::ElementPtr WayfindingScoringPlugin::sdf

private

Pointer to the sdf plugin element.

Definition at line 83 of file wayfinding_scoring_plugin.hh.

std::vector<ignition::math::Vector3d> WayfindingScoringPlugin::sphericalWaypoints

private

Vector containing waypoints as 3D vectors of doubles representing Lattitude Longitude yaw, where lattitude and longitude are given in spherical (WGS84) coordinates.

Definition at line 114 of file wayfinding_scoring_plugin.hh.

gazebo::common::Timer WayfindingScoringPlugin::timer

private

Timer used to calculate the elapsed time docked in the bay.

Definition at line 124 of file wayfinding_scoring_plugin.hh.

gazebo::event::ConnectionPtr WayfindingScoringPlugin::updateConnection

private

Pointer to the update event connection.

Definition at line 80 of file wayfinding_scoring_plugin.hh.

WaypointMarkers WayfindingScoringPlugin::waypointMarkers

private

Waypoint visualization markers.