diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp index 8af9a63f3a1fa..d929647d9bcd0 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp +++ b/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 pose = std::nullopt); } // namespace obstacle_cruise_utils #endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 80fb85d4e30c9..da5d083d6ddcf 100644 --- a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -326,6 +326,10 @@ std::vector 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 diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index 140396a09e042..61946a8e4b37f 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -87,26 +87,6 @@ StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) return stop_reason_array; } -VelocityFactorArray makeVelocityFactorArray( - const rclcpp::Time & time, const std::optional 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::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,7 +227,8 @@ std::vector PlannerInterface::generateStopTrajectory( 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); @@ -403,7 +384,8 @@ std::vector PlannerInterface::generateStopTrajectory( 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 @@ std::vector PlannerInterface::generateSlowDownTrajectory( 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); } diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp index bc52e800c8fc5..3bf6ee311ca13 100644 --- a/planning/autoware_obstacle_cruise_planner/src/utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -113,4 +113,26 @@ std::vector getClosestStopObstacles(const std::vector 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::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