chore: sync upstream

localization/ndt_scan_matcher/src/map_update_module.cpp

@@ -163,7 +163,8 @@ bool MapUpdateModule::should_update_map(const geometry_msgs::msg::Point & positi
 void MapUpdateModule::update_map(const geometry_msgs::msg::Point & position)
 {
   auto request = std::make_shared<autoware_map_msgs::srv::GetDifferentialPointCloudMap::Request>();
-  request->area.center = position;
+  request->area.center_x = position.x;
+  request->area.center_y = position.y;
   request->area.radius = dynamic_map_loading_map_radius_;
   request->cached_ids = ndt_ptr_->getCurrentMapIDs();
 

map/map_height_fitter/src/map_height_fitter.cpp

@@ -97,7 +97,8 @@ void MapHeightFitter::Impl::get_partial_point_cloud_map(const Point & point)
   }
 
   const auto req = std::make_shared<autoware_map_msgs::srv::GetPartialPointCloudMap::Request>();
-  req->area.center = point;
+  req->area.center_x = point.x;
+  req->area.center_y = point.y;
   req->area.radius = 50;
 
   RCLCPP_INFO(logger, "Send request to map_loader");

map/map_loader/CMakeLists.txt

@@ -67,7 +67,7 @@ if(BUILD_TESTING)
 
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
-  add_testcase(test/test_sphere_box_overlap.cpp)
+  add_testcase(test/test_cylinder_box_overlap.cpp)
   add_testcase(test/test_replace_with_absolute_path.cpp)
   add_testcase(test/test_load_pcd_metadata.cpp)
   add_testcase(test/test_pointcloud_map_loader_module.cpp)

map/map_loader/include/map_loader/lanelet2_map_loader_node.hpp

@@ -18,28 +18,34 @@
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
+#include <tier4_map_msgs/msg/map_projector_info.hpp>
 
 #include <lanelet2_projection/UTM.h>
 
 #include <memory>
 #include <string>
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
+using tier4_map_msgs::msg::MapProjectorInfo;
 
 class Lanelet2MapLoaderNode : public rclcpp::Node
 {
 public:
   explicit Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options);
 
   static lanelet::LaneletMapPtr load_map(
-    rclcpp::Node & node, const std::string & lanelet2_filename,
-    const std::string & lanelet2_map_projector_type);
+    const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
+    const double & map_origin_lat = 0.0, const double & map_origin_lon = 0.0);
+  static const MapProjectorInfo get_map_projector_type(
+    const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
+    const double & map_origin_lat, const double & map_origin_lon);
   static HADMapBin create_map_bin_msg(
     const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename,
     const rclcpp::Time & now);
 
 private:
   rclcpp::Publisher<HADMapBin>::SharedPtr pub_map_bin_;
+  rclcpp::Publisher<MapProjectorInfo>::SharedPtr pub_map_projector_type_;
 };
 
 #endif  // MAP_LOADER__LANELET2_MAP_LOADER_NODE_HPP_

map/map_loader/package.xml

@@ -27,6 +27,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>tier4_autoware_utils</depend>
+  <depend>tier4_map_msgs</depend>
   <depend>visualization_msgs</depend>
   <depend>yaml-cpp</depend>
 

map/map_loader/src/lanelet2_map_loader/lanelet2_map_loader_node.cpp

@@ -53,9 +53,12 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
   const auto lanelet2_filename = declare_parameter("lanelet2_map_path", "");
   const auto lanelet2_map_projector_type = declare_parameter("lanelet2_map_projector_type", "MGRS");
   const auto center_line_resolution = declare_parameter("center_line_resolution", 5.0);
+  const double map_origin_lat = declare_parameter("latitude", 0.0);
+  const double map_origin_lon = declare_parameter("longitude", 0.0);
 
   // load map from file
-  const auto map = load_map(*this, lanelet2_filename, lanelet2_map_projector_type);
+  const auto map =
+    load_map(lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon);
   if (!map) {
     return;
   }
@@ -66,15 +69,21 @@ Lanelet2MapLoaderNode::Lanelet2MapLoaderNode(const rclcpp::NodeOptions & options
   // create map bin msg
   const auto map_bin_msg = create_map_bin_msg(map, lanelet2_filename, now());
 
+  const auto map_projector_type_msg = get_map_projector_type(
+    lanelet2_filename, lanelet2_map_projector_type, map_origin_lat, map_origin_lon);
   // create publisher and publish
   pub_map_bin_ =
     create_publisher<HADMapBin>("output/lanelet2_map", rclcpp::QoS{1}.transient_local());
   pub_map_bin_->publish(map_bin_msg);
+  // create publisher and publish
+  pub_map_projector_type_ =
+    create_publisher<MapProjectorInfo>("map_projector_type", rclcpp::QoS{1}.transient_local());
+  pub_map_projector_type_->publish(map_projector_type_msg);
 }
 
 lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
