feat(obstacle_cruise_planner)!: add ego pose consideration (#5036)

planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml

@@ -99,6 +99,13 @@
         successive_num_to_entry_slow_down_condition: 5
         successive_num_to_exit_slow_down_condition: 5
 
+      # consider the current ego pose (it is not the nearest pose on the reference trajectory)
+      # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence"
+      # The both errors decrease with constant rates against the time.
+      consider_current_pose:
+        enable_to_consider_current_pose: false
+        time_to_convergence: 1.5 #[s]
+
     cruise:
       pid_based_planner:
         use_velocity_limit_based_planner: true

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -48,6 +48,10 @@
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
   // main functions
+  std::vector<Polygon2d> createOneStepPolygons(
+    const std::vector<TrajectoryPoint> & traj_points,
+    const vehicle_info_util::VehicleInfo & vehicle_info,
+    const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const;
   std::vector<Obstacle> convertToObstacles(const std::vector<TrajectoryPoint> & traj_points) const;
   std::tuple<std::vector<StopObstacle>, std::vector<CruiseObstacle>, std::vector<SlowDownObstacle>>
   determineEgoBehaviorAgainstObstacles(
@@ -189,6 +193,9 @@
     double lat_hysteresis_margin_for_slow_down;
     int successive_num_to_entry_slow_down_condition;
     int successive_num_to_exit_slow_down_condition;
+    // consideration for the current ego pose
+    bool enable_to_consider_current_pose{false};
+    double time_to_convergence{1.5};
   };
   BehaviorDeterminationParam behavior_determination_param_;
 
@@ -235,7 +242,7 @@
     }
     void reset(const std::string & uuid) { counter_.emplace(uuid, 0); }
 
     // NOTE: positive is for meeting entrying condition, and negative is for exiting.
     std::unordered_map<std::string, int> counter_;
     std::vector<std::string> current_uuids_;
   };

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp

@@ -33,6 +33,11 @@ namespace bg = boost::geometry;
 using tier4_autoware_utils::Point2d;
 using tier4_autoware_utils::Polygon2d;
 
+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);
+
 std::optional<geometry_msgs::msg::Point> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const Obstacle & obstacle, const bool is_driving_forward);
@@ -45,9 +50,6 @@ std::vector<PointWithStamp> getCollisionPoints(
   const double max_lat_dist = std::numeric_limits<double>::max(),
   const double max_prediction_time_for_collision_check = std::numeric_limits<double>::max());
 
-std::vector<Polygon2d> createOneStepPolygons(
-  const std::vector<TrajectoryPoint> & traj_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin = 0.0);
 }  // namespace polygon_utils
 
 #endif  // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_

planning/obstacle_cruise_planner/src/node.cpp

@@ -272,6 +272,10 @@
     "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition");
   successive_num_to_exit_slow_down_condition = node.declare_parameter<int>(
     "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition");
+  enable_to_consider_current_pose = node.declare_parameter<bool>(
+    "behavior_determination.consider_current_pose.enable_to_consider_current_pose");
+  time_to_convergence = node.declare_parameter<double>(
+    "behavior_determination.consider_current_pose.time_to_convergence");
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -330,6 +334,12 @@
   tier4_autoware_utils::updateParam<int>(
     parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition",
     successive_num_to_exit_slow_down_condition);
+  tier4_autoware_utils::updateParam<bool>(
+    parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose",
+    enable_to_consider_current_pose);
+  tier4_autoware_utils::updateParam<double>(
+    parameters, "behavior_determination.consider_current_pose.time_to_convergence",
+    time_to_convergence);
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -510,6 +520,57 @@
     get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time);
 }
 
