Skip to content

Commit 4b8eb93

Browse files
zulfaqar-azmi-t4zulfaqar-azmi-t4
committed
refactor(planning_test_utils): remove route_handler dependencies
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
1 parent 5889e9a commit 4b8eb93

File tree

4 files changed

+126
-93
lines changed

4 files changed

+126
-93
lines changed

‎planning/autoware_planning_test_manager/include/autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp

+119
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
// Copyright 2024 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_
16+
#define AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_
17+
#include <route_handler/route_handler.hpp>
18+
#include <nav_msgs/msg/odometry.hpp>
19+
#include <geometry_msgs/msg/pose.hpp>
20+
#include <autoware_planning_msgs/msg/lanelet_route.hpp>
21+
#include <planning_test_utils/planning_test_utils.hpp>
22+
23+
namespace autoware_planning_test_manager::utils
24+
{
25+
using route_handler::RouteHandler;
26+
using nav_msgs::msg::Odometry;
27+
using geometry_msgs::msg::Pose;
28+
using autoware_planning_msgs::msg::LaneletRoute;
29+
using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>;
30+
31+
Pose createPoseFromLaneID(const lanelet::Id & lane_id)
32+
{
33+
auto map_bin_msg = test_utils::makeMapBinMsg();
34+
// create route_handler
35+
auto route_handler = std::make_shared<RouteHandler>();
36+
route_handler->setMap(map_bin_msg);
37+
38+
// get middle idx of the lanelet
39+
const auto lanelet = route_handler->getLaneletsFromId(lane_id);
40+
const auto center_line = lanelet.centerline();
41+
const size_t middle_point_idx = std::floor(center_line.size() / 2.0);
42+
43+
// get middle position of the lanelet
44+
geometry_msgs::msg::Point middle_pos;
45+
middle_pos.x = center_line[middle_point_idx].x();
46+
middle_pos.y = center_line[middle_point_idx].y();
47+
48+
// get next middle position of the lanelet
49+
geometry_msgs::msg::Point next_middle_pos;
50+
next_middle_pos.x = center_line[middle_point_idx + 1].x();
51+
next_middle_pos.y = center_line[middle_point_idx + 1].y();
52+
53+
// calculate middle pose
54+
geometry_msgs::msg::Pose middle_pose;
55+
middle_pose.position = middle_pos;
56+
const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos);
57+
middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
58+
59+
return middle_pose;
60+
}
61+
62+
// Function to create a route from given start and goal lanelet ids
63+
// start pose and goal pose are set to the middle of the lanelet
64+
LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id)
65+
{
66+
LaneletRoute route;
67+
route.header.frame_id = "map";
68+
auto start_pose = createPoseFromLaneID(start_lane_id);
69+
auto goal_pose = createPoseFromLaneID(goal_lane_id);
70+
route.start_pose = start_pose;
71+
route.goal_pose = goal_pose;
72+
73+
auto map_bin_msg = test_utils::makeMapBinMsg();
74+
// create route_handler
75+
auto route_handler = std::make_shared<RouteHandler>();
76+
route_handler->setMap(map_bin_msg);
77+
78+
LaneletRoute route_msg;
79+
RouteSections route_sections;
80+
lanelet::ConstLanelets all_route_lanelets;
81+
82+
// Plan the path between checkpoints (start and goal poses)
83+
lanelet::ConstLanelets path_lanelets;
84+
if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) {
85+
return route_msg;
86+
}
87+
88+
// Add all path_lanelets to all_route_lanelets
89+
for (const auto & lane : path_lanelets) {
90+
all_route_lanelets.push_back(lane);
91+
}
92+
// create local route sections
93+
route_handler->setRouteLanelets(path_lanelets);
94+
const auto local_route_sections = route_handler->createMapSegments(path_lanelets);
95+
route_sections = test_utils::combineConsecutiveRouteSections(route_sections, local_route_sections);
96+
for (const auto & route_section : route_sections) {
97+
for (const auto & primitive : route_section.primitives) {
98+
std::cerr << "primitive: " << primitive.id << std::endl;
99+
}
100+
std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl;
101+
}
102+
route_handler->setRouteLanelets(all_route_lanelets);
103+
route.segments = route_sections;
104+
105+
route.allow_modification = false;
106+
return route;
107+
}
108+
109+
Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id)
110+
{
111+
Odometry current_odometry;
112+
current_odometry.pose.pose = createPoseFromLaneID(lane_id);
113+
current_odometry.header.frame_id = "map";
114+
115+
return current_odometry;
116+
}
117+
118+
} // namespace autoware_planning_test_manager::utils
119+
#endif // AUTOWARE_PLANNING_TEST_MANAGER__AUTOWARE_PLANNING_TEST_MANAGER_UTILS_HPP_

