diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp index 22e8e4b06..5b52ef8bb 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.cpp @@ -282,20 +282,19 @@ void OptimizationTrajectoryBasedCenterline::init_path_generator_node( const geometry_msgs::msg::Pose current_pose, LaneletMapBin::ConstSharedPtr & map_bin_ptr, const LaneletRoute & route) const { - autoware::path_generator::PathGenerator::InputData path_generator_input; + if (path_generator_node_) { + return; + } - if (!path_generator_node_) { - // initialize node, lanelet map and route - path_generator_node_ = - std::make_shared(create_node_options()); + // initialize node, lanelet map and route + path_generator_node_ = + std::make_shared(create_node_options()); - // NOTE: no need to register every time - path_generator_input.lanelet_map_bin_ptr = map_bin_ptr; - path_generator_input.route_ptr = std::make_shared(route); - } + autoware::path_generator::PathGenerator::RouteManagerData route_manager_data; + route_manager_data.lanelet_map_bin_ptr = map_bin_ptr; + route_manager_data.route_ptr = std::make_shared(route); - path_generator_input.odometry_ptr = utils::convert_to_odometry(current_pose); - path_generator_node_->set_planner_data(path_generator_input); + path_generator_node_->initialize_route_manager(route_manager_data, current_pose); } std::shared_ptr diff --git a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp index cae845ac6..bc76dd55c 100644 --- a/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp +++ b/planning/autoware_static_centerline_generator/src/centerline_source/optimization_trajectory_based_centerline.hpp @@ -17,9 +17,7 @@ #include "autoware/behavior_path_goal_planner_module/default_fixed_goal_planner.hpp" #include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp" -#include "autoware/path_generator/common_structs.hpp" #include "autoware/path_generator/node.hpp" -#include "autoware/path_generator/utils.hpp" #include "rclcpp/rclcpp.hpp" #include "type_alias.hpp"