Set a output port value always before a return. Fixed new_goal reset always before any return

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>

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>