From bd7378ebcca253467ef3ee1d297211213395b894 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Fri, 20 Dec 2024 12:42:51 +0900 Subject: [PATCH] fix(obstacle_cruise_planner): use planning factor interface (#1718) Signed-off-by: satoshi-ota --- .../obstacle_cruise_planner/planner_interface.hpp | 8 +++++++- .../autoware_obstacle_cruise_planner/src/node.cpp | 1 + .../src/pid_based_planner/pid_based_planner.cpp | 4 ++++ .../src/planner_interface.cpp | 11 +++++++++++ 4 files changed, 23 insertions(+), 1 deletion(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp index ef6ae1662dcee..764ad5531d76c 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -15,6 +15,7 @@ #ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#include "autoware/motion_utils/factor/planning_factor_interface.hpp" #include "autoware/motion_utils/trajectory/trajectory.hpp" #include "autoware/obstacle_cruise_planner/common_structs.hpp" #include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" @@ -47,7 +48,9 @@ class PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) - : longitudinal_info_(longitudinal_info), + : planning_factor_interface_{std::make_unique( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param), debug_data_ptr_(debug_data_ptr), @@ -101,6 +104,7 @@ class PlannerInterface const std::optional & stop_pose = std::nullopt, const std::optional & stop_obstacle = std::nullopt); void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } void clearMetrics(); void onParam(const std::vector & parameters) @@ -128,6 +132,8 @@ class PlannerInterface double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } protected: + std::unique_ptr planning_factor_interface_; + // Parameters bool enable_debug_info_{false}; bool enable_calculation_time_info_{false}; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 6931ff58e8457..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -726,6 +726,7 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms // 8. Publish debug data published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); publishDebugMarker(); publishDebugInfo(); objects_of_interest_marker_interface_.publishMarkerArray(); 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 544597f05ff75..b80baec50ec70 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 @@ -335,6 +335,10 @@ std::vector PIDBasedPlanner::planCruise( debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( planner_data.current_time, PlanningBehavior::ADAPTIVE_CRUISE, stop_traj_points.at(wall_idx).pose)); diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp index b08bb10ef69cf..b92ca185bf8eb 100644 --- a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -338,6 +338,10 @@ std::vector PlannerInterface::generateStopTrajectory( const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; velocity_factors_pub_->publish(obstacle_cruise_utils::makeVelocityFactorArray( planner_data.current_time, PlanningBehavior::ROUTE_OBSTACLE, stop_pose)); + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); // Store stop reason debug data debug_data_ptr_->stop_metrics = makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); @@ -594,6 +598,13 @@ std::vector PlannerInterface::generateSlowDownTrajectory( 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_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); } // add debug virtual wall