Merge branch 'main' into fix/freespace_planner_use_node_clock

build_depends.repos

@@ -28,7 +28,7 @@ repositories:
   core/autoware_adapi_msgs:
     type: git
     url: https://github.com/autowarefoundation/autoware_adapi_msgs.git
-    version: 1.3.0
+    version: beta/1.7.0
   core/autoware_internal_msgs:
     type: git
     url: https://github.com/autowarefoundation/autoware_internal_msgs.git

common/.pages

@@ -40,7 +40,7 @@ nav:
     - 'tier4_system_rviz_plugin': common/tier4_system_rviz_plugin
     - 'tier4_traffic_light_rviz_plugin': common/tier4_traffic_light_rviz_plugin
     - 'tier4_vehicle_rviz_plugin': common/tier4_vehicle_rviz_plugin
-    - 'traffic_light_recognition_marker_publisher': common/traffic_light_recognition_marker_publisher/Readme
+    - 'autoware_traffic_light_recognition_marker_publisher': common/autoware_traffic_light_recognition_marker_publisher/Readme
   - 'Node':
     - 'Goal Distance Calculator': common/autoware_goal_distance_calculator/Readme
     - 'Path Distance Calculator': common/autoware_path_distance_calculator/Readme

common/traffic_light_recognition_marker_publisher/CMakeLists.txt → common/autoware_traffic_light_recognition_marker_publisher/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(traffic_light_recognition_marker_publisher)
+project(autoware_traffic_light_recognition_marker_publisher)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()

common/traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml → common/autoware_traffic_light_recognition_marker_publisher/launch/traffic_light_recognition_marker_publisher.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <node pkg="traffic_light_recognition_marker_publisher" exec="traffic_light_recognition_marker_publisher_node" name="traffic_light_recognition_marker_publisher">
+  <node pkg="autoware_traffic_light_recognition_marker_publisher" exec="autoware_traffic_light_recognition_marker_publisher_node" name="traffic_light_recognition_marker_publisher">
     <remap from="~/input/lanelet2_map" to="/map/vector_map"/>
     <remap from="~/input/traffic_signals" to="/perception/traffic_light_recognition/traffic_signals"/>
     <remap from="~/output/marker" to="/perception/traffic_light_recognition/traffic_signals_marker"/>

common/traffic_light_recognition_marker_publisher/package.xml → common/autoware_traffic_light_recognition_marker_publisher/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>traffic_light_recognition_marker_publisher</name>
+  <name>autoware_traffic_light_recognition_marker_publisher</name>
   <version>0.38.0</version>
-  <description>The traffic_light_recognition_marker_publisher package</description>
+  <description>The autoware_traffic_light_recognition_marker_publisher package</description>
   <maintainer email="[email protected]">Tomoya Kimura</maintainer>
   <maintainer email="[email protected]">Takeshi Miura</maintainer>
   <maintainer email="[email protected]">Shumpei Wakabayashi</maintainer>

evaluator/autoware_planning_evaluator/package.xml

@@ -30,6 +30,7 @@
   <depend>tier4_metric_msgs</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

evaluator/kinematic_evaluator/package.xml

@@ -29,6 +29,7 @@
   <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

evaluator/localization_evaluator/package.xml

@@ -25,6 +25,7 @@
   <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

evaluator/perception_online_evaluator/package.xml

@@ -35,6 +35,7 @@
   <depend>tf2_ros</depend>
 
   <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_index_cpp</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

planning/autoware_freespace_planning_algorithms/CMakeLists.txt

@@ -60,6 +60,8 @@ find_package(Python3 REQUIRED COMPONENTS Interpreter Development)
 find_package(pybind11_vendor REQUIRED)
 find_package(pybind11 REQUIRED)
 
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wno-ignored-optimization-argument")
+
 pybind11_add_module(${PROJECT_NAME}_pybind SHARED
   scripts/bind/astar_search_pybind.cpp
 )

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

@@ -42,6 +42,7 @@ using MetricArray = tier4_metric_msgs::msg::MetricArray;
 class PlannerInterface
 {
 public:
+  virtual ~PlannerInterface() = default;
   PlannerInterface(
     rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info,

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

@@ -87,26 +87,6 @@ StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time)
   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,7 +227,8 @@ std::vector<TrajectoryPoint> 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<TrajectoryPoint> 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<TrajectoryPoint> 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);
     }
 

planning/autoware_obstacle_cruise_planner/src/utils.cpp

@@ -113,4 +113,26 @@ std::vector<StopObstacle> getClosestStopObstacles(const std::vector<StopObstacle
   }
   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

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/pull_over_planner/shift_pull_over.hpp

