feat(obstacle_cruise_planner): separate data (tier4#1209)

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp

@@ -71,10 +71,16 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg);
 
   // member Functions
-  ObstacleCruisePlannerData createPlannerData(
+  ObstacleCruisePlannerData createCruiseData(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
-    DebugData & debug_data);
+    const std::vector<TargetObstacle> & obstacles);
+  ObstacleCruisePlannerData createStopData(
+    const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
+    const std::vector<TargetObstacle> & obstacles);
   double calcCurrentAccel() const;
+  std::vector<TargetObstacle> getTargetObstacles(
+    const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
+    const double current_vel, DebugData & debug_data);
   std::vector<TargetObstacle> filterObstacles(
     const PredictedObjects & predicted_objects, const Trajectory & traj,
     const geometry_msgs::msg::Pose & current_pose, const double current_vel,

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

@@ -49,8 +49,8 @@ class OptimizationBasedPlanner : public PlannerInterface
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
   Trajectory generateCruiseTrajectory(
-    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
-    boost::optional<VelocityLimit> & vel_limit, DebugData & debug_data) override;
+    const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
+    DebugData & debug_data) override;
 
 private:
   struct TrajectoryData
@@ -72,13 +72,12 @@ class OptimizationBasedPlanner : public PlannerInterface
   // Member Functions
   std::vector<double> createTimeVector();
   std::tuple<double, double> calcInitialMotion(
-    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
-    const size_t input_closest, const Trajectory & prev_traj);
+    const ObstacleCruisePlannerData & planner_data, const size_t input_closest,
+    const Trajectory & prev_traj);
 
   TrajectoryPoint calcInterpolatedTrajectoryPoint(
     const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose);
-  bool checkHasReachedGoal(
-    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj);
+  bool checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data);
   TrajectoryData getTrajectoryData(
     const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose);
 

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

@@ -56,8 +56,8 @@ class PIDBasedPlanner : public PlannerInterface
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
   Trajectory generateCruiseTrajectory(
-    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
-    boost::optional<VelocityLimit> & vel_limit, DebugData & debug_data) override;
+    const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
+    DebugData & debug_data) override;
 
   void updateParam(const std::vector<rclcpp::Parameter> & parameters) override;
 

planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp

@@ -66,8 +66,8 @@ class PlannerInterface
     const ObstacleCruisePlannerData & planner_data, DebugData & debug_data);
 
   virtual Trajectory generateCruiseTrajectory(
-    const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
-    boost::optional<VelocityLimit> & vel_limit, DebugData & debug_data) = 0;
+    const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
+    DebugData & debug_data) = 0;
 
   void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
   {

planning/obstacle_cruise_planner/src/node.cpp

@@ -482,17 +482,24 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms
 
   stop_watch_.tic(__func__);
 
-  // create algorithmic data
+  // Get Target Obstacles
   DebugData debug_data;
-  const auto planner_data = createPlannerData(*msg, current_pose_ptr->pose, debug_data);
+  const auto target_obstacles = getTargetObstacles(
+    *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, debug_data);
+
+  // create data for stop
+  const auto stop_data = createStopData(*msg, current_pose_ptr->pose, target_obstacles);
 
   // stop planning
-  const auto stop_traj = planner_ptr_->generateStopTrajectory(planner_data, debug_data);
+  const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data);
+
+  // create data for cruise
+  const auto cruise_data = createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles);
 
   // cruise planning
   boost::optional<VelocityLimit> vel_limit;
   const auto output_traj =
-    planner_ptr_->generateCruiseTrajectory(planner_data, stop_traj, vel_limit, debug_data);
+    planner_ptr_->generateCruiseTrajectory(cruise_data, vel_limit, debug_data);
 
   // publisher external velocity limit if required
   publishVelocityLimit(vel_limit);
@@ -523,33 +530,50 @@ bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label)
   return std::find(types.begin(), types.end(), label) != types.end();
 }
 
