feat(behavior_path_planner, obstacle_avoidance_planner): add new driv…

…able area (autowarefoundation#2472) * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update obstacle avoidance planner Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * clean code Signed-off-by: yutaka <purewater0901@gmail.com> * uddate Signed-off-by: yutaka <purewater0901@gmail.com> * clean code Signed-off-by: yutaka <purewater0901@gmail.com> * remove resample Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * add orientation Signed-off-by: yutaka <purewater0901@gmail.com> * change color Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * remove drivable area Signed-off-by: yutaka <purewater0901@gmail.com> * add flag Signed-off-by: yutaka <purewater0901@gmail.com> * update Signed-off-by: yutaka <purewater0901@gmail.com> * update color Signed-off-by: yutaka <purewater0901@gmail.com> * fix some codes Signed-off-by: yutaka <purewater0901@gmail.com> * change to makerker array Signed-off-by: yutaka <purewater0901@gmail.com> * change avoidance utils Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com>
HansRobo · Dec 12, 2022 · 7f0138c · 7f0138c
1 parent c855e23
commit 7f0138c
Show file tree

Hide file tree

Showing 42 changed files with 715 additions and 1,654 deletions.
diff --git a/common/motion_utils/src/resample/resample.cpp b/common/motion_utils/src/resample/resample.cpp
@@ -260,7 +260,8 @@ autoware_auto_planning_msgs::msg::PathWithLaneId resamplePath(
 
   autoware_auto_planning_msgs::msg::PathWithLaneId resampled_path;
   resampled_path.header = input_path.header;
-  resampled_path.drivable_area = input_path.drivable_area;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = input_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;
@@ -402,7 +403,8 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
 
   autoware_auto_planning_msgs::msg::Path resampled_path;
   resampled_path.header = input_path.header;
-  resampled_path.drivable_area = input_path.drivable_area;
+  resampled_path.left_bound = input_path.left_bound;
+  resampled_path.right_bound = resampled_path.right_bound;
   resampled_path.points.resize(interpolated_pose.size());
   for (size_t i = 0; i < resampled_path.points.size(); ++i) {
     autoware_auto_planning_msgs::msg::PathPoint path_point;

diff --git a/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp b/common/tier4_planning_rviz_plugin/src/drivable_area/display.cpp
@@ -308,9 +308,8 @@ bool validateFloats(const nav_msgs::msg::OccupancyGrid & msg)
 }
 
 void AutowareDrivableAreaDisplay::processMessage(
-  autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg)
+  [[maybe_unused]] autoware_auto_planning_msgs::msg::Path::ConstSharedPtr msg)
 {
-  current_map_ = msg->drivable_area;
   loaded_ = true;
   // updated via signal in case ros spinner is in a different thread
   Q_EMIT mapUpdated();

diff --git a/...nning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/...nning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml
@@ -50,6 +50,9 @@
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
       max_avoidance_acceleration: 0.5  # [m/ss]
 
+      # bound clipping for objects
+      enable_bound_clipping: false
+
       # for debug
       publish_debug_marker: false
       print_debug_info: false

diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml
@@ -50,6 +50,9 @@
       min_avoidance_speed_for_acc_prevention: 3.0  # [m/s]
       max_avoidance_acceleration: 0.5  # [m/s2]
 
+      # bound clipping for objects
+      enable_bound_clipping: false
+
       # for debug
       publish_debug_marker: false
       print_debug_info: false

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp
@@ -43,6 +43,7 @@
 #include <tier4_planning_msgs/msg/lane_change_debug_msg_array.hpp>
 #include <tier4_planning_msgs/msg/path_change_module.hpp>
 #include <tier4_planning_msgs/msg/scenario.hpp>
+#include <visualization_msgs/msg/marker.hpp>
 
 #include <memory>
 #include <mutex>
@@ -80,6 +81,7 @@ using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 using tier4_planning_msgs::msg::PathChangeModule;
 using tier4_planning_msgs::msg::Scenario;
+using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
 
 class BehaviorPathPlannerNode : public rclcpp::Node
@@ -99,6 +101,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   rclcpp::Publisher<Path>::SharedPtr path_candidate_publisher_;
   rclcpp::Publisher<TurnIndicatorsCommand>::SharedPtr turn_signal_publisher_;
   rclcpp::Publisher<HazardLightsCommand>::SharedPtr hazard_signal_publisher_;
+  rclcpp::Publisher<MarkerArray>::SharedPtr bound_publisher_;
   rclcpp::TimerBase::SharedPtr timer_;
 
   std::shared_ptr<PlannerData> planner_data_;
@@ -184,6 +187,11 @@ class BehaviorPathPlannerNode : public rclcpp::Node
    */
   void publish_steering_factor(const TurnIndicatorsCommand & turn_signal);
 
+  /**
+   * @brief publish left and right bound
+   */
+  void publish_bounds(const PathWithLaneId & path);
+
   /**
    * @brief publish debug messages
    */

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp
@@ -24,6 +24,7 @@
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <geometry_msgs/msg/accel_with_covariance_stamped.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
 #include <lanelet2_core/geometry/Lanelet.h>

diff --git a/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/...or_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp
@@ -275,7 +275,7 @@ class AvoidanceModule : public SceneModuleInterface
 
   // -- path generation --
   ShiftedPath generateAvoidancePath(PathShifter & shifter) const;
-  void generateExtendedDrivableArea(ShiftedPath * shifted_path) const;
+  void generateExtendedDrivableArea(PathWithLaneId & path) const;
 
   // -- velocity planning --
   std::shared_ptr<double> ego_velocity_starting_avoidance_ptr_;

diff --git a/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/...th_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp
@@ -179,6 +179,9 @@ struct AvoidanceParameters
   double drivable_area_right_bound_offset;
   double drivable_area_left_bound_offset;
 
+  // clip left and right bounds for objects
+  bool enable_bound_clipping{false};
+
   // debug
   bool publish_debug_marker = false;
   bool print_debug_info = false;

diff --git a/...ior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp b/...ior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_utils.hpp
@@ -25,6 +25,14 @@
 namespace behavior_path_planner
 {
 using behavior_path_planner::PlannerData;
+
+struct PolygonPoint
+{
+  geometry_msgs::msg::Point point;
+  double lat_dist_to_bound;
+  double lon_dist;
+};
+
 bool isOnRight(const ObjectData & obj);
 
 double calcShiftLength(
@@ -73,6 +81,26 @@ std::vector<std::string> getUuidStr(const ObjectDataArray & objs);
 
 Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
+
+void getEdgePoints(
+  const Polygon2d & object_polygon, const double threshold, std::vector<Point> & edge_points);
+
+void getEdgePoints(
+  const std::vector<Point> & bound, const std::vector<Point> & edge_points,
+  const double lat_dist_to_path, std::vector<PolygonPoint> & edge_points_data,
+  size_t & start_segment_idx, size_t & end_segment_idx);
+
+void sortPolygonPoints(
+  const std::vector<PolygonPoint> & points, std::vector<PolygonPoint> & sorted_points);
+
+std::vector<Point> updateBoundary(
+  const std::vector<Point> & original_bound, const std::vector<PolygonPoint> & points,
+  const size_t start_segment_idx, const size_t end_segment_idx);
+
+void generateDrivableArea(
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
+  const std::shared_ptr<const PlannerData> planner_data, const ObjectDataArray & objects,
+  const bool enable_bound_clipping);
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp
@@ -66,7 +66,6 @@ using geometry_msgs::msg::PoseArray;
 using geometry_msgs::msg::PoseStamped;
 using geometry_msgs::msg::Twist;
 using geometry_msgs::msg::Vector3;
-using nav_msgs::msg::OccupancyGrid;
 using route_handler::RouteHandler;
 using tier4_autoware_utils::LineString2d;
 using tier4_autoware_utils::Point2d;
@@ -293,16 +292,9 @@ size_t getOverlappedLaneletId(const std::vector<DrivableLanes> & lanes);
 std::vector<DrivableLanes> cutOverlappedLanes(
   PathWithLaneId & path, const std::vector<DrivableLanes> & lanes);
 
-void occupancyGridToImage(const OccupancyGrid & occupancy_grid, cv::Mat * cv_image);
-
-void imageToOccupancyGrid(const cv::Mat & cv_image, OccupancyGrid * occupancy_grid);
-
-cv::Point toCVPoint(
-  const Point & geom_point, const double width_m, const double height_m, const double resolution);
-
-OccupancyGrid generateDrivableArea(
-  const PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double resolution,
-  const double vehicle_length, const std::shared_ptr<const PlannerData> planner_data);
+void generateDrivableArea(
+  PathWithLaneId & path, const std::vector<DrivableLanes> & lanes, const double vehicle_length,
+  const std::shared_ptr<const PlannerData> planner_data);
 
 lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets(
   const std::shared_ptr<const PlannerData> & planner_data);

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -75,6 +75,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
       create_publisher<MarkerArray>("~/drivable_area_boundary", 1);
   }
 
+  bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
+
   // subscriber
   velocity_subscriber_ = create_subscription<Odometry>(
     "~/input/odometry", 1, std::bind(&BehaviorPathPlannerNode::onVelocity, this, _1),
@@ -335,6 +337,8 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
   p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0);
   p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0);
 
+  p.enable_bound_clipping = dp("enable_bound_clipping", false);
+
   p.avoidance_execution_lateral_threshold = dp("avoidance_execution_lateral_threshold", 0.499);
 
   return p;
@@ -625,6 +629,9 @@ void BehaviorPathPlannerNode::run()
   planner_data_->prev_output_path = path;
   mutex_pd_.unlock();
 
+  // publish drivable bounds
+  publish_bounds(*path);
+
   const size_t target_idx = findEgoIndex(path->points);
   util::clipPathLength(*path, target_idx, planner_data_->parameters);
 
@@ -700,6 +707,39 @@ void BehaviorPathPlannerNode::publish_steering_factor(const TurnIndicatorsComman
   steering_factor_interface_ptr_->publishSteeringFactor(get_clock()->now());
 }
 
+void BehaviorPathPlannerNode::publish_bounds(const PathWithLaneId & path)
+{
+  constexpr double scale_x = 0.1;
+  constexpr double scale_y = 0.1;
+  constexpr double scale_z = 0.1;
+  constexpr double color_r = 0.0 / 256.0;
+  constexpr double color_g = 148.0 / 256.0;
+  constexpr double color_b = 205.0 / 256.0;
+  constexpr double color_a = 0.999;
+
+  const auto current_time = path.header.stamp;
+  auto left_marker = tier4_autoware_utils::createDefaultMarker(
+    "map", current_time, "left_bound", 0L, Marker::LINE_STRIP,
+    tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z),
+    tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a));
+  for (const auto lb : path.left_bound) {
+    left_marker.points.push_back(lb);
+  }
+
+  auto right_marker = tier4_autoware_utils::createDefaultMarker(
+    "map", current_time, "right_bound", 0L, Marker::LINE_STRIP,
+    tier4_autoware_utils::createMarkerScale(scale_x, scale_y, scale_z),
+    tier4_autoware_utils::createMarkerColor(color_r, color_g, color_b, color_a));
+  for (const auto rb : path.right_bound) {
+    right_marker.points.push_back(rb);
+  }
+
+  MarkerArray msg;
+  msg.markers.push_back(left_marker);
+  msg.markers.push_back(right_marker);
+  bound_publisher_->publish(msg);
+}
+
 void BehaviorPathPlannerNode::publishSceneModuleDebugMsg()
 {
   {

diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp
@@ -123,7 +123,8 @@ Path toPath(const PathWithLaneId & input)
 {
   Path output{};
   output.header = input.header;
-  output.drivable_area = input.drivable_area;
+  output.left_bound = input.left_bound;
+  output.right_bound = input.right_bound;
   output.points.resize(input.points.size());
   for (size_t i = 0; i < input.points.size(); ++i) {
     output.points.at(i) = input.points.at(i).point;

diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp
@@ -1902,7 +1902,7 @@ double AvoidanceModule::getLeftShiftBound() const
 // TODO(murooka) freespace during turning in intersection where there is no neighbour lanes
 // NOTE: Assume that there is no situation where there is an object in the middle lane of more than
 // two lanes since which way to avoid is not obvious
-void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) const
+void AvoidanceModule::generateExtendedDrivableArea(PathWithLaneId & path) const
 {
   const auto has_same_lane =
     [](const lanelet::ConstLanelets lanes, const lanelet::ConstLanelet & lane) {
@@ -2007,17 +2007,17 @@ void AvoidanceModule::generateExtendedDrivableArea(ShiftedPath * shifted_path) c
     drivable_lanes.push_back(current_drivable_lanes);
   }
 
-  const auto shorten_lanes = util::cutOverlappedLanes(shifted_path->path, drivable_lanes);
+  const auto shorten_lanes = util::cutOverlappedLanes(path, drivable_lanes);
 
   const auto extended_lanes = util::expandLanelets(
     shorten_lanes, parameters_->drivable_area_left_bound_offset,
     parameters_->drivable_area_right_bound_offset, {"road_border"});
 
   {
     const auto & p = planner_data_->parameters;
-    shifted_path->path.drivable_area = util::generateDrivableArea(
-      shifted_path->path, extended_lanes, p.drivable_area_resolution, p.vehicle_length,
-      planner_data_);
+    generateDrivableArea(
+      path, drivable_lanes, p.vehicle_length, planner_data_, avoidance_data_.target_objects,
+      parameters_->enable_bound_clipping);
   }
 }
 
@@ -2261,9 +2261,6 @@ BehaviorModuleOutput AvoidanceModule::plan()
   auto avoidance_path = generateAvoidancePath(path_shifter_);
   debug_data_.output_shift = avoidance_path.shift_length;
 
-  // Drivable area generation.
-  generateExtendedDrivableArea(&avoidance_path);
-
   // modify max speed to prevent acceleration in avoidance maneuver.
   modifyPathVelocityToPreventAccelerationOnAvoidance(avoidance_path);
 
@@ -2293,6 +2290,9 @@ BehaviorModuleOutput AvoidanceModule::plan()
   const size_t ego_idx = findEgoIndex(output.path->points);
   util::clipPathLength(*output.path, ego_idx, planner_data_->parameters);
 
+  // Drivable area generation.
+  generateExtendedDrivableArea(*output.path);
+
   DEBUG_PRINT("exit plan(): set prev output (back().lat = %f)", prev_output_.shift_length.back());
 
   updateRegisteredRTCStatus(avoidance_path.path);