diff --git a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp b/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp index 6b77bb4c24931..093449176b047 100644 --- a/common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp +++ b/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 #include #include @@ -51,7 +48,6 @@ #include #include #include -#include #include 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. * diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index c7435be5afcec..563350dbe53cc 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/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 +#include +#include +#include +#include +#include + +#include + 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; diff --git a/localization/localization_util/CMakeLists.txt b/localization/localization_util/CMakeLists.txt index fe65077d89649..9a9086314f42e 100644 --- a/localization/localization_util/CMakeLists.txt +++ b/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( diff --git a/localization/localization_util/README.md b/localization/localization_util/README.md index a3e980464d0c6..a7daea33e0273 100644 --- a/localization/localization_util/README.md +++ b/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. diff --git a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp b/localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp similarity index 90% rename from localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp rename to localization/localization_util/include/localization_util/tree_structured_parzen_estimator.hpp index 3853fed2c3943..ee25ac175c0b4 100644 --- a/localization/tree_structured_parzen_estimator/include/tree_structured_parzen_estimator/tree_structured_parzen_estimator.hpp +++ b/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_ diff --git a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp b/localization/localization_util/src/tree_structured_parzen_estimator.cpp similarity index 98% rename from localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp rename to localization/localization_util/src/tree_structured_parzen_estimator.cpp index f7cdcbacd3e04..aab78f0aaec71 100644 --- a/localization/tree_structured_parzen_estimator/src/tree_structured_parzen_estimator.cpp +++ b/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 #include diff --git a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp b/localization/localization_util/test/test_tpe.cpp similarity index 97% rename from localization/tree_structured_parzen_estimator/test/test_tpe.cpp rename to localization/localization_util/test/test_tpe.cpp index 357c86289ef1f..fd9afe8b2a75f 100644 --- a/localization/tree_structured_parzen_estimator/test/test_tpe.cpp +++ b/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 diff --git a/localization/ndt_scan_matcher/package.xml b/localization/ndt_scan_matcher/package.xml index b62e810926331..6b0a44bdf1c55 100644 --- a/localization/ndt_scan_matcher/package.xml +++ b/localization/ndt_scan_matcher/package.xml @@ -40,7 +40,6 @@ tier4_autoware_utils tier4_debug_msgs tier4_localization_msgs - tree_structured_parzen_estimator visualization_msgs ament_cmake_cppcheck diff --git a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp b/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp index e84d6709c3151..f87b1cd8e98a8 100644 --- a/localization/ndt_scan_matcher/src/ndt_scan_matcher_core.cpp +++ b/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 #include diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp index c366a7f02f4fe..b4ad7111e4ba0 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/grid_key.hpp +++ b/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(x) + 0.5f); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp index 72071b23b467f..4b0188a1638f6 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.cpp @@ -20,6 +20,11 @@ #include +#include +#include +#include +#include + namespace pose_estimator_arbiter::rule_helper { PcdOccupancy::PcdOccupancy(int pcd_density_upper_threshold, int pcd_density_lower_threshold) diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp index e5eb4ff091a31..098704e11aba9 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pcd_occupancy.hpp @@ -24,6 +24,8 @@ #include #include +#include + 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; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp index 4eea34f9ae1ee..df7f9c6c1035f 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.cpp +++ b/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; struct PoseEstimatorArea::Impl { - explicit Impl(rclcpp::Logger logger) : logger_(logger) {} + explicit Impl(const rclcpp::Logger & logger) : logger_(logger) {} std::multimap 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(marker_array_.markers.size()); // Enqueue the vertices of the polygon to bounding box and visualization marker BoostPolygon poly; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp index 392219b2281e1..74843c66a4eba 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/rule_helper/pose_estimator_area.hpp +++ b/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; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp index 4c3829316230b..0d8880d5c4d16 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.cpp @@ -16,12 +16,16 @@ #include +#include + namespace pose_estimator_arbiter::switch_rule { PcdMapBasedRule::PcdMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr 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(node, "pcd_density_upper_threshold"); @@ -63,14 +67,14 @@ std::unordered_map 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 diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp index 23fd0f812f700..ab4d18dcad66a 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/pcd_map_based_rule.hpp +++ b/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 & running_estimator_list, + rclcpp::Node & node, std::unordered_set running_estimator_list, const std::shared_ptr shared_data); std::unordered_map update() override; diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp index 675842662acf8..dffb738e87685 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.cpp @@ -16,12 +16,16 @@ #include +#include + namespace pose_estimator_arbiter::switch_rule { VectorMapBasedRule::VectorMapBasedRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list), shared_data_(shared_data) + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr shared_data) +: BaseSwitchRule(node), + running_estimator_list_(std::move(running_estimator_list)), + shared_data_(std::move(shared_data)) { pose_estimator_area_ = std::make_unique(node.get_logger()); diff --git a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp b/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp index e3360f9692f86..83723a346054b 100644 --- a/localization/pose_estimator_arbiter/example_rule/src/pose_estimator_arbiter/switch_rule/vector_map_based_rule.hpp +++ b/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 & running_estimator_list, - const std::shared_ptr shared_data); + rclcpp::Node & node, std::unordered_set running_estimator_list, + std::shared_ptr shared_data); std::unordered_map update() override; diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp index 5febfa403363e..f53e01dc9c548 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_pcd_map_based_rule.cpp @@ -19,10 +19,10 @@ #include -class MockNode : public ::testing::Test +class PcdMapBasedRuleMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -46,10 +46,10 @@ class MockNode : public ::testing::Test std::shared_ptr shared_data_; std::shared_ptr rule_; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, pcdMapBasedRule) +TEST_F(PcdMapBasedRuleMockNode, pcdMapBasedRule) { // Create dummy pcd and set { diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp index beda4d720b7f6..0001ce674a2e8 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_rule_helper.cpp @@ -28,10 +28,10 @@ #include -class MockNode : public ::testing::Test +class RuleHelperMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -39,10 +39,10 @@ class MockNode : public ::testing::Test std::shared_ptr node{nullptr}; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, poseEstimatorArea) +TEST_F(RuleHelperMockNode, poseEstimatorArea) { auto create_polygon3d = []() -> lanelet::Polygon3d { lanelet::Polygon3d polygon; @@ -73,7 +73,7 @@ TEST_F(MockNode, poseEstimatorArea) EXPECT_FALSE(pose_estimator_area.within(Point().set__x(-5).set__y(-5).set__z(0), "yabloc")); } -TEST_F(MockNode, pcdOccupancy) +TEST_F(RuleHelperMockNode, pcdOccupancy) { using pose_estimator_arbiter::rule_helper::PcdOccupancy; const int pcd_density_upper_threshold = 20; @@ -89,7 +89,7 @@ TEST_F(MockNode, pcdOccupancy) EXPECT_FALSE(pcd_occupancy.ndt_can_operate(point, &message)); } -TEST_F(MockNode, gridKey) +TEST_F(RuleHelperMockNode, gridKey) { using pose_estimator_arbiter::rule_helper::GridKey; EXPECT_TRUE(GridKey(10., -5.) == GridKey(10., -10.)); diff --git a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp index 66d867883272b..a4fd91365f433 100644 --- a/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp +++ b/localization/pose_estimator_arbiter/example_rule/test/test_vector_map_based_rule.cpp @@ -24,10 +24,10 @@ #include -class MockNode : public ::testing::Test +class VectorMapBasedRuleMockNode : public ::testing::Test { protected: - virtual void SetUp() + void SetUp() override { rclcpp::init(0, nullptr); node = std::make_shared("test_node"); @@ -49,10 +49,10 @@ class MockNode : public ::testing::Test std::shared_ptr shared_data_; std::shared_ptr rule_; - virtual void TearDown() { rclcpp::shutdown(); } + void TearDown() override { rclcpp::shutdown(); } }; -TEST_F(MockNode, vectorMapBasedRule) +TEST_F(VectorMapBasedRuleMockNode, vectorMapBasedRule) { // Create dummy lanelet2 and set { diff --git a/localization/pose_estimator_arbiter/package.xml b/localization/pose_estimator_arbiter/package.xml index 70896d93d9cb8..134cad7534502 100644 --- a/localization/pose_estimator_arbiter/package.xml +++ b/localization/pose_estimator_arbiter/package.xml @@ -4,7 +4,13 @@ pose_estimator_arbiter 0.1.0 The arbiter of multiple pose estimators + Yamato Ando Kento Yabuuchi + Masahiro Sakamoto + NGUYEN Viet Anh + Taiki Yamada + Shintaro Sakoda + Ryu Yamamoto Apache License 2.0 Kento Yabuuchi diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp index edf12537198a3..d78bfeb05b4f0 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/pose_estimator_type.hpp @@ -18,6 +18,6 @@ namespace pose_estimator_arbiter { enum class PoseEstimatorType : int { ndt = 1, yabloc = 2, eagleye = 4, artag = 8 }; -} +} // namespace pose_estimator_arbiter #endif // POSE_ESTIMATOR_ARBITER__POSE_ESTIMATOR_TYPE_HPP_ diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp index a72dc5585877b..0d3dbfab11cbe 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/shared_data.hpp @@ -30,29 +30,29 @@ namespace pose_estimator_arbiter template struct CallbackInvokingVariable { - CallbackInvokingVariable() {} + CallbackInvokingVariable() = default; - explicit CallbackInvokingVariable(T initial_data) : value(initial_data) {} + explicit CallbackInvokingVariable(T initial_data) : value_(initial_data) {} // Set data and invoke all callbacks void set_and_invoke(T new_value) { - value = new_value; + value_ = new_value; // Call all callbacks with the new value - for (const auto & callback : callbacks) { - callback(value.value()); + for (const auto & callback : callbacks_) { + callback(value_.value()); } } // Same as std::optional::has_value() - bool has_value() const { return value.has_value(); } + bool has_value() const { return value_.has_value(); } // Same as std::optional::value() - const T operator()() const { return value.value(); } + T operator()() const { return value_.value(); } // Register callback function which is invoked when set_and_invoke() is called - void register_callback(std::function callback) const { callbacks.push_back(callback); } + void register_callback(std::function callback) const { callbacks_.push_back(callback); } // Create subscription callback function which is used as below: // auto subscriber = create_subscription("topic_name", 10, @@ -64,10 +64,10 @@ struct CallbackInvokingVariable private: // The latest data - std::optional value{std::nullopt}; + std::optional value_{std::nullopt}; // These functions are expected not to change the value variable - mutable std::vector> callbacks; + mutable std::vector> callbacks_; }; // This structure is handed to several modules as shared_ptr so that all modules can access data. @@ -80,7 +80,7 @@ struct SharedData using HADMapBin = autoware_map_msgs::msg::LaneletMapBin; using InitializationState = autoware_adapi_v1_msgs::msg::LocalizationInitializationState; - SharedData() {} + SharedData() = default; // Used for stoppers CallbackInvokingVariable eagleye_output_pose_cov; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp index fc2242a0b27e9..d64592a12308e 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/base_stopper.hpp @@ -20,6 +20,7 @@ #include #include +#include namespace pose_estimator_arbiter::stopper { @@ -28,11 +29,16 @@ class BaseStopper public: using SharedPtr = std::shared_ptr; - explicit BaseStopper(rclcpp::Node * node, const std::shared_ptr shared_data) - : logger_(node->get_logger()), shared_data_(shared_data) + explicit BaseStopper(rclcpp::Node * node, std::shared_ptr shared_data) + : logger_(node->get_logger()), shared_data_(std::move(shared_data)) { } + virtual ~BaseStopper() = default; + BaseStopper(const BaseStopper & other) = default; + BaseStopper(BaseStopper && other) noexcept = default; + BaseStopper & operator=(const BaseStopper & other) = default; + BaseStopper & operator=(BaseStopper && other) noexcept = default; void enable() { set_enable(true); } void disable() { set_enable(false); } diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp index 72a3f5412add0..f334983f33aad 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_artag.hpp @@ -29,7 +29,7 @@ class StopperArTag : public BaseStopper using SetBool = std_srvs::srv::SetBool; public: - explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperArTag(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { ar_tag_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp index 12bbe8c9769a1..cc800ad6bdee4 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_eagleye.hpp @@ -27,7 +27,8 @@ class StopperEagleye : public BaseStopper using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped; public: - explicit StopperEagleye(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperEagleye( + rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { eagleye_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp index dacea02f77bde..3b7c083ea31e3 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_ndt.hpp @@ -27,7 +27,7 @@ class StopperNdt : public BaseStopper using PointCloud2 = sensor_msgs::msg::PointCloud2; public: - explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperNdt(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { ndt_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp index 2cd8b66f4ffd4..808a5bf9407ca 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/stopper/stopper_yabloc.hpp @@ -29,7 +29,7 @@ class StopperYabLoc : public BaseStopper using SetBool = std_srvs::srv::SetBool; public: - explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr shared_data) + explicit StopperYabLoc(rclcpp::Node * node, const std::shared_ptr & shared_data) : BaseStopper(node, shared_data) { yabloc_is_enabled_ = true; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp index 5770f18efa32c..95d10c2b741a8 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/base_switch_rule.hpp @@ -40,12 +40,16 @@ class BaseSwitchRule } virtual ~BaseSwitchRule() = default; + BaseSwitchRule(const BaseSwitchRule & other) = default; + BaseSwitchRule(BaseSwitchRule && other) noexcept = default; + BaseSwitchRule & operator=(const BaseSwitchRule & other) = default; + BaseSwitchRule & operator=(BaseSwitchRule && other) noexcept = default; virtual std::unordered_map update() = 0; virtual std::string debug_string() { return std::string{}; } virtual MarkerArray debug_marker_array() { return MarkerArray{}; } protected: - rclcpp::Logger get_logger() const { return *logger_ptr_; } + [[nodiscard]] rclcpp::Logger get_logger() const { return *logger_ptr_; } std::shared_ptr logger_ptr_{nullptr}; }; diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp index 2b2e325c3d94b..aebad094a0eca 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.cpp @@ -19,14 +19,15 @@ #include #include #include +#include #include namespace pose_estimator_arbiter::switch_rule { EnableAllRule::EnableAllRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr) -: BaseSwitchRule(node), running_estimator_list_(running_estimator_list) + rclcpp::Node & node, std::unordered_set running_estimator_list, + const std::shared_ptr &) +: BaseSwitchRule(node), running_estimator_list_(std::move(running_estimator_list)) { } diff --git a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp index 568226985b2ff..63142b0e662e1 100644 --- a/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp +++ b/localization/pose_estimator_arbiter/src/pose_estimator_arbiter/switch_rule/enable_all_rule.hpp @@ -29,10 +29,8 @@ class EnableAllRule : public BaseSwitchRule { public: EnableAllRule( - rclcpp::Node & node, const std::unordered_set & running_estimator_list, - const std::shared_ptr shared_data); - - virtual ~EnableAllRule() = default; + rclcpp::Node & node, std::unordered_set running_estimator_list, + const std::shared_ptr & shared_data); std::unordered_map update() override; diff --git a/localization/tree_structured_parzen_estimator/CMakeLists.txt b/localization/tree_structured_parzen_estimator/CMakeLists.txt deleted file mode 100644 index 7b687a12a003c..0000000000000 --- a/localization/tree_structured_parzen_estimator/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(tree_structured_parzen_estimator) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -ament_auto_add_library(tree_structured_parzen_estimator SHARED - src/tree_structured_parzen_estimator.cpp -) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - ament_add_gtest(test_tpe - test/test_tpe.cpp - src/tree_structured_parzen_estimator.cpp - ) - target_include_directories(test_tpe PRIVATE include) - target_link_libraries(test_tpe) -endif() - -ament_auto_package( - INSTALL_TO_SHARE -) diff --git a/localization/tree_structured_parzen_estimator/README.md b/localization/tree_structured_parzen_estimator/README.md deleted file mode 100644 index 2b21e65929485..0000000000000 --- a/localization/tree_structured_parzen_estimator/README.md +++ /dev/null @@ -1,5 +0,0 @@ -# tree_structured_parzen_estimator - -`tree_structured_parzen_estimator`` is a package for black-box optimization. - -This package does not have a node, it is just a library. diff --git a/localization/tree_structured_parzen_estimator/package.xml b/localization/tree_structured_parzen_estimator/package.xml deleted file mode 100644 index 96d2b9ecf54cc..0000000000000 --- a/localization/tree_structured_parzen_estimator/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - tree_structured_parzen_estimator - 0.1.0 - The tree_structured_parzen_estimator package - Yamato Ando - Masahiro Sakamoto - Shintaro Sakoda - Kento Yabuuchi - NGUYEN Viet Anh - Taiki Yamada - Ryu Yamamoto - Apache License 2.0 - Shintaro Sakoda - - ament_cmake_auto - autoware_cmake - - ament_cmake_cppcheck - ament_lint_auto - - - ament_cmake - - diff --git a/planning/autoware_behavior_path_lane_change_module/package.xml b/planning/autoware_behavior_path_lane_change_module/package.xml index dca36f99e3faa..ca338771064b9 100644 --- a/planning/autoware_behavior_path_lane_change_module/package.xml +++ b/planning/autoware_behavior_path_lane_change_module/package.xml @@ -12,6 +12,7 @@ Tomoya Kimura Shumpei Wakabayashi Tomohito Ando + Alqudah Mohammad Apache License 2.0 diff --git a/planning/autoware_route_handler/package.xml b/planning/autoware_route_handler/package.xml index 61a8a43b31461..64cdaf596b52d 100644 --- a/planning/autoware_route_handler/package.xml +++ b/planning/autoware_route_handler/package.xml @@ -10,6 +10,7 @@ Takayuki Murooka Mamoru Sobue Go Sakayori + Alqudah Mohammad Apache License 2.0 diff --git a/planning/behavior_velocity_occlusion_spot_module/plugins.xml b/planning/behavior_velocity_occlusion_spot_module/plugins.xml deleted file mode 100644 index 53a2c460c2e44..0000000000000 --- a/planning/behavior_velocity_occlusion_spot_module/plugins.xml +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/system/default_ad_api/src/vehicle.cpp b/system/default_ad_api/src/vehicle.cpp index e25933e6ca2ad..47932f969c172 100644 --- a/system/default_ad_api/src/vehicle.cpp +++ b/system/default_ad_api/src/vehicle.cpp @@ -167,7 +167,7 @@ void VehicleNode::publish_status() { if ( !steering_status_msgs_ || !gear_status_msgs_ || !turn_indicator_status_msgs_ || - !hazard_light_status_msgs_ || !energy_status_msgs_) + !hazard_light_status_msgs_) return; autoware_ad_api::vehicle::VehicleStatus::Message vehicle_status; @@ -178,7 +178,6 @@ void VehicleNode::publish_status() mapping(turn_indicator_type_, turn_indicator_status_msgs_->report, ApiTurnIndicator::UNKNOWN); vehicle_status.hazard_lights.status = mapping(hazard_light_type_, hazard_light_status_msgs_->report, ApiHazardLight::UNKNOWN); - vehicle_status.energy_percentage = energy_status_msgs_->energy_level; pub_status_->publish(vehicle_status); }