Skip to content
Snippets Groups Projects
Commit b05d3980 authored by Alejandro Lopez Gestoso's avatar Alejandro Lopez Gestoso
Browse files

Adapted toreceive an empty name

parent de195c44
No related branches found
No related tags found
No related merge requests found
#ifndef _NAV_MODULE_H #ifndef _NAV_MODULE_H
#define _NAV_MODULE_H #define _NAV_MODULE_H
#include<ctype.h>
// IRI ROS headers // IRI ROS headers
#include <iri_ros_tools/module.h> #include <iri_ros_tools/module.h>
#include <iri_ros_tools/module_action.h> #include <iri_ros_tools/module_action.h>
...@@ -847,56 +849,63 @@ class CNavModule : public CModule<ModuleCfg> ...@@ -847,56 +849,63 @@ class CNavModule : public CModule<ModuleCfg>
params=ModuleCfg::__getParamDescriptions__(); params=ModuleCfg::__getParamDescriptions__();
CModule<ModuleCfg>::dynamic_reconfigure(config,name+"_module"); std::string prefix;
this->move_base_action.dynamic_reconfigure(config,name+"_move_base");
this->make_plan_service.dynamic_reconfigure(config,name+"_make_plan"); if (name.length() == 0)
this->clear_costmaps.dynamic_reconfigure(config,name+"_clear_costmap"); 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++) 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); (*param)->getValue(config,value);
if(value.type()==typeid(std::string)) if(value.type()==typeid(std::string))
this->goal_frame_id=boost::any_cast<std::string &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(std::string)) if(value.type()==typeid(std::string))
this->global_frame_id=boost::any_cast<std::string &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(std::string)) if(value.type()==typeid(std::string))
this->robot_frame_id=boost::any_cast<std::string &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(bool)) if(value.type()==typeid(bool))
this->move_base_cancel_prev=boost::any_cast<bool &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(double)) if(value.type()==typeid(double))
this->odom_watchdog_time_s=boost::any_cast<double &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(double)) if(value.type()==typeid(double))
this->tf_timeout_time_s=boost::any_cast<double &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(double)) if(value.type()==typeid(double))
this->auto_clear_rate_hz=boost::any_cast<double &>(value); 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); (*param)->getValue(config,value);
if(value.type()==typeid(bool)) if(value.type()==typeid(bool))
......
...@@ -4,36 +4,47 @@ from iri_ros_tools.dyn_params import add_module_service_params,add_module_action ...@@ -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): 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") odom = new_group.add_group(prefix+"Odometry")
tf = new_group.add_group(name+"_TF") 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_module_action_params(new_group,prefix+"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(prefix+"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(prefix+"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(prefix+"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(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_module_service_params(new_group,prefix+"clear_costmap")
costmap.add(name+"_clear_costmap_enable_auto_clear",bool_t, 0, "Periodically clear the costmaps",False) costmap.add(prefix+"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(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 return new_group
def add_opendrive_gp_module_params(gen,name): 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") new_group = gen.add_group(prefix)
add_module_service_params(new_group,name+"_get_nodes")
add_module_service_params(new_group,prefix+"_get_map")
add_module_service_params(new_group,prefix+"_get_nodes")
return new_group return new_group
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment