A plugin for computing the score of the navigation task. This plugin derives from the generic ScoringPlugin class. Check out that plugin for other required SDF elements. This plugin requires the following SDF parameters: More...
#include <navigation_scoring_plugin.hh>
Classes | |
class | Gate |
A gate that is part of the navigation challenge. More... | |
Public Member Functions | |
void | Load (gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf) |
NavigationScoringPlugin () | |
Public Member Functions inherited from ScoringPlugin | |
ScoringPlugin ()=default | |
Class constructor. More... | |
Private Types | |
enum | GateState { GateState::VEHICLE_OUTSIDE, GateState::VEHICLE_BEFORE, GateState::VEHICLE_AFTER, GateState::CROSSED, GateState::INVALID } |
All gate states. More... | |
Private Member Functions | |
bool | AddGate (const std::string &_leftMarkerName, const std::string &_rightMarkerName) |
Register a new gate. More... | |
void | OnFinished () override |
Callback executed when the task state transition into "finished". More... | |
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... | |
bool | ParseGates (sdf::ElementPtr _sdf) |
Parse the gates from SDF. More... | |
void | Update () |
Callback executed at every world update. More... | |
Private Attributes | |
std::vector< Gate > | gates |
All the gates. More... | |
gazebo::event::ConnectionPtr | updateConnection |
Pointer to the update event connection. 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... | |
void | Load (gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf) |
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... | |
std::string | TaskName () const |
Get the task name. More... | |
std::string | TaskState () const |
Get the task state. More... | |
Protected Attributes inherited from ScoringPlugin | |
double | score = 0.0 |
The score. More... | |
std::string | taskName = "undefined" |
The name of the task. More... | |
std::string | topic = "/vmrc/task/info" |
Topic where the task stats are published. More... | |
gazebo::event::ConnectionPtr | updateConnection |
Pointer to the update event connection. 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... | |
A plugin for computing the score of the navigation task. This plugin derives from the generic ScoringPlugin class. Check out that plugin for other required SDF elements. This plugin requires the following SDF parameters:
<gates>: Specifies the collection of gates delimiting the course.
Each gate accepts the following elements:
<gate>: A gate is delimited by two markers (left and right). The vessel should pass through the gate with the markers on the defined right and left sides. E.g.:
<left_marker>: The name of the marker that should stay on the left side of the vessel. <right_marker> The name of the marker that should stay on the right side of the vessel.
Here's an example: <plugin name="navigation_scoring_plugin" filename="libnavigation_scoring_plugin.so"> <vehicle>wamv</vehicle> <task_name>navigation_scoring_plugin</task_name> <gates> <gate> <left_marker>red_bound_0</left_marker> <right_marker>green_bound_0</right_marker> </gate> <gate> <left_marker>red_bound_1</left_marker> <right_marker>green_bound_1</right_marker> </gate> <gate> <left_marker>red_bound_2</left_marker> <right_marker>green_bound_2</right_marker> </gate> <gate> <left_marker>red_bound_3</left_marker> <right_marker>green_bound_3</right_marker> </gate> <gate> <left_marker>red_bound_4</left_marker> <right_marker>green_bound_4</right_marker> </gate> <gate> <left_marker>red_bound_5</left_marker> <right_marker>green_bound_5</right_marker> </gate> <gate> <left_marker>red_bound_6</left_marker> <right_marker>green_bound_6</right_marker> </gate> </gates> </plugin>
Definition at line 84 of file navigation_scoring_plugin.hh.
|
strongprivate |
All gate states.
Enumerator | |
---|---|
VEHICLE_OUTSIDE |
Not "in" the gate. |
VEHICLE_BEFORE |
Before the gate. |
VEHICLE_AFTER |
After the gate. |
CROSSED |
Gate crossed! |
INVALID |
Gate invalid. E.g.: if crossed in the wrong direction. |
Definition at line 87 of file navigation_scoring_plugin.hh.
NavigationScoringPlugin::NavigationScoringPlugin | ( | ) |
Definition at line 86 of file navigation_scoring_plugin.cc.
|
private |
Register a new gate.
[in] | _leftMarkerName | The name of the left marker. |
[in] | _rightMarkerName | The name of the right marker. |
Definition at line 166 of file navigation_scoring_plugin.cc.
void NavigationScoringPlugin::Load | ( | gazebo::physics::WorldPtr | _world, |
sdf::ElementPtr | _sdf | ||
) |
Definition at line 92 of file navigation_scoring_plugin.cc.
|
overrideprivatevirtual |
Callback executed when the task state transition into "finished".
Reimplemented from ScoringPlugin.
Definition at line 252 of file navigation_scoring_plugin.cc.
|
overrideprivatevirtual |
Callback executed when the task state transition into "ready".
Reimplemented from ScoringPlugin.
Definition at line 240 of file navigation_scoring_plugin.cc.
|
overrideprivatevirtual |
Callback executed when the task state transition into "running".
Reimplemented from ScoringPlugin.
Definition at line 246 of file navigation_scoring_plugin.cc.
|
private |
Parse the gates from SDF.
[in] | _sdf | The current SDF element. |
Definition at line 119 of file navigation_scoring_plugin.cc.
|
private |
Callback executed at every world update.
Definition at line 196 of file navigation_scoring_plugin.cc.
|
private |
All the gates.
Definition at line 173 of file navigation_scoring_plugin.hh.
|
private |
Pointer to the update event connection.
Definition at line 176 of file navigation_scoring_plugin.hh.