@@ -50,7 +50,7 @@ class ShiftPullOver : public PullOverPlannerBase
     const GoalCandidate & goal_candidate, const size_t id,
     const std::shared_ptr<const PlannerData> planner_data,
     const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const;
+    const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const;
   static double calcBeforeShiftedArcLength(
     const PathWithLaneId & path, const double after_shifted_arc_length, const double dr);
   static std::vector<double> splineTwoPoints(

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/pull_over_planner/shift_pull_over.cpp

@@ -132,7 +132,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   const GoalCandidate & goal_candidate, const size_t id,
   const std::shared_ptr<const PlannerData> planner_data,
   const BehaviorModuleOutput & previous_module_output, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const double lateral_jerk) const
+  const lanelet::ConstLanelets & pull_over_lanes, const double lateral_jerk) const
 {
   const double pull_over_velocity = parameters_.pull_over_velocity;
   const double after_shift_straight_distance = parameters_.after_shift_straight_distance;
@@ -206,9 +206,6 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   shifted_path.path.points = autoware::motion_utils::removeOverlapPoints(shifted_path.path.points);
   autoware::motion_utils::insertOrientation(shifted_path.path.points, true);
 
-  // set same orientation, because the reference center line orientation is not same to the
-  shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation;
-
   // for debug. result of shift is not equal to the target
   const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose;
 
@@ -227,7 +224,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
     p.point.longitudinal_velocity_mps = 0.0;
     p.point.pose = goal_pose;
     p.lane_ids = shifted_path.path.points.back().lane_ids;
-    for (const auto & lane : shoulder_lanes) {
+    for (const auto & lane : pull_over_lanes) {
       p.lane_ids.push_back(lane.id());
     }
     shifted_path.path.points.push_back(p);
@@ -249,7 +246,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
       }
     }
     // add shoulder lane_id if not found
