From e5018aec4dc314eeafcf29179d43d1503b9aeb43 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Wed, 14 Jul 2021 17:36:32 +0200
Subject: [PATCH] Access by getParam in the module constructor to avoid
 uninitialized config variables problems

---
 src/tiago_nav_module.cpp | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp
index 510c4a0..b446084 100644
--- a/src/tiago_nav_module.cpp
+++ b/src/tiago_nav_module.cpp
@@ -32,7 +32,15 @@ CTiagoNavModule::CTiagoNavModule(const std::string &name,const std::string &name
   this->cancel_pending=false;
 
   // initialize odometry subscriber
-  this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
+  if(!this->module_nh.getParam("odom_watchdog_time_s", this->config.odom_watchdog_time_s))
+  {
+    ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'odom_watchdog_time_s' not found. Configuring wathcdog with 1.0 sec");
+    this->odom_watchdog.reset(ros::Duration(1.0));
+  }
+  else
+    this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
+
+  // this->odom_watchdog.reset(ros::Duration(this->config.odom_watchdog_time_s));
   this->odom_subscriber = this->module_nh.subscribe("odom",1,&CTiagoNavModule::odom_callback,this);
 
   // tf listener
@@ -40,7 +48,14 @@ CTiagoNavModule::CTiagoNavModule(const std::string &name,const std::string &name
   this->transform_orientation=false;
 
   // costmaps
-  this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
+  if(!this->module_nh.getParam("clear_costmap_auto_clear_rate_hz", this->config.clear_costmap_auto_clear_rate_hz))
+  {
+    ROS_WARN("CTiagoNavModule::CTiagoNavModule: param 'clear_costmap_auto_clear_rate_hz' not found. Configuring wathcdog with 0.1 Hz");
+    this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/0.1),&CTiagoNavModule::clear_costmaps_call,this);
+  }
+  else
+    this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
+  // this->clear_costmaps_timer=this->module_nh.createTimer(ros::Duration(1.0/this->config.clear_costmap_auto_clear_rate_hz),&CTiagoNavModule::clear_costmaps_call,this);
   this->clear_costmaps_timer.stop();
   this->enable_auto_clear=false;
 
-- 
GitLab