feat(obstacle_cruise_planner): outputs velocity factor when the ego follows front vehicle.

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp

@@ -93,6 +93,10 @@ size_t getIndexWithLongitudinalOffset(
   }
   return 0;
 }
+
+VelocityFactorArray makeVelocityFactorArray(
+  const rclcpp::Time & time, const std::string & behavior = PlanningBehavior::ROUTE_OBSTACLE,
+  const std::optional<geometry_msgs::msg::Pose> pose = std::nullopt);
 }  // namespace obstacle_cruise_utils
 
 #endif  // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

@@ -326,6 +326,10 @@ std::vector<TrajectoryPoint> PIDBasedPlanner::planCruise(
       // cruise obstacle
       debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle);
       debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data);
+
+      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
+        planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE,
+        stop_traj_points.at(wall_idx).pose));
     }
 
     // do cruise planning

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

@@ -1,4 +1,4 @@
 // Copyright 2022 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -87,26 +87,6 @@
   return stop_reason_array;
 }
 
-VelocityFactorArray makeVelocityFactorArray(
-  const rclcpp::Time & time, const std::optional<geometry_msgs::msg::Pose> pose = std::nullopt)
-{
-  VelocityFactorArray velocity_factor_array;
-  velocity_factor_array.header.frame_id = "map";
-  velocity_factor_array.header.stamp = time;
-
-  if (pose) {
-    using distance_type = VelocityFactor::_distance_type;
-    VelocityFactor velocity_factor;
-    velocity_factor.behavior = PlanningBehavior::ROUTE_OBSTACLE;
-    velocity_factor.pose = pose.value();
-    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
-    velocity_factor.status = VelocityFactor::UNKNOWN;
-    velocity_factor.detail = std::string();
-    velocity_factor_array.factors.push_back(velocity_factor);
-  }
-  return velocity_factor_array;
-}
-
 double calcMinimumDistanceToStop(
   const double initial_vel, const double max_acc, const double min_acc)
 {
@@ -247,163 +227,165 @@
 
   if (stop_obstacles.empty()) {
     stop_reasons_pub_->publish(makeEmptyStopReasonArray(planner_data.current_time));
-    velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time));
+    velocity_factors_pub_->publish(
+      obstacle_cruise_utils::makeVelocityFactorArray(planner_data.current_time));
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
     autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
 
     prev_stop_distance_info_ = std::nullopt;
     return planner_data.traj_points;
   }
 
   RCLCPP_INFO_EXPRESSION(
     rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_,
     "stop planning");
 
   std::optional<StopObstacle> determined_stop_obstacle{};
   std::optional<double> determined_zero_vel_dist{};
   std::optional<double> determined_desired_margin{};
 
   const auto closest_stop_obstacles =
     obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles);
   for (const auto & stop_obstacle : closest_stop_obstacles) {
     const auto ego_segment_idx =
       ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose);
     const double dist_to_collide_on_ref_traj =
       autoware::motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) +
       stop_obstacle.dist_to_collide_on_decimated_traj;
 
     const double desired_margin = [&]() {
       const double margin_from_obstacle =
         calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle);
       // Use terminal margin (terminal_safe_distance_margin) for obstacle stop
       const auto ref_traj_length = autoware::motion_utils::calcSignedArcLength(
         planner_data.traj_points, 0, planner_data.traj_points.size() - 1);
       if (dist_to_collide_on_ref_traj > ref_traj_length) {
         return longitudinal_info_.terminal_safe_distance_margin;
       }
 
       // If behavior stop point is ahead of the closest_obstacle_stop point within a certain
       // margin we set closest_obstacle_stop_distance to closest_behavior_stop_distance
       const auto closest_behavior_stop_idx = autoware::motion_utils::searchZeroVelocityIndex(
         planner_data.traj_points, ego_segment_idx + 1);
       if (closest_behavior_stop_idx) {
         const double closest_behavior_stop_dist_on_ref_traj =
           autoware::motion_utils::calcSignedArcLength(
             planner_data.traj_points, 0, *closest_behavior_stop_idx);
         const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj -
                                       (dist_to_collide_on_ref_traj - margin_from_obstacle);
         if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) {
           return min_behavior_stop_margin_;
         }
       }
       return margin_from_obstacle;
     }();
 
     // calc stop point against the obstacle
     double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin);
     if (suppress_sudden_obstacle_stop_) {
       const auto acceptable_stop_acc = [&]() -> std::optional<double> {
         if (stop_param_.getParamType(stop_obstacle.classification) == "default") {
           return longitudinal_info_.limit_min_accel;
         }
         const double distance_to_judge_suddenness = std::min(
           calcMinimumDistanceToStop(
             planner_data.ego_vel, longitudinal_info_.limit_max_accel,
             stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold),
           stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold);
         if (candidate_zero_vel_dist > distance_to_judge_suddenness) {
           return longitudinal_info_.limit_min_accel;
         }
         if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) {
           RCLCPP_WARN(
             rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"),
             "[Cruise] abandon to stop against %s object",
             stop_param_.types_maps.at(stop_obstacle.classification.label).c_str());
           return std::nullopt;
         } else {
           return stop_param_.getParam(stop_obstacle.classification).limit_min_acc;
         }
       }();
       if (!acceptable_stop_acc) {
         continue;
       }
 
       const double acceptable_stop_pos =
         autoware::motion_utils::calcSignedArcLength(
           planner_data.traj_points, 0, planner_data.ego_pose.position) +
         calcMinimumDistanceToStop(
           planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value());
       if (acceptable_stop_pos > candidate_zero_vel_dist) {
         candidate_zero_vel_dist = acceptable_stop_pos;
       }
     }
 
     if (determined_stop_obstacle) {
       const bool is_same_param_types =
         (stop_obstacle.classification.label == determined_stop_obstacle->classification.label);
       if (
         (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj >
                                   determined_stop_obstacle->dist_to_collide_on_decimated_traj) ||
         (!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) {
         continue;
       }
     }
     determined_zero_vel_dist = candidate_zero_vel_dist;
     determined_stop_obstacle = stop_obstacle;
     determined_desired_margin = desired_margin;
   }
 
   if (!determined_zero_vel_dist) {
     // delete marker
     const auto markers =
       autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0);
     autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
 
     prev_stop_distance_info_ = std::nullopt;
     return planner_data.traj_points;
   }
 
   // Hold previous stop distance if necessary
   if (
     std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold &&
     prev_stop_distance_info_) {
     // NOTE: We assume that the current trajectory's front point is ahead of the previous
     // trajectory's front point.
     const size_t traj_front_point_prev_seg_idx =
       autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
         prev_stop_distance_info_->first, planner_data.traj_points.front().pose);
     const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength(
       prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position,
       traj_front_point_prev_seg_idx);
 
     const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points;
     if (
       std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) <
       longitudinal_info_.hold_stop_distance_threshold) {
       determined_zero_vel_dist.value() = prev_zero_vel_dist;
     }
   }
 
   // Insert stop point
   auto output_traj_points = planner_data.traj_points;
   const auto zero_vel_idx =
     autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points);
   if (zero_vel_idx) {
     // virtual wall marker for stop obstacle
     const auto markers = autoware::motion_utils::createStopVirtualWallMarker(
       output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0,
       abs_ego_offset, "", planner_data.is_driving_forward);
     autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker);
     debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle);
 
     // Publish Stop Reason
     const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose;
     const auto stop_reasons_msg =
       makeStopReasonArray(planner_data, stop_pose, *determined_stop_obstacle);
     stop_reasons_pub_->publish(stop_reasons_msg);
