Skip to content

Commit 41ca768

Browse files
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 <kyoichi.sugahara@tier4.jp> * feat: set default package name to 'autoware_test_utils' in makeMapBinMsg and related functions Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> --------- Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
1 parent 3c94dbf commit 41ca768

File tree

3 files changed

+24
-13
lines changed

3 files changed

+24
-13
lines changed

common/autoware_test_utils/include/autoware_test_utils/autoware_test_utils.hpp

+10-4
Original file line numberDiff line numberDiff line change
@@ -213,15 +213,21 @@ LaneletMapBin make_map_bin_msg(
213213
const std::string & absolute_path, const double center_line_resolution = 5.0);
214214

215215
/**
216-
* @brief Creates a LaneletMapBin message using a predefined Lanelet2 map file.
216+
* @brief Creates a LaneletMapBin message using a Lanelet2 map file.
217217
*
218-
* This function loads a lanelet2_map.osm from the test_map folder in the
219-
* autoware_test_utils package, overwrites the centerline with a resolution of 5.0,
218+
* This function loads a specified map file from the test_map folder in the
219+
* specified package (or autoware_test_utils if no package is specified),
220+
* overwrites the centerline with a resolution of 5.0,
220221
* and converts the map to a LaneletMapBin message.
221222
*
223+
* @param package_name The name of the package containing the map file. If empty, defaults to
224+
* "autoware_test_utils".
225+
* @param map_filename The name of the map file (e.g. "lanelet2_map.osm")
222226
* @return A LaneletMapBin message containing the map data.
223227
*/
224-
LaneletMapBin makeMapBinMsg();
228+
LaneletMapBin makeMapBinMsg(
229+
const std::string & package_name = "autoware_test_utils",
230+
const std::string & map_filename = "lanelet2_map.osm");
225231

226232
/**
227233
* @brief Creates an Odometry message with a specified shift.

common/autoware_test_utils/src/autoware_test_utils.cpp

+2-3
Original file line numberDiff line numberDiff line change
@@ -180,10 +180,9 @@ LaneletMapBin make_map_bin_msg(
180180
return map_bin_msg;
181181
}
182182

183-
LaneletMapBin makeMapBinMsg()
183+
LaneletMapBin makeMapBinMsg(const std::string & package_name, const std::string & map_filename)
184184
{
185-
return make_map_bin_msg(
186-
get_absolute_path_to_lanelet_map("autoware_test_utils", "lanelet2_map.osm"));
185+
return make_map_bin_msg(get_absolute_path_to_lanelet_map(package_name, map_filename));
187186
}
188187

189188
Odometry makeOdometry(const double shift)

planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp

+12-6
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@
2424

2525
#include <iostream>
2626
#include <memory>
27+
#include <string>
2728
#include <vector>
2829

2930
namespace autoware_planning_test_manager::utils
@@ -34,9 +35,11 @@ using geometry_msgs::msg::Pose;
3435
using nav_msgs::msg::Odometry;
3536
using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
3637

37-
Pose createPoseFromLaneID(const lanelet::Id & lane_id)
38+
Pose createPoseFromLaneID(
39+
const lanelet::Id & lane_id, const std::string & package_name = "autoware_test_utils",
40+
const std::string & map_filename = "lanelet2_map.osm")
3841
{
39-
auto map_bin_msg = autoware::test_utils::makeMapBinMsg();
42+
auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename);
4043
// create route_handler
4144
auto route_handler = std::make_shared<RouteHandler>();
4245
route_handler->setMap(map_bin_msg);
@@ -67,16 +70,19 @@ Pose createPoseFromLaneID(const lanelet::Id & lane_id)
6770

6871
// Function to create a route from given start and goal lanelet ids
6972
// start pose and goal pose are set to the middle of the lanelet
70-
LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id)
73+
LaneletRoute makeBehaviorRouteFromLaneId(
74+
const int & start_lane_id, const int & goal_lane_id,
75+
const std::string & package_name = "autoware_test_utils",
76+
const std::string & map_filename = "lanelet2_map.osm")
7177
{
7278
LaneletRoute route;
7379
route.header.frame_id = "map";
74-
auto start_pose = createPoseFromLaneID(start_lane_id);
75-
auto goal_pose = createPoseFromLaneID(goal_lane_id);
80+
auto start_pose = createPoseFromLaneID(start_lane_id, package_name, map_filename);
81+
auto goal_pose = createPoseFromLaneID(goal_lane_id, package_name, map_filename);
7682
route.start_pose = start_pose;
7783
route.goal_pose = goal_pose;
7884

79-
auto map_bin_msg = autoware::test_utils::makeMapBinMsg();
85+
auto map_bin_msg = autoware::test_utils::makeMapBinMsg(package_name, map_filename);
8086
// create route_handler
8187
auto route_handler = std::make_shared<RouteHandler>();
8288
route_handler->setMap(map_bin_msg);

0 commit comments

Comments
 (0)