test(static_obstacle_avoidance): add unit test for utils functions (a…

…utowarefoundation#9134) * docs(static_obstacle_avoidance): add doxygen Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * test: add test Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: assert and expect Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * fix: wrong comment Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor: use autoware test utils Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
1222-takeshi · Oct 22, 2024 · 99b8664 · 99b8664
1 parent e5ad593
commit 99b8664
Show file tree

Hide file tree

Showing 5 changed files with 1,694 additions and 26 deletions.
diff --git a/...nce_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp b/...nce_module/include/autoware/behavior_path_static_obstacle_avoidance_module/type_alias.hpp
@@ -34,8 +34,10 @@
 namespace autoware::behavior_path_planner
 {
 // auto msgs
+using autoware_perception_msgs::msg::ObjectClassification;
 using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedPath;
+using autoware_perception_msgs::msg::Shape;
 using tier4_planning_msgs::msg::PathWithLaneId;
 
 // ROS 2 general msgs

diff --git a/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
@@ -36,17 +36,41 @@ using autoware::behavior_path_planner::utils::path_safety_checker::PredictedPath
 static constexpr const char * logger_namespace =
   "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.static_obstacle_"
   "avoidance.utils";
-
+/**
+ * @brief check object offset direction.
+ * @param object data.
+ * @return if the object is on right side of ego path, return true.
+ */
 bool isOnRight(const ObjectData & obj);
 
+/**
+ * @brief calculate shift length from centerline of current lane.
+ * @param object offset direction.
+ * @param distance between object polygon and centerline of current lane. (signed)
+ * @param margin distance between ego and object.
+ * @return necessary shift length. (signed)
+ */
 double calcShiftLength(
   const bool & is_object_on_right, const double & overhang_dist, const double & avoid_margin);
 
 bool isWithinLanes(
   const lanelet::ConstLanelets & lanelets, const std::shared_ptr<const PlannerData> & planner_data);
 
+/**
+ * @brief check if the ego has to shift driving position.
+ * @param if object is on right side of ego path.
+ * @param ego shift length.
+ * @return necessity of shifting.
+ */
 bool isShiftNecessary(const bool & is_object_on_right, const double & shift_length);
 
+/**
+ * @brief check if the ego has to avoid object with the trajectory whose shift direction is same as
+ * object offset.
+ * @param object offset direction.
+ * @param ego shift length.
+ * @return if the direction of shift and object offset, return true.
+ */
 bool isSameDirectionShift(const bool & is_object_on_right, const double & shift_length);
 
 size_t findPathIndexFromArclength(
@@ -62,9 +86,23 @@ std::vector<UUID> calcParentIds(const AvoidLineArray & lines1, const AvoidLine &
 
 double lerpShiftLengthOnArc(double arc, const AvoidLine & al);
 
+/**
+ * @brief calculate distance between ego and object. object length along with the path is calculated
+ * as well.
+ * @param current path.
+ * @param ego position.
+ * @param object data.
+ */
 void fillLongitudinalAndLengthByClosestEnvelopeFootprint(
   const PathWithLaneId & path, const Point & ego_pos, ObjectData & obj);
 
+/**
+ * @brief calculate overhang distance for all of the envelope polygon outer points.
+ * @param object data.
+ * @param current path.
+ * @return first: overhang distance, second: outer point. this vector is sorted by overhang
+ * distance.
+ */
 std::vector<std::pair<double, Point>> calcEnvelopeOverhangDistance(
   const ObjectData & object_data, const PathWithLaneId & path);
 
@@ -76,12 +114,33 @@ void setStartData(
   AvoidLine & al, const double start_shift_length, const geometry_msgs::msg::Pose & start,
   const size_t start_idx, const double start_dist);
 
+/**
+ * @brief create envelope polygon which is parallel to current path.
+ * @param object polygon.
+ * @param closest point pose of the current path.
+ * @param buffer.
+ * @return envelope polygon.
+ */
 Polygon2d createEnvelopePolygon(
   const Polygon2d & object_polygon, const Pose & closest_pose, const double envelope_buffer);
 
+/**
+ * @brief create envelope polygon which is parallel to current path.
+ * @param object data.
+ * @param closest point pose of the current path.
+ * @param buffer.
+ * @return envelope polygon.
+ */
 Polygon2d createEnvelopePolygon(
   const ObjectData & object_data, const Pose & closest_pose, const double envelope_buffer);
 
+/**
+ * @brief create data structs which are used in clipping drivable area process.
+ * @param objects.
+ * @param avoidance module parameters.
+ * @param ego vehicle width.
+ * @return struct which includes expanded polygon.
+ */
 std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
   const ObjectDataArray & objects, const std::shared_ptr<AvoidanceParameters> & parameters,
   const double vehicle_width);
@@ -98,18 +157,45 @@ lanelet::ConstLanelets getExtendLanes(
   const lanelet::ConstLanelets & lanelets, const Pose & ego_pose,
   const std::shared_ptr<const PlannerData> & planner_data);
 
+/**
+ * @brief insert target stop/decel point.
+ * @param ego current position.
+ * @param distance between ego and stop/decel position.
+ * @param target velocity.
+ * @param target path.
+ * @param insert point.
+ */
 void insertDecelPoint(
   const Point & p_src, const double offset, const double velocity, PathWithLaneId & path,
   std::optional<Pose> & p_out);
 
+/**
+ * @brief update envelope polygon based on object position reliability.
+ * @param current detected object.
+ * @param previous stopped objects.
+ * @param base pose to create envelope polygon.
+ * @param threshold parameters.
+ */
 void fillObjectEnvelopePolygon(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const Pose & closest_pose,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+/**
+ * @brief fill stopping duration.
+ * @param current detected object.
+ * @param previous stopped objects.
+ * @param threshold parameters.
+ */
 void fillObjectMovingTime(
   ObjectData & object_data, ObjectDataArray & stopped_objects,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+/**
+ * @brief check whether ego has to avoid the objects.
+ * @param current detected object.
+ * @param previous stopped objects.
+ * @param threshold parameters.
+ */
 void fillAvoidanceNecessity(
   ObjectData & object_data, const ObjectDataArray & registered_objects, const double vehicle_width,
   const std::shared_ptr<AvoidanceParameters> & parameters);
@@ -120,6 +206,13 @@ void fillObjectStoppableJudge(
 
 void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data);
 
+/**
+ * @brief compensate lost objects until a certain time elapses.
+ * @param previous stopped object.
+ * @param avoidance planning data.
+ * @param current time.
+ * @param avoidance parameters which includes duration of compensation.
+ */
 void compensateLostTargetObjects(
   ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
   const std::shared_ptr<const PlannerData> & planner_data,
@@ -175,6 +268,11 @@ double calcDistanceToAvoidStartLine(
   const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters);
 
+/**
+ * @brief calculate error eclipse radius based on object pose covariance.
+ * @param pose with covariance.
+ * @return error eclipse long radius.
+ */
 double calcErrorEclipseLongRadius(const PoseWithCovariance & pose);
 
 }  // namespace autoware::behavior_path_planner::utils::static_obstacle_avoidance

diff --git a/...behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/...behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/package.xml
@@ -30,6 +30,7 @@
   <depend>autoware_route_handler</depend>
   <depend>autoware_rtc_interface</depend>
   <depend>autoware_signal_processing</depend>
+  <depend>autoware_test_utils</depend>
   <depend>autoware_universe_utils</depend>
   <depend>autoware_vehicle_info_utils</depend>
   <depend>geometry_msgs</depend>

diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -162,6 +162,12 @@ void pushUniqueVector(T & base_vector, const T & additional_vector)
 
 namespace filtering_utils
 {
+/**
+ * @brief check whether the object is avoidance target object type.
+ * @param object data.
+ * @param parameters.
+ * @return if the object is avoidance target object type, return true.
+ */
 bool isAvoidanceTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
@@ -174,6 +180,12 @@ bool isAvoidanceTargetObjectType(
   return parameters->object_parameters.at(object_type).is_avoidance_target;
 }
 
+/**
+ * @brief check whether the object is safety check target object type.
+ * @param object data.
+ * @param parameters.
+ * @return if the object is safety check target object type, return true.
+ */
 bool isSafetyCheckTargetObjectType(
   const PredictedObject & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
@@ -186,12 +198,23 @@ bool isSafetyCheckTargetObjectType(
   return parameters->object_parameters.at(object_type).is_safety_check_target;
 }
 
+/**
+ * @brief check whether the object type is ObjectClassification::UNKNOWN.
+ * @param object data.
+ * @return if the object label whose probability is the highest among the candidate is UNKNOWN,
+ * return true.
+ */
 bool isUnknownTypeObject(const ObjectData & object)
 {
   const auto object_type = utils::getHighestProbLabel(object.object.classification);
   return object_type == ObjectClassification::UNKNOWN;
 }
 
+/**
+ * @brief classify object by whether it's vehicle or not.
+ * @param object data.
+ * @return if the object type is vehicle, return true.
+ */
 bool isVehicleTypeObject(const ObjectData & object)
 {
   const auto object_type = utils::getHighestProbLabel(object.object.classification);
@@ -207,6 +230,14 @@ bool isVehicleTypeObject(const ObjectData & object)
   return true;
 }
 
+/**
+ * @brief check whether the object is moving or not.
+ * @param object data.
+ * @param parameters.
+ * @return if the object keeps moving more than threshold time duration, return true. if the object
+ * hasn't been moving for more than threshold time, this function return false even if the object
+ * speed is NOT zero.
+ */
 bool isMovingObject(
   const ObjectData & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
@@ -291,6 +322,12 @@ bool isWithinFreespace(
     lanelet::utils::to2D(polygons.front().basicPolygon()));
 }
 
+/**
+ * @brief check whether the object is on ego driving lane.
+ * @param object data.
+ * @param route handler.
+ * @return if the object is on ego lane, return true.
+ */
 bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
   if (boost::geometry::within(
@@ -370,6 +407,14 @@ bool isMergingToEgoLane(const ObjectData & object)
   return true;
 }
 
+/**
+ * @brief check whether the object is parking on road shoulder.
+ * @param object polygon.
+ * @param avoidance module data.
+ * @param route handler.
+ * @param parameters.
+ * @return if the object is close to road shoulder of the lane, return true.
+ */
 bool isParkedVehicle(
   ObjectData & object, const AvoidancePlanningData & data,
   const std::shared_ptr<RouteHandler> & route_handler,
@@ -749,6 +794,18 @@ bool isObviousAvoidanceTarget(
   return false;
 }
 
+/**
+ * @brief this function includes some conditions which apply to both vehicle and non-vehicle object.
+ * @param object data.
+ * @param current reference path.
+ * @param object detection range.
+ * @param distance between object and goal point.
+ * @param ego position.
+ * @param if the goal point can be moved by external module when there is obstacle around the goal,
+ * this flag will be true.
+ * @param parameters.
+ * @return if the object is potentially target object, return true.
+ */
 bool isSatisfiedWithCommonCondition(
   ObjectData & object, const PathWithLaneId & path, const double forward_detection_range,
   const double to_goal_distance, const Point & ego_pos, const bool is_allowed_goal_modification,
@@ -860,6 +917,13 @@ bool isSatisfiedWithNonVehicleCondition(
   return true;
 }
 
+/**
+ * @brief estimate object's behavior based on its relative yaw angle to lane.
+ * @param object data.
+ * @param parameters.
+ * @return return DEVIATING, MERGING and NONE. NONE means the object has no intent to leave or merge
+ * the ego lane.
+ */
 ObjectData::Behavior getObjectBehavior(
   const ObjectData & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
@@ -927,6 +991,13 @@ bool isSatisfiedWithVehicleCondition(
   return false;
 }
 
+/**
+ * @brief check the ego has to avoid the object for lateral margin.
+ * @param object data.
+ * @param parameters.
+ * @return if the ego doesn't have to shift driving position to avoid object, return false. if the
+ * shift length is less than threshold, this fuction returns false.
+ */
 bool isNoNeedAvoidanceBehavior(
   ObjectData & object, const std::shared_ptr<AvoidanceParameters> & parameters)
 {
@@ -949,6 +1020,13 @@ bool isNoNeedAvoidanceBehavior(
   return false;
 }
 
+/**
+ * @brief get avoidance lateral margin based on road width.
+ * @param object data.
+ * @param planner data, which includes ego vehicle footprint info.
+ * @param parameters.
+ * @return if this function finds there is no enough space to avoid, return nullopt.
+ */
 std::optional<double> getAvoidMargin(
   const ObjectData & object, const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
@@ -982,6 +1060,13 @@ std::optional<double> getAvoidMargin(
   return std::min(soft_lateral_distance_limit, max_avoid_margin);
 }
 
+/**
+ * @brief get avoidance lateral margin based on road width.
+ * @param object data.
+ * @param avoidance module data, which includes current reference path.
+ * @param planner data, which includes ego vehicle footprint info.
+ * @return if this function finds there is no enough space to avoid, return nullopt.
+ */
 double getRoadShoulderDistance(
   ObjectData & object, const AvoidancePlanningData & data,
   const std::shared_ptr<const PlannerData> & planner_data)