fix(static_obstacle_avoidance): fix issues in target object filtering…

… logic (#7830) * fix(static_obstacle_avoidance): check if object is inside/outside by its position point instead of its polygon Signed-off-by: satoshi-ota <[email protected]> * refactor(static_obstacle_avoidance): add getter functions Signed-off-by: satoshi-ota <[email protected]> * fix(static_obstacle_avoidance): check next lane without route if the current lane is not preferred Signed-off-by: satoshi-ota <[email protected]> * fix(static_obstacle_avoidance): fix parked vehicle check Signed-off-by: satoshi-ota <[email protected]> --------- Signed-off-by: satoshi-ota <[email protected]>
autowarefoundation · Jul 4, 2024 · 99c7c57 · 99c7c57
1 parent 5df28cc
commit 99c7c57
Show file tree

Hide file tree

Showing 4 changed files with 59 additions and 55 deletions.
diff --git a/...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/...e_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
@@ -352,6 +352,10 @@ struct ObjectData  // avoidance target
   {
   }
 
+  Pose getPose() const { return object.kinematics.initial_pose_with_covariance.pose; }
+
+  Point getPosition() const { return object.kinematics.initial_pose_with_covariance.pose.position; }
+
   PredictedObject object;
 
   // object behavior.

diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
@@ -64,7 +64,7 @@ MarkerArray createObjectsCubeMarkerArray(
     }
 
     marker.id = uuidToInt32(object.object.object_id);
-    marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+    marker.pose = object.getPose();
     msg.markers.push_back(marker);
   }
 
