Skip to content

Commit

Permalink
add additional test
Browse files Browse the repository at this point in the history 
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed May 17, 2024
1 parent 00235b9 commit 16579ec
Show file tree
Hide file tree
Showing 2 changed files with 105 additions and 190 deletions.
81 changes: 78 additions & 3 deletions planning/route_handler/test/test_route_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,84 @@ TEST_F(TestRouteHandler, isRouteHandlerReadyTest)

TEST_F(TestRouteHandler, checkIfIDReturned)
{
const auto lanelet = route_handler_->getLaneletsFromId(4870);
const auto is_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet);
const auto lanelet1 = route_handler_->getLaneletsFromId(4785);
const auto is_lanelet1_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet1);
ASSERT_TRUE(is_lanelet1_in_goal_route_section);

ASSERT_TRUE(is_in_goal_route_section);
const auto lanelet2 = route_handler_->getLaneletsFromId(4780);
const auto is_lanelet2_in_goal_route_section = route_handler_->isInGoalRouteSection(lanelet2);
ASSERT_FALSE(is_lanelet2_in_goal_route_section);
}

TEST_F(TestRouteHandler, getGoalLaneId)
{
lanelet::ConstLanelet goal_lane;

const auto goal_lane_obtained = route_handler_->getGoalLanelet(&goal_lane);
ASSERT_TRUE(goal_lane_obtained);
ASSERT_EQ(goal_lane.id(), 5088);
}

// TEST_F(TestRouteHandler, getClosestLaneletWithinRouteWhenPointsInRoute)
// {
// lanelet::ConstLanelet closest_lane;

// Pose search_pose;

// search_pose.position = tier4_autoware_utils::createPoint(-1.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained7 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained7);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(-0.5, 1.75, 0);
// const auto closest_lane_obtained =
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(-.01, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained3 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained3);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(0.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained1 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained1);
// ASSERT_EQ(closest_lane.id(), 4775);

// search_pose.position = tier4_autoware_utils::createPoint(0.01, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained2 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained2);
// ASSERT_EQ(closest_lane.id(), 4424);

// search_pose.position = tier4_autoware_utils::createPoint(0.5, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained4 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained4);
// ASSERT_EQ(closest_lane.id(), 4424);

// search_pose.position = tier4_autoware_utils::createPoint(1.0, 1.75, 0);
// search_pose.orientation = tier4_autoware_utils::createQuaternion(0.0, 0.0, 0.0, 1.0);
// const auto closest_lane_obtained5 =
// route_handler_->getClosestLaneletWithinRoute(search_pose, &closest_lane);

// ASSERT_TRUE(closest_lane_obtained5);
// ASSERT_EQ(closest_lane.id(), 4424);
// }
} // namespace route_handler::test
214 changes: 27 additions & 187 deletions planning/route_handler/test_route/lane_change_test_route.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,11 @@
header:
stamp:
sec: 1715936953
nanosec: 67206425
frame_id: map
start_pose:
position:
x: -200.0
x: -50.0
y: 1.75
z: 0.0
orientation:
Expand All @@ -10,7 +15,7 @@ start_pose:
w: 1.0
goal_pose:
position:
x: 500.0
x: 70.0
y: -1.75
z: 0.0
orientation:
Expand All @@ -19,54 +24,6 @@ goal_pose:
z: 0.0
w: 1.0
segments:
- preferred_primitive:
id: 4735
primitive_type: ""
primitives:
- id: 4735
primitive_type: lane
- id: 5044
primitive_type: lane
- preferred_primitive:
id: 4740
primitive_type: ""
primitives:
- id: 4740
primitive_type: lane
- id: 5048
primitive_type: lane
- preferred_primitive:
id: 4745
primitive_type: ""
primitives:
- id: 4745
primitive_type: lane
- id: 5052
primitive_type: lane
- preferred_primitive:
id: 4750
primitive_type: ""
primitives:
- id: 4750
primitive_type: lane
- id: 5056
primitive_type: lane
- preferred_primitive:
id: 4755
primitive_type: ""
primitives:
- id: 4755
primitive_type: lane
- id: 5060
primitive_type: lane
- preferred_primitive:
id: 4760
primitive_type: ""
primitives:
- id: 4760
primitive_type: lane
- id: 5064
primitive_type: lane
- preferred_primitive:
id: 4765
primitive_type: ""
Expand All @@ -84,7 +41,7 @@ segments:
- id: 5072
primitive_type: lane
- preferred_primitive:
id: 9594
id: 4775
primitive_type: ""
primitives:
- id: 4775
Expand Down Expand Up @@ -115,139 +72,22 @@ segments:
primitive_type: lane
- id: 5088
primitive_type: lane
- preferred_primitive:
id: 5092
primitive_type: ""
primitives:
- id: 4790
primitive_type: lane
- id: 5092
primitive_type: lane
- preferred_primitive:
id: 5096
primitive_type: ""
primitives:
- id: 4795
primitive_type: lane
- id: 5096
primitive_type: lane
- preferred_primitive:
id: 5104
primitive_type: ""
primitives:
- id: 4800
primitive_type: lane
- id: 5104
primitive_type: lane
- preferred_primitive:
id: 5100
primitive_type: ""
primitives:
- id: 4805
primitive_type: lane
- id: 5100
primitive_type: lane
- preferred_primitive:
id: 9813
primitive_type: ""
primitives:
- id: 4810
primitive_type: lane
- id: 9813
primitive_type: lane
- preferred_primitive:
id: 5112
primitive_type: ""
primitives:
- id: 4815
primitive_type: lane
- id: 5112
primitive_type: lane
- preferred_primitive:
id: 5116
primitive_type: ""
primitives:
- id: 4820
primitive_type: lane
- id: 5116
primitive_type: lane
- preferred_primitive:
id: 5120
primitive_type: ""
primitives:
- id: 4825
primitive_type: lane
- id: 5120
primitive_type: lane
- preferred_primitive:
id: 5124
primitive_type: ""
primitives:
- id: 4830
primitive_type: lane
- id: 5124
primitive_type: lane
- preferred_primitive:
id: 5128
primitive_type: ""
primitives:
- id: 4835
primitive_type: lane
- id: 5128
primitive_type: lane
- preferred_primitive:
id: 5132
primitive_type: ""
primitives:
- id: 4840
primitive_type: lane
- id: 5132
primitive_type: lane
- preferred_primitive:
id: 5136
primitive_type: ""
primitives:
- id: 4845
primitive_type: lane
- id: 5136
primitive_type: lane
- preferred_primitive:
id: 5140
primitive_type: ""
primitives:
- id: 4850
primitive_type: lane
- id: 5140
primitive_type: lane
- preferred_primitive:
id: 9817
primitive_type: ""
primitives:
- id: 4855
primitive_type: lane
- id: 9817
primitive_type: lane
- preferred_primitive:
id: 9821
primitive_type: ""
primitives:
- id: 4860
primitive_type: lane
- id: 9821
primitive_type: lane
- preferred_primitive:
id: 9825
primitive_type: ""
primitives:
- id: 4865
primitive_type: lane
- id: 9825
primitive_type: lane
- preferred_primitive:
id: 5156
primitive_type: ""
primitives:
- id: 4870
primitive_type: lane
- id: 5156
primitive_type: lane
uuid:
uuid:
- 231
- 254
- 143
- 227
- 194
- 8
- 220
- 88
- 30
- 194
- 172
- 147
- 127
- 143
- 176
- 133
allow_modification: false

0 comments on commit 16579ec

Please sign in to comment.