From 41ca7686d7439b5ebb5a9ad903a3bd827289427b Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara <32741405+kyoichi-sugahara@users.noreply.github.com> Date: Thu, 12 Dec 2024 13:34:48 +0900 Subject: [PATCH] refactor(autoware_test_utils): enhance makeMapBinMsg to accept package name and map filename parameters (#9617) * feat: enhance makeMapBinMsg to accept package name and map filename parameters Signed-off-by: kyoichi-sugahara * feat: set default package name to 'autoware_test_utils' in makeMapBinMsg and related functions Signed-off-by: kyoichi-sugahara --------- Signed-off-by: kyoichi-sugahara --- .../autoware_test_utils.hpp | 14 ++++++++++---- .../src/autoware_test_utils.cpp | 5 ++--- .../autoware_planning_test_manager_utils.hpp | 18 ++++++++++++------ 3 files changed, 24 insertions(+), 13 deletions(-) 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 3721dc67526b5..b98af2133f175 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 @@ -213,15 +213,21 @@ LaneletMapBin make_map_bin_msg( const std::string & absolute_path, const double center_line_resolution = 5.0); /** - * @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file. + * @brief Creates a LaneletMapBin message using a Lanelet2 map file. * - * This function loads a lanelet2_map.osm from the test_map folder in the - * autoware_test_utils package, overwrites the centerline with a resolution of 5.0, + * This function loads a specified map file from the test_map folder in the + * specified package (or autoware_test_utils if no package is specified), + * overwrites the centerline with a resolution of 5.0, * and converts the map to a LaneletMapBin message. * + * @param package_name The name of the package containing the map file. If empty, defaults to + * "autoware_test_utils". + * @param map_filename The name of the map file (e.g. "lanelet2_map.osm") * @return A LaneletMapBin message containing the map data. */ -LaneletMapBin makeMapBinMsg(); +LaneletMapBin makeMapBinMsg( + const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm"); /** * @brief Creates an Odometry message with a specified shift. diff --git a/common/autoware_test_utils/src/autoware_test_utils.cpp b/common/autoware_test_utils/src/autoware_test_utils.cpp index d42a5219e374c..08c4a3b5e9264 100644 --- a/common/autoware_test_utils/src/autoware_test_utils.cpp +++ b/common/autoware_test_utils/src/autoware_test_utils.cpp @@ -180,10 +180,9 @@ LaneletMapBin make_map_bin_msg( return map_bin_msg; } -LaneletMapBin makeMapBinMsg() +LaneletMapBin makeMapBinMsg(const std::string & package_name, const std::string & map_filename) { - return make_map_bin_msg( - get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm")); + return make_map_bin_msg(get_absolute_path_to_lanelet_map(package_name, map_filename)); } Odometry makeOdometry(const double shift) diff --git a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp index b61c579df83b9..5081b14dda63b 100644 --- a/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp +++ b/planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp @@ -24,6 +24,7 @@ #include #include +#include #include namespace autoware_planning_test_manager::utils @@ -34,9 +35,11 @@ using geometry_msgs::msg::Pose; using nav_msgs::msg::Odometry; using RouteSections = std::vector; -Pose createPoseFromLaneID(const lanelet::Id & lane_id) +Pose createPoseFromLaneID( + const lanelet::Id & lane_id, const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm") { - auto map_bin_msg = autoware::test_utils::makeMapBinMsg(); + auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename); // create route_handler auto route_handler = std::make_shared(); route_handler->setMap(map_bin_msg); @@ -67,16 +70,19 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id) // Function to create a route from given start and goal lanelet ids // start pose and goal pose are set to the middle of the lanelet -LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id) +LaneletRoute makeBehaviorRouteFromLaneId( + const int & start_lane_id, const int & goal_lane_id, + const std::string & package_name = "autoware_test_utils", + const std::string & map_filename = "lanelet2_map.osm") { LaneletRoute route; route.header.frame_id = "map"; - auto start_pose = createPoseFromLaneID(start_lane_id); - auto goal_pose = createPoseFromLaneID(goal_lane_id); + auto start_pose = createPoseFromLaneID(start_lane_id, package_name, map_filename); + auto goal_pose = createPoseFromLaneID(goal_lane_id, package_name, map_filename); route.start_pose = start_pose; route.goal_pose = goal_pose; - auto map_bin_msg = autoware::test_utils::makeMapBinMsg(); + auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename); // create route_handler auto route_handler = std::make_shared(); route_handler->setMap(map_bin_msg);