19 #include <gazebo/common/Assert.hh> 20 #include <gazebo/common/Console.hh> 25 const gazebo::physics::ModelPtr _leftMarkerModel,
26 const gazebo::physics::ModelPtr _rightMarkerModel)
27 : leftMarkerModel(_leftMarkerModel),
28 rightMarkerModel(_rightMarkerModel)
44 auto v1 = leftMarkerPose.pos - rightMarkerPose.pos;
48 const auto v2 = gazebo::math::Vector3::UnitZ.Cross(v1);
51 const auto middle = (leftMarkerPose.pos + rightMarkerPose.pos) / 2.0;
54 const auto yaw =
atan2(v2.y, v2.x);
60 this->
width = leftMarkerPose.pos.Distance(rightMarkerPose.pos);
65 const gazebo::math::Pose &_robotWorldPose)
const 69 this->
pose.rot.GetInverse().RotateVector(_robotWorldPose.pos -
73 if (fabs(robotLocalPosition.y) <= this->width / 2.0)
75 if (robotLocalPosition.x >= 0.0)
88 gzmsg <<
"Navigation scoring plugin loaded" << std::endl;
98 if (!_sdf->HasElement(
"gates"))
100 gzerr <<
"Unable to find <gates> element in SDF." << std::endl;
105 auto const &gatesElem = _sdf->GetElement(
"gates");
108 gzerr <<
"Score has been disabled" << std::endl;
112 gzmsg <<
"Task [" << this->
TaskName() <<
"]" << std::endl;
121 GZ_ASSERT(_sdf,
"NavigationScoringPlugin::ParseGates(): NULL _sdf pointer");
124 if (!_sdf->HasElement(
"gate"))
126 gzerr <<
"Unable to find <gate> element in SDF." << std::endl;
130 auto gateElem = _sdf->GetElement(
"gate");
136 if (!gateElem->HasElement(
"left_marker"))
138 gzerr <<
"Unable to find <left_marker> element in SDF." << std::endl;
142 const std::string leftMarkerName =
143 gateElem->Get<std::string>(
"left_marker");
146 if (!gateElem->HasElement(
"right_marker"))
148 gzerr <<
"Unable to find <right_marker> element in SDF." << std::endl;
152 const std::string rightMarkerName =
153 gateElem->Get<std::string>(
"right_marker");
155 if (!this->
AddGate(leftMarkerName, rightMarkerName))
159 gateElem = gateElem->GetNextElement(
"gate");
167 const std::string &_rightMarkerName)
170 this->
world->GetModel(_leftMarkerName);
173 if (!leftMarkerModel)
175 gzerr <<
"Unable to find model [" << _leftMarkerName <<
"]" << std::endl;
180 this->
world->GetModel(_rightMarkerName);
183 if (!rightMarkerModel)
185 gzerr <<
"Unable to find model [" << _rightMarkerName <<
"]" << std::endl;
190 this->
gates.push_back(
Gate(leftMarkerModel, rightMarkerModel));
206 const auto robotPose = this->
vehicleModel->GetWorldPose();
209 for (
auto &gate : this->
gates)
219 auto currentState = gate.IsPoseInGate(robotPose);
224 gzmsg <<
"New gate crossed!" << std::endl;
231 gzmsg <<
"Transited the gate in the wrong direction. Gate invalidated!" 235 gate.state = currentState;
242 gzmsg <<
"OnReady" << std::endl;
248 gzmsg <<
"OnRunning" << std::endl;
254 gzmsg <<
"OnFinished" << std::endl;
std::vector< Gate > gates
All the gates.
std::string TaskName() const
Get the task name.
Gate invalid. E.g.: if crossed in the wrong direction.
double width
The width of the gate in meters.
GateState
All gate states.
GateState IsPoseInGate(const gazebo::math::Pose &_robotWorldPose) const
Where is the given robot pose with respect to the gate?
void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
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)
Gate(const gazebo::physics::ModelPtr _leftMarkerModel, const gazebo::physics::ModelPtr _rightMarkerModel)
Constructor.
gazebo::physics::WorldPtr world
A world pointer.
gazebo::physics::ModelPtr vehicleModel
Pointer to the vehicle to score.
void Update()
Recalculate the pose and width of the gate.
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.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
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.
std::string vehicleName
The name of the vehicle to score.
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...