diff --git a/config/config.yaml b/config/config.yaml deleted file mode 100644 index 15b08f5..0000000 --- a/config/config.yaml +++ /dev/null @@ -1,26 +0,0 @@ -inflation_layer: - enabled: true - inflate_unknown: false - cost_scaling_factor: 15.0 - inflation_radius: 0.55 - -costmap_2d: - plugins: - - name: static_layer - type: create_static_layer - - - name: inflation_layer - type: create_inflation_layer - - - name: obstacle_layer - type: create_obstacle_layer - - - name: preferred_layer - type: create_preferred_layer - - path: ./src/costmap_2d/libplugins.so - - # robot_time_source: true - # update_frequency: 5.0 - # publish_frequency: 2.0 - # transform_tolerance: 0.5 \ No newline at end of file diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml new file mode 100644 index 0000000..02a0cbf --- /dev/null +++ b/config/costmap_params.yaml @@ -0,0 +1,38 @@ +costmap_2d: + global_frame: map + robot_base_frame: base_link + rolling_window: false + track_unknown_space: false + + plugins: + - name: static_layer + type: create_static_layer + + - name: inflation_layer + type: create_inflation_layer + + - name: obstacle_layer + type: create_obstacle_layer + + - name: voxel_layer + type: create_voxel_layer + + path_plugins: ./src/costmap_2d/libplugins.so + foot_print: + - [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: 0.0 + width: 0.0 + height: 0.0 + resolution: 0.0 + origin_x: 0.0 + origin_y: 0.0 + + footprint_padding: 0.0 + robot_radius: 0.0 \ No newline at end of file diff --git a/config/layer_params.yaml b/config/layer_params.yaml new file mode 100644 index 0000000..b96fc5a --- /dev/null +++ b/config/layer_params.yaml @@ -0,0 +1,46 @@ +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/include/costmap_2d/inflation_layer.h b/include/costmap_2d/inflation_layer.h index 44cdabb..9edbbc4 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(); + bool getParams(const std::string& config_file_name); bool need_reinflation_; ///< Indicates that the entire costmap should be reinflated next time around. }; diff --git a/include/costmap_2d/obstacle_layer.h b/include/costmap_2d/obstacle_layer.h index 4665f4f..af284d7 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(); + bool getParams(const std::string& config_file_name); }; } // namespace costmap_2d diff --git a/include/costmap_2d/static_layer.h b/include/costmap_2d/static_layer.h index 4174dbc..e1e6075 100755 --- a/include/costmap_2d/static_layer.h +++ b/include/costmap_2d/static_layer.h @@ -88,7 +88,7 @@ protected: unsigned char lethal_threshold_, unknown_cost_value_; private: - bool getParams(); + bool getParams(const std::string& config_file_name); }; diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h index ee5a852..73ecb22 100644 --- a/include/costmap_2d/utils.h +++ b/include/costmap_2d/utils.h @@ -15,6 +15,25 @@ namespace costmap_2d return default_value; } + inline std::vector loadFootprint(const YAML::Node& node, const std::vector& default_value) + { + if( !node || !node.IsDefined()) + return default_value; + + std::vector fp; + + for (const auto& p : node) { + if (p.size() != 2) + throw std::runtime_error("Footprint point must be [x, y]"); + + fp.push_back(geometry_msgs::Point{p[0].as(), p[1].as()}); + } + std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl; + + return fp; + } + + inline std::string getSourceFile(const std::string& root, const std::string& config_file_name) { std::string path_source = " "; @@ -30,11 +49,13 @@ namespace costmap_2d if (entry.is_regular_file() && entry.path().filename() == config_file_name) { path_source = entry.path().string(); + std::cout << "Path source: " << path_source << std::endl; break; // tìm thấy thì dừng } } } - + if(path_source == " ") + std::cout<< config_file_name << " file not found at path: "<< cfg_dir << std::endl; return path_source; } diff --git a/include/costmap_2d/voxel_layer.h b/include/costmap_2d/voxel_layer.h index 7e5de7b..eb043d2 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(); + bool getParams(const std::string& config_file_name); 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/directional_layer.cpp b/plugins/directional_layer.cpp index d22dc65..70a0a82 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -15,10 +15,8 @@ using costmap_2d::NO_INFORMATION; namespace costmap_2d { DirectionalLayer::DirectionalLayer() - { - // ros::NodeHandle nh("~/" + name_); - // lane_mask_pub_ = nh.advertise("/direction_zones/lanes", 1); - } + {} + DirectionalLayer::~DirectionalLayer() {} bool DirectionalLayer::laneFilter(const std::vector plan) diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index f9b514a..28dedd7 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -79,26 +79,19 @@ void InflationLayer::onInitialize() 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); } matchSize(); } -bool InflationLayer::getParams() +bool InflationLayer::getParams(const std::string& config_file_name) { try { - std::string config_file_name = "config.yaml"; std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - if(path_source != " ") - { - std::cout << "Path source: " << path_source << std::endl; - } - else - { - std::cout << "/cfg folder not found!" << std::endl; - } YAML::Node config = YAML::LoadFile(path_source); YAML::Node layer = config["inflation_layer"]; diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index 8e1a1d9..6b95bf6 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -62,26 +62,18 @@ void ObstacleLayer::onInitialize() current_ = true; stop_receiving_data_ = false; global_frame_ = layered_costmap_->getGlobalFrameID(); - getParams(); + std::string config_file_name = name_ + ".yaml"; + getParams(config_file_name); } ObstacleLayer::~ObstacleLayer() {} -bool ObstacleLayer::getParams() +bool ObstacleLayer::getParams(const std::string& config_file_name) { try { - std::string config_file_name = "config.yaml"; std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - if(path_source != " ") - { - std::cout << "Path source: " << path_source << std::endl; - } - else - { - std::cout << "/cfg folder not found!" << std::endl; - } YAML::Node config = YAML::LoadFile(path_source); @@ -94,9 +86,9 @@ bool ObstacleLayer::getParams() default_value_ = FREE_SPACE; double transform_tolerance = loadParam(layer,"transform_tolerance", 0.2); - // get the topics that we'll subscribe to from the parameter server - std::string topics_string = loadParam(layer,"observation_sources", std::string("")); - printf(" Subscribed to Topics: %s\n", topics_string.c_str()); + // // get the topics that we'll subscribe to from the parameter server + // std::string topics_string = loadParam(layer,"observation_sources", std::string("")); + // printf("Subscribed to Topics: %s\n", topics_string.c_str()); // get the parameters for the specific topic double observation_keep_time = 0, expected_update_rate = 0, min_obstacle_height = 0, max_obstacle_height = 2; diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 47d95dd..02e61f3 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -65,24 +65,16 @@ void StaticLayer::onInitialize() current_ = true; global_frame_ = layered_costmap_->getGlobalFrameID(); - getParams(); + std::string config_file_name = name_ + ".yaml"; + getParams(config_file_name); } -bool StaticLayer::getParams() +bool StaticLayer::getParams(const std::string& config_file_name) { try { - std::string config_file_name = "config.yaml"; std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - if(path_source != " ") - { - std::cout << "Path source: " << path_source << std::endl; - } - else - { - std::cout << "/cfg folder not found!" << std::endl; - } YAML::Node config = YAML::LoadFile(path_source); diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index 01f11eb..d2f1d9f 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -54,26 +54,18 @@ namespace costmap_2d void VoxelLayer::onInitialize() { ObstacleLayer::onInitialize(); - getParams(); + std::string config_file_name = name_ + ".yaml"; + getParams(config_file_name); } VoxelLayer::~VoxelLayer() {} -bool VoxelLayer::getParams() +bool VoxelLayer::getParams(const std::string& config_file_name) { try { - std::string config_file_name = "config.yaml"; std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - if(path_source != " ") - { - std::cout << "Path source: " << path_source << std::endl; - } - else - { - std::cout << "/cfg folder not found!" << std::endl; - } YAML::Node config = YAML::LoadFile(path_source); YAML::Node layer = config["voxel_layer"]; @@ -82,12 +74,12 @@ bool VoxelLayer::getParams() 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); + 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, "max_obstacle_height", 15); + 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", true); + combination_method_ = loadParam(layer, "combination_method", 0.0); this->matchSize(); } catch (const YAML::BadFile& e) { diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index 751b77d..fc02e33 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -66,7 +66,8 @@ Costmap2DROBOT::Costmap2DROBOT(const std::string& name, tf3::BufferCore& tf) : map_update_thread_(NULL), footprint_padding_(0.0) { - getParams("config.yaml"); + std::string config_file_name = name_ + ".yaml"; + getParams(config_file_name); // create a thread to handle updating the map stop_updates_ = false; @@ -80,14 +81,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) { std::string folder = COSTMAP_2D_DIR; std::string path_source = getSourceFile(folder,config_file_name); - if(path_source != " ") - { - std::cout << "Path source: " << path_source << std::endl; - } - else - { - std::cout<< config_file_name << " file not found!" << std::endl; - } YAML::Node config = YAML::LoadFile(path_source); @@ -99,27 +92,28 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) 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()) - { - printf("%f: 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()) + // { + // printf("%f: 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, always_send_full_costmap; + bool rolling_window, track_unknown_space; rolling_window = loadParam(layer, "rolling_window", false); track_unknown_space = loadParam(layer, "track_unknown_space", false); - always_send_full_costmap = loadParam(layer, "always_send_full_costmap", false); + layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space); + struct PluginConfig { std::string name; std::string type; @@ -136,8 +130,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) } } - std::string path = loadParam(layer, "path", std::string(" ")); - std::cout << "Plugin to load: " << path << std::endl; + 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) { @@ -146,22 +140,24 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) // copyParentParameters(pname, type, private_nh); creators_.push_back( boost::dll::import_alias( - path, info.type, boost::dll::load_mode::append_decorations) + path_plugins, info.type, boost::dll::load_mode::append_decorations) ); PluginLayerPtr plugin = creators_.back()(); std::cout << "Plugin created: " << info.name << std::endl; - plugin->initialize(layered_costmap_, info.name, &tf_); + plugin->initialize(layered_costmap_, layer_config_file_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", ex.what()); + 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()); } } - // setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh)); + std::vector new_footprint;// = loadParam(layer, "footprint", 0.0); + new_footprint = loadFootprint(layer["foot_print"], new_footprint); + setUnpaddedRobotFootprint(new_footprint); - transform_tolerance_ = loadParam(layer, "transform_tolerance", false); + transform_tolerance_ = loadParam(layer, "transform_tolerance", 0.0); if (map_update_thread_ != NULL) { map_update_thread_shutdown_ = true; @@ -170,14 +166,14 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) map_update_thread_ = NULL; } map_update_thread_shutdown_ = false; - double map_update_frequency = loadParam(layer, "update_frequency", false); + double map_update_frequency = loadParam(layer, "update_frequency", 0.0); // find size parameters - double map_width_meters = loadParam(layer, "width", false); - double map_height_meters = loadParam(layer, "height", false); - double resolution = loadParam(layer, "resolution", false); - double origin_x = loadParam(layer, "origin_x", false); - double origin_y = loadParam(layer, "origin_y", false); + double map_width_meters = loadParam(layer, "width", 0.0); + double map_height_meters = loadParam(layer, "height", 0.0); + double resolution = loadParam(layer, "resolution", 0.0); + double origin_x = loadParam(layer, "origin_x", 0.0); + double origin_y = loadParam(layer, "origin_y", 0.0); if (!layered_costmap_->isSizeLocked()) { @@ -195,7 +191,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) } double robot_radius = loadParam(layer, "robot_radius", 0.0); - std::vector new_footprint;// = loadParam(layer, "footprint", 0.0); readFootprintFromConfig(new_footprint, unpadded_footprint_, robot_radius); // only construct the thread if the frequency is positive @@ -279,7 +274,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency) while (!map_update_thread_shutdown_) { updateMap(); - + std::cout << "Costmap2DROBOT::mapUpdateLoop updateMap\n"; r.sleep(); // make sure to sleep for the remainder of our cycle time if (r.cycleTime() > robot::Duration(1 / frequency)) diff --git a/src/footprint.cpp b/src/footprint.cpp index 9029803..0bef986 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -27,13 +27,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include #include #include #include #include #include -#include +#include namespace costmap_2d { @@ -207,8 +207,6 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector makeFootprintFromParams(ros::NodeHandle& nh) // { // std::string full_param_name; diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 11ad1e4..ffa8494 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -18,7 +18,7 @@ class CostmapTester : public testing::Test { costmap_2d::Costmap2DROBOT costmap_ros_; }; -CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("test_costmap", tf){} +CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_global_params", tf){} void CostmapTester::checkConsistentCosts(){ costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();