fix(obstacle_cruise_planner): compensate time delay for predicted object and fix stop condition

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp

@@ -73,8 +73,7 @@ class OptimizationBasedPlanner : public PlannerInterface
   std::vector<double> createTimeVector();
 
   double getClosestStopDistance(
-    const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data,
-    const std::vector<double> & resolutions);
+    const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data);
 
   std::tuple<double, double> calcInitialMotion(
     const double current_vel, const Trajectory & input_traj, const size_t input_closest,
@@ -116,7 +115,7 @@ class OptimizationBasedPlanner : public PlannerInterface
     const TargetObstacle & object, const rclcpp::Time & obj_base_time,
     const PredictedPath & predicted_path);
 
-  boost::optional<PredictedPath> resampledPredictedPath(
+  boost::optional<PredictedPath> resamplePredictedPath(
     const TargetObstacle & object, const rclcpp::Time & obj_base_time,
     const rclcpp::Time & current_time, const std::vector<double> & resolutions,
     const double horizon);

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/resample.hpp

@@ -34,13 +34,6 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
   const autoware_auto_perception_msgs::msg::PredictedPath & input_path,
   const std::vector<rclcpp::Duration> & rel_time_vec);
 
-geometry_msgs::msg::Pose lerpByPose(
-  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t);
-
-boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
-  const autoware_auto_perception_msgs::msg::PredictedPath & path,
-  const rclcpp::Duration & rel_time);
-
 autoware_auto_planning_msgs::msg::Trajectory applyLinearInterpolation(
   const std::vector<double> & base_index,
   const autoware_auto_planning_msgs::msg::Trajectory & base_trajectory,

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp

@@ -78,7 +78,6 @@ class PIDBasedPlanner : public PlannerInterface
     boost::optional<CruiseObstacleInfo> & cruise_obstacle_info);
   double calcDistanceToObstacle(
     const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle);
-  bool isStopRequired(const TargetObstacle & obstacle);
 
   void planCruise(
     const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -167,6 +167,21 @@ class PlannerInterface
     return std::find(types.begin(), types.end(), label) != types.end();
   }
 
+  // Note: If stop planning is not required, cruise planning will be done instead.
+  bool isStopRequired(const TargetObstacle & obstacle)
+  {
+    const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label);
+    const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label);
+
+    if (is_cruise_obstacle) {
+      return std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_;
+    } else if (is_stop_obstacle && !is_cruise_obstacle) {
+      return true;
+    }
+
+    return false;
+  }
+
 protected:
   // Parameters
   bool is_showing_debug_info_{false};

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp

@@ -18,6 +18,7 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include "autoware_auto_perception_msgs/msg/object_classification.hpp"
+#include "autoware_auto_perception_msgs/msg/predicted_object.hpp"
 #include "autoware_auto_perception_msgs/msg/predicted_path.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
 #include "geometry_msgs/msg/pose.hpp"
@@ -26,6 +27,7 @@
 #include <boost/optional.hpp>
 
 #include <string>
+#include <vector>
 
 namespace obstacle_cruise_utils
 {
@@ -41,9 +43,24 @@ boost::optional<geometry_msgs::msg::Pose> calcForwardPose(
   const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx,
   const double target_length);
 
+geometry_msgs::msg::Pose lerpByPose(
+  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t);
+
+boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
+  const autoware_auto_perception_msgs::msg::PredictedPath & path,
+  const rclcpp::Duration & rel_time);
+
 boost::optional<geometry_msgs::msg::Pose> getCurrentObjectPoseFromPredictedPath(
   const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path,
   const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time);
+
+boost::optional<geometry_msgs::msg::Pose> getCurrentObjectPoseFromPredictedPath(
+  const std::vector<autoware_auto_perception_msgs::msg::PredictedPath> & predicted_paths,
+  const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time);
+
+geometry_msgs::msg::Pose getCurrentObjectPoseFromPredictedPath(
+  const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object,
+  const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time);
 }  // namespace obstacle_cruise_utils
 
 #endif  // OBSTACLE_CRUISE_PLANNER__UTILS_HPP_

planning/obstacle_cruise_planner/src/node.cpp

