fix: fix data types for lane ids (#4096)

Signed-off-by: Ryohsuke Mitsudome <ryohsuke.mitsudome@tier4.jp>

Author: mitsudome-r

control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp

@@ -53,13 +53,13 @@ std::array<geometry_msgs::msg::Point, 3> triangle2points(
   return points;
 }
 
-std::map<int, lanelet::ConstLanelet> getRouteLanelets(
+std::map<lanelet::Id, lanelet::ConstLanelet> getRouteLanelets(
   const lanelet::LaneletMapPtr & lanelet_map,
   const lanelet::routing::RoutingGraphPtr & routing_graph,
   const autoware_planning_msgs::msg::LaneletRoute::ConstSharedPtr & route_ptr,
   const double vehicle_length)
 {
-  std::map<int, lanelet::ConstLanelet> route_lanelets;
+  std::map<lanelet::Id, lanelet::ConstLanelet> route_lanelets;
 
   bool is_route_valid = lanelet::utils::route::isRouteValid(*route_ptr, lanelet_map);
   if (!is_route_valid) {
@@ -315,7 +315,7 @@ void LaneDepartureCheckerNode::onTimer()
 
   // In order to wait for both of map and route will be ready, write this not in callback but here
   if (last_route_ != route_ && !route_->segments.empty()) {
-    std::map<int, lanelet::ConstLanelet>::iterator itr;
+    std::map<lanelet::Id, lanelet::ConstLanelet>::iterator itr;
 
     auto map_route_lanelets_ =
       getRouteLanelets(lanelet_map_, routing_graph_, route_, vehicle_length_m_);

planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_path.hpp

@@ -42,8 +42,8 @@ struct LaneChangeStatus
   LaneChangePath lane_change_path{};
   lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets target_lanes{};
-  std::vector<uint64_t> lane_follow_lane_ids{};
-  std::vector<uint64_t> lane_change_lane_ids{};
+  std::vector<lanelet::Id> lane_follow_lane_ids{};
+  std::vector<lanelet::Id> lane_change_lane_ids{};
   bool is_safe{false};
   bool is_valid_path{true};
   double start_distance{0.0};

planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp

@@ -122,7 +122,7 @@ FrenetPoint convertToFrenetPoint(
   return frenet_point;
 }
 
-std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets);
+std::vector<lanelet::Id> getIds(const lanelet::ConstLanelets & lanelets);
 
 // distance (arclength) calculation
 

planning/behavior_path_planner/src/utils/utils.cpp

@@ -778,9 +778,9 @@ double getSignedDistance(
   return arc_goal.length - arc_current.length;
 }
 
-std::vector<uint64_t> getIds(const lanelet::ConstLanelets & lanelets)
+std::vector<lanelet::Id> getIds(const lanelet::ConstLanelets & lanelets)
 {
-  std::vector<uint64_t> ids;
+  std::vector<lanelet::Id> ids;
   ids.reserve(lanelets.size());
   for (const auto & llt : lanelets) {
     ids.push_back(llt.id());
@@ -1274,7 +1274,7 @@ lanelet::ConstLanelets getCurrentLanesFromPath(
   const auto & current_pose = planner_data->self_odometry->pose.pose;
   const auto & p = planner_data->parameters;
 
-  std::set<uint64_t> lane_ids;
+  std::set<lanelet::Id> lane_ids;
   for (const auto & p : path.points) {
     for (const auto & id : p.lane_ids) {
       lane_ids.insert(id);

planning/behavior_velocity_blind_spot_module/src/scene.cpp

@@ -629,7 +629,7 @@ bool BlindSpotModule::isTargetObjectType(
 }
 
 boost::optional<geometry_msgs::msg::Pose> BlindSpotModule::getStartPointFromLaneLet(
-  const int lane_id) const
+  const lanelet::Id lane_id) const
 {
   lanelet::ConstLanelet lanelet =
     planner_data_->route_handler_->getLaneletMapPtr()->laneletLayer.get(lane_id);
@@ -653,7 +653,7 @@ boost::optional<geometry_msgs::msg::Pose> BlindSpotModule::getStartPointFromLane
 
 lanelet::ConstLanelets BlindSpotModule::getStraightLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
-  const int lane_id)
+  const lanelet::Id lane_id)
 {
   lanelet::ConstLanelets straight_lanelets;
   const auto intersection_lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);

planning/behavior_velocity_blind_spot_module/src/scene.hpp

@@ -201,14 +201,15 @@ class BlindSpotModule : public SceneModuleInterface
    * @param lane_id lane id of objective lanelet
    * @return end point of lanelet
    */
-  boost::optional<geometry_msgs::msg::Pose> getStartPointFromLaneLet(const int lane_id) const;
+  boost::optional<geometry_msgs::msg::Pose> getStartPointFromLaneLet(
+    const lanelet::Id lane_id) const;
 
   /**
    * @brief get straight lanelets in intersection
    */
   lanelet::ConstLanelets getStraightLanelets(
     lanelet::LaneletMapConstPtr lanelet_map_ptr,
-    lanelet::routing::RoutingGraphPtr routing_graph_ptr, const int lane_id);
+    lanelet::routing::RoutingGraphPtr routing_graph_ptr, const lanelet::Id lane_id);
 
   /**
    * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr.

planning/behavior_velocity_crosswalk_module/include/behavior_velocity_crosswalk_module/util.hpp

@@ -105,7 +105,7 @@ std::vector<geometry_msgs::msg::Point> getLinestringIntersects(
   const geometry_msgs::msg::Point & ego_pos, const size_t max_num);
 
 std::optional<lanelet::ConstLineString3d> getStopLineFromMap(
-  const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const std::string & attribute_name);
 }  // namespace behavior_velocity_planner
 

planning/behavior_velocity_crosswalk_module/src/util.cpp

@@ -209,7 +209,7 @@ std::vector<geometry_msgs::msg::Point> getLinestringIntersects(
 }
 
 std::optional<lanelet::ConstLineString3d> getStopLineFromMap(
-  const int lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
+  const lanelet::Id lane_id, const lanelet::LaneletMapPtr & lanelet_map_ptr,
   const std::string & attribute_name)
 {
   lanelet::ConstLanelet lanelet = lanelet_map_ptr->laneletLayer.get(lane_id);

planning/behavior_velocity_intersection_module/src/scene_intersection.cpp

@@ -95,7 +95,7 @@ static bool isTargetCollisionVehicleType(
 IntersectionModule::IntersectionModule(
   const int64_t module_id, const int64_t lane_id,
   [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
-  const PlannerParam & planner_param, const std::set<int> & associative_ids,
+  const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
   const std::string & turn_direction, const bool has_traffic_light,
   const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node,
   const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)

planning/behavior_velocity_intersection_module/src/scene_intersection.hpp

@@ -259,7 +259,7 @@ class IntersectionModule : public SceneModuleInterface
 
   IntersectionModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
-    const PlannerParam & planner_param, const std::set<int> & associative_ids,
+    const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
     const std::string & turn_direction, const bool has_traffic_light,
     const bool enable_occlusion_detection, const bool is_private_area, rclcpp::Node & node,
     const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
@@ -273,7 +273,7 @@ class IntersectionModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   motion_utils::VirtualWalls createVirtualWalls() override;
 
-  const std::set<int> & getAssociativeIds() const { return associative_ids_; }
+  const std::set<lanelet::Id> & getAssociativeIds() const { return associative_ids_; }
 
   UUID getOcclusionUUID() const { return occlusion_uuid_; }
   bool getOcclusionSafety() const { return occlusion_safety_; }
@@ -284,7 +284,7 @@ class IntersectionModule : public SceneModuleInterface
 private:
   rclcpp::Node & node_;
   const int64_t lane_id_;
-  const std::set<int> associative_ids_;
+  const std::set<lanelet::Id> associative_ids_;
   const std::string turn_direction_;
   const bool has_traffic_light_;
 

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.cpp

@@ -39,7 +39,7 @@ namespace bg = boost::geometry;
 MergeFromPrivateRoadModule::MergeFromPrivateRoadModule(
   const int64_t module_id, const int64_t lane_id,
   [[maybe_unused]] std::shared_ptr<const PlannerData> planner_data,
-  const PlannerParam & planner_param, const std::set<int> & associative_ids,
+  const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
   const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),

planning/behavior_velocity_intersection_module/src/scene_merge_from_private_road.hpp

@@ -66,7 +66,7 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
 
   MergeFromPrivateRoadModule(
     const int64_t module_id, const int64_t lane_id, std::shared_ptr<const PlannerData> planner_data,
-    const PlannerParam & planner_param, const std::set<int> & associative_ids,
+    const PlannerParam & planner_param, const std::set<lanelet::Id> & associative_ids,
     const rclcpp::Logger logger, const rclcpp::Clock::SharedPtr clock);
 
   /**
@@ -78,11 +78,11 @@ class MergeFromPrivateRoadModule : public SceneModuleInterface
   visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
   motion_utils::VirtualWalls createVirtualWalls() override;
 
-  const std::set<int> & getAssociativeIds() const { return associative_ids_; }
+  const std::set<lanelet::Id> & getAssociativeIds() const { return associative_ids_; }
 
 private:
   const int64_t lane_id_;
-  const std::set<int> associative_ids_;
+  const std::set<lanelet::Id> associative_ids_;
 
   autoware_auto_planning_msgs::msg::PathWithLaneId extractPathNearExitOfPrivateRoad(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const double extend_length);

planning/behavior_velocity_intersection_module/src/util.cpp

@@ -117,7 +117,8 @@ static std::optional<size_t> insertPointIndex(
 }
 
 bool hasLaneIds(
-  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set<int> & ids)
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p,
+  const std::set<lanelet::Id> & ids)
 {
   for (const auto & pid : p.lane_ids) {
     if (ids.find(pid) != ids.end()) {
@@ -128,7 +129,7 @@ bool hasLaneIds(
 }
 
 std::optional<std::pair<size_t, size_t>> findLaneIdsInterval(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set<int> & ids)
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set<lanelet::Id> & ids)
 {
   bool found = false;
   size_t start = 0;
@@ -684,7 +685,7 @@ mergeLaneletsByTopologicalSort(
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path,
-  const std::set<int> & associative_ids, const double detection_area_length,
+  const std::set<lanelet::Id> & associative_ids, const double detection_area_length,
   const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle)
 {
   const auto turn_direction = assigned_lanelet.attributeOr("turn_direction", "else");
@@ -1098,7 +1099,7 @@ std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
 }
 
 std::optional<InterpolatedPathInfo> generateInterpolatedPath(
-  const int lane_id, const std::set<int> & associative_lane_ids,
+  const lanelet::Id lane_id, const std::set<lanelet::Id> & associative_lane_ids,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds,
   const rclcpp::Logger logger)
 {
@@ -1315,7 +1316,7 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const lanelet::Id lane_id,
-  const std::set<int> & associative_ids, const size_t closest_idx,
+  const std::set<lanelet::Id> & associative_ids, const size_t closest_idx,
   const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
   const double intersection_velocity, const double minimum_ego_velocity,
   const bool use_upstream_velocity, const double minimum_upstream_velocity,
@@ -1496,7 +1497,7 @@ void IntersectionLanelets::update(
 }
 
 static lanelet::ConstLanelets getPrevLanelets(
-  const lanelet::ConstLanelets & lanelets_on_path, const std::set<int> & associative_ids)
+  const lanelet::ConstLanelets & lanelets_on_path, const std::set<lanelet::Id> & associative_ids)
 {
   lanelet::ConstLanelets previous_lanelets;
   for (const auto & ll : lanelets_on_path) {
@@ -1541,7 +1542,8 @@ lanelet::ConstLanelet generatePathLanelet(
 
 std::optional<PathLanelets> generatePathLanelets(
   const lanelet::ConstLanelets & lanelets_on_path,
-  const util::InterpolatedPathInfo & interpolated_path_info, const std::set<int> & associative_ids,
+  const util::InterpolatedPathInfo & interpolated_path_info,
+  const std::set<lanelet::Id> & associative_ids,
   const lanelet::CompoundPolygon3d & first_conflicting_area,
   const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
   const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,

planning/behavior_velocity_intersection_module/src/util.hpp

@@ -40,17 +40,18 @@ std::optional<size_t> insertPoint(
   autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
 
 bool hasLaneIds(
-  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p, const std::set<int> & ids);
+  const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p,
+  const std::set<lanelet::Id> & ids);
 std::optional<std::pair<size_t, size_t>> findLaneIdsInterval(
-  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set<int> & ids);
+  const autoware_auto_planning_msgs::msg::PathWithLaneId & p, const std::set<lanelet::Id> & ids);
 
 /**
  * @brief get objective polygons for detection area
  */
 IntersectionLanelets getObjectiveLanelets(
   lanelet::LaneletMapConstPtr lanelet_map_ptr, lanelet::routing::RoutingGraphPtr routing_graph_ptr,
   const lanelet::ConstLanelet assigned_lanelet, const lanelet::ConstLanelets & lanelets_on_path,
-  const std::set<int> & associative_ids, const double detection_area_length,
+  const std::set<lanelet::Id> & associative_ids, const double detection_area_length,
   const double occlusion_detection_area_length, const bool consider_wrong_direction_vehicle);
 
 /**
@@ -117,7 +118,7 @@ std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
   const double curvature_threshold, const double curvature_calculation_ds);
 
 std::optional<InterpolatedPathInfo> generateInterpolatedPath(
-  const int lane_id, const std::set<int> & associative_lane_ids,
+  const lanelet::Id lane_id, const std::set<lanelet::Id> & associative_lane_ids,
   const autoware_auto_planning_msgs::msg::PathWithLaneId & input_path, const double ds,
   const rclcpp::Logger logger);
 
@@ -150,7 +151,7 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const lanelet::Id lane_id,
-  const std::set<int> & associative_ids, const size_t closest_idx,
+  const std::set<lanelet::Id> & associative_ids, const size_t closest_idx,
   const size_t last_intersection_stop_line_candidate_idx, const double time_delay,
   const double intersection_velocity, const double minimum_ego_velocity,
   const bool use_upstream_velocity, const double minimum_upstream_velocity,
@@ -166,7 +167,8 @@ lanelet::ConstLanelet generatePathLanelet(
 
 std::optional<PathLanelets> generatePathLanelets(
   const lanelet::ConstLanelets & lanelets_on_path,
-  const util::InterpolatedPathInfo & interpolated_path_info, const std::set<int> & associative_ids,
+  const util::InterpolatedPathInfo & interpolated_path_info,
+  const std::set<lanelet::Id> & associative_ids,
   const lanelet::CompoundPolygon3d & first_conflicting_area,
   const std::vector<lanelet::CompoundPolygon3d> & conflicting_areas,
   const std::optional<lanelet::CompoundPolygon3d> & first_attention_area,

planning/behavior_velocity_intersection_module/src/util_type.hpp

@@ -64,8 +64,8 @@ struct InterpolatedPathInfo
 {
   autoware_auto_planning_msgs::msg::PathWithLaneId path;
   double ds{0.0};
-  int lane_id{0};
-  std::set<int> associative_lane_ids{};
+  lanelet::Id lane_id{0};
+  std::set<lanelet::Id> associative_lane_ids{};
   std::optional<std::pair<size_t, size_t>> lane_id_interval{std::nullopt};
 };
 
@@ -175,7 +175,8 @@ struct PathLanelets
   lanelet::ConstLanelets all;
   lanelet::ConstLanelets
     conflicting_interval_and_remaining;  // the left/right-most interval of path conflicting with
-                                         // conflicting lanelets plus the next lane part of the path
+                                         // conflicting lanelets plus the next lane part of the
+                                         // path
 };
 
 struct TargetObject

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/utilization/util.hpp

@@ -224,13 +224,13 @@ boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   @brief return 'associative' lanes in the intersection. 'associative' means that a lane shares same
   or lane-changeable parent lanes with `lane` and has same turn_direction value.
  */
-std::set<int> getAssociativeIntersectionLanelets(
+std::set<lanelet::Id> getAssociativeIntersectionLanelets(
   lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map,
   const lanelet::routing::RoutingGraphPtr routing_graph);
 
 template <template <class> class Container>
 lanelet::ConstLanelets getConstLaneletsFromIds(
-  lanelet::LaneletMapConstPtr map, const Container<int> & ids)
+  lanelet::LaneletMapConstPtr map, const Container<lanelet::Id> & ids)
 {
   lanelet::ConstLanelets ret{};
   for (const auto & id : ids) {

planning/behavior_velocity_planner_common/src/utilization/util.cpp

@@ -663,7 +663,7 @@ boost::optional<geometry_msgs::msg::Pose> insertStopPoint(
   return tier4_autoware_utils::getPose(output.points.at(insert_idx.get()));
 }
 
-std::set<int> getAssociativeIntersectionLanelets(
+std::set<lanelet::Id> getAssociativeIntersectionLanelets(
   lanelet::ConstLanelet lane, const lanelet::LaneletMapPtr lanelet_map,
   const lanelet::routing::RoutingGraphPtr routing_graph)
 {
@@ -673,12 +673,12 @@ std::set<int> getAssociativeIntersectionLanelets(
   }
 
   const auto parents = routing_graph->previous(lane);
-  std::set<int> parent_neighbors;
+  std::set<lanelet::Id> parent_neighbors;
   for (const auto & parent : parents) {
     const auto neighbors = routing_graph->besides(parent);
     for (const auto & neighbor : neighbors) parent_neighbors.insert(neighbor.id());
   }
-  std::set<int> associative_intersection_lanelets;
+  std::set<lanelet::Id> associative_intersection_lanelets;
   associative_intersection_lanelets.insert(lane.id());
   for (const auto & parent_neighbor_id : parent_neighbors) {
     const auto parent_neighbor = lanelet_map->laneletLayer.get(parent_neighbor_id);

planning/planning_test_utils/include/planning_interface_test_manager/planning_interface_test_manager_utils.hpp

@@ -268,7 +268,7 @@ Scenario makeScenarioMsg(const std::string scenario)
   return scenario_msg;
 }
 
-Pose createPoseFromLaneID(const int & lane_id)
+Pose createPoseFromLaneID(const lanelet::Id & lane_id)
 {
   auto map_bin_msg = makeMapBinMsg();
   // create route_handler
@@ -299,7 +299,7 @@ Pose createPoseFromLaneID(const int & lane_id)
   return middle_pose;
 }
 
-Odometry makeInitialPoseFromLaneId(const int & lane_id)
+Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id)
 {
   Odometry current_odometry;
   current_odometry.pose.pose = createPoseFromLaneID(lane_id);

planning/route_handler/src/route_handler.cpp

@@ -1676,7 +1676,7 @@ PathWithLaneId RouteHandler::getCenterLinePath(
 
   // append a point only when having one point so that yaw calculation would work
   if (reference_path.points.size() == 1) {
-    const int lane_id = static_cast<int>(reference_path.points.front().lane_ids.front());
+    const lanelet::Id lane_id = static_cast<int>(reference_path.points.front().lane_ids.front());
     const auto lanelet = lanelet_map_ptr_->laneletLayer.get(lane_id);
     const auto point = reference_path.points.front().point.pose.position;
     const auto lane_yaw = lanelet::utils::getLaneletAngle(lanelet, point);