From c53db2280e318cb2095c9a55c987db98c315b139 Mon Sep 17 00:00:00 2001 From: duongtd Date: Thu, 11 Dec 2025 17:16:46 +0700 Subject: [PATCH] update load param using node handle --- CMakeLists.txt | 4 + config/costmap_params.yaml | 4 +- config/inflation_layer_params.yaml | 5 + config/layer_params.yaml | 46 ------ config/obstacle_layer_params.yaml | 18 +++ config/static_layer_params.yaml | 10 ++ config/voxel_layer_params.yaml | 11 ++ include/costmap_2d/costmap_2d_robot.h | 4 +- include/costmap_2d/footprint.h | 11 +- include/costmap_2d/inflation_layer.h | 2 +- include/costmap_2d/layer.h | 3 +- include/costmap_2d/obstacle_layer.h | 2 +- include/costmap_2d/static_layer.h | 3 +- include/costmap_2d/voxel_layer.h | 2 +- plugins/inflation_layer.cpp | 38 +++-- plugins/obstacle_layer.cpp | 43 +++++- plugins/static_layer.cpp | 49 ++++-- plugins/voxel_layer.cpp | 86 ++++++++--- src/costmap_2d_robot.cpp | 212 +++++++++++++++++++------- src/footprint.cpp | 57 +++---- 20 files changed, 397 insertions(+), 213 deletions(-) create mode 100644 config/inflation_layer_params.yaml delete mode 100644 config/layer_params.yaml create mode 100644 config/obstacle_layer_params.yaml create mode 100644 config/static_layer_params.yaml create mode 100644 config/voxel_layer_params.yaml diff --git a/CMakeLists.txt b/CMakeLists.txt index 89bf7e1..2a3f533 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -71,6 +71,8 @@ target_link_libraries(costmap_2d xmlrpcpp # XMLRPC yaml-cpp dl + robot::node_handle + robot::console ) # --- Include directories cho target --- @@ -125,6 +127,8 @@ target_link_libraries(plugins ${Boost_LIBRARIES} yaml-cpp robot_time + robot::node_handle + robot::console ) diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml index d2f5164..5b60d79 100644 --- a/config/costmap_params.yaml +++ b/config/costmap_params.yaml @@ -18,14 +18,12 @@ costmap_2d: type: create_voxel_layer path_plugins: ./src/costmap_2d/libplugins.so - foot_print: + footprint: - [0.3, 0.3] - [0.3, -0.3] - [-0.3, -0.3] - [-0.3, 0.3] - layer_config_file_name: layer_params - transform_tolerance: 0.0 update_frequency: 1.0 width: 0.0 diff --git a/config/inflation_layer_params.yaml b/config/inflation_layer_params.yaml new file mode 100644 index 0000000..d17193e --- /dev/null +++ b/config/inflation_layer_params.yaml @@ -0,0 +1,5 @@ +inflation_layer: + enabled: true + inflate_unknown: false + cost_scaling_factor: 15.0 + inflation_radius: 0.55 \ No newline at end of file diff --git a/config/layer_params.yaml b/config/layer_params.yaml deleted file mode 100644 index b96fc5a..0000000 --- a/config/layer_params.yaml +++ /dev/null @@ -1,46 +0,0 @@ -static_layer: - enabled: true - first_map_only: false - subscribe_to_updates: false - track_unknown_space: true - use_maximum: false - lethal_cost_threshold: 100 - unknown_cost_value: -1 - trinary_costmap: true - base_frame_id: "map" - -inflation_layer: - enabled: true - inflate_unknown: false - cost_scaling_factor: 15.0 - inflation_radius: 0.55 - -obstacle_layer: - track_unknown_space: true - transform_tolerance: 0.2 - topic: "map" - sensor_frame: laser_frame - observation_persistence: 0.0 - expected_update_rate: 0.0 - data_type: PointCloud - min_obstacle_height: 0.0 - max_obstacle_height: 2.0 - inf_is_valid: false - clearing: false - marking: true - obstacle_range: 2.5 - raytrace_range: 3.0 - footprint_clearing_enabled: true - combination_method: 1 - -voxel_layer: - enabled: true - footprint_clearing_enabled: true - max_obstacle_height: 3.0 - origin_z: 0.0 - z_resolution: 0.2 - z_voxels: 16 - unknown_threshold: 15.0 - mark_threshold: 0 - combination_method: 3 - diff --git a/config/obstacle_layer_params.yaml b/config/obstacle_layer_params.yaml new file mode 100644 index 0000000..8fa749a --- /dev/null +++ b/config/obstacle_layer_params.yaml @@ -0,0 +1,18 @@ +obstacle_layer: + track_unknown_space: true + transform_tolerance: 0.2 + topic: "map" + sensor_frame: laser_frame + observation_persistence: 0.0 + expected_update_rate: 0.0 + data_type: PointCloud + min_obstacle_height: 0.0 + max_obstacle_height: 2.0 + inf_is_valid: false + clearing: false + marking: true + obstacle_range: 2.5 + raytrace_range: 3.0 + footprint_clearing_enabled: true + combination_method: 1 + diff --git a/config/static_layer_params.yaml b/config/static_layer_params.yaml new file mode 100644 index 0000000..ccfac89 --- /dev/null +++ b/config/static_layer_params.yaml @@ -0,0 +1,10 @@ +static_layer: + enabled: true + first_map_only: false + subscribe_to_updates: false + track_unknown_space: true + use_maximum: false + lethal_cost_threshold: 100 + unknown_cost_value: -1 + trinary_costmap: true + base_frame_id: "map" diff --git a/config/voxel_layer_params.yaml b/config/voxel_layer_params.yaml new file mode 100644 index 0000000..1fab3e5 --- /dev/null +++ b/config/voxel_layer_params.yaml @@ -0,0 +1,11 @@ +voxel_layer: + enabled: true + footprint_clearing_enabled: true + max_obstacle_height: 3.0 + origin_z: 0.0 + z_resolution: 0.2 + z_voxels: 16 + unknown_threshold: 15.0 + mark_threshold: 0 + combination_method: 3 + diff --git a/include/costmap_2d/costmap_2d_robot.h b/include/costmap_2d/costmap_2d_robot.h index da158b6..cdc83f8 100755 --- a/include/costmap_2d/costmap_2d_robot.h +++ b/include/costmap_2d/costmap_2d_robot.h @@ -52,6 +52,8 @@ #include #include + +#include class SuperValue : public XmlRpc::XmlRpcValue { public: @@ -269,7 +271,7 @@ private: float footprint_padding_; private: - void getParams(const std::string& config_file_name); + void getParams(const std::string& config_file_name, robot::NodeHandle& nh); }; // class Costmap2DROBOT } // namespace costmap_2d diff --git a/include/costmap_2d/footprint.h b/include/costmap_2d/footprint.h index 297fef1..5c7a4df 100755 --- a/include/costmap_2d/footprint.h +++ b/include/costmap_2d/footprint.h @@ -43,6 +43,7 @@ #include #include +#include #include namespace costmap_2d @@ -118,11 +119,11 @@ std::vector makeFootprintFromRadius(double radius); */ bool makeFootprintFromString(const std::string& footprint_string, std::vector& footprint); -// /** -// * @brief Read the ros-params "footprint" and/or "robot_radius" from -// * the given NodeHandle using searchParam() to go up the tree. -// */ -// std::vector makeFootprintFromParams(ros::NodeHandle& nh); +/** + * @brief Read the ros-params "footprint" and/or "robot_radius" from + * the given NodeHandle using searchParam() to go up the tree. + */ +std::vector makeFootprintFromParams(robot::NodeHandle& nh); /** * @brief Create the footprint from the given XmlRpcValue. diff --git a/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h index 9edbbc4..7b1f798 100755 --- a/include/costmap_2d/inflation_layer.h +++ b/include/costmap_2d/inflation_layer.h @@ -187,7 +187,7 @@ private: double** cached_distances_; double last_min_x_, last_min_y_, last_max_x_, last_max_y_; - bool getParams(const std::string& config_file_name); + bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. }; diff --git a/include/costmap_2d/layer.h b/include/costmap_2d/layer.h index 4754d52..ea8f3aa 100755 --- a/include/costmap_2d/layer.h +++ b/include/costmap_2d/layer.h @@ -42,8 +42,7 @@ #include #include #include - - +#include namespace costmap_2d { class LayeredCostmap; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index af284d7..087412a 100755 --- a/include/costmap_2d/obstacle_layer.h +++ b/include/costmap_2d/obstacle_layer.h @@ -164,7 +164,7 @@ protected: int combination_method_; private: - bool getParams(const std::string& config_file_name); + bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); }; } // namespace costmap_2d diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index e1e6075..c0dfb7f 100755 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -88,8 +88,7 @@ protected: unsigned char lethal_threshold_, unknown_cost_value_; private: - bool getParams(const std::string& config_file_name); - + bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); }; } // namespace costmap_2d diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h index eb043d2..946c59d 100755 --- a/include/costmap_2d/voxel_layer.h +++ b/include/costmap_2d/voxel_layer.h @@ -83,7 +83,7 @@ protected: virtual void resetMaps(); private: - bool getParams(const std::string& config_file_name); + bool getParams(const std::string& config_file_name, robot::NodeHandle &nh); void clearNonLethal(double wx, double wy, double w_size_x, double w_size_y, bool clear_no_info); virtual void raytraceFreespace(const costmap_2d::Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y); diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index d42017e..8cf1fc6 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -71,23 +71,24 @@ InflationLayer::InflationLayer() void InflationLayer::onInitialize() { - { - boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); - current_ = true; - if (seen_) - delete[] seen_; - seen_ = NULL; - seen_size_ = 0; - need_reinflation_ = false; - std::string config_file_name = name_ + ".yaml"; - std::cout << "InflationLayer: " << config_file_name << std::endl; - getParams(config_file_name); - } + robot::NodeHandle nh("~"); + robot::NodeHandle priv_nh(nh, name_); + + boost::unique_lock < boost::recursive_mutex > lock(*inflation_access_); + current_ = true; + if (seen_) + delete[] seen_; + seen_ = NULL; + seen_size_ = 0; + need_reinflation_ = false; + std::string config_file_name = "inflation_layer_params.yaml"; + // std::cout << "InflationLayer: " << config_file_name << std::endl; + getParams(config_file_name, priv_nh); matchSize(); } -bool InflationLayer::getParams(const std::string& config_file_name) +bool InflationLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) { try { std::string folder = COSTMAP_2D_DIR; @@ -97,11 +98,22 @@ bool InflationLayer::getParams(const std::string& config_file_name) YAML::Node layer = config["inflation_layer"]; double cost_scaling_factor = loadParam(layer, "cost_scaling_factor", 15.0); double inflation_radius = loadParam(layer, "inflation_radius", 0.55); + + if (nh.hasParam("cost_scaling_factor")) + nh.getParam("cost_scaling_factor", cost_scaling_factor); + if (nh.hasParam("inflation_radius")) + nh.getParam("inflation_radius", inflation_radius); + setInflationParameters(inflation_radius, cost_scaling_factor); bool enabled = loadParam(layer, "enabled", true); bool inflate_unknown = loadParam(layer, "inflate_unknown", false); + if (nh.hasParam("enabled")) + nh.getParam("enabled", enabled); + if (nh.hasParam("inflate_unknown")) + nh.getParam("inflate_unknown", inflate_unknown); + if (enabled_ != enabled || inflate_unknown_ != inflate_unknown) { enabled_ = enabled; inflate_unknown_ = inflate_unknown; diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 1b53d48..bb3d10b 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -56,20 +56,23 @@ namespace costmap_2d void ObstacleLayer::onInitialize() { + robot::NodeHandle nh("~"); + robot::NodeHandle priv_nh(nh, name_); + rolling_window_ = layered_costmap_->isRolling(); ObstacleLayer::matchSize(); current_ = true; stop_receiving_data_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); - std::string config_file_name = name_ + ".yaml"; - getParams(config_file_name); + std::string config_file_name = "obstacle_layer_params.yaml"; + getParams(config_file_name, priv_nh); } ObstacleLayer::~ObstacleLayer() {} -bool ObstacleLayer::getParams(const std::string& config_file_name) +bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) { try { std::string folder = COSTMAP_2D_DIR; @@ -104,6 +107,30 @@ bool ObstacleLayer::getParams(const std::string& config_file_name) inf_is_valid = loadParam(layer,"inf_is_valid", false); clearing = loadParam(layer,"clearing", false); marking = loadParam(layer,"marking", true); + + if (nh.hasParam("transform_tolerance")) + nh.getParam("transform_tolerance", transform_tolerance); + if (nh.hasParam("topic")) + nh.getParam("topic", topic); + if (nh.hasParam("sensor_frame")) + nh.getParam("sensor_frame", sensor_frame); + if (nh.hasParam("observation_persistence")) + nh.getParam("observation_persistence", observation_keep_time); + if (nh.hasParam("expected_update_rate")) + nh.getParam("expected_update_rate", expected_update_rate); + if (nh.hasParam("data_type")) + nh.getParam("data_type", data_type); + if (nh.hasParam("min_obstacle_height")) + nh.getParam("min_obstacle_height", min_obstacle_height); + if (nh.hasParam("max_obstacle_height")) + nh.getParam("max_obstacle_height", max_obstacle_height); + if (nh.hasParam("inf_is_valid")) + nh.getParam("inf_is_valid", inf_is_valid); + if (nh.hasParam("clearing")) + nh.getParam("clearing", clearing); + if (nh.hasParam("marking")) + nh.getParam("marking", marking); + if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")) { printf("Only topics that use point clouds or laser scans are currently supported\n"); @@ -117,10 +144,18 @@ bool ObstacleLayer::getParams(const std::string& config_file_name) double raytrace_range = 3.0; raytrace_range = loadParam(layer,"raytrace_range", raytrace_range); - bool footprint_clearing_enabled = loadParam(layer, "footprint_clearing_enabled", true); int combination_method = loadParam(layer, "combination_method", 1); + if (nh.hasParam("obstacle_range")) + nh.getParam("obstacle_range", obstacle_range); + if (nh.hasParam("raytrace_range")) + nh.getParam("raytrace_range", raytrace_range); + if (nh.hasParam("footprint_clearing_enabled")) + nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled); + if (nh.hasParam("combination_method")) + nh.getParam("combination_method", combination_method); + // enabled_ = enabled; footprint_clearing_enabled_ = footprint_clearing_enabled; max_obstacle_height_ = max_obstacle_height; diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index d697a06..adc722d 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -62,34 +62,55 @@ StaticLayer::~StaticLayer() void StaticLayer::onInitialize() { + robot::NodeHandle nh("~"); + robot::NodeHandle priv_nh(nh, name_); + current_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); - std::string config_file_name = name_ + ".yaml"; - getParams(config_file_name); - + std::string config_file_name = "static_layer_params.yaml"; + getParams(config_file_name, priv_nh); } -bool StaticLayer::getParams(const std::string& config_file_name) +bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) { try { std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - YAML::Node config = YAML::LoadFile(path_source); YAML::Node layer = config["static_layer"]; + // ===== 1. Load default từ layer (yaml / plugin config) ===== + bool enabled = loadParam(layer, "enabled", true); + bool first_map_only = loadParam(layer, "first_map_only", false); + bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false); + bool track_unknown_space = loadParam(layer, "track_unknown_space", true); + bool use_maximum = loadParam(layer, "use_maximum", false); + int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100); + int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1); + bool trinary_costmap = loadParam(layer, "trinary_costmap", true); + std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map")); - bool enabled = loadParam(layer, "enabled", true); - bool first_map_only = loadParam(layer, "first_map_only", false); - bool subscribe_to_updates = loadParam(layer, "subscribe_to_updates", false); - bool track_unknown_space = loadParam(layer, "track_unknown_space", true); - bool use_maximum = loadParam(layer, "use_maximum", false); - int lethal_cost_threshold = loadParam(layer, "lethal_cost_threshold", 100); - int unknown_cost_value = loadParam(layer, "unknown_cost_value", -1); - bool trinary_costmap = loadParam(layer, "trinary_costmap", true); - std::string base_frame_id = loadParam(layer, "base_frame_id", std::string("map")); + // ===== 2. Override từ ROS param server (chỉ khi param tồn tại) ===== + if (nh.hasParam("enabled")) + nh.getParam("enabled", enabled, enabled); + if (nh.hasParam("first_map_only")) + nh.getParam("first_map_only", first_map_only); + if (nh.hasParam("subscribe_to_updates")) + nh.getParam("subscribe_to_updates", subscribe_to_updates); + if (nh.hasParam("track_unknown_space")) + nh.getParam("track_unknown_space", track_unknown_space); + if (nh.hasParam("use_maximum")) + nh.getParam("use_maximum", use_maximum); + if (nh.hasParam("trinary_costmap")) + nh.getParam("trinary_costmap", trinary_costmap); + if (nh.hasParam("base_frame_id")) + nh.getParam("base_frame_id", base_frame_id); + if (nh.hasParam("lethal_cost_threshold")) + nh.getParam("lethal_cost_threshold", lethal_cost_threshold); + if (nh.hasParam("unknown_cost_value")) + nh.getParam("unknown_cost_value", unknown_cost_value); first_map_only_ = first_map_only; subscribe_to_updates_ = subscribe_to_updates; diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 3a5fbd6..10525da 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -53,41 +53,77 @@ namespace costmap_2d void VoxelLayer::onInitialize() { + robot::NodeHandle nh("~"); + robot::NodeHandle priv_nh(nh, name_); + ObstacleLayer::onInitialize(); - std::string config_file_name = name_ + ".yaml"; - getParams(config_file_name); + std::string config_file_name = "voxel_layer_params.yaml"; + getParams(config_file_name, priv_nh); } VoxelLayer::~VoxelLayer() {} -bool VoxelLayer::getParams(const std::string& config_file_name) +bool VoxelLayer::getParams(const std::string& config_file_name, robot::NodeHandle &nh) { - try { - std::string folder = COSTMAP_2D_DIR; - std::string path_source = getSourceFile(folder,config_file_name); - - YAML::Node config = YAML::LoadFile(path_source); - YAML::Node layer = config["voxel_layer"]; + try + { + std::string folder = COSTMAP_2D_DIR; + std::string path_source = getSourceFile(folder,config_file_name); + + YAML::Node config = YAML::LoadFile(path_source); + YAML::Node layer = config["voxel_layer"]; - // publish_voxel_ = loadParam(layer, "publish_voxel_map", false); - enabled_ = loadParam(layer, "enabled", true); - footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true); - max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0); - size_z_ = loadParam(layer, "z_voxels", 10.0); - origin_z_ = loadParam(layer, "origin_z", 0.0); - z_resolution_ = loadParam(layer, "z_resolution", 0.2); - unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_); - mark_threshold_ = loadParam(layer, "mark_threshold", 0); - combination_method_ = loadParam(layer, "combination_method", 0.0); - this->matchSize(); - } - catch (const YAML::BadFile& e) { - std::cerr << "Cannot open YAML file: " << e.what() << std::endl; - return false; + // publish_voxel_ = loadParam(layer, "publish_voxel_map", false); + enabled_ = loadParam(layer, "enabled", true); + footprint_clearing_enabled_ = loadParam(layer, "footprint_clearing_enabled", true); + max_obstacle_height_ = loadParam(layer, "max_obstacle_height", 2.0); + size_z_ = loadParam(layer, "z_voxels", 10); + origin_z_ = loadParam(layer, "origin_z", 0.0); + z_resolution_ = loadParam(layer, "z_resolution", 0.2); + unknown_threshold_ = loadParam(layer, "unknown_threshold", 15.0) + (VOXEL_BITS - size_z_); + mark_threshold_ = loadParam(layer, "mark_threshold", 0); + combination_method_ = loadParam(layer, "combination_method", 0.0); + + int size_z, unknown_threshold, mark_threshold; + if (nh.hasParam("enabled")) + nh.getParam("enabled", enabled_); + if (nh.hasParam("footprint_clearing_enabled")) + nh.getParam("footprint_clearing_enabled", footprint_clearing_enabled_); + if (nh.hasParam("max_obstacle_height")) + nh.getParam("max_obstacle_height", max_obstacle_height_); + if (nh.hasParam("z_voxels")) + { + nh.getParam("z_voxels", size_z); + size_z_ = size_z; } + if (nh.hasParam("origin_z")) + nh.getParam("origin_z", origin_z_); + if (nh.hasParam("unknown_threshold")) + { + nh.getParam("unknown_threshold", unknown_threshold); + unknown_threshold_ = unknown_threshold; + } + if (nh.hasParam("mark_threshold")) + { + nh.getParam("mark_threshold", mark_threshold); + mark_threshold_ = mark_threshold; + } + if (nh.hasParam("combination_method")) + nh.getParam("combination_method", combination_method_); - return true; + + + + + this->matchSize(); + } + catch (const YAML::BadFile& e) { + std::cerr << "Cannot open YAML file: " << e.what() << std::endl; + return false; + } + + return true; } void VoxelLayer::matchSize() diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 055ae06..7891211 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -66,8 +66,11 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : map_update_thread_(NULL), footprint_padding_(0.0) { - std::string config_file_name = name_ + ".yaml"; - getParams(config_file_name); + robot::NodeHandle nh("~"); + robot::NodeHandle priv_nh(nh, name); + name_ = name; + std::string config_file_name = "costmap_params.yaml"; + getParams(config_file_name, priv_nh); // create a thread to handle updating the map stop_updates_ = false; @@ -75,7 +78,7 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : stopped_ = false; } -void Costmap2DROBOT::getParams(const std::string& config_file_name) +void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeHandle& nh) { try { @@ -85,53 +88,118 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) YAML::Node config = YAML::LoadFile(path_source); YAML::Node layer = config["costmap_2d"]; - global_frame_ = loadParam(layer, "global_frame", std::string("map")); - robot_base_frame_ = loadParam(layer, "robot_base_frame", std::string("base_link")); + std::string global_frame = + loadParam(layer, "global_frame", std::string("map")); + std::string robot_base_frame = + loadParam(layer, "robot_base_frame", std::string("base_link")); + + if (nh.hasParam("global_frame")) + nh.getParam("global_frame", global_frame); + + if (nh.hasParam("robot_base_frame")) + nh.getParam("robot_base_frame", robot_base_frame); + + global_frame_ = global_frame; + robot_base_frame_ = robot_base_frame; robot::Time last_error = robot::Time::now(); std::string tf_error; - // we need to make sure that the transform between the robot base frame and the global frame is available - while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error)) - { - if (last_error + robot::Duration(5.0) < robot::Time::now()) - { - // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl; - printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", - robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); - last_error = robot::Time::now(); - } - // The error string will accumulate and errors will typically be the same, so the last - // will do for the warning above. Reset the string here to avoid accumulation. - tf_error.clear(); - } + // // we need to make sure that the transform between the robot base frame and the global frame is available + // while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error)) + // { + // if (last_error + robot::Duration(5.0) < robot::Time::now()) + // { + // // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl; + // printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", + // robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + // last_error = robot::Time::now(); + // } + // // The error string will accumulate and errors will typically be the same, so the last + // // will do for the warning above. Reset the string here to avoid accumulation. + // tf_error.clear(); + // } // check if we want a rolling window version of the costmap - bool rolling_window, track_unknown_space; - rolling_window = loadParam(layer, "rolling_window", false); - track_unknown_space = loadParam(layer, "track_unknown_space", false); + bool rolling_window = loadParam(layer, "rolling_window", false); + bool track_unknown_space = loadParam(layer, "track_unknown_space", false); + std::string path_plugins = loadParam(layer, "path_plugins", std::string(" ")); + + if (nh.hasParam("rolling_window")) + nh.getParam("rolling_window", rolling_window); + + if (nh.hasParam("track_unknown_space")) + nh.getParam("track_unknown_space", track_unknown_space); + + if (nh.hasParam("path_plugins")) + nh.getParam("path_plugins", path_plugins); - layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space); - - struct PluginConfig { - std::string name; - std::string type; + + struct PluginConfig + { + std::string name; + std::string type; }; std::vector my_list; - if (layer["plugins"] && layer["plugins"].IsSequence()) + + if(nh.hasParam("plugins")) { - for (const auto& plugin_node : layer["plugins"]) { + my_list.clear(); + YAML::Node my_plugins = nh.getParamValue("plugins"); + if (my_plugins && my_plugins.IsSequence()) + { + for (size_t i = 0; i < my_plugins.size(); ++i) + { + YAML::Node plugin_i = my_plugins[i].as(); + PluginConfig p; - p.name = loadParam(plugin_node, "name", std::string(" ")); - p.type = loadParam(plugin_node, "type", std::string(" ")); + p.type = plugin_i["type"].as(); + p.name = plugin_i["name"].as(); my_list.push_back(p); - std::cout << "Plugin to load: " << p.name << ", Type: " << p.type << std::endl; + if (!plugin_i.IsMap()) + { + std::cerr << "Plugins must be specified as maps. We'll use the default recovery behaviors instead." << std::endl; + return; + } + + if (!plugin_i["name"] || !plugin_i["type"]) + { + std::cerr << "Plugins must have a name and a type. Using the default recovery behaviors instead." << std::endl; + return; + } + + // check for recovery behaviors with the same name + std::string name_i = plugin_i["name"].as(); + for (size_t j = i + 1; j < my_plugins.size(); ++j) + { + YAML::Node plugin_j = my_plugins[j].as(); + if (plugin_j.IsMap() && plugin_j["name"]) + { + std::string name_j = plugin_j["name"].as(); + if (name_i == name_j) + { + std::cerr << "A plugin with the name " << name_i << " already exists, this is not allowed. Using the default recovery behaviors instead." << std::endl; + return; + } + } + } + } + } + } + else + { + my_list.clear(); + if (layer["plugins"] && layer["plugins"].IsSequence()) + { + for (const auto& plugin_node : layer["plugins"]) { + PluginConfig p; + p.name = loadParam(plugin_node, "name", std::string(" ")); + p.type = loadParam(plugin_node, "type", std::string(" ")); + my_list.push_back(p); + } } } - - std::string path_plugins = loadParam(layer, "path_plugins", std::string(" ")); - std::string layer_config_file_name = loadParam(layer, "layer_config_file_name", std::string("layer_params")); for (auto& info : my_list) { @@ -144,20 +212,33 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) ); PluginLayerPtr plugin = creators_.back()(); std::cout << "Plugin created: " << info.name << std::endl; - plugin->initialize(layered_costmap_, layer_config_file_name, &tf_); + + plugin->initialize(layered_costmap_,name_ + "/" + info.name, &tf_); layered_costmap_->addPlugin(plugin); } catch (std::exception &ex) { printf("Failed to create the %s, are you sure it is properly registered and that the containing library is built? Exception: %s\n", info.name.c_str(), ex.what()); + return; } } std::vector new_footprint;// = loadParam(layer, "footprint", 0.0); - new_footprint = loadFootprint(layer["foot_print"], new_footprint); - setUnpaddedRobotFootprint(new_footprint); - + new_footprint = loadFootprint(layer["footprint"], new_footprint); transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0); + + if (nh.hasParam("footprint")) + { + std::cout <<"FOOT PRINT ROBOT:"<isSizeLocked()) - { - layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution), - (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y); - } + if (nh.hasParam("update_frequency")) + nh.getParam("update_frequency", map_update_frequency); + if (nh.hasParam("width")) + nh.getParam("width", map_width_meters); + if (nh.hasParam("height")) + nh.getParam("height", map_height_meters); + if (nh.hasParam("resolution")) + nh.getParam("resolution", resolution); + if (nh.hasParam("origin_x")) + nh.getParam("origin_x", origin_x); + if (nh.hasParam("origin_y")) + nh.getParam("origin_y", origin_y); - // If the padding has changed, call setUnpaddedRobotFootprint() to - // re-apply the padding. - float footprint_padding = loadParam(layer, "footprint_padding", 0.0); - if (footprint_padding_ != footprint_padding) - { - footprint_padding_ = footprint_padding; - setUnpaddedRobotFootprint(unpadded_footprint_); - } + if (!layered_costmap_->isSizeLocked()) + { + layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution), + (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y); + } - double robot_radius = loadParam(layer, "robot_radius", 0.0); - readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius); + // If the padding has changed, call setUnpaddedRobotFootprint() to + // re-apply the padding. + float footprint_padding = loadParam(layer, "footprint_padding", 0.0); + if (nh.hasParam("footprint_padding")) + nh.getParam("footprint_padding", footprint_padding); + if (footprint_padding_ != footprint_padding) + { + footprint_padding_ = footprint_padding; + setUnpaddedRobotFootprint(unpadded_footprint_); + } - // only construct the thread if the frequency is positive - if(map_update_frequency > 0.0) - map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency)); + double robot_radius = loadParam(layer, "robot_radius", 0.0); + if (nh.hasParam("robot_radius")) + nh.getParam("robot_radius", robot_radius); + readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius); + + // only construct the thread if the frequency is positive + if(map_update_frequency > 0.0) + map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROBOT::mapUpdateLoop, this, map_update_frequency)); } catch (const YAML::BadFile& e) { diff --git a/src/footprint.cpp b/src/footprint.cpp index 0bef986..b77b3e0 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -207,45 +207,26 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(ros::NodeHandle& nh) -// { -// std::string full_param_name; -// std::string full_radius_param_name; -// std::vector points; +std::vector makeFootprintFromParams(robot::NodeHandle& nh) +{ + YAML::Node ft = nh.getParamValue("footprint"); + std::vector footprint; -// if (nh.searchParam("footprint", full_param_name)) -// { -// XmlRpc::XmlRpcValue footprint_xmlrpc; -// nh.getParam(full_param_name, footprint_xmlrpc); -// if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeString && -// footprint_xmlrpc != "" && footprint_xmlrpc != "[]") -// { -// if (makeFootprintFromString(std::string(footprint_xmlrpc), points)) -// { -// writeFootprintToParam(nh, points); -// return points; -// } -// } -// else if (footprint_xmlrpc.getType() == XmlRpc::XmlRpcValue::TypeArray) -// { -// points = makeFootprintFromXMLRPC(footprint_xmlrpc, full_param_name); -// writeFootprintToParam(nh, points); -// return points; -// } -// } - -// if (nh.searchParam("robot_radius", full_radius_param_name)) -// { -// double robot_radius; -// nh.param(full_radius_param_name, robot_radius, 1.234); -// points = makeFootprintFromRadius(robot_radius); -// nh.setParam("robot_radius", robot_radius); -// } -// // Else neither param was found anywhere this knows about, so -// // defaults will come from dynamic_reconfigure stuff, set in -// // cfg/Costmap2D.cfg and read in this file in reconfigureCB(). -// return points; -// } + if (ft && ft.IsSequence()) + { + for (size_t i = 0; i < ft.size(); ++i) + { + auto pt = ft[i]; + geometry_msgs::Point p; + p.x = pt[0].as(); + p.y = pt[1].as(); + p.z = 0.0; + footprint.push_back(p); + std::cout << "Pose[" << i << "]" << p.x << ", " << p.y << std::endl; + } + } + return footprint; +} // void writeFootprintToParam(ros::NodeHandle& nh, const std::vector& footprint) // {