18 #ifndef VMRC_GAZEBO_NAVIGATION_SCORING_PLUGIN_HH_ 19 #define VMRC_GAZEBO_NAVIGATION_SCORING_PLUGIN_HH_ 23 #include <gazebo/common/Events.hh> 24 #include <gazebo/math/Pose.hh> 25 #include <gazebo/physics/Model.hh> 26 #include <gazebo/physics/World.hh> 111 public:
Gate(
const gazebo::physics::ModelPtr _leftMarkerModel,
112 const gazebo::physics::ModelPtr _rightMarkerModel);
117 public:
GateState IsPoseInGate(
const gazebo::math::Pose &_robotWorldPose)
132 public: gazebo::math::Pose
pose;
145 public:
void Load(gazebo::physics::WorldPtr _world,
146 sdf::ElementPtr _sdf);
151 private:
bool ParseGates(sdf::ElementPtr _sdf);
157 private:
bool AddGate(
const std::string &_leftMarkerName,
158 const std::string &_rightMarkerName);
164 private:
void OnReady()
override;
std::vector< Gate > gates
All the gates.
Gate invalid. E.g.: if crossed in the wrong direction.
double width
The width of the gate in meters.
GateState
All gate states.
void OnReady() override
Callback executed when the task state transition into "ready".
gazebo::physics::ModelPtr rightMarkerModel
The right marker model.
bool ParseGates(sdf::ElementPtr _sdf)
Parse the gates from SDF.
void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
A plugin that provides common functionality to any scoring plugin. This plugin defines four different...
void OnFinished() override
Callback executed when the task state transition into "finished".
bool AddGate(const std::string &_leftMarkerName, const std::string &_rightMarkerName)
Register a new gate.
gazebo::math::Pose pose
The center of the gate in the world frame. Note that the roll and pitch are ignored. Only yaw is relevant and it points into the direction in which the gate should be crossed.
void OnRunning() override
Callback executed when the task state transition into "running".
void Update()
Callback executed at every world update.
A gate that is part of the navigation challenge.
gazebo::physics::ModelPtr leftMarkerModel
The left marker model.
NavigationScoringPlugin()
gazebo::event::ConnectionPtr updateConnection
Pointer to the update event connection.
A plugin for computing the score of the navigation task. This plugin derives from the generic Scoring...