fix(behavior_velocity): occlusion spot offset (#290)

* fix(behavior velocity): occlusion spot offset Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): remove-lanelet-feature Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * ci(pre-commit): autofix * doc(behavior_velocity): update occlusion spot doc Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * doc(behavior_velocity): cosmetic change Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * feat(behavior_velocity): early return at no target road type Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * doc(behavior_velocity): fix typo Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): remove unused param Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * fix(behavior_velocity): consider ego offset after pass collision Signed-off-by: taikitanaka <ttatcoder@outlook.jp> * feat(behavior_velocity): sort distance closest then length closest Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): param to pass scenario sim Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * doc(behavior_velocity): fix doc Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * fix(behavior_velocity): consider ego at final using .at Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): fix Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * chore(behavior_velocity): better naming Signed-off-by: tanaka3 <ttatcoder@outlook.jp> * fix(behavior_velocity): fix sorting by abs distance Signed-off-by: taikitanaka <ttatcoder@outlook.jp> * fix(behavior_velocity): virtual wall and slice size Signed-off-by: taikitanaka <ttatcoder@outlook.jp> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
autowarefoundation · Feb 4, 2022 · 61ebaf7 · 61ebaf7
1 parent d69147a
commit 61ebaf7
Show file tree

Hide file tree

Showing 11 changed files with 519 additions and 909 deletions.
diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
@@ -2,24 +2,24 @@
   ros__parameters:
     occlusion_spot:
       pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
-      safety_time_buffer: 0.1                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
+      safety_time_buffer: 0.1              # [s] delay for the system response
       threshold:
-        detection_area_length: 200.0        # [m] the length of path to consider occlusion spot
+        detection_area_length: 100.0        # [m] the length of path to consider perception range
         stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop
         lateral_distance: 1.5               # [m] maximum lateral distance to consider hidden collision
       # road type parameters
       public_road:
-        min_velocity: 2.78                  # [m/s] minimum velocity to ignore occlusion spot
-        ebs_decel: -6.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
+        min_velocity: 1.5                   # [m/s] minimum velocity to ignore occlusion spot
+        ebs_decel: -2.5                     # [m/s^2] maximum deceleration to assume for emergency stops.
         pbs_decel: -1.0                     # [m/s^2] deceleration to assume for predictive braking stops
       private_road:
         min_velocity: 1.5                   # [m/s] minimum velocity to ignore occlusion spot
-        ebs_decel: -4.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
-        pbs_decel: -0.5                     # [m/s^2] deceleration to assume for predictive braking stops.
+        ebs_decel: -2.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
+        pbs_decel: -1.0                     # [m/s^2] deceleration to assume for predictive braking stops.
       sidewalk:
         min_occlusion_spot_size: 1.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
-        slice_size: 0.5     # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.
-        focus_range: 2.5            # [m] buffer around the ego path used to build the sidewalk area.
+        slice_size: 1.5     # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.
+        focus_range: 3.0            # [m] buffer around the ego path used to build the sidewalk area.
       grid:
         free_space_max: 40  # maximum value of a free space cell in the occupancy grid
         occupied_min: 60   # minimum value of an occupied cell in the occupancy grid
diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/occlusion_spot.svg
diff --git a/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/...ng/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
@@ -20,8 +20,13 @@
 #include <lanelet2_extension/visualization/visualization.hpp>
 #include <scene_module/occlusion_spot/geometry.hpp>
 #include <scene_module/occlusion_spot/grid_utils.hpp>
+#include <tier4_autoware_utils/geometry/geometry.hpp>
+#include <utilization/util.hpp>
 
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+#include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
+#include <autoware_auto_planning_msgs/msg/path.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 
@@ -32,11 +37,30 @@
 
 #include <algorithm>
 #include <chrono>