-ObstacleCruisePlannerData ObstacleCruisePlannerNode::createPlannerData(
+ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData(
   const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
-  DebugData & debug_data)
+  const std::vector<TargetObstacle> & obstacles)
 {
-  stop_watch_.tic(__func__);
-
   const auto current_time = now();
   const double current_vel = current_twist_ptr_->twist.linear.x;
   const double current_accel = calcCurrentAccel();
-  auto target_obstacles =
-    filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data);
-  updateHasStopped(target_obstacles);
 
-  // create planner_data
+  // create planner_stop data
   ObstacleCruisePlannerData planner_data;
   planner_data.current_time = current_time;
   planner_data.traj = trajectory;
   planner_data.current_pose = current_pose;
   planner_data.current_vel = current_vel;
   planner_data.current_acc = current_accel;
-  planner_data.target_obstacles = target_obstacles;
+  for (const auto & obstacle : obstacles) {
+    if (obstacle.has_stopped) {
+      planner_data.target_obstacles.push_back(obstacle);
+    }
+  }
 
-  // print calculation time
-  const double calculation_time = stop_watch_.toc(__func__);
-  RCLCPP_INFO_EXPRESSION(
-    rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, "  %s := %f [ms]",
-    __func__, calculation_time);
+  return planner_data;
+}
+
+ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData(
+  const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
+  const std::vector<TargetObstacle> & obstacles)
+{
+  const auto current_time = now();
+  const double current_vel = current_twist_ptr_->twist.linear.x;
+  const double current_accel = calcCurrentAccel();
+
+  // create planner_stop data
+  ObstacleCruisePlannerData planner_data;
+  planner_data.current_time = current_time;
+  planner_data.traj = trajectory;
+  planner_data.current_pose = current_pose;
+  planner_data.current_vel = current_vel;
+  planner_data.current_acc = current_accel;
+  for (const auto & obstacle : obstacles) {
+    if (!obstacle.has_stopped) {
+      planner_data.target_obstacles.push_back(obstacle);
+    }
+  }
 
   return planner_data;
 }
@@ -567,6 +591,25 @@ double ObstacleCruisePlannerNode::calcCurrentAccel() const
   return lpf_acc_ptr_->filter(accel);
 }
 
+std::vector<TargetObstacle> ObstacleCruisePlannerNode::getTargetObstacles(
+  const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose,
+  const double current_vel, DebugData & debug_data)
+{
+  stop_watch_.tic(__func__);
+
+  auto target_obstacles =
+    filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data);
+  updateHasStopped(target_obstacles);
+
+  // print calculation time
+  const double calculation_time = stop_watch_.toc(__func__);
+  RCLCPP_INFO_EXPRESSION(
+    rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, "  %s := %f [ms]",
+    __func__, calculation_time);
+
+  return target_obstacles;
+}
+
 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)

planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp

@@ -113,7 +113,7 @@ OptimizationBasedPlanner::OptimizationBasedPlanner(
 }
 
 Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
-  const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
+  const ObstacleCruisePlannerData & planner_data,
   [[maybe_unused]] boost::optional<VelocityLimit> & vel_limit,
   [[maybe_unused]] DebugData & debug_data)
 {
@@ -123,57 +123,57 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
     RCLCPP_ERROR(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "Resolution size is not enough");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Get the nearest point on the trajectory
   const auto closest_idx = tier4_autoware_utils::findNearestIndex(
-    stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
+    planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
     nearest_yaw_deviation_threshold_);
   if (!closest_idx) {  // Check validity of the closest index
     RCLCPP_ERROR(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "Closest Index is Invalid");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Transform original trajectory to TrajectoryData
-  const auto base_traj_data = getTrajectoryData(stop_traj, planner_data.current_pose);
+  const auto base_traj_data = getTrajectoryData(planner_data.traj, planner_data.current_pose);
   if (base_traj_data.traj.points.size() < 2) {
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "The number of points on the trajectory data is too small");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Compute maximum velocity
   double v_max = 0.0;
-  for (const auto & point : stop_traj.points) {
+  for (const auto & point : planner_data.traj.points) {
     v_max = std::max(v_max, static_cast<double>(point.longitudinal_velocity_mps));
   }
 
   // Get Current Velocity
   double v0;
   double a0;
-  std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj, *closest_idx, prev_output_);
+  std::tie(v0, a0) = calcInitialMotion(planner_data, *closest_idx, prev_output_);
   a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel));
 
   // Check trajectory size
