Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

CanonicalizedLaneletPose unit tests #32

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions simulation/traffic_simulator/test/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -5,3 +5,4 @@ add_subdirectory(src/behavior)
add_subdirectory(src/job)
add_subdirectory(src/simulation_clock)
add_subdirectory(src/hdmap_utils)
add_subdirectory(src/data_type)
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
ament_add_gtest(test_lanelet_pose test_lanelet_pose.cpp)
target_link_libraries(test_lanelet_pose traffic_simulator)

384 changes: 384 additions & 0 deletions simulation/traffic_simulator/test/src/data_type/test_lanelet_pose.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,384 @@
// Copyright 2015 TIER IV, Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <gtest/gtest.h>

#include <traffic_simulator/data_type/lanelet_pose.hpp>

#include "../helper_functions.hpp"

using CanonicalizedLaneletPose = traffic_simulator::lanelet_pose::CanonicalizedLaneletPose;

int main(int argc, char ** argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}
class CanonicalizedLaneletPoseTest : public testing::Test
{
protected:
CanonicalizedLaneletPoseTest() : hdmap_utils(makeHdMapUtilsSharedPointer()) {}

std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils;
};

/**
* @note Test constructor behavior with route_lanelets argument present when canonicalization function fails.
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute_invalid)
{
EXPECT_THROW(
CanonicalizedLaneletPose(
traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), lanelet::Ids{},
hdmap_utils),
std::runtime_error);
gmajrobotec marked this conversation as resolved.
Show resolved Hide resolved
}

/**
* @note Test constructor behavior with route_lanelets argument present when canonicalization function succeeds
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withRoute)
{
std::shared_ptr<CanonicalizedLaneletPose> pose;
EXPECT_NO_THROW(
pose = std::make_shared<CanonicalizedLaneletPose>(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), lanelet::Ids{120659},
hdmap_utils));
EXPECT_LANELET_POSE_EQ(
static_cast<traffic_simulator::LaneletPose>(*pose),
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0));
}

/**
* @note Test constructor behavior with route_lanelets argument absent when canonicalization function fails
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute_invalid)
{
EXPECT_THROW(
CanonicalizedLaneletPose(
traffic_simulator::helper::constructLaneletPose(100000000000, 0.0, 0.0), hdmap_utils),
std::runtime_error);
gmajrobotec marked this conversation as resolved.
Show resolved Hide resolved
}

/**
* @note Test constructor behavior with route_lanelets argument absent when canonicalization function succeeds
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_withoutRoute)
{
std::shared_ptr<CanonicalizedLaneletPose> pose;
EXPECT_NO_THROW(
pose = std::make_shared<CanonicalizedLaneletPose>(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils));
EXPECT_LANELET_POSE_EQ(
static_cast<traffic_simulator::LaneletPose>(*pose),
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0));
}

/**
* @note Test copy constructor behavior
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_copyConstructor)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_copy(pose);
EXPECT_LANELET_POSE_EQ(
static_cast<traffic_simulator::LaneletPose>(pose),
static_cast<traffic_simulator::LaneletPose>(CanonicalizedLaneletPose(pose)));
}

/**
* @note Test move constructor behavior
*/
TEST_F(CanonicalizedLaneletPoseTest, CanonicalizedLaneletPose_moveConstructor)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose2(pose);
const CanonicalizedLaneletPose pose_move = std::move(pose2);
EXPECT_LANELET_POSE_EQ(
static_cast<traffic_simulator::LaneletPose>(pose),
static_cast<traffic_simulator::LaneletPose>(pose_move));
}

/**
* @note Test copy assignment operator behavior
*/
TEST_F(CanonicalizedLaneletPoseTest, copyAssignment)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
CanonicalizedLaneletPose pose_assign(
traffic_simulator::helper::constructLaneletPose(34468, 0.0, 0.0), hdmap_utils);

pose_assign = pose;

EXPECT_LANELET_POSE_EQ(
static_cast<traffic_simulator::LaneletPose>(pose),
static_cast<traffic_simulator::LaneletPose>(pose_assign));
}

/**
* @note Test conversion operator behavior using static_cast<LaneletPose>
*/
TEST_F(CanonicalizedLaneletPoseTest, conversionLaneletPose)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const traffic_simulator::LaneletPose pose_casted =
static_cast<traffic_simulator::LaneletPose>(pose);

EXPECT_EQ(pose_casted.lanelet_id, 120659);
EXPECT_DOUBLE_EQ(pose_casted.s, 0.0);
EXPECT_DOUBLE_EQ(pose_casted.offset, 0.0);
EXPECT_VECTOR3_EQ(pose_casted.rpy, geometry_msgs::msg::Vector3());
}

/**
* @note Test conversion operator behavior using static_cast<Pose>
*/
TEST_F(CanonicalizedLaneletPoseTest, conversionPose)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);

const geometry_msgs::msg::Pose pose1 =
makePose(makePoint(3822.3815, 73784.9618, -1.761), makeQuaternionFromYaw(2.060578777273));

EXPECT_POSE_NEAR(static_cast<geometry_msgs::msg::Pose>(pose), pose1, 0.01);
}

/**
* @note Test function behavior when alternative poses are present
*/
TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_true)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, -10.0, 0.0), hdmap_utils);

EXPECT_TRUE(pose.hasAlternativeLaneletPose());
}