+#include <utility>
 #include <vector>
 
 namespace behavior_velocity_planner
 {
+using autoware_auto_perception_msgs::msg::ObjectClassification;
+using autoware_auto_perception_msgs::msg::PredictedObject;
+using autoware_auto_perception_msgs::msg::PredictedObjects;
+using autoware_auto_planning_msgs::msg::Path;
+using autoware_auto_planning_msgs::msg::PathPoint;
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using geometry_msgs::msg::Point;
+using geometry_msgs::msg::Pose;
+using lanelet::ArcCoordinates;
+using lanelet::BasicLineString2d;
+using lanelet::BasicPoint2d;
+using lanelet::BasicPolygon2d;
+using lanelet::ConstLineString2d;
+using lanelet::LaneletMapPtr;
+using lanelet::geometry::fromArcCoordinates;
+using lanelet::geometry::toArcCoordinates;
+using DetectionAreaIdx = boost::optional<std::pair<double, double>>;
+
 namespace occlusion_spot_utils
 {
 enum ROAD_TYPE { PRIVATE, PUBLIC, HIGHWAY, UNKNOWN };
@@ -118,53 +142,98 @@ struct PossibleCollisionInfo
   }
 };
 
+lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
+
+// Note : consider offset_from_start_to_ego and safety margin for collision here
+inline void handleCollisionOffset(
+  std::vector<PossibleCollisionInfo> & possible_collisions, double offset, double margin)
+{
+  for (auto & pc : possible_collisions) {
+    pc.arc_lane_dist_at_collision.length -= offset;
+    pc.arc_lane_dist_at_collision.length -= margin;
+  }
+}
+
+inline double offsetFromStartToEgo(
+  const PathWithLaneId & path, const Pose & ego_pose, const int closest_idx)
+{
+  double offset_from_ego_to_closest = 0;
+  for (int i = 0; i < closest_idx; i++) {
+    const auto & curr_p = path.points.at(i).point.pose.position;
+    const auto & next_p = path.points.at(i + 1).point.pose.position;
+    offset_from_ego_to_closest += tier4_autoware_utils::calcDistance2d(curr_p, next_p);
+  }
+  const double offset_from_closest_to_target =
+    -planning_utils::transformRelCoordinate2D(ego_pose, path.points[closest_idx].point.pose)
+       .position.x;
+  return offset_from_ego_to_closest + offset_from_closest_to_target;
+}
+
+inline void clipPathByLength(
+  const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length = 100.0)
+{
+  double length_sum = 0;
+  for (int i = 0; i < static_cast<int>(path.points.size()) - 1; i++) {
+    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+    if (length_sum > max_length) return;
+    clipped.points.emplace_back(path.points.at(i));
+  }
+}
+
+inline bool isStuckVehicle(PredictedObject obj, const double min_vel)
+{
+  if (
+    obj.classification.at(0).label == ObjectClassification::CAR ||
+    obj.classification.at(0).label == ObjectClassification::TRUCK ||
+    obj.classification.at(0).label == ObjectClassification::BUS) {
+    if (std::abs(obj.kinematics.initial_twist_with_covariance.twist.linear.x) < min_vel) {
+      return true;
+    }
+  }
+  return false;
+}
+void filterCollisionByRoadType(
+  std::vector<PossibleCollisionInfo> & possible_collisions, const DetectionAreaIdx road_type);
 bool splineInterpolate(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & input, const double interval,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * output, const rclcpp::Logger logger);
