feat(obstacle_cruise): ignore right beside objects (autowarefoundation#6754) (#1267)

* ignore near beside objects
* change a magic num

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -87,7 +87,7 @@
         obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop
 
       stop:
-        max_lat_margin: 0.2 # lateral margin between obstacle and trajectory band with ego's width
+        max_lat_margin: 0.3 # lateral margin between obstacle and trajectory band with ego's width
         crossing_obstacle:
           collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s]
 

planning/obstacle_cruise_planner/src/node.cpp

@@ -556,9 +556,10 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
   const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const
 {
   const auto & p = behavior_determination_param_;
-  const bool is_enable_current_pose_consideration = p.enable_to_consider_current_pose;
-  const double step_length = p.decimate_trajectory_step_length;
-  const double time_to_convergence = p.time_to_convergence;
+
+  const double front_length = vehicle_info.max_longitudinal_offset_m;
+  const double rear_length = vehicle_info.rear_overhang_m;
+  const double vehicle_width = vehicle_info.vehicle_width_m;
 
   const size_t nearest_idx =
     motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position);
@@ -567,41 +568,68 @@ std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
     tier4_autoware_utils::inverseTransformPose(current_ego_pose, nearest_pose);
   const double current_ego_lat_error = current_ego_pose_error.position.y;
   const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation);
-  double time_elapsed = 0.0;
-
-  std::vector<Polygon2d> polygons;
-  std::vector<geometry_msgs::msg::Pose> last_poses = {traj_points.at(0).pose};
-  if (is_enable_current_pose_consideration) {
-    last_poses.push_back(current_ego_pose);
-  }
+  double time_elapsed{0.0};
 
+  std::vector<Polygon2d> output_polygons;
+  Polygon2d tmp_polys{};
   for (size_t i = 0; i < traj_points.size(); ++i) {
     std::vector<geometry_msgs::msg::Pose> current_poses = {traj_points.at(i).pose};
 
     // estimate the future ego pose with assuming that the pose error against the reference path
     // will decrease to zero by the time_to_convergence
-    if (is_enable_current_pose_consideration && time_elapsed < time_to_convergence) {
-      const double rem_ratio = (time_to_convergence - time_elapsed) / time_to_convergence;
+    if (p.enable_to_consider_current_pose && time_elapsed < p.time_to_convergence) {
+      const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence;
       geometry_msgs::msg::Pose indexed_pose_err;
       indexed_pose_err.set__orientation(
         tier4_autoware_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio));
       indexed_pose_err.set__position(
         tier4_autoware_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0));
-
       current_poses.push_back(
         tier4_autoware_utils::transformPose(indexed_pose_err, traj_points.at(i).pose));
-
       if (traj_points.at(i).longitudinal_velocity_mps != 0.0) {
-        time_elapsed += step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps);
+        time_elapsed +=
+          p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps);
       } else {
         time_elapsed = std::numeric_limits<double>::max();
       }
     }
-    polygons.push_back(
-      polygon_utils::createOneStepPolygon(last_poses, current_poses, vehicle_info, lat_margin));
-    last_poses = current_poses;
+
+    Polygon2d idx_poly{};
+    for (const auto & pose : current_poses) {
+      if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) {
+        boost::geometry::append(
+          idx_poly,
+          tier4_autoware_utils::toFootprint(pose, front_length, rear_length, vehicle_width)
+            .outer());
+        boost::geometry::append(
+          idx_poly,
+          tier4_autoware_utils::fromMsg(tier4_autoware_utils::calcOffsetPose(
+                                          pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0)
+                                          .position)
+            .to_2d());
+        boost::geometry::append(
+          idx_poly, tier4_autoware_utils::fromMsg(
+                      tier4_autoware_utils::calcOffsetPose(
+                        pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0)
+                        .position)
+                      .to_2d());
+      } else {
+        boost::geometry::append(
+          idx_poly, tier4_autoware_utils::toFootprint(
+                      pose, front_length, rear_length, vehicle_width + lat_margin * 2.0)
+                      .outer());
+      }
+    }
+
+    boost::geometry::append(tmp_polys, idx_poly.outer());
+    Polygon2d hull_polygon;
+    boost::geometry::convex_hull(tmp_polys, hull_polygon);
+    boost::geometry::correct(hull_polygon);
+
+    output_polygons.push_back(hull_polygon);
+    tmp_polys = std::move(idx_poly);
   }
-  return polygons;
+  return output_polygons;
 }
 
 std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -20,18 +20,6 @@
 
 namespace
 {
-void appendPointToPolygon(Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point)
-{
-  Point2d point(geom_point.x, geom_point.y);
-  bg::append(polygon.outer(), point);
-}
-
-geometry_msgs::msg::Point calcOffsetPosition(
-  const geometry_msgs::msg::Pose & pose, const double offset_x, const double offset_y)
-{
-  return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position;
-}
-
 PointWithStamp calcNearestCollisionPoint(
   const size_t first_within_idx, const std::vector<PointWithStamp> & collision_points,
   const std::vector<TrajectoryPoint> & decimated_traj_points, const bool is_driving_forward)
@@ -104,38 +92,6 @@ std::optional<std::pair<size_t, std::vector<PointWithStamp>>> getCollisionIndex(
 
 namespace polygon_utils
 {
-Polygon2d createOneStepPolygon(
-  const std::vector<geometry_msgs::msg::Pose> & last_poses,
-  const std::vector<geometry_msgs::msg::Pose> & current_poses,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
-{
-  Polygon2d polygon;
-
-  const double base_to_front = vehicle_info.max_longitudinal_offset_m;
-  const double width = vehicle_info.vehicle_width_m / 2.0 + lat_margin;
-  const double base_to_rear = vehicle_info.rear_overhang_m;
-
-  for (auto & pose : last_poses) {
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width));
-  }
-  for (auto & pose : current_poses) {
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, base_to_front, -width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, -width));
-    appendPointToPolygon(polygon, calcOffsetPosition(pose, -base_to_rear, width));
-  }
-
-  bg::correct(polygon);
-
-  Polygon2d hull_polygon;
-  bg::convex_hull(polygon, hull_polygon);
-
-  return hull_polygon;
-}
-
 std::optional<std::pair<geometry_msgs::msg::Point, double>> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const Obstacle & obstacle, const bool is_driving_forward,