diff --git a/src/tiago_nav_module.cpp b/src/tiago_nav_module.cpp index 510c4a0b0fd8d5cf7e2906a2a717cbac11e35781..b446084fc102733396923801682e28a968fd3ced 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;