-    for (const auto & lane : shoulder_lanes) {
+    for (const auto & lane : pull_over_lanes) {
       if (
         std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) ==
         point.lane_ids.end()) {
@@ -297,7 +294,7 @@ std::optional<PullOverPath> ShiftPullOver::generatePullOverPath(
   });
   const bool is_in_lanes = std::invoke([&]() -> bool {
     const auto drivable_lanes =
-      utils::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes);
+      utils::generateDrivableLanesWithShoulderLanes(road_lanes, pull_over_lanes);
     const auto & dp = planner_data->drivable_area_expansion_parameters;
     const auto expanded_lanes = utils::expandLanelets(
       drivable_lanes, dp.drivable_area_left_bound_offset, dp.drivable_area_right_bound_offset,

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/utils/parking_departure/geometric_parallel_parking.hpp

@@ -70,11 +70,11 @@ class GeometricParallelParking
   bool isParking() const;
   bool planPullOver(
     const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward,
+    const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
     const bool left_side_parking);
   bool planPullOut(
     const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-    const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+    const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
       autoware_lane_departure_checker);
   void setParameters(const ParallelParkingParameters & parameters) { parameters_ = parameters; }
@@ -117,7 +117,7 @@ class GeometricParallelParking
   void clearPaths();
   std::vector<PathWithLaneId> planOneTrial(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
     const double lane_departure_margin, const double arc_path_interval,
     const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
@@ -134,7 +134,7 @@ class GeometricParallelParking
     const bool left_side_parking);
   std::vector<PathWithLaneId> generatePullOverPaths(
     const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+    const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
     const bool is_forward, const bool left_side_parking, const double end_pose_offset,
     const double velocity);
   PathWithLaneId generateStraightPath(

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/utils/parking_departure/geometric_parallel_parking.cpp

@@ -105,7 +105,7 @@ void GeometricParallelParking::setVelocityToArcPaths(
 
 std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
   const double velocity)
 {
@@ -115,7 +115,7 @@ std::vector<PathWithLaneId> GeometricParallelParking::generatePullOverPaths(
   const double arc_path_interval = is_forward ? parameters_.forward_parking_path_interval
                                               : parameters_.backward_parking_path_interval;
   auto arc_paths = planOneTrial(
-    start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
+    start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
     end_pose_offset, lane_departure_margin, arc_path_interval, {});
   if (arc_paths.empty()) {
     return std::vector<PathWithLaneId>{};
@@ -156,7 +156,7 @@ void GeometricParallelParking::clearPaths()
 
 bool GeometricParallelParking::planPullOver(
   const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool is_forward,
+  const lanelet::ConstLanelets & pull_over_lanes, const bool is_forward,
   const bool left_side_parking)
 {
   const auto & common_params = planner_data_->parameters;
@@ -186,7 +186,7 @@ bool GeometricParallelParking::planPullOver(
       }
 
       const auto paths = generatePullOverPaths(
-        *start_pose, goal_pose, R_E_far, road_lanes, shoulder_lanes, is_forward, left_side_parking,
+        *start_pose, goal_pose, R_E_far, road_lanes, pull_over_lanes, is_forward, left_side_parking,
         end_pose_offset, parameters_.forward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
@@ -208,8 +208,8 @@ bool GeometricParallelParking::planPullOver(
       }
 
       const auto paths = generatePullOverPaths(
-        *start_pose, goal_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_parking,
-        end_pose_offset, parameters_.backward_parking_velocity);
+        *start_pose, goal_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward,
+        left_side_parking, end_pose_offset, parameters_.backward_parking_velocity);
       if (!paths.empty()) {
         paths_ = paths;
         return true;
@@ -222,7 +222,7 @@ bool GeometricParallelParking::planPullOver(
 
 bool GeometricParallelParking::planPullOut(
   const Pose & start_pose, const Pose & goal_pose, const lanelet::ConstLanelets & road_lanes,
-  const lanelet::ConstLanelets & shoulder_lanes, const bool left_side_start,
+  const lanelet::ConstLanelets & pull_over_lanes, const bool left_side_start,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
     lane_departure_checker)
 {
@@ -242,7 +242,7 @@ bool GeometricParallelParking::planPullOut(
 
     // plan reverse path of parking. end_pose <-> start_pose
     auto arc_paths = planOneTrial(
-      *end_pose, start_pose, R_E_min_, road_lanes, shoulder_lanes, is_forward, left_side_start,
+      *end_pose, start_pose, R_E_min_, road_lanes, pull_over_lanes, is_forward, left_side_start,
       start_pose_offset, parameters_.pull_out_lane_departure_margin,
       parameters_.pull_out_arc_path_interval, lane_departure_checker);
     if (arc_paths.empty()) {
@@ -374,7 +374,7 @@ PathWithLaneId GeometricParallelParking::generateStraightPath(
 
 std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
   const Pose & start_pose, const Pose & goal_pose, const double R_E_far,
-  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes,
+  const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & pull_over_lanes,
   const bool is_forward, const bool left_side_parking, const double end_pose_offset,
   const double lane_departure_margin, const double arc_path_interval,
   const std::shared_ptr<autoware::lane_departure_checker::LaneDepartureChecker>
@@ -419,15 +419,15 @@ std::vector<PathWithLaneId> GeometricParallelParking::planOneTrial(
     }
     lanes.push_back(lane);
   }
-  lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end());
+  lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end());
 
   // If start_pose is parallel to goal_pose, we can know lateral deviation of edges of vehicle,
   // and detect lane departure.
   if (is_forward) {  // Check near bound
     const double R_front_near =
       std::hypot(R_E_far + common_params.vehicle_width / 2, common_params.base_link2front);
     const double distance_to_near_bound =
-      utils::getSignedDistanceFromBoundary(shoulder_lanes, arc_end_pose, left_side_parking);
+      utils::getSignedDistanceFromBoundary(pull_over_lanes, arc_end_pose, left_side_parking);
     const double near_deviation = R_front_near - R_E_far;
     if (std::abs(distance_to_near_bound) - near_deviation < lane_departure_margin) {
       return std::vector<PathWithLaneId>{};

planning/behavior_path_planner/autoware_behavior_path_sampling_planner_module/include/autoware/behavior_path_sampling_planner_module/sampling_planner_module.hpp

@@ -155,8 +155,6 @@ class SamplingPlannerModule : public SceneModuleInterface
   bool canTransitSuccessState() override
   {
     std::vector<DrivableLanes> drivable_lanes{};
-    const auto & prev_module_path =
-      std::make_shared<PathWithLaneId>(getPreviousModuleOutput().path);
     const auto prev_module_reference_path =
       std::make_shared<PathWithLaneId>(getPreviousModuleOutput().reference_path);
 

planning/behavior_velocity_planner/autoware_behavior_velocity_run_out_module/src/scene.cpp

@@ -50,7 +50,7 @@ RunOutModule::RunOutModule(
   debug_ptr_(debug_ptr),
   state_machine_(std::make_unique<run_out_utils::StateMachine>(planner_param.approaching.state))
 {
-  velocity_factor_.init(PlanningBehavior::ROUTE_OBSTACLE);
+  velocity_factor_.init(PlanningBehavior::RUN_OUT);
 
   if (planner_param.run_out.use_partition_lanelet) {
     const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr();