feat(avoidance): improve avoidance target filter (autowarefoundation#…

…2282) * feat(avoidance): use envelope polygon for measure against perception noise Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(avoidance): use moving time for measure against perception noise Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * feat(tier4_planning_launch): add new params for avoidance Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix(avoidance): reserve marker array size Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
YoshiRi · Jan 11, 2023 · da17d79 · da17d79
1 parent 4ba67fb
commit da17d79
Show file tree

Hide file tree

Showing 10 changed files with 398 additions and 58 deletions.
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
@@ -12,8 +12,10 @@
 
       threshold_distance_object_is_on_center: 1.0 # [m]
       threshold_speed_object_is_stopped: 1.0      # [m/s]
+      threshold_time_object_is_moving: 1.0        # [s]
       object_check_forward_distance: 150.0        # [m]
       object_check_backward_distance: 2.0         # [m]
+      object_envelope_buffer: 0.3                 # [m]
       lateral_collision_margin: 1.0               # [m]
       lateral_collision_safety_buffer: 0.7        # [m]
 

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
@@ -192,6 +192,15 @@ class AvoidanceModule : public SceneModuleInterface
     }
   }
 
+  /**
+   * object pre-process
+   */
+  void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const;
+  void fillObjectEnvelopePolygon(const Pose & closest_pose, ObjectData & object_data) const;
+  void fillObjectMovingTime(ObjectData & object_data) const;
+  void compensateDetectionLost(
+    ObjectDataArray & target_objects, ObjectDataArray & other_objects) const;
+
   // data used in previous planning
   ShiftedPath prev_output_;
   ShiftedPath prev_linear_shift_path_;  // used for shift point check
@@ -209,13 +218,9 @@ class AvoidanceModule : public SceneModuleInterface
   // -- for pre-processing --
   void initVariables();
   AvoidancePlanningData calcAvoidancePlanningData(DebugData & debug) const;
-  ObjectDataArray calcAvoidanceTargetObjects(
-    const lanelet::ConstLanelets & lanelets, const PathWithLaneId & reference_path,
-    DebugData & debug) const;
 
   ObjectDataArray registered_objects_;
   void updateRegisteredObject(const ObjectDataArray & objects);
-  void CompensateDetectionLost(ObjectDataArray & objects) const;
 
   // -- for shift point generation --
   AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const;
@@ -288,7 +293,8 @@ class AvoidanceModule : public SceneModuleInterface
   // debug
   mutable DebugData debug_data_;
   mutable std::shared_ptr<AvoidanceDebugMsgArray> debug_msg_ptr_;
-  void setDebugData(const PathShifter & shifter, const DebugData & debug);
+  void setDebugData(
+    const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const;
   void updateAvoidanceDebugData(std::vector<AvoidanceDebugMsg> & avoidance_debug_msg_array) const;
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
   mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
@@ -318,6 +324,11 @@ class AvoidanceModule : public SceneModuleInterface
   double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); }
   double getCurrentShift() const;
   double getCurrentLinearShift() const;
+
+  /**
+   * avoidance module misc data
+   */
+  mutable ObjectDataArray stopped_objects_;
 };
 
 }  // namespace behavior_path_planner

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
@@ -18,6 +18,7 @@
 #include "behavior_path_planner/scene_module/utils/path_shifter.hpp"
 
 #include <rclcpp/rclcpp.hpp>
+#include <tier4_autoware_utils/tier4_autoware_utils.hpp>
 
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
@@ -34,10 +35,12 @@ namespace behavior_path_planner
 using autoware_auto_perception_msgs::msg::PredictedObject;
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
 
