chore: sync upstream

common/component_interface_specs/include/component_interface_specs/map.hpp

@@ -25,7 +25,7 @@ namespace map_interface
 struct MapProjectorInfo
 {
   using Message = tier4_map_msgs::msg::MapProjectorInfo;
-  static constexpr char name[] = "/map/map_projector_type";
+  static constexpr char name[] = "/map/map_projector_info";
   static constexpr size_t depth = 1;
   static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
   static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;

map/map_loader/README.md

@@ -140,7 +140,7 @@ Please see [the description of `GetSelectedPointCloudMap.srv`](https://github.co
 ### Feature
 
 lanelet2_map_loader loads Lanelet2 file and publishes the map data as autoware_auto_mapping_msgs/HADMapBin message.
-The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_type` from `map_projection_loader`.
+The node projects lan/lon coordinates into arbitrary coordinates defined in `/map/map_projector_info` from `map_projection_loader`.
 The node supports the following three types of coordinate systems:
 
 - MGRS

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -47,7 +47,7 @@ class Lanelet2MapLoaderNode : public rclcpp::Node
 
   void on_map_projector_info(const MapProjectorInfo::Message::ConstSharedPtr msg);
 
-  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_type_;
+  component_interface_utils::Subscription<MapProjectorInfo>::SharedPtr sub_map_projector_info_;
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
 };
 

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -55,7 +55,7 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
 
   // subscription
   adaptor.init_sub(
-    sub_map_projector_type_,
+    sub_map_projector_info_,
     [this](const MapProjectorInfo::Message::ConstSharedPtr msg) { on_map_projector_info(msg); });
 
   declare_parameter("lanelet2_map_path", "");

map/map_projection_loader/test/test_node_load_local_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map/map_projector_type",
+            "/map/map_projector_info",
             self.callback,
             custom_qos_profile,
         )

map/map_projection_loader/test/test_node_load_mgrs_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map/map_projector_type",
+            "/map/map_projector_info",
             self.callback,
             custom_qos_profile,
         )

map/map_projection_loader/test/test_node_load_utm_from_yaml.test.py

@@ -110,7 +110,7 @@ def test_node_link(self):
         # Create subscription to map_projector_info topic
         subscription = self.test_node.create_subscription(
             MapProjectorInfo,
-            "/map/map_projector_type",
+            "/map/map_projector_info",
             self.callback,
             custom_qos_profile,
         )

perception/compare_map_segmentation/include/compare_map_segmentation/voxel_grid_map_loader.hpp

@@ -260,7 +260,8 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
       int index = static_cast<int>(
         std::floor((kv.second.min_b_x - origin_x_) / map_grid_size_x_) +
         map_grids_x_ * std::floor((kv.second.min_b_y - origin_y_) / map_grid_size_y_));
-      if (index >= map_grids_x_ * map_grids_y_) {
+      // TODO(1222-takeshi): check if index is valid
+      if (index >= map_grids_x_ * map_grids_y_ || index < 0) {
         continue;
       }
       current_voxel_grid_array_.at(index) = std::make_shared<MapGridVoxelInfo>(kv.second);

planning/route_handler/src/route_handler.cpp

@@ -969,13 +969,18 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getRightLanelet(
     return adjacent_right_lane;
   }
 
-  lanelet::ConstLanelet next_lanelet;
-  if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
+  lanelet::ConstLanelets prev_lanelet;
+  if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
     return adjacent_right_lane;
   }
 
-  lanelet::ConstLanelets prev_lanelet;
-  if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
+  lanelet::ConstLanelet next_lanelet;
+  if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
+    for (const auto & lane : getNextLanelets(prev_lanelet.front())) {
+      if (lanelet.rightBound().back().id() == lane.leftBound().back().id()) {
+        return lane;
+      }
+    }
     return adjacent_right_lane;
   }
 
@@ -1036,13 +1041,18 @@ boost::optional<lanelet::ConstLanelet> RouteHandler::getLeftLanelet(
     return adjacent_left_lane;
   }
 
-  lanelet::ConstLanelet next_lanelet;
-  if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
+  lanelet::ConstLanelets prev_lanelet;
+  if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
     return adjacent_left_lane;
   }
 
-  lanelet::ConstLanelets prev_lanelet;
-  if (!getPreviousLaneletsWithinRoute(lanelet, &prev_lanelet)) {
+  lanelet::ConstLanelet next_lanelet;
+  if (!getNextLaneletWithinRoute(lanelet, &next_lanelet)) {
+    for (const auto & lane : getNextLanelets(prev_lanelet.front())) {
+      if (lanelet.leftBound().back().id() == lane.rightBound().back().id()) {
+        return lane;
+      }
+    }
     return adjacent_left_lane;
   }