-  if (stop_traj.points.size() - *closest_idx <= 2) {
+  if (planner_data.traj.points.size() - *closest_idx <= 2) {
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "The number of points on the trajectory is too small");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Check if reached goal
-  if (checkHasReachedGoal(planner_data, stop_traj)) {
-    prev_output_ = stop_traj;
-    return stop_traj;
+  if (checkHasReachedGoal(planner_data)) {
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Resample base trajectory data by time
@@ -183,8 +183,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "The number of points on the resampled trajectory data is too small");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Get S Boundaries from the obstacle
@@ -193,8 +193,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "No Dangerous Objects around the ego vehicle");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Optimization
@@ -232,23 +232,24 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
 
   // Publish Debug trajectories
   publishDebugTrajectory(
-    planner_data.current_time, stop_traj, *closest_idx, time_vec, *s_boundaries, optimized_result);
+    planner_data.current_time, planner_data.traj, *closest_idx, time_vec, *s_boundaries,
+    optimized_result);
 
   // Transformation from t to s
   const auto processed_result = processOptimizedResult(data.v0, optimized_result);
   if (!processed_result) {
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "Processed Result is empty");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
   const auto & opt_position = processed_result->s;
   const auto & opt_velocity = processed_result->v;
 
   // Check Size
   if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) {
-    auto output = stop_traj;
+    auto output = planner_data.traj;
     output.points.at(*closest_idx).longitudinal_velocity_mps = data.v0;
     for (size_t i = *closest_idx + 1; i < output.points.size(); ++i) {
       output.points.at(i).longitudinal_velocity_mps = 0.0;
@@ -259,8 +260,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
     RCLCPP_DEBUG(
       rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"),
       "Optimized Trajectory is too small");
-    prev_output_ = stop_traj;
-    return stop_traj;
+    prev_output_ = planner_data.traj;
+    return planner_data.traj;
   }
 
   // Get Zero Velocity Position
@@ -273,7 +274,7 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
     }
   }
   const auto traj_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint(
-    stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
+    planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
     nearest_yaw_deviation_threshold_);
   if (traj_stop_dist) {
     closest_stop_dist = std::min(*traj_stop_dist, closest_stop_dist);
@@ -332,9 +333,9 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory(
   }
 
   Trajectory output;
-  output.header = stop_traj.header;
+  output.header = planner_data.traj.header;
   for (size_t i = 0; i < *closest_idx; ++i) {
-    auto point = stop_traj.points.at(i);
+    auto point = planner_data.traj.points.at(i);
     point.longitudinal_velocity_mps = data.v0;
     output.points.push_back(point);
   }
@@ -385,11 +386,11 @@ std::vector<double> OptimizationBasedPlanner::createTimeVector()
 
 // v0, a0
 std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
-  const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj,
-  const size_t input_closest, const Trajectory & prev_traj)
+  const ObstacleCruisePlannerData & planner_data, const size_t input_closest,
+  const Trajectory & prev_traj)
 {
   const auto & current_vel = planner_data.current_vel;
-  const auto & input_traj = stop_traj;
+  const auto & input_traj = planner_data.traj;
   const double vehicle_speed{std::abs(current_vel)};
   const double target_vel{std::abs(input_traj.points.at(input_closest).longitudinal_velocity_mps)};
 
@@ -504,12 +505,11 @@ TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint(
   return traj_p;
 }
 
-bool OptimizationBasedPlanner::checkHasReachedGoal(
-  const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj)
+bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data)
 {
   // If goal is close and current velocity is low, we don't optimize trajectory
   const auto closest_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint(
-    stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
+    planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_,
     nearest_yaw_deviation_threshold_);
   if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) {
     return true;