+using tier4_autoware_utils::Polygon2d;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
 
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
+using geometry_msgs::msg::TransformStamped;
 
 struct AvoidanceParameters
 {
@@ -74,6 +77,12 @@ struct AvoidanceParameters
   // continue to detect backward vehicles as avoidance targets until they are this distance away
   double object_check_backward_distance;
 
+  // object's enveloped polygon
+  double object_envelope_buffer;
+
+  // vehicles which is moving more than this parameter will not be avoided
+  double threshold_time_object_is_moving;
+
   // we want to keep this lateral margin when avoiding
   double lateral_collision_margin;
   // a buffer in case lateral_collision_margin is set to 0. Will throw error
@@ -191,12 +200,19 @@ struct ObjectData  // avoidance target
   rclcpp::Time last_seen;
   double lost_time{0.0};
 
+  // count up when object moved. Removed when it exceeds threshold.
+  rclcpp::Time last_stop;
+  double move_time{0.0};
+
   // store the information of the lanelet which the object's overhang is currently occupying
   lanelet::ConstLanelet overhang_lanelet;
 
   // the position of the overhang
   Pose overhang_pose;
 
+  // envelope polygon
+  Polygon2d envelope_poly{};
+
   // lateral distance from overhang to the road shoulder
   double to_road_shoulder_distance{0.0};
 };
@@ -252,7 +268,10 @@ struct AvoidancePlanningData
   lanelet::ConstLanelets current_lanelets;
 
   // avoidance target objects
-  ObjectDataArray objects;
+  ObjectDataArray target_objects;
+
+  // the others
+  ObjectDataArray other_objects;
 };
 
 /*

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
@@ -50,9 +50,15 @@ void fillLongitudinalAndLengthByClosestFootprint(
   const PathWithLaneId & path, const PredictedObject & object, const Point & ego_pos,
   ObjectData & obj);
 
+void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
+  const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
+
 double calcOverhangDistance(
   const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
 
+double calcEnvelopeOverhangDistance(
+  const ObjectData & object_data, const Pose & base_pose, Point & overhang_pose);
+
 void setEndData(
   AvoidLine & al, const double length, const geometry_msgs::msg::Pose & end, const size_t end_idx,
   const double end_dist);
@@ -64,6 +70,9 @@ void setStartData(
 std::string getUuidStr(const ObjectData & obj);
 
 std::vector<std::string> getUuidStr(const ObjectDataArray & objs);
+
+Polygon2d createEnvelopePolygon(
+  const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 }  // namespace behavior_path_planner
 
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__AVOIDANCE__AVOIDANCE_UTILS_HPP_
diff --git a/...ning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp b/...ning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/debug.hpp
@@ -47,7 +47,10 @@ MarkerArray createAvoidLineMarkerArray(
   const AvoidLineArray & shift_points, std::string && ns, const float & r, const float & g,
   const float & b, const double & w);
 
-MarkerArray createAvoidanceObjectsMarkerArray(
+MarkerArray createTargetObjectsMarkerArray(
+  const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
+
+MarkerArray createOtherObjectsMarkerArray(
   const behavior_path_planner::ObjectDataArray & objects, std::string && ns);
 
 MarkerArray makeOverhangToRoadShoulderMarkerArray(

diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml
@@ -44,6 +44,7 @@
   <depend>autoware_adapi_v1_msgs</depend>
   <depend>autoware_auto_perception_msgs</depend>
   <depend>autoware_auto_planning_msgs</depend>
+  <depend>autoware_auto_tf2</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>behaviortree_cpp_v3</depend>
   <depend>geometry_msgs</depend>

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -274,8 +274,10 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam()
 
   p.threshold_distance_object_is_on_center = dp("threshold_distance_object_is_on_center", 1.0);
   p.threshold_speed_object_is_stopped = dp("threshold_speed_object_is_stopped", 1.0);
+  p.threshold_time_object_is_moving = dp("threshold_time_object_is_moving", 1.0);
   p.object_check_forward_distance = dp("object_check_forward_distance", 150.0);
   p.object_check_backward_distance = dp("object_check_backward_distance", 2.0);
+  p.object_envelope_buffer = dp("object_envelope_buffer", 0.1);
   p.lateral_collision_margin = dp("lateral_collision_margin", 2.0);
   p.lateral_collision_safety_buffer = dp("lateral_collision_safety_buffer", 0.5);