From 3408b8bbe3d794dbcdee14241dea57d9f8307f6d Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Carlos=20Ag=C3=BCero?= <caguero@openrobotics.org>
Date: Tue, 12 Apr 2022 20:48:11 +0200
Subject: [PATCH] Fix timeout score issue (#456)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Fix timeout score issue.

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
---
 vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh | 2 +-
 vrx_gazebo/src/gymkhana_scoring_plugin.cc       | 8 +++-----
 vrx_gazebo/src/navigation_scoring_plugin.cc     | 5 +++++
 3 files changed, 9 insertions(+), 6 deletions(-)

diff --git a/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh b/vrx_gazebo/include/vrx_gazebo/scoring_plugin.hh
index e996b70cc..c9dd37ce4 100644
--- 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
index 563f35d7b..78ae9d32c 100644
--- 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
index 8e9d008d8..cddbe3c8e 100644
--- 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(