From b05d3980dee39d161640fe9de9f69e39dfb92236 Mon Sep 17 00:00:00 2001
From: Alopez <alopez@iri.upc.edu>
Date: Thu, 15 Dec 2022 10:24:57 +0100
Subject: [PATCH] Adapted toreceive an empty name
---
include/iri_nav_module/nav_module.h | 33 ++++++++++++--------
src/iri_nav_module/dyn_params.py | 47 ++++++++++++++++++-----------
2 files changed, 50 insertions(+), 30 deletions(-)
diff --git a/include/iri_nav_module/nav_module.h b/include/iri_nav_module/nav_module.h
index a15258e..c867aa6 100644
--- a/include/iri_nav_module/nav_module.h
+++ b/include/iri_nav_module/nav_module.h
@@ -1,6 +1,8 @@
#ifndef _NAV_MODULE_H
#define _NAV_MODULE_H
+#include<ctype.h>
+
// IRI ROS headers
#include <iri_ros_tools/module.h>
#include <iri_ros_tools/module_action.h>
@@ -847,56 +849,63 @@ class CNavModule : public CModule<ModuleCfg>
params=ModuleCfg::__getParamDescriptions__();
- CModule<ModuleCfg>::dynamic_reconfigure(config,name+"_module");
- this->move_base_action.dynamic_reconfigure(config,name+"_move_base");
- this->make_plan_service.dynamic_reconfigure(config,name+"_make_plan");
- this->clear_costmaps.dynamic_reconfigure(config,name+"_clear_costmap");
+ std::string prefix;
+
+ if (name.length() == 0)
+ prefix = "";
+ else
+ prefix = name + "_";
+
+ CModule<ModuleCfg>::dynamic_reconfigure(config,prefix+"module");
+ this->move_base_action.dynamic_reconfigure(config,prefix+"move_base");
+ this->make_plan_service.dynamic_reconfigure(config,prefix+"make_plan");
+ this->clear_costmaps.dynamic_reconfigure(config,prefix+"clear_costmap");
for(typename std::vector<typename ModuleCfg::AbstractParamDescriptionConstPtr>::iterator param=params.begin();param!=params.end();param++)
{
- if((*param)->name==(name+"_move_base_goal_frame_id"))
+ if((*param)->name==(prefix+"move_base_goal_frame_id"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(std::string))
this->goal_frame_id=boost::any_cast<std::string &>(value);
}
- else if((*param)->name==(name+"_move_base_global_frame_id"))
+ else if((*param)->name==(prefix+"move_base_global_frame_id"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(std::string))
this->global_frame_id=boost::any_cast<std::string &>(value);
}
- else if((*param)->name==(name+"_move_base_robot_frame_id"))
+ else if((*param)->name==(prefix+"move_base_robot_frame_id"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(std::string))
this->robot_frame_id=boost::any_cast<std::string &>(value);
}
- else if((*param)->name==(name+"_move_base_cancel_prev"))
+ else if((*param)->name==(prefix+"move_base_cancel_prev"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(bool))
this->move_base_cancel_prev=boost::any_cast<bool &>(value);
}
- else if((*param)->name==(name+"_odom_watchdog_time_s"))
+ else if((*param)->name==(prefix+"odom_watchdog_time_s"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(double))
this->odom_watchdog_time_s=boost::any_cast<double &>(value);
}
- else if((*param)->name==(name+"_tf_timeout_time_s"))
+ else if((*param)->name==(prefix+"tf_timeout_time_s"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(double))
this->tf_timeout_time_s=boost::any_cast<double &>(value);
}
- else if((*param)->name==(name+"_clear_costmap_auto_clear_rate_hz"))
+ else if((*param)->name==(prefix+"clear_costmap_auto_clear_rate_hz"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(double))
this->auto_clear_rate_hz=boost::any_cast<double &>(value);
}
- else if((*param)->name==(name+"_clear_costmap_enable_auto_clear"))
+ else if((*param)->name==(prefix+"clear_costmap_enable_auto_clear"))
{
(*param)->getValue(config,value);
if(value.type()==typeid(bool))
diff --git a/src/iri_nav_module/dyn_params.py b/src/iri_nav_module/dyn_params.py
index 9deeb34..ded8131 100644
--- a/src/iri_nav_module/dyn_params.py
+++ b/src/iri_nav_module/dyn_params.py
@@ -4,36 +4,47 @@ from iri_ros_tools.dyn_params import add_module_service_params,add_module_action
def add_nav_module_params(gen,name):
- new_group = gen.add_group(name)
+ if len(name) == 0:
+ new_group = gen.add_group("nav")
+ prefix = ""
+ else:
+ new_group = gen.add_group(name)
+ prefix = name + "_"
+
- odom = new_group.add_group(name+"_Odometry")
- tf = new_group.add_group(name+"_TF")
+ odom = new_group.add_group(prefix+"Odometry")
+ tf = new_group.add_group(prefix+"TF")
- add_module_params(new_group,name+"_module")
+ add_module_params(new_group,prefix+"module")
- move_base_action=add_module_action_params(new_group,name+"_move_base")
- move_base_action.add(name+"_move_base_goal_frame_id",str_t, 0, "Reference frame of the position goals","map")
- move_base_action.add(name+"_move_base_global_frame_id",str_t, 0, "Global reference frame of the TF","map")
- move_base_action.add(name+"_move_base_robot_frame_id",str_t, 0, "Main reference frame of the robot","map")
- move_base_action.add(name+"_move_base_cancel_prev", bool_t, 0, "Cancel previous action", True)
+ move_base_action=add_module_action_params(new_group,prefix+"move_base")
+ move_base_action.add(prefix+"move_base_goal_frame_id",str_t, 0, "Reference frame of the position goals","map")
+ move_base_action.add(prefix+"move_base_global_frame_id",str_t, 0, "Global reference frame of the TF","map")
+ move_base_action.add(prefix+"move_base_robot_frame_id",str_t, 0, "Main reference frame of the robot","map")
+ move_base_action.add(prefix+"move_base_cancel_prev", bool_t, 0, "Cancel previous action", True)
- odom.add(name+"_odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50)
+ odom.add(prefix+"odom_watchdog_time_s", double_t, 0, "Maximum time between odom messages",1, 0.01, 50)
- tf.add(name+"_tf_timeout_time_s", double_t, 0, "Maximum time to wait for transform",5, 0.01, 50)
+ tf.add(prefix+"tf_timeout_time_s", double_t, 0, "Maximum time to wait for transform",5, 0.01, 50)
- costmap=add_module_service_params(new_group,name+"_clear_costmap")
- costmap.add(name+"_clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False)
- costmap.add(name+"_clear_costmap_auto_clear_rate_hz",double_t, 0, "Clear costmaps period", 0.1, 0.01, 1.0)
+ costmap=add_module_service_params(new_group,prefix+"clear_costmap")
+ costmap.add(prefix+"clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False)
+ costmap.add(prefix+"clear_costmap_auto_clear_rate_hz",double_t, 0, "Clear costmaps period", 0.1, 0.01, 1.0)
- make_plan=add_module_service_params(new_group,name+"_make_plan")
+ make_plan=add_module_service_params(new_group,prefix+"make_plan")
return new_group
def add_opendrive_gp_module_params(gen,name):
- new_group = gen.add_group(name)
+ if len(name) == 0:
+ prefix = "opendrive_gp"
+ else:
+ prefix = name
- add_module_service_params(new_group,name+"_get_map")
- add_module_service_params(new_group,name+"_get_nodes")
+ new_group = gen.add_group(prefix)
+
+ add_module_service_params(new_group,prefix+"_get_map")
+ add_module_service_params(new_group,prefix+"_get_nodes")
return new_group
--
GitLab