From 19a459f5204d412da741b47a61f71588f88ab5dd Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Mon, 13 Feb 2023 17:11:32 +0100
Subject: [PATCH] Set a output port value always before a return. Fixed
 new_goal reset always before any return

---
 .../iri_nav_module/nav_costmap_module_bt.h    | 28 +++++++++++++++++++
 include/iri_nav_module/nav_module_bt.h        | 23 +++++++++++----
 2 files changed, 45 insertions(+), 6 deletions(-)

diff --git a/include/iri_nav_module/nav_costmap_module_bt.h b/include/iri_nav_module/nav_costmap_module_bt.h
index 9676f61..b0025e2 100644
--- a/include/iri_nav_module/nav_costmap_module_bt.h
+++ b/include/iri_nav_module/nav_costmap_module_bt.h
@@ -132,7 +132,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("width", width);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -165,7 +168,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("height", height);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -198,7 +204,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("footprint", footprint);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -231,7 +240,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("tolerance", tolerance);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -264,7 +276,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("frequency", freq);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -297,7 +312,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("frequency", freq);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -330,7 +348,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("resolution", res);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -365,7 +386,11 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("x", x);
+      self.setOutput("y", y);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -400,7 +425,10 @@ class CNavCostmapModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("padding", pad);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
diff --git a/include/iri_nav_module/nav_module_bt.h b/include/iri_nav_module/nav_module_bt.h
index a17ae64..02f6279 100644
--- a/include/iri_nav_module/nav_module_bt.h
+++ b/include/iri_nav_module/nav_module_bt.h
@@ -478,11 +478,9 @@ class CNavModuleBT
     ROS_DEBUG("CNavModuleBT::get_goal_distance-> get_goal_distance");
 
     double dist = this->nav_module.get_goal_distance();
+    self.setOutput("distance", dist);
     if(dist!=-1.0)
-    {
-      self.setOutput("distance", dist);
       return BT::NodeStatus::SUCCESS;
-    }
     else
       return BT::NodeStatus::FAILURE;
   }
@@ -493,11 +491,9 @@ class CNavModuleBT
     ROS_DEBUG("CNavModuleBT::get_goal_distance-> get_goal_distance");
 
     double length = this->nav_module.get_path_length();
+    self.setOutput("length", length);
     if(length!=-1.0)
-    {
-      self.setOutput("length", length);
       return BT::NodeStatus::SUCCESS;
-    }
     else
       return BT::NodeStatus::FAILURE;
   }
@@ -532,7 +528,10 @@ class CNavModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("retries", num);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -565,7 +564,10 @@ class CNavModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("freq", freq);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -598,7 +600,10 @@ class CNavModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("time", pat);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -631,7 +636,10 @@ class CNavModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("freq", freq);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
@@ -664,7 +672,10 @@ class CNavModuleBT
       return BT::NodeStatus::SUCCESS;
     }
     else
+    {
+      self.setOutput("time", pat);
       return BT::NodeStatus::FAILURE;
+    }
   }
 
   template <class ModuleCfg>
-- 
GitLab