-    velocity_factors_pub_->publish(makeVelocityFactorArray(planner_data.current_time, stop_pose));
+    velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
+      planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose));
     // Store stop reason debug data
     debug_data_ptr_->stop_metrics =
       makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle);
@@ -656,6 +638,9 @@
       const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker(
         slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down",
         planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward);
+      velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray(
+        planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE,
+        slow_down_traj_points.at(slow_down_wall_idx).pose));
       autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker);
     }
 

planning/autoware_obstacle_cruise_planner/src/utils.cpp

@@ -113,4 +113,26 @@
   }
   return candidates;
 }
+
+VelocityFactorArray makeVelocityFactorArray(
+  const rclcpp::Time & time, const std::string & behavior,
+  const std::optional<geometry_msgs::msg::Pose> pose)
+{
+  VelocityFactorArray velocity_factor_array;
+  velocity_factor_array.header.frame_id = "map";
+  velocity_factor_array.header.stamp = time;
+
+  if (pose) {
+    using distance_type = VelocityFactor::_distance_type;
+    VelocityFactor velocity_factor;
+    velocity_factor.behavior = behavior;
+    velocity_factor.pose = pose.value();
+    velocity_factor.distance = std::numeric_limits<distance_type>::quiet_NaN();
+    velocity_factor.status = VelocityFactor::UNKNOWN;
+    velocity_factor.detail = std::string();
+    velocity_factor_array.factors.push_back(velocity_factor);
+  }
+  return velocity_factor_array;
+}
+
 }  // namespace obstacle_cruise_utils