@@ -80,10 +80,8 @@ MarkerArray createObjectPolygonMarkerArray(const ObjectDataArray & objects, std:
       "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::LINE_STRIP,
       createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
 
-    const auto pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-
     for (const auto & p : object.envelope_poly.outer()) {
-      marker.points.push_back(createPoint(p.x(), p.y(), pos.z));
+      marker.points.push_back(createPoint(p.x(), p.y(), object.getPosition().z));
     }
 
     marker.points.push_back(marker.points.front());
@@ -142,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(
   for (const auto & object : objects) {
     if (verbose) {
       marker.id = uuidToInt32(object.object.object_id);
-      marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+      marker.pose = object.getPose();
       std::ostringstream string_stream;
       string_stream << std::fixed << std::setprecision(2) << std::boolalpha;
       string_stream << "ratio:" << object.shiftable_ratio << " [-]\n"
@@ -162,7 +160,7 @@ MarkerArray createObjectInfoMarkerArray(
 
     {
       marker.id = uuidToInt32(object.object.object_id);
-      marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
+      marker.pose = object.getPose();
       marker.pose.position.z += 2.0;
       std::ostringstream string_stream;
       string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : "");

diff --git a/...nner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/...nner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -165,9 +165,9 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     }
 
     // the avoidance path is already approved
-    const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
-    const auto is_approved = (helper_->getShift(object_pos) > 0.0 && is_object_on_right) ||
-                             (helper_->getShift(object_pos) < 0.0 && !is_object_on_right);
+    const auto is_approved =
+      (helper_->getShift(object.getPosition()) > 0.0 && is_object_on_right) ||
+      (helper_->getShift(object.getPosition()) < 0.0 && !is_object_on_right);
     if (is_approved) {
       return std::make_pair(desire_shift_length, avoidance_distance);
     }
@@ -363,9 +363,8 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       if (is_return_shift_to_goal) {
         return true;
       }
-      const auto & object_pos = o.object.kinematics.initial_pose_with_covariance.pose.position;
       const bool has_object_near_goal =
-        autoware::universe_utils::calcDistance2d(goal_pose.position, object_pos) <
+        autoware::universe_utils::calcDistance2d(goal_pose.position, o.getPosition()) <
         parameters_->object_check_goal_distance;
       return has_object_near_goal;
     }();
@@ -1027,8 +1026,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
     const auto has_object_near_goal =
       std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
         return autoware::universe_utils::calcDistance2d(
-                 data_->route_handler->getGoalPose().position,
-                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+                 data_->route_handler->getGoalPose().position, o.getPosition()) <
                parameters_->object_check_goal_distance;
       });
     if (has_object_near_goal) {
@@ -1097,9 +1095,7 @@ AvoidLineArray ShiftLineGenerator::addReturnShiftLine(
   if (utils::isAllowedGoalModification(data_->route_handler)) {
     const bool has_last_shift_near_goal =
       std::any_of(data.target_objects.begin(), data.target_objects.end(), [&](const auto & o) {
-        return autoware::universe_utils::calcDistance2d(
-                 last_sl.end.position,
-                 o.object.kinematics.initial_pose_with_covariance.pose.position) <
+        return autoware::universe_utils::calcDistance2d(last_sl.end.position, o.getPosition()) <
                parameters_->object_check_goal_distance;
       });
     if (has_last_shift_near_goal) {

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
@@ -293,8 +293,7 @@ bool isWithinCrosswalk(
 {
   using Point = boost::geometry::model::d2::point_xy<double>;
 
-  const auto & p = object.object.kinematics.initial_pose_with_covariance.pose.position;
-  const Point p_object{p.x, p.y};
+  const Point p_object{object.getPosition().x, object.getPosition().y};
 
   // get conflicting crosswalk crosswalk
   constexpr int PEDESTRIAN_GRAPH_ID = 1;
@@ -335,14 +334,16 @@ bool isWithinIntersection(
     route_handler->getLaneletMapPtr()->polygonLayer.get(std::atoi(area_id.c_str()));
 
   return boost::geometry::within(
-    object_polygon, utils::toPolygon2d(lanelet::utils::to2D(polygon.basicPolygon())));
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+      .basicPoint(),
+    lanelet::utils::to2D(polygon.basicPolygon()));
 }
 
 bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
-  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
   if (boost::geometry::within(
-        lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+        lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+          .basicPoint(),
         object.overhang_lanelet.polygon2d().basicPolygon())) {
     return true;
   }
@@ -351,7 +352,8 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler>
   lanelet::ConstLanelets prev_lanelet;
   if (route_handler->getPreviousLaneletsWithinRoute(object.overhang_lanelet, &prev_lanelet)) {
     if (boost::geometry::within(
-          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+            .basicPoint(),
           prev_lanelet.front().polygon2d().basicPolygon())) {
       return true;
     }
@@ -361,31 +363,39 @@ bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler>
   lanelet::ConstLanelet next_lanelet;
   if (route_handler->getNextLaneletWithinRoute(object.overhang_lanelet, &next_lanelet)) {
     if (boost::geometry::within(
-          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object_pos)).basicPoint(),
+          lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+            .basicPoint(),
           next_lanelet.polygon2d().basicPolygon())) {
       return true;
     }
+  } else {
+    for (const auto & lane : route_handler->getNextLanelets(object.overhang_lanelet)) {
+      if (boost::geometry::within(
+            lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+              .basicPoint(),
+            lane.polygon2d().basicPolygon())) {
+        return true;
+      }
+    }
   }
 
   return false;
 }
 
 bool isParallelToEgoLane(const ObjectData & object, const double threshold)
 {
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto closest_pose =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
-  const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object_pose));
+    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
+  const auto yaw_deviation = std::abs(calcYawDeviation(closest_pose, object.getPose()));
 
   return yaw_deviation < threshold || yaw_deviation > M_PI - threshold;
 }
 
 bool isMergingToEgoLane(const ObjectData & object)
 {
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto closest_pose =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
-  const auto yaw_deviation = calcYawDeviation(closest_pose, object_pose);
+    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
+  const auto yaw_deviation = calcYawDeviation(closest_pose, object.getPose());
 
   if (isOnRight(object)) {
     if (yaw_deviation < 0.0 && -1.0 * M_PI_2 < yaw_deviation) {
@@ -422,9 +432,8 @@ bool isParkedVehicle(
     return false;
   }
 
-  const auto & object_pos = object.object.kinematics.initial_pose_with_covariance.pose.position;
   const auto centerline_pos =
-    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pos).position;
+    lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition()).position;
 
   bool is_left_side_parked_vehicle = false;
   if (!isOnRight(object)) {
@@ -442,7 +451,8 @@ bool isParkedVehicle(
         return same_direction_lane;
       }
 
-      return static_cast<lanelet::ConstLanelet>(opposite_lanes.front().invert());
+      return static_cast<lanelet::ConstLanelet>(
+        route_handler->getMostRightLanelet(opposite_lanes.front()).invert());
     }();
 
     const auto center_to_left_boundary = distance2d(
@@ -457,7 +467,7 @@ bool isParkedVehicle(
     if (sub_type == "road_shoulder") {
       // assuming it's parked vehicle if its CoG is within road shoulder lanelet.
       if (boost::geometry::within(
-            to2D(toLaneletPoint(object_pos)).basicPoint(),
+            to2D(toLaneletPoint(object.getPosition())).basicPoint(),
             most_left_lanelet.polygon2d().basicPolygon())) {
         return true;
       }
@@ -468,7 +478,7 @@ bool isParkedVehicle(
 
     const auto arc_coordinates = toArcCoordinates(
       to2D(object.overhang_lanelet.centerline().basicLineString()),
-      to2D(toLaneletPoint(object_pos)).basicPoint());
+      to2D(toLaneletPoint(object.getPosition())).basicPoint());
     object.shiftable_ratio = arc_coordinates.distance / object_shiftable_distance;
 
     is_left_side_parked_vehicle = object.shiftable_ratio > parameters->object_check_shiftable_ratio;
@@ -490,7 +500,8 @@ bool isParkedVehicle(
         return same_direction_lane;
       }
 
-      return static_cast<lanelet::ConstLanelet>(opposite_lanes.front().invert());
+      return static_cast<lanelet::ConstLanelet>(
+        route_handler->getMostLeftLanelet(opposite_lanes.front()).invert());
     }();
 
     const auto center_to_right_boundary = distance2d(
@@ -505,7 +516,7 @@ bool isParkedVehicle(
     if (sub_type == "road_shoulder") {
       // assuming it's parked vehicle if its CoG is within road shoulder lanelet.
       if (boost::geometry::within(
-            to2D(toLaneletPoint(object_pos)).basicPoint(),
+            to2D(toLaneletPoint(object.getPosition())).basicPoint(),
             most_right_lanelet.polygon2d().basicPolygon())) {
         return true;
       }
@@ -516,7 +527,7 @@ bool isParkedVehicle(
 
     const auto arc_coordinates = toArcCoordinates(
       to2D(object.overhang_lanelet.centerline().basicLineString()),
-      to2D(toLaneletPoint(object_pos)).basicPoint());
+      to2D(toLaneletPoint(object.getPosition())).basicPoint());
     object.shiftable_ratio = -1.0 * arc_coordinates.distance / object_shiftable_distance;
 
     is_right_side_parked_vehicle =
@@ -527,9 +538,8 @@ bool isParkedVehicle(
     return false;
   }
 
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   object.to_centerline =
-    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
+    lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance;
   if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
     return false;
   }
@@ -544,13 +554,13 @@ bool isCloseToStopFactor(
 {
   const auto & rh = planner_data->route_handler;
   const auto & ego_pose = planner_data->self_odometry->pose.pose;
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
 
   // force avoidance for stopped vehicle
   bool is_close_to_stop_factor = false;
 
   // check traffic light
-  const auto to_traffic_light = getDistanceToNextTrafficLight(object_pose, data.extend_lanelets);
+  const auto to_traffic_light =
+    getDistanceToNextTrafficLight(object.getPose(), data.extend_lanelets);
   {
     is_close_to_stop_factor =
       to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance;
@@ -792,9 +802,8 @@ bool isSatisfiedWithNonVehicleCondition(
   }
 
   // Object is on center line -> ignore.
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   object.to_centerline =
-    lanelet::utils::getArcCoordinates(data.current_lanelets, object_pose).distance;
+    lanelet::utils::getArcCoordinates(data.current_lanelets, object.getPose()).distance;
   if (std::abs(object.to_centerline) < parameters->threshold_distance_object_is_on_center) {
     object.info = ObjectInfo::TOO_NEAR_TO_CENTERLINE;
     return false;
@@ -844,9 +853,8 @@ bool isSatisfiedWithVehicleCondition(
     return false;
   }
 
-  const auto & current_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto is_moving_distance_longer_than_threshold =
-    calcDistance2d(object.init_pose, current_pose) >
+    calcDistance2d(object.init_pose, object.getPose()) >
     parameters->distance_threshold_for_ambiguous_vehicle;
   if (is_moving_distance_longer_than_threshold) {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
@@ -944,9 +952,8 @@ double getRoadShoulderDistance(
   using autoware::universe_utils::Point2d;
   using lanelet::utils::to2D;
 
-  const auto & object_pose = object.object.kinematics.initial_pose_with_covariance.pose;
   const auto object_closest_index =
-    autoware::motion_utils::findNearestIndex(data.reference_path.points, object_pose.position);
+    autoware::motion_utils::findNearestIndex(data.reference_path.points, object.getPosition());
   const auto object_closest_pose = data.reference_path.points.at(object_closest_index).point.pose;
 
   const auto rh = planner_data->route_handler;
@@ -957,7 +964,7 @@ double getRoadShoulderDistance(
   std::vector<std::tuple<double, Point, Point>> intersects;
   for (const auto & p1 : object.overhang_points) {
     const auto centerline_pose =
-      lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object_pose.position);
+      lanelet::utils::getClosestCenterPose(object.overhang_lanelet, object.getPosition());
     const auto p_tmp =
       geometry_msgs::build<Pose>().position(p1.second).orientation(centerline_pose.orientation);
 
@@ -1326,8 +1333,7 @@ std::vector<DrivableAreaInfo::Obstacle> generateObstaclePolygonsForDrivableArea(
       object.avoid_margin.value() - object_parameter.envelope_buffer_margin - vehicle_width / 2.0;
     const auto obj_poly =
       autoware::universe_utils::expandPolygon(object.envelope_poly, diff_poly_buffer);
-    obstacles_for_drivable_area.push_back(
-      {object.object.kinematics.initial_pose_with_covariance.pose, obj_poly, !isOnRight(object)});
+    obstacles_for_drivable_area.push_back({object.getPose(), obj_poly, !isOnRight(object)});
   }
   return obstacles_for_drivable_area;
 }
@@ -1495,7 +1501,7 @@ void fillObjectMovingTime(
     object_data.last_stop = now;
     object_data.move_time = 0.0;
     if (is_new_object) {
-      object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
+      object_data.init_pose = object_data.getPose();
       object_data.stop_time = 0.0;
       object_data.last_move = now;
       stopped_objects.push_back(object_data);
@@ -1510,7 +1516,7 @@ void fillObjectMovingTime(
   }
 
   if (is_new_object) {
-    object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
+    object_data.init_pose = object_data.getPose();
     object_data.move_time = std::numeric_limits<double>::infinity();
     object_data.stop_time = 0.0;
     object_data.last_move = now;
@@ -1520,7 +1526,7 @@ void fillObjectMovingTime(
   object_data.last_stop = same_id_obj->last_stop;
   object_data.move_time = (now - same_id_obj->last_stop).seconds();
   object_data.stop_time = 0.0;
-  object_data.init_pose = object_data.object.kinematics.initial_pose_with_covariance.pose;
+  object_data.init_pose = object_data.getPose();
 
   if (object_data.move_time > object_parameter.moving_time_threshold) {
     stopped_objects.erase(same_id_obj);
@@ -1617,9 +1623,9 @@ void updateRegisteredObject(
     }
 
     constexpr auto POS_THR = 1.5;
-    const auto r_pos = registered_object.object.kinematics.initial_pose_with_covariance.pose;
+    const auto r_pos = registered_object.getPose();
     const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) {
-      return calcDistance2d(r_pos, o.object.kinematics.initial_pose_with_covariance.pose) < POS_THR;
+      return calcDistance2d(r_pos, o.getPose()) < POS_THR;
     });
 
     // same id object is not detected, but object is found around registered. update registered.