+  const PathWithLaneId & input, const double interval, PathWithLaneId * output,
+  const rclcpp::Logger logger);
+std::vector<PredictedObject> getParkedVehicles(
+  const PredictedObjects & dyn_objects, const PlannerParam & param,
+  std::vector<Point> & debug_point);
+std::vector<PossibleCollisionInfo> generatePossibleCollisionBehindParkedVehicle(
+  const PathWithLaneId & path, const PlannerParam & param, const double offset_from_start_to_ego,
+  const std::vector<PredictedObject> & dyn_objects);
 ROAD_TYPE getCurrentRoadType(
-  const lanelet::ConstLanelet & current_lanelet, const lanelet::LaneletMapPtr & lanelet_map_ptr);
-//!< @brief build a Lanelet from a interpolated path
-lanelet::ConstLanelet buildPathLanelet(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path);
+  const lanelet::ConstLanelet & current_lanelet, const LaneletMapPtr & lanelet_map_ptr);
 //!< @brief calculate intersection and collision point from occlusion spot
 void calculateCollisionPathPointFromOcclusionSpot(
   PossibleCollisionInfo & pc, const lanelet::BasicPoint2d & obstacle_point,
   const double offset_from_ego_to_target, const lanelet::ConstLanelet & path_lanelet,
   const PlannerParam & param);
 //!< @brief create hidden collision behind parked car
 void createPossibleCollisionBehindParkedVehicle(
-  std::vector<PossibleCollisionInfo> & possible_collisions,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const PlannerParam & param,
-  const double offset_from_ego_to_target,
-  const autoware_auto_perception_msgs::msg::PredictedObjects::ConstSharedPtr & dyn_obj_arr);
+  std::vector<PossibleCollisionInfo> & possible_collisions, const PathWithLaneId & path,
+  const PlannerParam & param, const double offset_from_ego_to_target,
+  const PredictedObjects::ConstSharedPtr & dyn_obj_arr);
 //!< @brief set velocity and orientation to collision point based on previous Path with laneId
 void calcSlowDownPointsForPossibleCollision(
-  const int closest_idx, const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
-  const double offset_from_ego_to_target, std::vector<PossibleCollisionInfo> & possible_collisions);
+  const int closest_idx, const PathWithLaneId & path, const double offset,
+  std::vector<PossibleCollisionInfo> & possible_collisions);
 //!< @brief extract lanelet that includes target_road_type only
-bool extractTargetRoad(
-  const int closest_idx, const lanelet::LaneletMapPtr lanelet_map_ptr, const double max_range,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & src_path,
-  double & offset_from_closest_to_target,
-  autoware_auto_planning_msgs::msg::PathWithLaneId & tar_path, const ROAD_TYPE & target_road_type);
-//!< @brief generate collision coming from occlusion spots of the given grid map and lanelet map
-void generatePossibleCollisions(
-  std::vector<PossibleCollisionInfo> & possible_collisions,
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const grid_map::GridMap & grid,
-  const double offset_from_ego_to_closest, const double offset_from_closest_to_target,
-  const PlannerParam & param, std::vector<lanelet::BasicPolygon2d> & debug);
+DetectionAreaIdx extractTargetRoadArcLength(
+  const LaneletMapPtr lanelet_map_ptr, const double max_range, const PathWithLaneId & path,
+  const ROAD_TYPE & target_road_type);
 //!< @brief convert a set of occlusion spots found on sidewalk slice
 void generateSidewalkPossibleCollisionFromOcclusionSpot(
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
   const std::vector<grid_map::Position> & occlusion_spot_positions,
-  const double offset_form_ego_to_target, const lanelet::ConstLanelet & path_lanelet,
+  const double offset_from_start_to_ego, const lanelet::ConstLanelet & path_lanelet,
   const PlannerParam & param);
 //!< @brief generate possible collisions coming from occlusion spots on the side of the path
 void generateSidewalkPossibleCollisions(
   std::vector<PossibleCollisionInfo> & possible_collisions, const grid_map::GridMap & grid,
-  const double offset_from_ego_to_closest, const double offset_from_closest_to_target,
-  const lanelet::ConstLanelet & path_lanelet, const PlannerParam & param,
-  std::vector<lanelet::BasicPolygon2d> & debug);
+  const PathWithLaneId & path, const double offset_from_start_to_ego, const PlannerParam & param,
+  std::vector<BasicPolygon2d> & debug);
 
 }  // namespace occlusion_spot_utils
 }  // namespace behavior_velocity_planner

diff --git a/...ocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp b/...ocity_planner/include/scene_module/occlusion_spot/scene_occlusion_spot_in_public_road.hpp
@@ -46,6 +46,7 @@ class OcclusionSpotInPublicModule : public SceneModuleInterface
     std::string road_type = "public";
     double z;
     std::vector<lanelet::BasicPolygon2d> sidewalks;
+    std::vector<geometry_msgs::msg::Point> parked_vehicle_point;
     std::vector<occlusion_spot_utils::PossibleCollisionInfo> possible_collisions;
   };