Public Member Functions | Public Attributes | Protected Member Functions | Private Member Functions | Private Attributes | List of all members
PerceptionScoringPlugin Class Reference

A plugin that allows models to be spawned at a given location in a specific simulation time and then takes care of scoring correct identification and localization of the objects. More...

#include <perception_scoring_plugin.hh>

Inheritance diagram for PerceptionScoringPlugin:
Inheritance graph
[legend]

Public Member Functions

virtual void Load (gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
 
 PerceptionScoringPlugin ()
 Constructor. More...
 
virtual ~PerceptionScoringPlugin ()
 Destructor. More...
 
- Public Member Functions inherited from ScoringPlugin
void Exit ()
 Shutdown Gazebo and ROS. More...
 
 ScoringPlugin ()
 Class constructor. More...
 

Public Attributes

gazebo::event::ConnectionPtr connection
 Connection event. More...
 
gazebo::physics::EntityPtr frame
 Link/model that the object poses use as their frame of reference. More...
 
std::string frameName = std::string()
 Link/model name for the object poses use as their frame of reference. More...
 
gazebo::common::Time lastUpdateTime
 Last time (sim time) that the plugin was updated. More...
 
bool loopForever = false
 When true, "objects" will be repopulated when the object queue is empty, creating an infinite supply of objects. More...
 
std::vector< PerceptionObjectobjects
 Collection of objects to be spawned. More...
 
sdf::ElementPtr sdf
 SDF pointer. More...
 
gazebo::common::Time startTime
 The time specified in the object is relative to this time. More...
 
gazebo::physics::WorldPtr world
 World pointer. More...
 

Protected Member Functions

void OnUpdate ()
 Update the plugin. More...
 
- 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)
 
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...
 

Private Member Functions

void OnAttempt (const geographic_msgs::GeoPoseStamped::ConstPtr &_msg)
 
void OnRunning () override
 Callback executed when the task state transition into "running". More...
 
void ReleaseVehicle () override
 Tries to release the vehicle in case is locked. More...
 
void Restart ()
 Restart the object population list. More...
 

Private Attributes

int attemptBal = 0
 
ros::NodeHandle nh
 ROS Node handle. More...
 
std::string ns
 ROS namespace. More...
 
int objectsDespawned =0
 \ brief count of how many objects have been despawned More...
 
ros::Subscriber objectSub
 ROS subscriber. More...
 
std::string objectTopic
 ROS topic where the object id/pose is received. More...
 

Additional Inherited Members

- 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 that allows models to be spawned at a given location in a specific simulation time and then takes care of scoring correct identification and localization of the objects.

The plugin accepts the following SDF parameters:

<object_sequence>: Contains the list of objects to be populated. An object should be declared as an <object> element with the following parameters: <time> Simulation time to be spawned. <type> Model. <name> Landmark name. <pose> Initial object pose.

<loop_forever>: Optional parameter. If true, all objects will be spawned as a circular buffer. After spawning the last element of the collection, the first one will be inserted.

<frame>: Optional parameter. If present, the poses of the objects will be in the frame of this link/model. Otherwise the world frame is used.

<robot_namespace>: Optional parameter. If present, specifies ROS namespace.

<landmark_topic>: Optional parameter. Specify the topic to which the plugin subscribes for receiving identification and localization msgs. Default is "/vrx/perception/landmark"

<duration>: Optional parameter. Specify the time an object sticks around. defaults to 5

Here's an example of a valid SDF:

<plugin filename="libperception_scoring_plugin.so" name="perception_scoring_plugin"> <vehicle>wamv</vehicle> <task_name>perception</task_name> <initial_state_duration>1</initial_state_duration> <ready_state_duration>1</ready_state_duration> <running_state_duration>300</running_state_duration>

<loop_forever>false</loop_forever> <frame>wamv</frame> <object_sequence> <object> <time>10.0</time> <type>surmark950410</type> <name>red_0</name> <pose>6 0 1 0 0 0</pose> </object> <object> <time>10.0</time> <type>surmark950400</type> <name>green_0</name> <pose>6 6 1 0 0 0</pose> </object> </object_sequence> </plugin>

