20 #include <gazebo/common/Assert.hh> 21 #include <gazebo/common/Console.hh> 22 #include <gazebo/physics/Link.hh> 26 const gazebo::physics::LinkPtr _leftMarkerModel,
27 const gazebo::physics::LinkPtr _rightMarkerModel)
28 : leftMarkerModel(_leftMarkerModel),
29 rightMarkerModel(_rightMarkerModel)
41 #if GAZEBO_MAJOR_VERSION >= 8 45 const auto leftMarkerPose = this->
leftMarkerModel->GetWorldPose().Ign();
50 auto v1 = leftMarkerPose.Pos() - rightMarkerPose.Pos();
54 const auto v2 = ignition::math::Vector3d::UnitZ.Cross(v1);
57 const auto middle = (leftMarkerPose.Pos() + rightMarkerPose.Pos()) / 2.0;
60 const auto yaw =
atan2(v2.Y(), v2.X());
63 this->
pose.Set(middle, ignition::math::Vector3d(0, 0, yaw));
66 this->
width = leftMarkerPose.Pos().Distance(rightMarkerPose.Pos());
71 const ignition::math::Pose3d &_robotWorldPose)
const 74 const ignition::math::Vector3d robotLocalPosition =
75 this->
pose.Rot().Inverse().RotateVector(_robotWorldPose.Pos() -
79 if (fabs(robotLocalPosition.Y()) <= this->
width / 2.0)
81 if (robotLocalPosition.X() >= 0.0)
93 gzmsg <<
"Navigation scoring plugin loaded" << std::endl;
103 if (!_sdf->HasElement(
"course_name"))
105 gzerr <<
"Unable to find <course_name> element in SDF." << std::endl;
108 #if GAZEBO_MAJOR_VERSION >= 8 110 this->
world->ModelByName(_sdf->Get<std::string>(
"course_name"));
113 this->
world->GetModel(_sdf->Get<std::string>(
"course_name"));
117 gzerr <<
"could not find " <<
118 _sdf->Get<std::string>(
"course_name") << std::endl;
122 if (_sdf->HasElement(
"obstacle_penalty"))
126 if (!_sdf->HasElement(
"gates"))
128 gzerr <<
"Unable to find <gates> element in SDF." << std::endl;
133 auto const &gatesElem = _sdf->GetElement(
"gates");
136 gzerr <<
"Score has been disabled" << std::endl;
145 static_cast<double>(this->
numGates);
146 gzmsg <<
"Setting timeoutScore = " << timeoutScore << std::endl;
149 gzmsg <<
"Task [" << this->
TaskName() <<
"]" << std::endl;
158 GZ_ASSERT(_sdf,
"NavigationScoringPlugin::ParseGates(): NULL _sdf pointer");
161 if (!_sdf->HasElement(
"gate"))
163 gzerr <<
"Unable to find <gate> element in SDF." << std::endl;
167 auto gateElem = _sdf->GetElement(
"gate");
173 if (!gateElem->HasElement(
"left_marker"))
175 gzerr <<
"Unable to find <left_marker> element in SDF." << std::endl;
179 const std::string leftMarkerName =
180 gateElem->Get<std::string>(
"left_marker");
183 if (!gateElem->HasElement(
"right_marker"))
185 gzerr <<
"Unable to find <right_marker> element in SDF." << std::endl;
189 const std::string rightMarkerName =
190 gateElem->Get<std::string>(
"right_marker");
192 if (!this->
AddGate(leftMarkerName, rightMarkerName))
196 gateElem = gateElem->GetNextElement(
"gate");
204 const std::string &_rightMarkerName)
208 _leftMarkerName +
"::link");
211 if (!leftMarkerModel)
213 gzerr <<
"Unable to find model [" << _leftMarkerName <<
"]" << std::endl;
219 _rightMarkerName +
"::link");
222 if (!rightMarkerModel)
224 gzerr <<
"Unable to find model [" << _rightMarkerName <<
"]" << std::endl;
229 this->
gates.push_back(
Gate(leftMarkerModel, rightMarkerModel));
240 #if GAZEBO_MAJOR_VERSION >= 8 258 #if GAZEBO_MAJOR_VERSION >= 8 261 const auto robotPose = this->
vehicleModel->GetWorldPose().Ign();
265 auto iter = std::begin(this->
gates);
266 while (iter != std::end(this->
gates))
279 gzmsg <<
"New gate crossed!" << std::endl;
282 if (iter != this->
gates.begin())
284 gzmsg <<
"Gate crossed in the wrong order" << std::endl;
289 iter = this->
gates.erase(iter);
296 gzmsg <<
"Transited the gate in the wrong direction. Gate invalidated!" 304 gate.
state = currentState;
308 if (this->
gates.empty())
310 gzmsg <<
"Course completed!" << std::endl;
std::vector< Gate > gates
All the gates.
std::string TaskName() const
Get the task name.
gazebo::physics::LinkPtr rightMarkerModel
The right marker model.
Gate invalid. E.g.: if crossed in the wrong direction.
double width
The width of the gate in meters.
void SetTimeoutScore(double _timeoutScore)
Set the score in case of timeout.
GZ_REGISTER_WORLD_PLUGIN(UsvWindPlugin)
GateState
All gate states.
std::string TaskState() const
Get the task state.
double timeoutScore
Score in case of timeout - added for Navigation task.
ignition::math::Pose3d 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 Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
gazebo::physics::LinkPtr leftMarkerModel
The left marker model.
void Finish()
Finish the current task. This will set the "finished" flag in the task message to true...
gazebo::physics::ModelPtr course
GateState state
The state of this gate.
gazebo::common::Time ElapsedTime() const
Elapsed time in the running state.
bool ParseGates(sdf::ElementPtr _sdf)
Parse the gates from SDF.
void Load(gazebo::physics::WorldPtr _world, sdf::ElementPtr _sdf)
GateState IsPoseInGate(const ignition::math::Pose3d &_robotWorldPose) const
Where is the given robot pose with respect to the gate?
int numGates
Number of gates.
void Fail()
Set the score to 0 and change to state to "finish".
gazebo::physics::WorldPtr world
A world pointer.
gazebo::physics::ModelPtr vehicleModel
Pointer to the vehicle to score.
unsigned int numCollisions
The number of WAM-V collisions.
double GetTimeoutScore()
Get the timeoutScore.
double obstaclePenalty
Number of points deducted per collision.
void Update()
Recalculate the pose and width of the gate.
bool AddGate(const std::string &_leftMarkerName, const std::string &_rightMarkerName)
Register a new gate.
void OnCollision() override
Callback executed when a collision is detected for the WAMV.
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
void Update()
Callback executed at every world update.
void SetScore(double _newScore)
Set the score.
A gate that is part of the navigation challenge.
std::string vehicleName
The name of the vehicle to score.
double GetRunningStateDuration()
Get running duration.
NavigationScoringPlugin()
Gate(const gazebo::physics::LinkPtr _leftMarkerModel, const gazebo::physics::LinkPtr _rightMarkerModel)
Constructor.
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...