From 1e9e9af75999f06dced5866c2e89a00e1d9654c6 Mon Sep 17 00:00:00 2001 From: NorahXiong <103234047+NorahXiong@users.noreply.github.com> Date: Fri, 5 Aug 2022 12:06:45 +0800 Subject: [PATCH] fix(costmap_generator): restrict costmap within parking lot (#996) * fix(costmap_generator): restrict costmap within parking lot Signed-off-by: NorahXiong * add parameters for free space planning area selection Signed-off-by: NorahXiong * ci(pre-commit): autofix Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../scenario_planning/parking.launch.py | 1 + planning/costmap_generator/README.md | 5 ++-- .../costmap_generator/costmap_generator.hpp | 7 +++--- .../costmap_generator/object_map_utils.hpp | 10 ++++---- .../launch/costmap_generator.launch.xml | 1 + .../costmap_generator_node.cpp | 24 +++++++++++-------- .../costmap_generator/object_map_utils.cpp | 4 ++-- 7 files changed, 30 insertions(+), 22 deletions(-) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index 16283674908d1..81758eec72485 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -73,6 +73,7 @@ def launch_setup(context, *args, **kwargs): "map_frame": "map", "update_rate": 10.0, "use_wayarea": True, + "use_parkinglot": True, "use_objects": True, "use_points": True, "grid_min_value": 0.0, diff --git a/planning/costmap_generator/README.md b/planning/costmap_generator/README.md index c4be20c32dbff..a38a1a4d7ff9b 100644 --- a/planning/costmap_generator/README.md +++ b/planning/costmap_generator/README.md @@ -38,6 +38,7 @@ None | `use_objects` | bool | whether using `~input/objects` or not | | `use_points` | bool | whether using `~input/points_no_ground` or not | | `use_wayarea` | bool | whether using `wayarea` from `~input/vector_map` or not | +| `use_parkinglot` | bool | whether using `parkinglot` from `~input/vector_map` or not | | `costmap_frame` | string | created costmap's coordinate | | `vehicle_frame` | string | vehicle's coordinate | | `map_frame` | string | map's coordinate | @@ -69,8 +70,8 @@ endif :set the center of costmap to current pose; -if (use wayarea?) then (yes) - :generate wayarea costmap; +if (use wayarea or use parkinglot?) then (yes) + :generate map primitives costmap; endif if (use objects?) then (yes) diff --git a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp index 92092b721c1da..87fded0c01891 100644 --- a/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp +++ b/planning/costmap_generator/include/costmap_generator/costmap_generator.hpp @@ -81,6 +81,7 @@ class CostmapGenerator : public rclcpp::Node bool use_objects_; bool use_points_; bool use_wayarea_; + bool use_parkinglot_; lanelet::LaneletMapPtr lanelet_map_; autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr objects_; @@ -123,7 +124,7 @@ class CostmapGenerator : public rclcpp::Node tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; - std::vector> area_points_; + std::vector> primitives_points_; PointsToCostmap points2costmap_; ObjectsToCostmap objects2costmap_; @@ -134,7 +135,7 @@ class CostmapGenerator : public rclcpp::Node { static constexpr const char * objects = "objects"; static constexpr const char * points = "points"; - static constexpr const char * wayarea = "wayarea"; + static constexpr const char * primitives = "primitives"; static constexpr const char * combined = "combined"; }; @@ -192,7 +193,7 @@ class CostmapGenerator : public rclcpp::Node const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr in_objects); /// \brief calculate cost from lanelet2 map - grid_map::Matrix generateWayAreaCostmap(); + grid_map::Matrix generatePrimitivesCostmap(); /// \brief calculate cost for final output grid_map::Matrix generateCombinedCostmap(); diff --git a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp b/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp index 869a801b2361f..282bd6dcc1beb 100644 --- a/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp +++ b/planning/costmap_generator/include/costmap_generator/object_map_utils.hpp @@ -78,19 +78,19 @@ void PublishOccupancyGrid( /*! * Projects the in_area_points forming the road, stores the result in out_grid_map. * @param[out] out_grid_map GridMap object to add the road grid - * @param[in] in_area_points Array of points containing the wayareas + * @param[in] in_points Array of points containing the selected primitives * @param[in] in_grid_layer_name Name to assign to the layer * @param[in] in_layer_background_value Empty state value - * @param[in] in_fill_color Value to fill on wayareas + * @param[in] in_fill_color Value to fill on selected primitives * @param[in] in_layer_min_value Minimum value in the layer - * @param[in] in_layer_max_value Maximum value in the later - * @param[in] in_tf_target_frame Target frame to transform the wayarea points + * @param[in] in_layer_max_value Maximum value in the layer + * @param[in] in_tf_target_frame Target frame to transform the points * @param[in] in_tf_source_frame Source frame, where the points are located * @param[in] in_tf_listener Valid listener to obtain the transformation */ void FillPolygonAreas( grid_map::GridMap & out_grid_map, - const std::vector> & in_area_points, + const std::vector> & in_points, const std::string & in_grid_layer_name, const int in_layer_background_value, const int in_fill_color, const int in_layer_min_value, const int in_layer_max_value, const std::string & in_tf_target_frame, const std::string & in_tf_source_frame, diff --git a/planning/costmap_generator/launch/costmap_generator.launch.xml b/planning/costmap_generator/launch/costmap_generator.launch.xml index 75b16f4fc2e51..7da0a258b37e0 100644 --- a/planning/costmap_generator/launch/costmap_generator.launch.xml +++ b/planning/costmap_generator/launch/costmap_generator.launch.xml @@ -21,6 +21,7 @@ + diff --git a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp index f632045e3d479..6999cd54d57c2 100644 --- a/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/costmap_generator_node.cpp @@ -178,6 +178,7 @@ CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & node_options) use_objects_ = this->declare_parameter("use_objects", true); use_points_ = this->declare_parameter("use_points", true); use_wayarea_ = this->declare_parameter("use_wayarea", true); + use_parkinglot_ = this->declare_parameter("use_parkinglot", true); expand_polygon_size_ = this->declare_parameter("expand_polygon_size", 1.0); size_of_expansion_kernel_ = this->declare_parameter("size_of_expansion_kernel", 9); @@ -268,8 +269,11 @@ void CostmapGenerator::onLaneletMapBin( lanelet::utils::conversion::fromBinMsg(*msg, lanelet_map_); if (use_wayarea_) { - loadRoadAreasFromLaneletMap(lanelet_map_, &area_points_); - loadParkingAreasFromLaneletMap(lanelet_map_, &area_points_); + loadRoadAreasFromLaneletMap(lanelet_map_, &primitives_points_); + } + + if (use_parkinglot_) { + loadParkingAreasFromLaneletMap(lanelet_map_, &primitives_points_); } } @@ -311,8 +315,8 @@ void CostmapGenerator::onTimer() p.y() = tf.transform.translation.y; costmap_.setPosition(p); - if (use_wayarea_ && lanelet_map_) { - costmap_[LayerName::wayarea] = generateWayAreaCostmap(); + if ((use_wayarea_ || use_parkinglot_) && lanelet_map_) { + costmap_[LayerName::primitives] = generatePrimitivesCostmap(); } if (use_objects_ && objects_) { @@ -355,7 +359,7 @@ void CostmapGenerator::initGridmap() costmap_.add(LayerName::points, grid_min_value_); costmap_.add(LayerName::objects, grid_min_value_); - costmap_.add(LayerName::wayarea, grid_min_value_); + costmap_.add(LayerName::primitives, grid_min_value_); costmap_.add(LayerName::combined, grid_min_value_); } @@ -418,15 +422,15 @@ grid_map::Matrix CostmapGenerator::generateObjectsCostmap( return objects_costmap; } -grid_map::Matrix CostmapGenerator::generateWayAreaCostmap() +grid_map::Matrix CostmapGenerator::generatePrimitivesCostmap() { grid_map::GridMap lanelet2_costmap = costmap_; - if (!area_points_.empty()) { + if (!primitives_points_.empty()) { object_map::FillPolygonAreas( - lanelet2_costmap, area_points_, LayerName::wayarea, grid_max_value_, grid_min_value_, + lanelet2_costmap, primitives_points_, LayerName::primitives, grid_max_value_, grid_min_value_, grid_min_value_, grid_max_value_, costmap_frame_, map_frame_, tf_buffer_); } - return lanelet2_costmap[LayerName::wayarea]; + return lanelet2_costmap[LayerName::primitives]; } grid_map::Matrix CostmapGenerator::generateCombinedCostmap() @@ -440,7 +444,7 @@ grid_map::Matrix CostmapGenerator::generateCombinedCostmap() combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::points]); combined_costmap[LayerName::combined] = - combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::wayarea]); + combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::primitives]); combined_costmap[LayerName::combined] = combined_costmap[LayerName::combined].cwiseMax(combined_costmap[LayerName::objects]); diff --git a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp index 3680662b94803..0dbf1feb5561b 100644 --- a/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp +++ b/planning/costmap_generator/nodes/costmap_generator/object_map_utils.cpp @@ -59,7 +59,7 @@ void PublishOccupancyGrid( void FillPolygonAreas( grid_map::GridMap & out_grid_map, - const std::vector> & in_area_points, + const std::vector> & in_points, const std::string & in_grid_layer_name, const int in_layer_background_value, const int in_layer_min_value, const int in_fill_color, const int in_layer_max_value, const std::string & in_tf_target_frame, const std::string & in_tf_source_frame, @@ -86,7 +86,7 @@ void FillPolygonAreas( const double origin_x_offset = out_grid_map.getLength().x() / 2.0 - map_pos.x(); const double origin_y_offset = out_grid_map.getLength().y() / 2.0 - map_pos.y(); - for (const auto & points : in_area_points) { + for (const auto & points : in_points) { std::vector cv_polygon; for (const auto & p : points) {