Merge branch 'main' into feature/warn_missing_pcd

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

@@ -15,9 +15,6 @@
 #ifndef AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_
 #define AUTOWARE_TEST_UTILS__AUTOWARE_TEST_UTILS_HPP_
 
-#include "ament_index_cpp/get_package_share_directory.hpp"
-#include "rclcpp/rclcpp.hpp"
-
 #include <component_interface_specs/planning.hpp>
 #include <lanelet2_extension/io/autoware_osm_parser.hpp>
 #include <lanelet2_extension/projection/mgrs_projector.hpp>
@@ -51,7 +48,6 @@
 #include <limits>
 #include <memory>
 #include <string>
-#include <utility>
 #include <vector>
 
 namespace autoware::test_utils
@@ -174,6 +170,45 @@ LaneletRoute makeNormalRoute();
  */
 OccupancyGrid makeCostMapMsg(size_t width = 150, size_t height = 150, double resolution = 0.2);
 
+/**
+ * @brief Get the absolute path to the lanelet map file.
+ *
+ * This function retrieves the absolute path to a lanelet map file located in the
+ * package's share directory under the "test_map" folder.
+ *
+ * @param package_name The name of the package containing the map file.
+ * @param map_filename The name of the map file.
+ * @return A string representing the absolute path to the lanelet map file.
+ */
+std::string get_absolute_path_to_lanelet_map(
+  const std::string & package_name, const std::string & map_filename);
+
+/**
+ * @brief Get the absolute path to the route file.
+ *
+ * This function retrieves the absolute path to a route file located in the
+ * package's share directory under the "test_route" folder.
+ *
+ * @param package_name The name of the package containing the route file.
+ * @param route_filename The name of the route file.
+ * @return A string representing the absolute path to the route file.
+ */
+std::string get_absolute_path_to_route(
+  const std::string & package_name, const std::string & route_filename);
+
+/**
+ * @brief Get the absolute path to the config file.
+ *
+ * This function retrieves the absolute path to a route file located in the
+ * package's share directory under the "config" folder.
+ *
+ * @param package_name The name of the package containing the route file.
+ * @param route_filename The name of the config file.
+ * @return A string representing the absolute path to the config file.
+ */
+std::string get_absolute_path_to_config(
+  const std::string & package_name, const std::string & config_filename);
+
 /**
  * @brief Creates a LaneletMapBin message from a Lanelet map file.
  *

common/autoware_test_utils/src/autoware_test_utils.cpp

@@ -12,7 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "autoware_test_utils/autoware_test_utils.hpp"
+#include <ament_index_cpp/get_package_share_directory.hpp>
+#include <autoware_test_utils/autoware_test_utils.hpp>
+#include <rclcpp/clock.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
+#include <rclcpp/logging.hpp>
+#include <rclcpp/node.hpp>
+
+#include <utility>
+
 namespace autoware::test_utils
 {
 
@@ -103,6 +111,27 @@ OccupancyGrid makeCostMapMsg(size_t width, size_t height, double resolution)
   return costmap_msg;
 }
 
+std::string get_absolute_path_to_lanelet_map(
+  const std::string & package_name, const std::string & map_filename)
+{
+  const auto dir = ament_index_cpp::get_package_share_directory(package_name);
+  return dir + "/test_map/" + map_filename;
+}
+
+std::string get_absolute_path_to_route(
+  const std::string & package_name, const std::string & route_filename)
+{
+  const auto dir = ament_index_cpp::get_package_share_directory(package_name);
+  return dir + "/test_route/" + route_filename;
+}
+
+std::string get_absolute_path_to_config(
+  const std::string & package_name, const std::string & config_filename)
+{
+  const auto dir = ament_index_cpp::get_package_share_directory(package_name);
+  return dir + "/config/" + config_filename;
+}
+
 LaneletMapBin make_map_bin_msg(
   const std::string & absolute_path, const double center_line_resolution)
 {
@@ -122,12 +151,8 @@ LaneletMapBin make_map_bin_msg(
 
 LaneletMapBin makeMapBinMsg()
 {
-  const auto autoware_test_utils_dir =
-    ament_index_cpp::get_package_share_directory("autoware_test_utils");
-  const auto lanelet2_path = autoware_test_utils_dir + "/test_map/lanelet2_map.osm";
-  double center_line_resolution = 5.0;
-
-  return make_map_bin_msg(lanelet2_path, center_line_resolution);
+  return make_map_bin_msg(
+    get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm"));
 }
 
 Odometry makeOdometry(const double shift)
@@ -251,9 +276,8 @@ void updateNodeOptions(
 
 PathWithLaneId loadPathWithLaneIdInYaml()
 {
-  const auto autoware_test_utils_dir =
-    ament_index_cpp::get_package_share_directory("autoware_test_utils");
-  const auto yaml_path = autoware_test_utils_dir + "/config/path_with_lane_id_data.yaml";
+  const auto yaml_path =
+    get_absolute_path_to_config("autoware_test_utils", "path_with_lane_id_data.yaml");
   YAML::Node yaml_node = YAML::LoadFile(yaml_path);
   PathWithLaneId path_msg;
 

localization/localization_util/CMakeLists.txt

@@ -7,6 +7,7 @@ autoware_package()
 ament_auto_add_library(localization_util SHARED
   src/util_func.cpp
   src/smart_pose_buffer.cpp
+  src/tree_structured_parzen_estimator.cpp
 )
 
 if(BUILD_TESTING)
@@ -15,6 +16,11 @@ if(BUILD_TESTING)
     test/test_smart_pose_buffer.cpp
     src/smart_pose_buffer.cpp
   )
+
+  ament_auto_add_gtest(test_tpe
+    test/test_tpe.cpp
+    src/tree_structured_parzen_estimator.cpp
+  )
 endif()
 
 ament_auto_package(

localization/localization_util/README.md

@@ -1,5 +1,5 @@
 # localization_util
 
-`localization_util`` is a localization utility package.
+`localization_util` is a localization utility package.
 
 This package does not have a node, it is just a library.

localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp → localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
-#define TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#ifndef LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#define LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
 
 /*
 A implementation of tree-structured parzen estimator (TPE)
@@ -81,4 +81,4 @@ class TreeStructuredParzenEstimator
   Input base_stddev_;
 };
 
-#endif  // TREE_STRUCTURED_PARZEN_ESTIMATOR__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_
+#endif  // LOCALIZATION_UTIL__TREE_STRUCTURED_PARZEN_ESTIMATOR_HPP_

localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp → localization/localization_util/src/tree_structured_parzen_estimator.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp"
+#include "localization_util/tree_structured_parzen_estimator.hpp"
 
 #include <algorithm>
 #include <cassert>

localization/tree_structured_parzen_estimator/test/test_tpe.cpp → localization/localization_util/test/test_tpe.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp"
+#include "localization_util/tree_structured_parzen_estimator.hpp"
 
 #include <gtest/gtest.h>
 

localization/ndt_scan_matcher/package.xml

@@ -40,7 +40,6 @@
   <depend>tier4_autoware_utils</depend>
   <depend>tier4_debug_msgs</depend>
   <depend>tier4_localization_msgs</depend>
-  <depend>tree_structured_parzen_estimator</depend>
   <depend>visualization_msgs</depend>
 
   <test_depend>ament_cmake_cppcheck</test_depend>

localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp

@@ -15,9 +15,9 @@
 #include "ndt_scan_matcher/ndt_scan_matcher_core.hpp"
 
 #include "localization_util/matrix_type.hpp"
+#include "localization_util/tree_structured_parzen_estimator.hpp"
 #include "localization_util/util_func.hpp"
 #include "ndt_scan_matcher/particle.hpp"
-#include "tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp"
 
 #include <tier4_autoware_utils/geometry/geometry.hpp>
 #include <tier4_autoware_utils/transform/transforms.hpp>

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp

@@ -30,7 +30,7 @@ struct GridKey
   GridKey() : x(0), y(0) {}
   GridKey(float x, float y) : x(std::floor(x / unit_length)), y(std::floor(y / unit_length)) {}
 
-  pcl::PointXYZ get_center_point() const
+  [[nodiscard]] pcl::PointXYZ get_center_point() const
   {
     pcl::PointXYZ xyz;
     xyz.x = unit_length * (static_cast<float>(x) + 0.5f);

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp

@@ -20,6 +20,11 @@
 
 #include <pcl_conversions/pcl_conversions.h>
 
+#include <memory>
+#include <string>
+#include <unordered_map>
+#include <vector>
+
 namespace pose_estimator_arbiter::rule_helper
 {
 PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold)

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp

@@ -24,6 +24,8 @@
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 
+#include <string>
+
 namespace pose_estimator_arbiter::rule_helper
 {
 class PcdOccupancy
@@ -34,7 +36,7 @@ class PcdOccupancy
 public:
   explicit PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold);
 
-  MarkerArray debug_marker_array() const;
+  [[nodiscard]] MarkerArray debug_marker_array() const;
   void init(PointCloud2::ConstSharedPtr msg);
   bool ndt_can_operate(
     const geometry_msgs::msg::Point & position, std::string * optional_message = nullptr) const;

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp

@@ -33,14 +33,14 @@ using BoostPolygon = boost::geometry::model::polygon<BoostPoint>;
 
 struct PoseEstimatorArea::Impl
 {
-  explicit Impl(rclcpp::Logger logger) : logger_(logger) {}
+  explicit Impl(const rclcpp::Logger & logger) : logger_(logger) {}
   std::multimap<std::string, BoostPolygon> bounding_boxes_;
 
   void init(HADMapBin::ConstSharedPtr msg);
-  bool within(
+  [[nodiscard]] bool within(
     const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const;
 
-  MarkerArray debug_marker_array() const { return marker_array_; }
+  [[nodiscard]] MarkerArray debug_marker_array() const { return marker_array_; }
 
 private:
   rclcpp::Logger logger_;
@@ -105,7 +105,7 @@ void PoseEstimatorArea::Impl::init(HADMapBin::ConstSharedPtr msg)
     marker.color.set__r(1.0f).set__g(1.0f).set__b(0.0f).set__a(1.0f);
     marker.ns = subtype;
     marker.header.frame_id = "map";
-    marker.id = marker_array_.markers.size();
+    marker.id = static_cast<int>(marker_array_.markers.size());
 
     // Enqueue the vertices of the polygon to bounding box and visualization marker
     BoostPolygon poly;

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp

@@ -40,10 +40,10 @@ class PoseEstimatorArea
   // This is assumed to be called only once in the vector map callback.
   void init(const HADMapBin::ConstSharedPtr msg);
 
-  bool within(
+  [[nodiscard]] bool within(
     const geometry_msgs::msg::Point & point, const std::string & pose_estimator_name) const;
 
-  MarkerArray debug_marker_array() const;
+  [[nodiscard]] MarkerArray debug_marker_array() const;
 
 private:
   struct Impl;

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp

@@ -16,12 +16,16 @@
 
 #include <tier4_autoware_utils/ros/parameter.hpp>
 
+#include <utility>
+
 namespace pose_estimator_arbiter::switch_rule
 {
 PcdMapBasedRule::PcdMapBasedRule(
-  rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
-  const std::shared_ptr<const SharedData> shared_data)
-: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data)
+  rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
+  std::shared_ptr<const SharedData> shared_data)
+: BaseSwitchRule(node),
+  running_estimator_list_(std::move(running_estimator_list)),
+  shared_data_(std::move(shared_data))
 {
   const int pcd_density_upper_threshold =
     tier4_autoware_utils::getOrDeclareParameter<int>(node, "pcd_density_upper_threshold");
@@ -63,14 +67,14 @@ std::unordered_map<PoseEstimatorType, bool> PcdMapBasedRule::update()
       {PoseEstimatorType::eagleye, false},
       {PoseEstimatorType::artag, false},
     };
-  } else {
-    debug_string_ = "Enable yabloc: ";
-    return {
-      {PoseEstimatorType::ndt, false},
-      {PoseEstimatorType::yabloc, true},
-      {PoseEstimatorType::eagleye, false},
-      {PoseEstimatorType::artag, false}};
   }
+
+  debug_string_ = "Enable yabloc: ";
+  return {
+    {PoseEstimatorType::ndt, false},
+    {PoseEstimatorType::yabloc, true},
+    {PoseEstimatorType::eagleye, false},
+    {PoseEstimatorType::artag, false}};
 }
 
 }  // namespace pose_estimator_arbiter::switch_rule

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp

@@ -31,7 +31,7 @@ class PcdMapBasedRule : public BaseSwitchRule
 {
 public:
   PcdMapBasedRule(
-    rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
+    rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
     const std::shared_ptr<const SharedData> shared_data);
 
   std::unordered_map<PoseEstimatorType, bool> update() override;

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp

@@ -16,12 +16,16 @@
 
 #include <magic_enum.hpp>
 
+#include <utility>
+
 namespace pose_estimator_arbiter::switch_rule
 {
 VectorMapBasedRule::VectorMapBasedRule(
-  rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
-  const std::shared_ptr<const SharedData> shared_data)
-: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data)
+  rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
+  std::shared_ptr<const SharedData> shared_data)
+: BaseSwitchRule(node),
+  running_estimator_list_(std::move(running_estimator_list)),
+  shared_data_(std::move(shared_data))
 {
   pose_estimator_area_ = std::make_unique<rule_helper::PoseEstimatorArea>(node.get_logger());
 

localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp

@@ -32,8 +32,8 @@ class VectorMapBasedRule : public BaseSwitchRule
 {
 public:
   VectorMapBasedRule(
-    rclcpp::Node & node, const std::unordered_set<PoseEstimatorType> & running_estimator_list,
-    const std::shared_ptr<const SharedData> shared_data);
+    rclcpp::Node & node, std::unordered_set<PoseEstimatorType> running_estimator_list,
+    std::shared_ptr<const SharedData> shared_data);
 
   std::unordered_map<PoseEstimatorType, bool> update() override;