navigation_scoring_plugin.cc
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2019 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
19 #include <cmath>
20 #include <gazebo/common/Assert.hh>
21 #include <gazebo/common/Console.hh>
22 #include <gazebo/physics/Link.hh>
23 
26  const gazebo::physics::LinkPtr _leftMarkerModel,
27  const gazebo::physics::LinkPtr _rightMarkerModel)
28  : leftMarkerModel(_leftMarkerModel),
29  rightMarkerModel(_rightMarkerModel)
30 {
31  this->Update();
32 }
33 
36 {
37  if (!this->leftMarkerModel || !this->rightMarkerModel)
38  return;
39 
40  // The pose of the markers delimiting the gate.
41 #if GAZEBO_MAJOR_VERSION >= 8
42  const auto leftMarkerPose = this->leftMarkerModel->WorldPose();
43  const auto rightMarkerPose = this->rightMarkerModel->WorldPose();
44 #else
45  const auto leftMarkerPose = this->leftMarkerModel->GetWorldPose().Ign();
46  const auto rightMarkerPose = this->rightMarkerModel->GetWorldPose().Ign();
47 #endif
48 
49  // Unit vector from the left marker to the right one.
50  auto v1 = leftMarkerPose.Pos() - rightMarkerPose.Pos();
51  v1.Normalize();
52 
53  // Unit vector perpendicular to v1 in the direction we like to cross gates.
54  const auto v2 = ignition::math::Vector3d::UnitZ.Cross(v1);
55 
56  // This is the center point of the gate.
57  const auto middle = (leftMarkerPose.Pos() + rightMarkerPose.Pos()) / 2.0;
58 
59  // Yaw of the gate in world coordinates.
60  const auto yaw = atan2(v2.Y(), v2.X());
61 
62  // The updated pose.
63  this->pose.Set(middle, ignition::math::Vector3d(0, 0, yaw));
64 
65  // The updated width.
66  this->width = leftMarkerPose.Pos().Distance(rightMarkerPose.Pos());
67 }
68 
71  const ignition::math::Pose3d &_robotWorldPose) const
72 {
73  // Transform to gate frame.
74  const ignition::math::Vector3d robotLocalPosition =
75  this->pose.Rot().Inverse().RotateVector(_robotWorldPose.Pos() -
76  this->pose.Pos());
77 
78  // Are we within the width?
79  if (fabs(robotLocalPosition.Y()) <= this->width / 2.0)
80  {
81  if (robotLocalPosition.X() >= 0.0)
83  else
85  }
86  else
88 }
89 
92 {
93  gzmsg << "Navigation scoring plugin loaded" << std::endl;
94 }
95 
97 void NavigationScoringPlugin::Load(gazebo::physics::WorldPtr _world,
98  sdf::ElementPtr _sdf)
99 {
100  ScoringPlugin::Load(_world, _sdf);
101 
102  // This is a required element.
103  if (!_sdf->HasElement("course_name"))
104  {
105  gzerr << "Unable to find <course_name> element in SDF." << std::endl;
106  return;
107  }
108 #if GAZEBO_MAJOR_VERSION >= 8
109  this->course =
110  this->world->ModelByName(_sdf->Get<std::string>("course_name"));
111 #else
112  this->course =
113  this->world->GetModel(_sdf->Get<std::string>("course_name"));
114 #endif
115  if (!this->course)
116  {
117  gzerr << "could not find " <<
118  _sdf->Get<std::string>("course_name") << std::endl;
119  }
120 
121  // Optional.
122  if (_sdf->HasElement("obstacle_penalty"))
123  this->obstaclePenalty = _sdf->Get<double>("obstacle_penalty");
124 
125  // This is a required element.
126  if (!_sdf->HasElement("gates"))
127  {
128  gzerr << "Unable to find <gates> element in SDF." << std::endl;
129  return;
130  }
131 
132  // Parse all the gates.
133  auto const &gatesElem = _sdf->GetElement("gates");
134  if (!this->ParseGates(gatesElem))
135  {
136  gzerr << "Score has been disabled" << std::endl;
137  return;
138  }
139 
140  // Save number of gates
141  this->numGates = this->gates.size();
142 
143  // Set default score in case of timeout.
144  double timeoutScore = 2.0 * this->GetRunningStateDuration() /
145  static_cast<double>(this->numGates);
146  gzmsg << "Setting timeoutScore = " << timeoutScore << std::endl;
147  this->ScoringPlugin::SetTimeoutScore(timeoutScore);
148 
149  gzmsg << "Task [" << this->TaskName() << "]" << std::endl;
150 
151  this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(
152  std::bind(&NavigationScoringPlugin::Update, this));
153 }
154 
156 bool NavigationScoringPlugin::ParseGates(sdf::ElementPtr _sdf)
157 {
158  GZ_ASSERT(_sdf, "NavigationScoringPlugin::ParseGates(): NULL _sdf pointer");
159 
160  // We need at least one gate.
161  if (!_sdf->HasElement("gate"))
162  {
163  gzerr << "Unable to find <gate> element in SDF." << std::endl;
164  return false;
165  }
166 
167  auto gateElem = _sdf->GetElement("gate");
168 
169  // Parse a new gate.
170  while (gateElem)
171  {
172  // The left marker's name.
173  if (!gateElem->HasElement("left_marker"))
174  {
175  gzerr << "Unable to find <left_marker> element in SDF." << std::endl;
176  return false;
177  }
178 
179  const std::string leftMarkerName =
180  gateElem->Get<std::string>("left_marker");
181 
182  // The right marker's name.
183  if (!gateElem->HasElement("right_marker"))
184  {
185  gzerr << "Unable to find <right_marker> element in SDF." << std::endl;
186  return false;
187  }
188 
189  const std::string rightMarkerName =
190  gateElem->Get<std::string>("right_marker");
191 
192  if (!this->AddGate(leftMarkerName, rightMarkerName))
193  return false;
194 
195  // Parse the next gate.
196  gateElem = gateElem->GetNextElement("gate");
197  }
198 
199  return true;
200 }
201 
203 bool NavigationScoringPlugin::AddGate(const std::string &_leftMarkerName,
204  const std::string &_rightMarkerName)
205 {
206  gazebo::physics::LinkPtr leftMarkerModel =
207  this->course->GetLink(this->course->GetName() + "::" +
208  _leftMarkerName + "::link");
209 
210  // Sanity check: Make sure that the model exists.
211  if (!leftMarkerModel)
212  {
213  gzerr << "Unable to find model [" << _leftMarkerName << "]" << std::endl;
214  return false;
215  }
216 
217  gazebo::physics::LinkPtr rightMarkerModel =
218  this->course->GetLink(this->course->GetName() + "::" +
219  _rightMarkerName + "::link");
220 
221  // Sanity check: Make sure that the model exists.
222  if (!rightMarkerModel)
223  {
224  gzerr << "Unable to find model [" << _rightMarkerName << "]" << std::endl;
225  return false;
226  }
227 
228  // Save the new gate.
229  this->gates.push_back(Gate(leftMarkerModel, rightMarkerModel));
230 
231  return true;
232 }
233 
236 {
237  // The vehicle might not be ready yet, let's try to get it.
238  if (!this->vehicleModel)
239  {
240 #if GAZEBO_MAJOR_VERSION >= 8
241  this->vehicleModel = this->world->ModelByName(this->vehicleName);
242 #else
243  this->vehicleModel = this->world->GetModel(this->vehicleName);
244 #endif
245  if (!this->vehicleModel)
246  return;
247  }
248 
249  // Skip if we're not in running mode.
250  if (this->TaskState() != "running")
251  return;
252 
253  // Current score
254  this->ScoringPlugin::SetScore(std::min(this->GetRunningStateDuration(),
255  this->ElapsedTime().Double() +
256  this->numCollisions * this->obstaclePenalty)/this->numGates);
257 
258 #if GAZEBO_MAJOR_VERSION >= 8
259  const auto robotPose = this->vehicleModel->WorldPose();
260 #else
261  const auto robotPose = this->vehicleModel->GetWorldPose().Ign();
262 #endif
263 
264  // Update the state of all gates.
265  auto iter = std::begin(this->gates);
266  while (iter != std::end(this->gates))
267  {
268  Gate &gate = *iter;
269 
270  // Update this gate (in case it moved).
271  gate.Update();
272 
273  // Check if we have crossed this gate.
274  auto currentState = gate.IsPoseInGate(robotPose);
275  if (currentState == GateState::VEHICLE_AFTER &&
277  {
278  currentState = GateState::CROSSED;
279  gzmsg << "New gate crossed!" << std::endl;
280 
281  // We need to cross all gates in order.
282  if (iter != this->gates.begin())
283  {
284  gzmsg << "Gate crossed in the wrong order" << std::endl;
285  this->Fail();
286  return;
287  }
288 
289  iter = this->gates.erase(iter);
290  }
291  // Just checking: did we go backward through the gate?
292  else if (currentState == GateState::VEHICLE_BEFORE &&
294  {
295  currentState = GateState::INVALID;
296  gzmsg << "Transited the gate in the wrong direction. Gate invalidated!"
297  << std::endl;
298  this->Fail();
299  return;
300  }
301  else
302  ++iter;
303 
304  gate.state = currentState;
305  }
306 
307  // Course completed!
308  if (this->gates.empty())
309  {
310  gzmsg << "Course completed!" << std::endl;
311  this->Finish();
312  }
313 }
314 
317 {
319  this->Finish();
320 }
321 
324 {
325  this->numCollisions++;
326 }
327 
328 // Register plugin with gazebo
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)
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?
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.
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...


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