+std::vector<Polygon2d> ObstacleCruisePlannerNode::createOneStepPolygons(
+  const std::vector<TrajectoryPoint> & traj_points,
+  const vehicle_info_util::VehicleInfo & vehicle_info,
+  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;
+
+  std::vector<Polygon2d> polygons;
+  const double current_ego_lat_error =
+    motion_utils::calcLateralOffset(traj_points, current_ego_pose.position);
+  const double current_ego_yaw_error =
+    motion_utils::calcYawDeviation(traj_points, current_ego_pose);
+  double time_elapsed = 0.0;
+
+  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);
+  }
+
+  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;
+      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 / 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;
+  }
+  return polygons;
+}
+
 std::vector<Obstacle> ObstacleCruisePlannerNode::convertToObstacles(
   const std::vector<TrajectoryPoint> & traj_points) const
 {
@@ -629,7 +690,7 @@
   // calculated decimated trajectory points and trajectory polygon
   const auto decimated_traj_points = decimateTrajectoryPoints(traj_points);
   const auto decimated_traj_polys =
-    polygon_utils::createOneStepPolygons(decimated_traj_points, vehicle_info_);
+    createOneStepPolygons(decimated_traj_points, vehicle_info_, ego_odom_ptr_->pose.pose);
   debug_data_ptr_->detection_polygons = decimated_traj_polys;
 
   // determine ego's behavior from stop, cruise and slow down
@@ -959,8 +1020,8 @@
   }
 
   // calculate collision points with trajectory with lateral stop margin
-  const auto traj_polys_with_lat_margin =
-    polygon_utils::createOneStepPolygons(traj_points, vehicle_info_, p.max_lat_margin_for_stop);
+  const auto traj_polys_with_lat_margin = createOneStepPolygons(
+    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose, p.max_lat_margin_for_stop);
   const auto collision_point = polygon_utils::getCollisionPoint(
     traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_);
   return collision_point;
@@ -999,7 +1060,7 @@
       }
       return true;
     }
     // check if entrying slow down
     if (is_lat_dist_low) {
       const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid);
       if (p.successive_num_to_entry_slow_down_condition <= count) {
@@ -1020,9 +1081,9 @@
   const auto obstacle_poly = obstacle.toPolygon();
 
   // calculate collision points with trajectory with lateral stop margin
-  // NOTE: For additional margin, hysteresis is not devided by two.
-  const auto traj_polys_with_lat_margin = polygon_utils::createOneStepPolygons(
-    traj_points, vehicle_info_,
+  // NOTE: For additional margin, hysteresis is not divided by two.
+  const auto traj_polys_with_lat_margin = createOneStepPolygons(
+    traj_points, vehicle_info_, ego_odom_ptr_->pose.pose,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down);
 
   std::vector<Polygon2d> front_collision_polygons;

planning/obstacle_cruise_planner/src/polygon_utils.cpp

@@ -31,36 +31,6 @@ geometry_msgs::msg::Point calcOffsetPosition(
   return tier4_autoware_utils::calcOffsetPose(pose, offset_x, offset_y, 0.0).position;
 }
 
-Polygon2d createOneStepPolygon(
-  const geometry_msgs::msg::Pose & base_step_pose, const geometry_msgs::msg::Pose & next_step_pose,
-  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;
-
-  // base step
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, base_to_front, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(base_step_pose, -base_to_rear, width));
-
-  // next step
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, base_to_front, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, -width));
-  appendPointToPolygon(polygon, calcOffsetPosition(next_step_pose, -base_to_rear, width));
-
-  bg::correct(polygon);
-
-  Polygon2d hull_polygon;
-  bg::convex_hull(polygon, hull_polygon);
-
-  return hull_polygon;
-}
-
 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)
@@ -132,6 +102,38 @@ 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<geometry_msgs::msg::Point> getCollisionPoint(
   const std::vector<TrajectoryPoint> & traj_points, const std::vector<Polygon2d> & traj_polygons,
   const Obstacle & obstacle, const bool is_driving_forward)
@@ -184,25 +186,4 @@ std::vector<PointWithStamp> getCollisionPoints(
   return collision_points;
 }
 
-std::vector<Polygon2d> createOneStepPolygons(
-  const std::vector<TrajectoryPoint> & traj_points,
-  const vehicle_info_util::VehicleInfo & vehicle_info, const double lat_margin)
-{
-  std::vector<Polygon2d> polygons;
-
-  for (size_t i = 0; i < traj_points.size(); ++i) {
-    const auto polygon = [&]() {
-      if (i == 0) {
-        return createOneStepPolygon(
-          traj_points.at(i).pose, traj_points.at(i).pose, vehicle_info, lat_margin);
-      }
-      return createOneStepPolygon(
-        traj_points.at(i - 1).pose, traj_points.at(i).pose, vehicle_info, lat_margin);
-    }();
-
-    polygons.push_back(polygon);
-  }
-  return polygons;
-}
-
 }  // namespace polygon_utils