Fix timeout score issue (#456)

* Fix timeout score issue. Signed-off-by: Carlos Agüero <[email protected]>
osrf · Apr 12, 2022 · 3408b8b · 3408b8b
1 parent d9db285
commit 3408b8b
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 6 deletions.
diff --git a/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh b/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh
@@ -331,7 +331,7 @@ class ScoringPlugin : public gazebo::WorldPlugin
   private: ros::Publisher contactPub;
 
   /// \brief Score in case of timeout - added for Navigation task
-  private: double timeoutScore = 200.0;
+  private: double timeoutScore = -1;
 
   /// \brief Whether to shut down after last gate is crossed.
   private: bool perPluginExitOnCompletion = true;

diff --git a/vrx_gazebo/src/gymkhana_scoring_plugin.cc b/vrx_gazebo/src/gymkhana_scoring_plugin.cc
@@ -110,7 +110,7 @@ void GymkhanaScoringPlugin::Update()
   }
   else
   {
-    this->ScoringPlugin::SetScore(this->ScoringPlugin::GetTimeoutScore());
+    this->ScoringPlugin::SetScore(200);
   }
 }
 
@@ -125,7 +125,7 @@ void GymkhanaScoringPlugin::ChannelCallback(
     {
       if (msg->state == "finished")
       {
-        if (msg->score == this->ScoringPlugin::GetTimeoutScore())
+        if (msg->score == 200)
           this->Finish();
         else
           this->channelCrossed = true;
@@ -158,9 +158,7 @@ void GymkhanaScoringPlugin::SetPingerPosition() const
 void GymkhanaScoringPlugin::OnFinished()
 {
   double penalty = this->GetNumCollisions() * this->obstaclePenalty;
-
-  if (this->Score() < std::numeric_limits<double>::max())
-    this->SetTimeoutScore(this->Score() + penalty);
+  this->SetTimeoutScore(this->Score() + penalty);
 
   ScoringPlugin::OnFinished();
 }

diff --git a/vrx_gazebo/src/navigation_scoring_plugin.cc b/vrx_gazebo/src/navigation_scoring_plugin.cc
@@ -140,6 +140,11 @@ void NavigationScoringPlugin::Load(gazebo::physics::WorldPtr _world,
   // Save number of gates
   this->numGates = this->gates.size();
 
+  // Set default score in case of timeout.
+  double timeoutScore = 200;
+  gzmsg << "Setting timeoutScore = " << timeoutScore << std::endl;
+  this->ScoringPlugin::SetTimeoutScore(timeoutScore);
+
   gzmsg << "Task [" << this->TaskName() << "]" << std::endl;
 
   this->updateConnection = gazebo::event::Events::ConnectWorldUpdateBegin(