‎planning/autoware_planning_test_manager/src/autoware_planning_test_manager.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,9 @@
1515
#include "motion_utils/trajectory/conversion.hpp"
1616

1717
#include <autoware_planning_test_manager/autoware_planning_test_manager.hpp>
18+
#include <autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp>
19+
#include <tier4_autoware_utils/geometry/geometry.hpp>
20+
1821
#include <planning_test_utils/planning_test_utils.hpp>
1922

2023
namespace planning_test_utils
@@ -115,7 +118,7 @@ void PlanningInterfaceTestManager::publishInitialPose(
115118
if (module_name == ModuleName::START_PLANNER) {
116119
test_utils::publishToTargetNode(
117120
test_node_, target_node, topic_name, initial_pose_pub_,
118-
test_utils::makeInitialPoseFromLaneId(10291));
121+
autoware_planning_test_manager::utils::makeInitialPoseFromLaneId(10291));
119122
} else {
120123
test_utils::publishToTargetNode(
121124
test_node_, target_node, topic_name, initial_pose_pub_, test_utils::makeInitialPose(shift));
@@ -256,7 +259,7 @@ void PlanningInterfaceTestManager::publishBehaviorNominalRoute(
256259
if (module_name == ModuleName::START_PLANNER) {
257260
test_utils::publishToTargetNode(
258261
test_node_, target_node, topic_name, behavior_normal_route_pub_,
259-
test_utils::makeBehaviorRouteFromLaneId(10291, 10333), 5);
262+
autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId(10291, 10333), 5);
260263
} else {
261264
test_utils::publishToTargetNode(
262265
test_node_, target_node, topic_name, behavior_normal_route_pub_,

‎planning/planning_test_utils/include/planning_test_utils/planning_test_utils.hpp

+2-89
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,6 @@
2323
#include <lanelet2_extension/projection/mgrs_projector.hpp>
2424
#include <lanelet2_extension/utility/message_conversion.hpp>
2525
#include <lanelet2_extension/utility/utilities.hpp>
26-
#include <route_handler/route_handler.hpp>
2726
#include <tier4_autoware_utils/geometry/geometry.hpp>
2827

2928
#include <autoware_adapi_v1_msgs/msg/operation_mode_state.hpp>
@@ -37,7 +36,9 @@
3736
#include <geometry_msgs/msg/pose_stamped.hpp>
3837
#include <nav_msgs/msg/occupancy_grid.hpp>
3938
#include <nav_msgs/msg/odometry.hpp>
39+
#include <sensor_msgs/msg/point_cloud2.hpp>
4040
#include <std_msgs/msg/bool.hpp>
41+
#include <tf2_msgs/msg/tf_message.hpp>
4142
#include <tier4_planning_msgs/msg/scenario.hpp>
4243
#include <unique_identifier_msgs/msg/uuid.hpp>
4344

@@ -71,7 +72,6 @@ using geometry_msgs::msg::PoseStamped;
7172
using geometry_msgs::msg::TransformStamped;
7273
using nav_msgs::msg::OccupancyGrid;
7374
using nav_msgs::msg::Odometry;
74-
using route_handler::RouteHandler;
7575
using sensor_msgs::msg::PointCloud2;
7676
using tf2_msgs::msg::TFMessage;
7777
using tier4_autoware_utils::createPoint;
@@ -267,46 +267,6 @@ Scenario makeScenarioMsg(const std::string scenario)
267267
return scenario_msg;
268268
}
269269

270-
Pose createPoseFromLaneID(const lanelet::Id & lane_id)
271-
{
272-
auto map_bin_msg = makeMapBinMsg();
273-
// create route_handler
274-
auto route_handler = std::make_shared<RouteHandler>();
275-
route_handler->setMap(map_bin_msg);
276-
277-
// get middle idx of the lanelet
278-
const auto lanelet = route_handler->getLaneletsFromId(lane_id);
279-
const auto center_line = lanelet.centerline();
280-
const size_t middle_point_idx = std::floor(center_line.size() / 2.0);
281-
282-
// get middle position of the lanelet
283-
geometry_msgs::msg::Point middle_pos;
284-
middle_pos.x = center_line[middle_point_idx].x();
285-
middle_pos.y = center_line[middle_point_idx].y();
286-
287-
// get next middle position of the lanelet
288-
geometry_msgs::msg::Point next_middle_pos;
289-
next_middle_pos.x = center_line[middle_point_idx + 1].x();
290-
next_middle_pos.y = center_line[middle_point_idx + 1].y();
291-
292-
// calculate middle pose
293-
geometry_msgs::msg::Pose middle_pose;
294-
middle_pose.position = middle_pos;
295-
const double yaw = tier4_autoware_utils::calcAzimuthAngle(middle_pos, next_middle_pos);
296-
middle_pose.orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw);
297-
298-
return middle_pose;
299-
}
300-
301-
Odometry makeInitialPoseFromLaneId(const lanelet::Id & lane_id)
302-
{
303-
Odometry current_odometry;
304-
current_odometry.pose.pose = createPoseFromLaneID(lane_id);
305-
current_odometry.header.frame_id = "map";
306-
307-
return current_odometry;
308-
}
309-
310270
RouteSections combineConsecutiveRouteSections(
311271
const RouteSections & route_sections1, const RouteSections & route_sections2)
312272
{
@@ -322,53 +282,6 @@ RouteSections combineConsecutiveRouteSections(
322282
return route_sections;
323283
}
324284

325-
// Function to create a route from given start and goal lanelet ids
326-
// start pose and goal pose are set to the middle of the lanelet
327-
LaneletRoute makeBehaviorRouteFromLaneId(const int & start_lane_id, const int & goal_lane_id)
328-
{
329-
LaneletRoute route;
330-
route.header.frame_id = "map";
331-
auto start_pose = createPoseFromLaneID(start_lane_id);
332-
auto goal_pose = createPoseFromLaneID(goal_lane_id);
333-
route.start_pose = start_pose;
334-
route.goal_pose = goal_pose;
335-
336-
auto map_bin_msg = makeMapBinMsg();
337-
// create route_handler
338-
auto route_handler = std::make_shared<RouteHandler>();
339-
route_handler->setMap(map_bin_msg);
340-
341-
LaneletRoute route_msg;
342-
RouteSections route_sections;
343-
lanelet::ConstLanelets all_route_lanelets;
344-
345-
// Plan the path between checkpoints (start and goal poses)
346-
lanelet::ConstLanelets path_lanelets;
347-
if (!route_handler->planPathLaneletsBetweenCheckpoints(start_pose, goal_pose, &path_lanelets)) {
348-
return route_msg;
349-
}
350-
351-
// Add all path_lanelets to all_route_lanelets
352-
for (const auto & lane : path_lanelets) {
353-
all_route_lanelets.push_back(lane);
354-
}
355-
// create local route sections
356-
route_handler->setRouteLanelets(path_lanelets);
357-
const auto local_route_sections = route_handler->createMapSegments(path_lanelets);
358-
route_sections = combineConsecutiveRouteSections(route_sections, local_route_sections);
359-
for (const auto & route_section : route_sections) {
360-
for (const auto & primitive : route_section.primitives) {
361-
std::cerr << "primitive: " << primitive.id << std::endl;
362-
}
363-
std::cerr << "preferred_primitive id : " << route_section.preferred_primitive.id << std::endl;
364-
}
365-
route_handler->setRouteLanelets(all_route_lanelets);
366-
route.segments = route_sections;
367-
368-
route.allow_modification = false;
369-
return route;
370-
}
371-
372285
// this is for the test lanelet2_map.osm
373286
// file hash: a9f84cff03b55a64917bc066451276d2293b0a54f5c088febca0c7fdf2f245d5
374287
LaneletRoute makeBehaviorNormalRoute()

‎planning/planning_test_utils/package.xml

-2
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,8 @@
2525
<depend>component_interface_utils</depend>
2626
<depend>lanelet2_extension</depend>
2727
<depend>lanelet2_io</depend>
28-
<depend>motion_utils</depend>
2928
<depend>nav_msgs</depend>
3029
<depend>rclcpp</depend>
31-
<depend>route_handler</depend>
3230
<depend>tf2_msgs</depend>
3331
<depend>tf2_ros</depend>
3432
<depend>tier4_api_msgs</depend>

0 commit comments

Comments
 (0)