diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 1d6f04b..461a38c 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -82,6 +82,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : name_ = name; std::string config_file_name = "costmap_params.yaml"; getParams(config_file_name, name_, nh); + nh.printParams(); // create a thread to handle updating the map stop_updates_ = false; @@ -366,14 +367,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, { std::string topics_string; move_parameter(priv_nh_2, target_layer, "observation_sources", topics_string); - robot::log_error("topics_string: %s", topics_string.c_str()); + // robot::log_error("topics_string: %s", topics_string.c_str()); std::stringstream ss(topics_string); std::string source; while (ss >> source) { robot::NodeHandle priv_nh_3(priv_nh_2, source); robot::NodeHandle target_layer_2(target_layer, source); - robot::log_warning("priv_nh_3: %s",priv_nh_3.getNamespace().c_str()); + // robot::log_warning("priv_nh_3: %s",priv_nh_3.getNamespace().c_str()); std::string topic; std::string data_type; bool clearing; @@ -381,15 +382,14 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, bool inf_is_valid; double min_obstacle_height; double max_obstacle_height; - move_parameter(priv_nh_2, target_layer_2, "topic", topic); + move_parameter(priv_nh_3, target_layer_2, "topic", topic); robot::log_error("topic: %s", topic.c_str()); - move_parameter(priv_nh_2, target_layer_2, "data_type", data_type); - move_parameter(priv_nh_2, target_layer_2, "clearing", clearing); - move_parameter(priv_nh_2, target_layer_2, "marking", marking); - move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid); - move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height); - move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height); - target_layer_2.printParams(); + move_parameter(priv_nh_3, target_layer_2, "data_type", data_type); + move_parameter(priv_nh_3, target_layer_2, "clearing", clearing); + move_parameter(priv_nh_3, target_layer_2, "marking", marking); + move_parameter(priv_nh_3, target_layer_2, "inf_is_valid", inf_is_valid); + move_parameter(priv_nh_3, target_layer_2, "min_obstacle_height", min_obstacle_height); + move_parameter(priv_nh_3, target_layer_2, "max_obstacle_height", max_obstacle_height); } } } @@ -421,16 +421,16 @@ void Costmap2DROBOT::copyParentParameters(const std::string& costmap_name, bool inf_is_valid; double min_obstacle_height; double max_obstacle_height; - move_parameter(priv_nh_2, target_layer_2, "topic", topic); - move_parameter(priv_nh_2, target_layer_2, "data_type", data_type); - move_parameter(priv_nh_2, target_layer_2, "clearing", clearing); - move_parameter(priv_nh_2, target_layer_2, "marking", marking); - move_parameter(priv_nh_2, target_layer_2, "inf_is_valid", inf_is_valid); - move_parameter(priv_nh_2, target_layer_2, "min_obstacle_height", min_obstacle_height); - move_parameter(priv_nh_2, target_layer_2, "max_obstacle_height", max_obstacle_height); + move_parameter(priv_nh_3, target_layer_2, "topic", topic); + // robot::log_error("topic: %s", topic.c_str()); + move_parameter(priv_nh_3, target_layer_2, "data_type", data_type); + move_parameter(priv_nh_3, target_layer_2, "clearing", clearing); + move_parameter(priv_nh_3, target_layer_2, "marking", marking); + move_parameter(priv_nh_3, target_layer_2, "inf_is_valid", inf_is_valid); + move_parameter(priv_nh_3, target_layer_2, "min_obstacle_height", min_obstacle_height); + move_parameter(priv_nh_3, target_layer_2, "max_obstacle_height", max_obstacle_height); } } - } else if(plugin_type == "InflationLayer") {