From 3e972bd12e6236bf69bd5290f5f45fbd89e11a48 Mon Sep 17 00:00:00 2001 From: Autumn60 Date: Mon, 2 Sep 2024 23:31:10 +0900 Subject: [PATCH] update booars_costmap_generator Signed-off-by: Autumn60 --- .../config/costmap_generator.param.yaml | 23 ++++++++++++++----- .../src/costmap_generator.cpp | 2 +- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml index 8173e2a4..a2b38ef1 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/config/costmap_generator.param.yaml @@ -1,17 +1,28 @@ /**: ros__parameters: - update_rate: 10.0 + update_rate: 20.0 map_frame_id: "map" costmap_center_frame_id: "base_link" - costmap_width: 20.0 - costmap_resolution: 0.1 + costmap_width: 50.0 + costmap_resolution: 0.2 multi_layered_costmap: layers: - - "lanelet_layer" + - "cached_lanelet_layer" + # - "lanelet_layer" - "predicted_object_layer" - lanelet_layer: - type: "lanelet" + cached_lanelet_layer: + type: "cached_lanelet" map_topic: "/map/vector_map" + costmap_topic: "~/debug/cached_costmap" + cached_costmap: + min_x: 89607.0 + max_x: 89687.0 + min_y: 43114.0 + max_y: 43194.0 + resolution: 0.1 + # lanelet_layer: + # type: "lanelet" + # map_topic: "/map/vector_map" predicted_object_layer: type: "predicted_object" predicted_objects_topic: "/perception/object_recognition/objects" diff --git a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp index aa86eee8..60b34007 100644 --- a/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp +++ b/aichallenge/workspace/src/aichallenge_submit/costmap/booars_costmap_generator/src/costmap_generator.cpp @@ -44,7 +44,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) double costmap_width = this->declare_parameter("costmap_width", 10.0); double costmap_resolution = this->declare_parameter("costmap_resolution", 0.1); costmap_parameters_ = - OccupancyGridParameters::create_parameters(costmap_width, costmap_resolution); + OccupancyGridParameters::create_parameters(costmap_width, costmap_width, costmap_resolution); index_to_point_table_ = booars_utils::nav::occupancy_grid_utils::get_index_to_point_table(costmap_parameters_); }