-  rclcpp::Node & node, const std::string & lanelet2_filename,
-  const std::string & lanelet2_map_projector_type)
+  const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
+  const double & map_origin_lat, const double & map_origin_lon)
 {
   lanelet::ErrorMessages errors{};
   if (lanelet2_map_projector_type == "MGRS") {
@@ -84,8 +93,6 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
       return map;
     }
   } else if (lanelet2_map_projector_type == "UTM") {
-    const double map_origin_lat = node.declare_parameter("latitude", 0.0);
-    const double map_origin_lon = node.declare_parameter("longitude", 0.0);
     lanelet::GPSPoint position{map_origin_lat, map_origin_lon};
     lanelet::Origin origin{position};
     lanelet::projection::UtmProjector projector{origin};
@@ -130,6 +137,27 @@ lanelet::LaneletMapPtr Lanelet2MapLoaderNode::load_map(
   return nullptr;
 }
 
+const MapProjectorInfo Lanelet2MapLoaderNode::get_map_projector_type(
+  const std::string & lanelet2_filename, const std::string & lanelet2_map_projector_type,
+  const double & map_origin_lat, const double & map_origin_lon)
+{
+  lanelet::ErrorMessages errors{};
+  MapProjectorInfo map_projector_type_msg;
+  if (lanelet2_map_projector_type == "MGRS") {
+    lanelet::projection::MGRSProjector projector{};
+    const lanelet::LaneletMapPtr map = lanelet::load(lanelet2_filename, projector, &errors);
+    map_projector_type_msg.type = "MGRS";
+    map_projector_type_msg.mgrs_grid = projector.getProjectedMGRSGrid();
+  } else if (lanelet2_map_projector_type == "UTM") {
+    map_projector_type_msg.type = "UTM";
+    map_projector_type_msg.map_origin.latitude = map_origin_lat;
+    map_projector_type_msg.map_origin.longitude = map_origin_lon;
+  } else {
+    map_projector_type_msg.type = "local";
+  }
+  return map_projector_type_msg;
+}
+
 HADMapBin Lanelet2MapLoaderNode::create_map_bin_msg(
   const lanelet::LaneletMapPtr map, const std::string & lanelet2_filename, const rclcpp::Time & now)
 {

map/map_loader/src/pointcloud_map_loader/utils.cpp

@@ -63,57 +63,39 @@ std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
   return absolute_path_map;
 }
 
-bool sphereAndBoxOverlapExists(
-  const geometry_msgs::msg::Point center, const double radius, const pcl::PointXYZ box_min_point,
-  const pcl::PointXYZ box_max_point)
+bool cylinderAndBoxOverlapExists(
+  const double center_x, const double center_y, const double radius,
+  const pcl::PointXYZ box_min_point, const pcl::PointXYZ box_max_point)
 {
-  // Collision detection with x-axis plane
+  // Collision detection with x-y plane (circular base of the cylinder)
   if (
-    box_min_point.x - radius <= center.x && center.x <= box_max_point.x + radius &&
-    box_min_point.y <= center.y && center.y <= box_max_point.y && box_min_point.z <= center.z &&
-    center.z <= box_max_point.z) {
+    box_min_point.x - radius <= center_x && center_x <= box_max_point.x + radius &&
+    box_min_point.y - radius <= center_y && center_y <= box_max_point.y + radius) {
     return true;
   }
 
-  // Collision detection with y-axis plane
-  if (
-    box_min_point.x <= center.x && center.x <= box_max_point.x &&
-    box_min_point.y - radius <= center.y && center.y <= box_max_point.y + radius &&
-    box_min_point.z <= center.z && center.z <= box_max_point.z) {
-    return true;
-  }
+  // Collision detection with box edges
+  const double dx0 = center_x - box_min_point.x;
+  const double dx1 = center_x - box_max_point.x;
+  const double dy0 = center_y - box_min_point.y;
+  const double dy1 = center_y - box_max_point.y;
 
-  // Collision detection with z-axis plane
   if (
-    box_min_point.x <= center.x && center.x <= box_max_point.x && box_min_point.y <= center.y &&
-    center.y <= box_max_point.y && box_min_point.z - radius <= center.z &&
-    center.z <= box_max_point.z + radius) {
+    std::hypot(dx0, dy0) <= radius || std::hypot(dx1, dy0) <= radius ||
+    std::hypot(dx0, dy1) <= radius || std::hypot(dx1, dy1) <= radius) {
     return true;
   }
 
-  // Collision detection with box edges
-  const double dx0 = center.x - box_min_point.x;
-  const double dx1 = center.x - box_max_point.x;
-  const double dy0 = center.y - box_min_point.y;
-  const double dy1 = center.y - box_max_point.y;
-  const double dz0 = center.z - box_min_point.z;
-  const double dz1 = center.z - box_max_point.z;
-  if (
-    std::hypot(dx0, dy0, dz0) <= radius || std::hypot(dx1, dy0, dz0) <= radius ||
-    std::hypot(dx0, dy1, dz0) <= radius || std::hypot(dx0, dy0, dz1) <= radius ||
-    std::hypot(dx0, dy1, dz1) <= radius || std::hypot(dx1, dy0, dz1) <= radius ||
-    std::hypot(dx1, dy1, dz0) <= radius || std::hypot(dx1, dy1, dz1) <= radius) {
-    return true;
-  }
   return false;
 }
 
 bool isGridWithinQueriedArea(
   const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata)
 {
-  // Currently, the area load only supports spherical area
-  geometry_msgs::msg::Point center = area.center;
+  // Currently, the area load only supports cylindrical area
+  double center_x = area.center_x;
+  double center_y = area.center_y;
   double radius = area.radius;
-  bool res = sphereAndBoxOverlapExists(center, radius, metadata.min, metadata.max);
+  bool res = cylinderAndBoxOverlapExists(center_x, center_y, radius, metadata.min, metadata.max);
   return res;
 }

map/map_loader/src/pointcloud_map_loader/utils.hpp

@@ -41,9 +41,9 @@ std::map<std::string, PCDFileMetadata> replaceWithAbsolutePath(
   const std::map<std::string, PCDFileMetadata> & pcd_metadata_path,
   const std::vector<std::string> & pcd_paths);
 
-bool sphereAndBoxOverlapExists(
-  const geometry_msgs::msg::Point position, const double radius, const pcl::PointXYZ position_min,
-  const pcl::PointXYZ position_max);
+bool cylinderAndBoxOverlapExists(
+  const double center_x, const double center_y, const double radius,
+  const pcl::PointXYZ position_min, const pcl::PointXYZ position_max);
 bool isGridWithinQueriedArea(
   const autoware_map_msgs::msg::AreaInfo area, const PCDFileMetadata metadata);
 

map/map_loader/test/test_cylinder_box_overlap.cpp

@@ -0,0 +1,84 @@
+// Copyright 2023 The Autoware Contributors
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "../src/pointcloud_map_loader/utils.hpp"
+
+#include <gmock/gmock.h>
+
+TEST(CylinderAndBoxOverlapExists, NoOverlap)
+{
+  // Cylinder: center = (5, 0), radius = 4 - 0.1
+  // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1)
+  double center_x = 5.0;
+  double center_y = 0.0;
+
+  const double radius = 4.0 - 0.1;
+
+  pcl::PointXYZ p_min;
+  p_min.x = -1.0;
+  p_min.y = -1.0;
+  p_min.z = -1.0;
+
+  pcl::PointXYZ p_max;
+  p_max.x = 1.0;
+  p_max.y = 1.0;
+  p_max.z = 1.0;
+
+  bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max);
+  EXPECT_FALSE(result);
+}
+
+TEST(CylinderAndBoxOverlapExists, Overlap1)
+{
+  // Cylinder: center = (0, 0), radius = 5
+  // Box: p_min = (-1, -1, -1), p_max = (1, 1, 1)
+  double center_x = 0.0;
+  double center_y = 0.0;
+  const double radius = 5.0;
+
+  pcl::PointXYZ p_min;
+  p_min.x = -1.0;
+  p_min.y = -1.0;
+  p_min.z = -1.0;
+
+  pcl::PointXYZ p_max;
+  p_max.x = 1.0;
+  p_max.y = 1.0;
+  p_max.z = 1.0;
+
+  bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max);
+  EXPECT_TRUE(result);
+}
+
+TEST(CylinderAndBoxOverlapExists, Overlap2)
+{
+  // Cylinder: center = (0, 0), radius = 5
+  // Box: p_min = (-1, -1, -100), p_max = (1, 1, -99)
+  double center_x = 0.0;
+  double center_y = 0.0;
+  const double radius = 5.0;
+
+  pcl::PointXYZ p_min;
+  p_min.x = -1.0;
+  p_min.y = -1.0;
+  p_min.z = -100.0;
+
+  pcl::PointXYZ p_max;
+  p_max.x = 1.0;
+  p_max.y = 1.0;
+  p_max.z = -99.0;
+
+  bool result = cylinderAndBoxOverlapExists(center_x, center_y, radius, p_min, p_max);
+  EXPECT_TRUE(result);
+}

map/map_loader/test/test_differential_map_loader_module.cpp

@@ -73,9 +73,8 @@ TEST_F(TestDifferentialMapLoaderModule, LoadDifferentialPCDFiles)
 
   // Prepare a request for the service
   auto request = std::make_shared<GetDifferentialPointCloudMap::Request>();
-  request->area.center.x = 0;
-  request->area.center.y = 0;
-  request->area.center.z = 0;
+  request->area.center_x = 0;
+  request->area.center_y = 0;
   request->area.radius = 2;
   request->cached_ids.clear();
 

map/map_loader/test/test_partial_map_loader_module.cpp

@@ -71,9 +71,8 @@ TEST_F(TestPartialMapLoaderModule, LoadPartialPCDFiles)
 
   // Prepare a request for the service
   auto request = std::make_shared<GetPartialPointCloudMap::Request>();
-  request->area.center.x = 0;
-  request->area.center.y = 0;
-  request->area.center.z = 0;
+  request->area.center_x = 0;
+  request->area.center_y = 0;
   request->area.radius = 2;
 
   // Call the service