@@ -488,6 +488,7 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
   const PredictedObjects & predicted_objects, const Trajectory & traj,
   const geometry_msgs::msg::Pose & current_pose, const double current_vel, DebugData & debug_data)
 {
+  const auto current_time = now();
   const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp);
 
   const size_t ego_idx = findExtendedNearestIndex(
@@ -518,7 +519,8 @@ std::vector<TargetObstacle> ObstacleCruisePlannerNode::filterObstacles(
       continue;
     }
 
-    const auto & object_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
+    const auto object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath(
+      predicted_object, time_stamp, current_time);
     const auto & object_velocity =
       predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
 

planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp

@@ -212,7 +212,7 @@ Trajectory OptimizationBasedPlanner::generateTrajectory(
   }
 
   // Get Closest Stop Point for static obstacles
-  double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data, time_vec);
+  double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data);
 
   // Compute maximum velocity
   double v_max = 0.0;
@@ -459,8 +459,7 @@ std::vector<double> OptimizationBasedPlanner::createTimeVector()
 }
 
 double OptimizationBasedPlanner::getClosestStopDistance(
-  const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data,
-  const std::vector<double> & resolutions)
+  const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data)
 {
   const auto & current_time = planner_data.current_time;
   double closest_stop_dist = ego_traj_data.s.back();
@@ -475,25 +474,14 @@ double OptimizationBasedPlanner::getClosestStopDistance(
   for (const auto & obj : planner_data.target_obstacles) {
     const auto obj_base_time = obj.time_stamp;
 
-    // Step1. Ignore obstacles that are not vehicles
-    if (
-      obj.classification.label != ObjectClassification::CAR &&
-      obj.classification.label != ObjectClassification::TRUCK &&
-      obj.classification.label != ObjectClassification::BUS &&
-      obj.classification.label != ObjectClassification::MOTORCYCLE) {
-      continue;
-    }
-
-    // Get the predicted path, which has the most high confidence
-    const auto predicted_path =
-      resampledPredictedPath(obj, obj_base_time, current_time, resolutions, max_time_horizon_);
-    if (!predicted_path) {
+    // Ignore obstacles that are not required to stop
+    if (!isStopRequired(obj)) {
       continue;
     }
 
     // Get current pose from object's predicted path
     const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath(
-      *predicted_path, obj_base_time, current_time);
+      obj.predicted_paths, obj_base_time, current_time);
     if (!current_object_pose) {
       continue;
     }
@@ -508,20 +496,17 @@ double OptimizationBasedPlanner::getClosestStopDistance(
 
     // Calculate Safety Distance
     const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin;
-    const double ego_vehicle_offset = vehicle_info_.wheel_base_m + vehicle_info_.front_overhang_m;
+    const auto & ego_vehicle_offset = vehicle_info_.max_longitudinal_offset_m;
     const double object_offset = obj_data.length / 2.0;
     const double safety_distance = ego_vehicle_offset + object_offset + safe_distance_margin;
 
     // If the object is on the current ego trajectory,
     // we assume the object travels along ego trajectory
-    const double obj_vel = std::abs(obj.velocity);
-    if (dist_to_collision_point && obj_vel < obstacle_velocity_threshold_from_cruise_to_stop_) {
-      const double current_stop_point = std::max(*dist_to_collision_point - safety_distance, 0.0);
-      closest_stop_dist = std::min(current_stop_point, closest_stop_dist);
-    }
-
-    // Update Distance to the closest object on the ego trajectory
     if (dist_to_collision_point) {
+      const double stop_dist = std::max(*dist_to_collision_point - safety_distance, 0.0);
+      closest_stop_dist = std::min(stop_dist, closest_stop_dist);
+
+      // Update Distance to the closest object on the ego trajectory
       const double current_obj_distance = std::max(
         *dist_to_collision_point - safety_distance + safe_distance_margin, -safety_distance);
       closest_obj_distance = std::min(closest_obj_distance, current_obj_distance);
@@ -832,9 +817,13 @@ boost::optional<SBoundaries> OptimizationBasedPlanner::getSBoundaries(
   double min_slow_down_point_length = std::numeric_limits<double>::max();
   boost::optional<size_t> min_slow_down_idx = {};
   for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) {
+    const auto & obj = planner_data.target_obstacles.at(o_idx);
     const auto obj_base_time = planner_data.target_obstacles.at(o_idx).time_stamp;
+    // Only see cruise obstacles
+    if (!isCruiseObstacle(obj.classification.label)) {
+      continue;
+    }
 
-    const auto & obj = planner_data.target_obstacles.at(o_idx);
     // Step1 Get S boundary from the obstacle
     const auto obj_s_boundaries =
       getSBoundaries(planner_data.current_time, ego_traj_data, obj, obj_base_time, time_vec);
@@ -852,11 +841,8 @@ boost::optional<SBoundaries> OptimizationBasedPlanner::getSBoundaries(
     // Step3 search nearest obstacle to follow for rviz marker
     const double object_offset = obj.shape.dimensions.x / 2.0;
 
-    const auto predicted_path =
-      resampledPredictedPath(obj, obj_base_time, current_time, time_vec, max_time_horizon_);
-
     const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath(
-      *predicted_path, obj_base_time, current_time);
+      obj.predicted_paths, obj_base_time, current_time);
 
     const double obj_vel = std::abs(obj.velocity);
     const double rss_dist = calcRSSDistance(planner_data.current_vel, obj_vel);
@@ -878,11 +864,8 @@ boost::optional<SBoundaries> OptimizationBasedPlanner::getSBoundaries(
   if (min_slow_down_idx) {
     const auto & obj = planner_data.target_obstacles.at(min_slow_down_idx.get());
 
-    const auto predicted_path =
-      resampledPredictedPath(obj, obj.time_stamp, current_time, time_vec, max_time_horizon_);
-
     const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPath(
-      *predicted_path, obj.time_stamp, current_time);
+      obj.predicted_paths, obj.time_stamp, current_time);
 
     const auto marker_pose = calcForwardPose(
       ego_traj_data.traj, planner_data.current_pose.position, min_slow_down_point_length);
@@ -917,7 +900,7 @@ boost::optional<SBoundaries> OptimizationBasedPlanner::getSBoundaries(
   // Get the predicted path, which has the most high confidence
   const double max_horizon = time_vec.back();
   const auto predicted_path =
-    resampledPredictedPath(object, obj_base_time, current_time, time_vec, max_horizon);
+    resamplePredictedPath(object, obj_base_time, current_time, time_vec, max_horizon);
   if (!predicted_path) {
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
@@ -1116,7 +1099,7 @@ boost::optional<size_t> OptimizationBasedPlanner::getCollisionIdx(
   return boost::none;
 }
 
-boost::optional<PredictedPath> OptimizationBasedPlanner::resampledPredictedPath(
+boost::optional<PredictedPath> OptimizationBasedPlanner::resamplePredictedPath(
   const TargetObstacle & object, const rclcpp::Time & obj_base_time,
   const rclcpp::Time & current_time, const std::vector<double> & resolutions, const double horizon)
 {

planning/obstacle_cruise_planner/src/optimization_based_planner/resample.cpp

@@ -16,37 +16,10 @@
 
 #include "interpolation/linear_interpolation.hpp"
 #include "interpolation/spline_interpolation.hpp"
+#include "obstacle_cruise_planner/utils.hpp"
 
 #include <vector>
 
-namespace
-{
-[[maybe_unused]] rclcpp::Duration safeSubtraction(const rclcpp::Time & t1, const rclcpp::Time & t2)
-{
-  rclcpp::Duration duration = rclcpp::Duration::from_seconds(0.0);
-  try {
-    duration = t1 - t2;
-  } catch (std::runtime_error & err) {
-    if (t1 > t2) {
-      duration = rclcpp::Duration::max() * -1.0;
-    } else {
-      duration = rclcpp::Duration::max();
-    }
-  }
-  return duration;
-}
-
-// tf2::toMsg does not have this type of function
-geometry_msgs::msg::Point toMsg(tf2::Vector3 vec)
-{
-  geometry_msgs::msg::Point point;
-  point.x = vec.x();
-  point.y = vec.y();
-  point.z = vec.z();
-  return point;
-}
-}  // namespace
-
 namespace resampling
 {
 std::vector<rclcpp::Duration> resampledValidRelativeTimeVector(
@@ -86,7 +59,7 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
   resampled_path.time_step = input_path.time_step;
 
   for (const auto & rel_time : rel_time_vec) {
-    const auto opt_pose = lerpByTimeStamp(input_path, rel_time);
+    const auto opt_pose = obstacle_cruise_utils::lerpByTimeStamp(input_path, rel_time);
     if (!opt_pose) {
       continue;
     }
@@ -97,68 +70,6 @@ autoware_auto_perception_msgs::msg::PredictedPath resamplePredictedPath(
   return resampled_path;
 }
 
-geometry_msgs::msg::Pose lerpByPose(
-  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t)
-{
-  tf2::Transform tf_transform1, tf_transform2;
-  tf2::fromMsg(p1, tf_transform1);
-  tf2::fromMsg(p2, tf_transform2);
-  const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t);
-  const auto & tf_quaternion =
-    tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t);
-
-  geometry_msgs::msg::Pose pose;
-  pose.position = ::toMsg(tf_point);
-  pose.orientation = tf2::toMsg(tf_quaternion);
-  return pose;
-}
-
-boost::optional<geometry_msgs::msg::Pose> lerpByTimeStamp(
-  const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time)
-{
-  auto clock{rclcpp::Clock{RCL_ROS_TIME}};
-  if (path.path.empty()) {
-    RCLCPP_WARN_STREAM_THROTTLE(
-      rclcpp::get_logger("DynamicAvoidance.resample"), clock, 1000,
-      "Empty path. Failed to interpolate path by time!");
-    return {};
-  }
-  if (rel_time < rclcpp::Duration::from_seconds(0.0)) {
-    RCLCPP_DEBUG_STREAM(
-      rclcpp::get_logger("DynamicAvoidance.resample"), "failed to interpolate path by time!"
-                                                         << std::endl
-                                                         << "query time: " << rel_time.seconds());
-
-    return {};
-  }
-
-  if (rel_time > rclcpp::Duration(path.time_step) * (static_cast<double>(path.path.size()) - 1)) {
-    RCLCPP_DEBUG_STREAM(
-      rclcpp::get_logger("DynamicAvoidance.resample"),
-      "failed to interpolate path by time!"
-        << std::endl
-        << "path max duration: " << path.path.size() * rclcpp::Duration(path.time_step).seconds()
-        << std::endl
-        << "query time       : " << rel_time.seconds());
-
-    return {};
-  }
-
-  for (size_t i = 1; i < path.path.size(); ++i) {
-    const auto & pt = path.path.at(i);
-    const auto & prev_pt = path.path.at(i - 1);
-    if (rel_time <= rclcpp::Duration(path.time_step) * static_cast<double>(i)) {
-      const auto offset = rel_time - rclcpp::Duration(path.time_step) * static_cast<double>(i - 1);
-      const auto ratio = offset.seconds() / rclcpp::Duration(path.time_step).seconds();
-      return lerpByPose(prev_pt, pt, ratio);
-    }
-  }
-
-  RCLCPP_ERROR_STREAM(
-    rclcpp::get_logger("DynamicAvoidance.resample"), "Something failed in function: " << __func__);
-  return {};
-}
-
 inline void convertEulerAngleToMonotonic(std::vector<double> & a)
 {
   for (unsigned int i = 1; i < a.size(); ++i) {

planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -301,21 +301,6 @@ double PIDBasedPlanner::calcDistanceToObstacle(
          offset;
 }
 
-// Note: If stop planning is not required, cruise planning will be done instead.
-bool PIDBasedPlanner::isStopRequired(const TargetObstacle & obstacle)
-{
-  const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label);
-  const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label);
-
-  if (is_cruise_obstacle) {
-    return std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_;
-  } else if (is_stop_obstacle && !is_cruise_obstacle) {
-    return true;
-  }
-
-  return false;
-}
-
 Trajectory PIDBasedPlanner::planStop(
   const ObstacleCruisePlannerData & planner_data,
   const boost::optional<StopObstacleInfo> & stop_obstacle_info, DebugData & debug_data)