/**
* @note Test function behavior when alternative poses are absent
*/
TEST_F(CanonicalizedLaneletPoseTest, hasAlternativeLaneletPose_false)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 10.0, 0.0), hdmap_utils);

EXPECT_FALSE(pose.hasAlternativeLaneletPose());
}

/**
* @note Test function behavior when there are no lanelet_poses
*/
TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_empty)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 20.0, 0.0), hdmap_utils);
const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);

const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);

gmajrobotec marked this conversation as resolved.
Show resolved Hide resolved
ASSERT_TRUE(result1.has_value());
ASSERT_TRUE(result2.has_value());

EXPECT_EQ(result1.value().lanelet_id, 120659);
EXPECT_EQ(result2.value().lanelet_id, 120659);
}

/**
* @note Test function behavior when there is only one lanelet_pose
*/
TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_single)
{
CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(34666, -20.0, 0.0), hdmap_utils);
const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);

const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);

ASSERT_TRUE(result1.has_value());
ASSERT_TRUE(result2.has_value());

EXPECT_EQ(result1.value().lanelet_id, 34603);
EXPECT_EQ(result2.value().lanelet_id, 34603);
}

/**
* @note Test function behavior when there are multiple lanelet_poses
*/
TEST_F(CanonicalizedLaneletPoseTest, getAlternativeLaneletPoseBaseOnShortestRouteFrom_multiple)
{
CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, -20.0, 0.0), hdmap_utils);
const auto from1 = traffic_simulator::helper::constructLaneletPose(34603, 10.0, 0.0);
const auto from2 = traffic_simulator::helper::constructLaneletPose(34579, 10.0, 0.0);

const auto result1 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from1, hdmap_utils);
const auto result2 = pose.getAlternativeLaneletPoseBaseOnShortestRouteFrom(from2, hdmap_utils);

ASSERT_TRUE(result1.has_value());
ASSERT_TRUE(result2.has_value());

EXPECT_EQ(result1.value().lanelet_id, 34603);
EXPECT_EQ(result2.value().lanelet_id, 34579);
}

/**
* @note Test accessor function
*/
TEST(CanonicalizedLaneletPose, getConsiderPoseByRoadSlope)
{
EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false);
}

/**
* @note Test accessor function
*/
TEST(CanonicalizedLaneletPose, setConsiderPoseByRoadSlope)
{
EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), false);
CanonicalizedLaneletPose::setConsiderPoseByRoadSlope(true);
EXPECT_EQ(CanonicalizedLaneletPose::getConsiderPoseByRoadSlope(), true);
}

/**
* @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id
*/
TEST_F(CanonicalizedLaneletPoseTest, operatorLessEqual)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_equal(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_less(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_greater(
traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils);

EXPECT_TRUE(pose_less <= pose);
EXPECT_TRUE(pose_equal <= pose);
EXPECT_FALSE(pose_greater <= pose);
}

/**
* @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id
*/
TEST_F(CanonicalizedLaneletPoseTest, operatorLess)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_equal(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_less(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_greater(
traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils);

EXPECT_TRUE(pose_less < pose);
EXPECT_FALSE(pose_equal < pose);
EXPECT_FALSE(pose_greater < pose);
}

/**
* @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id
*/
TEST_F(CanonicalizedLaneletPoseTest, operatorGreaterEqual)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_equal(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_less(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_greater(
traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils);

EXPECT_FALSE(pose_less >= pose);
EXPECT_TRUE(pose_equal >= pose);
EXPECT_TRUE(pose_greater >= pose);
}

/**
* @note Test operator calculation correctness with CanonicalizedLaneletPose of lesser, equal and greater lanelet_id
*/
TEST_F(CanonicalizedLaneletPoseTest, operatorGreater)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_equal(
traffic_simulator::helper::constructLaneletPose(120659, 5.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_less(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose_greater(
traffic_simulator::helper::constructLaneletPose(120659, 6.0, 0.0), hdmap_utils);

EXPECT_FALSE(pose_less > pose);
EXPECT_FALSE(pose_equal > pose);
EXPECT_TRUE(pose_greater > pose);
}

/**
* @note Test function behavior when provided two poses occupying the same lanelet_id
*/
TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_same)
{
const CanonicalizedLaneletPose pose1(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
const CanonicalizedLaneletPose pose2(
traffic_simulator::helper::constructLaneletPose(120659, 1.0, 0.0), hdmap_utils);

EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose1, pose2));
}

/**
* @note Test function behavior when provided two poses occupying different lanelet_ids
*/
TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withPose_different)
{
const CanonicalizedLaneletPose pose1(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);
CanonicalizedLaneletPose pose2(
traffic_simulator::helper::constructLaneletPose(34606, 1.0, 0.0), hdmap_utils);

EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose1, pose2));
}

/**
* @note Test function behavior when provided with a pose having lanelt_id equal to the lanelet_id argument
*/
TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_same)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);

EXPECT_TRUE(traffic_simulator::isSameLaneletId(pose, 120659));
}

/**
* @note Test function behavior when provided with a pose having lanelet_id different to the lanelet_id argument
*/
TEST_F(CanonicalizedLaneletPoseTest, isSameLaneletId_withLanelet_different)
{
const CanonicalizedLaneletPose pose(
traffic_simulator::helper::constructLaneletPose(120659, 0.0, 0.0), hdmap_utils);

EXPECT_FALSE(traffic_simulator::isSameLaneletId(pose, 34606));
}