Definition at line 150 of file perception_scoring_plugin.hh.

Constructor & Destructor Documentation

PerceptionScoringPlugin::PerceptionScoringPlugin ( )

Constructor.

Definition at line 148 of file perception_scoring_plugin.cc.

PerceptionScoringPlugin::~PerceptionScoringPlugin ( )
virtual

Destructor.

Definition at line 154 of file perception_scoring_plugin.cc.

Member Function Documentation

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

Definition at line 159 of file perception_scoring_plugin.cc.

void PerceptionScoringPlugin::OnAttempt ( const geographic_msgs::GeoPoseStamped::ConstPtr &  _msg)
private

Definition at line 379 of file perception_scoring_plugin.cc.

void PerceptionScoringPlugin::OnRunning ( )
overrideprivatevirtual

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

Reimplemented from ScoringPlugin.

Definition at line 429 of file perception_scoring_plugin.cc.

void PerceptionScoringPlugin::OnUpdate ( )
protected

Update the plugin.

Definition at line 291 of file perception_scoring_plugin.cc.

void PerceptionScoringPlugin::ReleaseVehicle ( )
overrideprivatevirtual

Tries to release the vehicle in case is locked.

Reimplemented from ScoringPlugin.

Definition at line 446 of file perception_scoring_plugin.cc.

void PerceptionScoringPlugin::Restart ( )
private

Restart the object population list.

Definition at line 274 of file perception_scoring_plugin.cc.

Member Data Documentation

int PerceptionScoringPlugin::attemptBal = 0
private

Definition at line 177 of file perception_scoring_plugin.hh.

gazebo::event::ConnectionPtr PerceptionScoringPlugin::connection

Connection event.

Definition at line 201 of file perception_scoring_plugin.hh.

gazebo::physics::EntityPtr PerceptionScoringPlugin::frame

Link/model that the object poses use as their frame of reference.

Definition at line 215 of file perception_scoring_plugin.hh.

std::string PerceptionScoringPlugin::frameName = std::string()

Link/model name for the object poses use as their frame of reference.

Definition at line 212 of file perception_scoring_plugin.hh.

gazebo::common::Time PerceptionScoringPlugin::lastUpdateTime

Last time (sim time) that the plugin was updated.

Definition at line 218 of file perception_scoring_plugin.hh.

bool PerceptionScoringPlugin::loopForever = false

When true, "objects" will be repopulated when the object queue is empty, creating an infinite supply of objects.

Definition at line 208 of file perception_scoring_plugin.hh.

ros::NodeHandle PerceptionScoringPlugin::nh
private

ROS Node handle.

Definition at line 186 of file perception_scoring_plugin.hh.

std::string PerceptionScoringPlugin::ns
private

ROS namespace.

Definition at line 180 of file perception_scoring_plugin.hh.

std::vector<PerceptionObject> PerceptionScoringPlugin::objects

Collection of objects to be spawned.

Definition at line 198 of file perception_scoring_plugin.hh.

int PerceptionScoringPlugin::objectsDespawned =0
private

\ brief count of how many objects have been despawned

Definition at line 221 of file perception_scoring_plugin.hh.

ros::Subscriber PerceptionScoringPlugin::objectSub
private

ROS subscriber.

Definition at line 189 of file perception_scoring_plugin.hh.

std::string PerceptionScoringPlugin::objectTopic
private

ROS topic where the object id/pose is received.

Definition at line 183 of file perception_scoring_plugin.hh.

sdf::ElementPtr PerceptionScoringPlugin::sdf

SDF pointer.

Definition at line 195 of file perception_scoring_plugin.hh.

gazebo::common::Time PerceptionScoringPlugin::startTime

The time specified in the object is relative to this time.

Definition at line 204 of file perception_scoring_plugin.hh.

gazebo::physics::WorldPtr PerceptionScoringPlugin::world

World pointer.

Definition at line 192 of file perception_scoring_plugin.hh.


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


vrx_gazebo
Author(s): Brian Bingham , Carlos Aguero
autogenerated on Thu May 7 2020 03:54:56