From 808a4b93ea883d247c7565870ab1c98627d284af Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 4 Sep 2024 14:25:01 +0200 Subject: [PATCH 1/5] Pose module unit tests --- .../traffic_simulator/test/CMakeLists.txt | 1 + .../test/src/utils/CMakeLists.txt | 2 + .../test/src/utils/test_pose.cpp | 669 ++++++++++++++++++ 3 files changed, 672 insertions(+) create mode 100644 simulation/traffic_simulator/test/src/utils/CMakeLists.txt create mode 100644 simulation/traffic_simulator/test/src/utils/test_pose.cpp diff --git a/simulation/traffic_simulator/test/CMakeLists.txt b/simulation/traffic_simulator/test/CMakeLists.txt index 713c8b40678..eb539f6e7df 100644 --- a/simulation/traffic_simulator/test/CMakeLists.txt +++ b/simulation/traffic_simulator/test/CMakeLists.txt @@ -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/utils) diff --git a/simulation/traffic_simulator/test/src/utils/CMakeLists.txt b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt new file mode 100644 index 00000000000..8c96fcb2eaa --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_pose test_pose.cpp) +target_link_libraries(test_pose traffic_simulator) diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp new file mode 100644 index 00000000000..b8182b2617a --- /dev/null +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -0,0 +1,669 @@ +// 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 + +#include + +#include "../helper_functions.hpp" + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} + +class PoseTest : public testing::Test +{ +protected: + PoseTest() + : hdmap_utils(std::make_shared( + ament_index_cpp::get_package_share_directory("traffic_simulator") + + "/map/four_track_highway/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.22312494055522) + .longitude(138.8024583466017) + .altitude(0.0))) + { + } + + std::shared_ptr hdmap_utils; +}; + +/** + * @note test created object values + */ +TEST(pose, quietNaNPose) +{ + const auto pose = traffic_simulator::pose::quietNaNPose(); + + EXPECT_TRUE(isnan(pose.position.x)); + EXPECT_TRUE(isnan(pose.position.y)); + EXPECT_TRUE(isnan(pose.position.z)); + + EXPECT_EQ(pose.orientation.x, 0); + EXPECT_EQ(pose.orientation.y, 0); + EXPECT_EQ(pose.orientation.z, 0); + EXPECT_EQ(pose.orientation.w, 1); +} + +/** + * @note test created object values + */ +TEST(pose, quietNaNLaneletPose) +{ + const auto pose = traffic_simulator::pose::quietNaNLaneletPose(); + + EXPECT_EQ(pose.lanelet_id, std::numeric_limits::max()); + EXPECT_TRUE(isnan(pose.s)); + EXPECT_TRUE(isnan(pose.offset)); + EXPECT_TRUE(isnan(pose.rpy.x)); + EXPECT_TRUE(isnan(pose.rpy.y)); + EXPECT_TRUE(isnan(pose.rpy.z)); +} + +/** + * @note test canonicalization with a default constructed LaneletPose + */ +TEST_F(PoseTest, canonicalize_default) +{ + const auto pose = + traffic_simulator::pose::canonicalize(traffic_simulator_msgs::msg::LaneletPose(), hdmap_utils); + + EXPECT_FALSE(pose.has_value()); +} + +/** + * @note test canonicalization with an invalid LaneletPose + */ +TEST_F(PoseTest, canonicalize_invalid) +{ + EXPECT_THROW( + traffic_simulator::pose::canonicalize( + traffic_simulator::pose::quietNaNLaneletPose(), hdmap_utils), + std::runtime_error); +} + +/** + * @note test canonicalization with a valid constructed LaneletPose + */ +TEST_F(PoseTest, canonicalize_valid) +{ + const auto pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); + std::optional canonicalized_pose; + + EXPECT_NO_THROW(canonicalized_pose = traffic_simulator::pose::canonicalize(pose, hdmap_utils)); + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_LANELET_POSE_EQ( + static_cast(canonicalized_pose.value()), pose); +} + +/** + * @note test conversion to geometry_msgs::msg::Pose from CanonicalizedLaneletPose + */ +TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) +{ + const traffic_simulator::lanelet_pose::CanonicalizedLaneletPose canonicalized_pose( + traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0), hdmap_utils); + + const geometry_msgs::msg::Pose pose = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(canonicalized_pose), pose, 0.01); +} + +/** + * @note test conversion to geometry_msgs::msg::Pose from LaneletPose with HdMapUtils pointer + */ +TEST_F(PoseTest, toMapPose_LaneletPose) +{ + const auto lanelet_pose = traffic_simulator::helper::constructLaneletPose(195, 0.0, 0.0); + + const geometry_msgs::msg::Pose pose = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + + EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(lanelet_pose, hdmap_utils), pose, 0.01); +} + +/** + * @note test function behavior with a pose that can be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note test function behavior with a pose that can not be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils), std::nullopt); +} + +/** + * @note test function behavior with a pose that can be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note test function behavior with a pose that can not be canonicalized, matching distance too large + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeSmallBoundingBox(), true, 0.0, hdmap_utils), + std::nullopt); +} + +/** + * @note test function behavior with an empty unique_route_lanelets vector and a pose that can not be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), {}, true, 1.0, hdmap_utils), + std::nullopt); +} + +/** + * @note test function behavior with an empty unique_route_lanelets vector and a pose that can be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), {}, true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note test function behavior with a non empty unique_route_lanelets vector and a pose that can not be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) +{ + const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + + EXPECT_EQ( + traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils), + std::nullopt); +} + +/** + * @note test function behavior with a non empty unique_route_lanelets vector and a pose that can be canonicalized + */ +TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) +{ + const auto lanelet_pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); + + const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( + pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils); + + ASSERT_TRUE(canonicalized_pose.has_value()); + EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); + EXPECT_LANELET_POSE_NEAR( + static_cast(canonicalized_pose.value()), + static_cast(lanelet_pose), 0.01); +} + +/** + * @note test calculation correctness with a non trivial example + */ +TEST(pose, transformRelativePoseToGlobal) +{ + const geometry_msgs::msg::Pose pose_relative = makePose(makePoint(4.9969, -28.1026, 0.214)); + const geometry_msgs::msg::Pose pose_global = makePose(makePoint(81601.0571, 50099.0981, 34.595)); + const geometry_msgs::msg::Pose pose_rel_global = + makePose(makePoint(81606.054, 50070.9955, 34.809)); + + EXPECT_POSE_EQ( + traffic_simulator::pose::transformRelativePoseToGlobal(pose_global, pose_relative), + pose_rel_global); +} + +/** + * @note test calculation correctness when only one pose is zeroed, the goal is to obtain a pose equal to the second argument + */ +TEST(pose, relativePose_poses_zeros) +{ + const auto from = makePose(makePoint(0, 0, 0)); + const auto to = makePose(makePoint(10, 51, 2)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_EQ(relative.value(), to); +} + +/** + * @note test calculation correctness when both poses are zeroed + */ +TEST(pose, relativePose_poses_zero) +{ + const auto from = makePose(makePoint(0, 0, 0)); + const auto to = makePose(makePoint(0, 0, 0)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_EQ(relative.value(), to); +} + +/** + * @note ttest calculation correctness with a non trivial example + */ +TEST(pose, relativePose_poses_complex) +{ + const auto pose_relative = makePose(makePoint(4.9969, -28.1026, 0.214)); + const auto from = makePose(makePoint(81601.0571, 50099.0981, 34.595)); + const auto to = makePose(makePoint(81606.054, 50070.9955, 34.809)); + + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note test calculation correctness with the overload + */ +TEST_F(PoseTest, relativePose_first) +{ + const auto pose_relative = makePose( + makePoint(9.9999, -0.0009, 0.0126), + geometry_msgs::build().x(0).y(0).z(0).w(1)); + const auto from = makePose( + makePoint(81585.1622, 50176.9202, 34.2595), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 10.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note test calculation correctness with the overload + */ +TEST_F(PoseTest, relativePose_second) +{ + const auto pose_relative = makePose( + makePoint(145244.7916, 786706.3326, 0.0127), + geometry_msgs::build().x(0).y(0).z(0).w(1)); + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = makePose( + makePoint(881586.9767, 50167.0862, 34.2722), + geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + const auto relative = traffic_simulator::pose::relativePose(from, to); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note test calculation correctness when bounding boxes are disjoint + */ +TEST_F(PoseTest, boundingBoxRelativePose_disjoint) +{ + const auto pose_relative = makePose(makePoint(0.1108, -20.5955, 0.0)); + const auto from = makePose(makePoint(81600.3967, 50094.4298, 34.6759)); + const auto to = makePose(makePoint(81604.5076, 50071.8343, 40.8482)); + const auto boundingBox = makeBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + + ASSERT_TRUE(relative.has_value()); + EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); +} + +/** + * @note test calculation correctness when bounding boxes share an edge + */ +TEST_F(PoseTest, boundingBoxRelativePose_commonEdge) +{ + const auto from = makePose(makePoint(81600, 50094, 34)); + const auto to = makePose(makePoint(81601, 50094, 34)); + const auto boundingBox = makeSmallBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + + EXPECT_FALSE(relative.has_value()); +} + +/** + * @note test calculation correctness when bounding boxes intersect + */ +TEST_F(PoseTest, boundingBoxRelativePose_intersect) +{ + const auto from = makePose(makePoint(81600, 50094, 34)); + const auto to = makePose(makePoint(81600.5, 50094, 34)); + const auto boundingBox = makeSmallBoundingBox(); + + const auto relative = + traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + + EXPECT_FALSE(relative.has_value()); +} + +/** + * @note test s-value calculation correctness when longitudinal distance between the poses can not be calculated + */ +TEST_F(PoseTest, relativeLaneletPose_s_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_TRUE(isnan(relative.s)); +} + +/** + * @note test s-value calculation correctness when longitudinal distance between the poses can be calculated + */ +TEST_F(PoseTest, relativeLaneletPose_s_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_NEAR(relative.s, 107.74, 0.001); +} + +/** + * @note test offset-value calculation correctness when lateral distance between the poses can not be calculated + */ +TEST_F(PoseTest, relativeLaneletPose_offset_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 0.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); + + EXPECT_TRUE(isnan(relative.offset)); +} + +/** + * @note test offset-value calculation correctness when lateral distance between the poses can be calculated + */ +TEST_F(PoseTest, relativeLaneletPose_offset_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + + const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, true, hdmap_utils); + + EXPECT_EQ(relative.offset, 1.0); +} + +/** + * @note test s-value calculation correctness when longitudinal distance between the poses can not be calculated + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto boundingBox = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, boundingBox, to, boundingBox, false, hdmap_utils); + + EXPECT_TRUE(isnan(relative.s)); +} + +/** + * @note test s-value calculation correctness when longitudinal distance between the poses can be calculated + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); + const auto boundingBox = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, boundingBox, to, boundingBox, false, hdmap_utils); + + EXPECT_NEAR(relative.s, 103.74, 0.01); +} + +/** + * @note test offset-value calculation correctness when lateral distance between the poses can not be calculated + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); + const auto boundingBox = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, boundingBox, to, boundingBox, false, hdmap_utils); + + EXPECT_TRUE(isnan(relative.s)); +} + +/** + * @note test offset-value calculation correctness when lateral distance between the poses can be calculated + */ +TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) +{ + const auto from = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils); + const auto to = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); + const auto boundingBox = makeBoundingBox(); + + const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( + from, boundingBox, to, boundingBox, false, hdmap_utils); + + EXPECT_EQ(relative.offset, 0.0); +} + +/** + * @note test calculation correctness when the pose lies within the lanelet + */ +TEST_F(PoseTest, isInLanelet_inside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 0, hdmap_utils)); +} + +/** + * @note test calculation correctness when the pose lies outside the front of the llanelet, distance greater than the tolerance + */ +TEST_F(PoseTest, isInLanelet_outsideFrontFar) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1, hdmap_utils)); +} + +/** + * @note test calculation correctness when the pose lies outside the front of the llanelet, distance smaller than the tolerance + */ +TEST_F(PoseTest, isInLanelet_outsideFrontClose) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2, hdmap_utils)); +} + +/** + * @note test calculation correctness when the pose lies outside the back of the llanelet, distance greater than the tolerance + */ +TEST_F(PoseTest, isInLanelet_outsideBackFar) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 195, 2, hdmap_utils)); +} + +/** + * @note test calculation correctness when the pose lies outside the back of the lanelet, distance smaller than the tolerance + */ +TEST_F(PoseTest, isInLanelet_outsideBackClose) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0, hdmap_utils); + + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10, hdmap_utils)); +} + +/** + * @note test calculation correctness when there are no following lanelets and the pose lies within the lanelet + */ +TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002171, 31.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note test calculation correctness when there is a single following lanelet and the pose lies within the lanelet + */ +TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 5.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note test calculation correctness when there is a signle following lanelet and the pose lies after the end of the lanelet + */ +TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(3002167, 20.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note test calculation correctness when there are multiple following lanelets and the pose lies within the lanelet + */ +TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 5.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note test calculation correctness when there are multiple following lanelets and the pose lies after the end of the lanelet + */ +TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) +{ + const auto pose = + traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 120.0, 0.0, hdmap_utils); + + EXPECT_FALSE(traffic_simulator::pose::isAtEndOfLanelets(pose, hdmap_utils)); +} + +/** + * @note test function behavior when non existent lanelet::Id has been passed + */ +TEST_F(PoseTest, laneletLength_invalid) +{ + EXPECT_THROW( + traffic_simulator::pose::laneletLength(10000000000000000, hdmap_utils), std::runtime_error); +} + +/** + * @note test calculation correctness when a correct lanelet::Id has been passed + */ +TEST_F(PoseTest, laneletLength_valid) +{ + EXPECT_NEAR(traffic_simulator::pose::laneletLength(195, hdmap_utils), 107.74, 0.01); +} From 18bfc25ae2cdefd84bce37750db5536eaf0e10b0 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Mon, 9 Sep 2024 14:50:45 +0200 Subject: [PATCH 2/5] CR requested changes --- .../test/src/utils/test_pose.cpp | 160 +++++++++--------- 1 file changed, 80 insertions(+), 80 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index b8182b2617a..5834df083b6 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -42,39 +42,39 @@ class PoseTest : public testing::Test }; /** - * @note test created object values + * @note Test created object values. */ TEST(pose, quietNaNPose) { const auto pose = traffic_simulator::pose::quietNaNPose(); - EXPECT_TRUE(isnan(pose.position.x)); - EXPECT_TRUE(isnan(pose.position.y)); - EXPECT_TRUE(isnan(pose.position.z)); + EXPECT_TRUE(std::isnan(pose.position.x)); + EXPECT_TRUE(std::isnan(pose.position.y)); + EXPECT_TRUE(std::isnan(pose.position.z)); - EXPECT_EQ(pose.orientation.x, 0); - EXPECT_EQ(pose.orientation.y, 0); - EXPECT_EQ(pose.orientation.z, 0); - EXPECT_EQ(pose.orientation.w, 1); + EXPECT_DOUBLE_EQ(pose.orientation.x, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.y, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.z, 0.0); + EXPECT_DOUBLE_EQ(pose.orientation.w, 1.0); } /** - * @note test created object values + * @note Test created object values. */ TEST(pose, quietNaNLaneletPose) { const auto pose = traffic_simulator::pose::quietNaNLaneletPose(); EXPECT_EQ(pose.lanelet_id, std::numeric_limits::max()); - EXPECT_TRUE(isnan(pose.s)); - EXPECT_TRUE(isnan(pose.offset)); - EXPECT_TRUE(isnan(pose.rpy.x)); - EXPECT_TRUE(isnan(pose.rpy.y)); - EXPECT_TRUE(isnan(pose.rpy.z)); + EXPECT_TRUE(std::isnan(pose.s)); + EXPECT_TRUE(std::isnan(pose.offset)); + EXPECT_TRUE(std::isnan(pose.rpy.x)); + EXPECT_TRUE(std::isnan(pose.rpy.y)); + EXPECT_TRUE(std::isnan(pose.rpy.z)); } /** - * @note test canonicalization with a default constructed LaneletPose + * @note Test canonicalization with a default constructed LaneletPose. */ TEST_F(PoseTest, canonicalize_default) { @@ -85,7 +85,7 @@ TEST_F(PoseTest, canonicalize_default) } /** - * @note test canonicalization with an invalid LaneletPose + * @note Test canonicalization with an invalid LaneletPose. */ TEST_F(PoseTest, canonicalize_invalid) { @@ -96,7 +96,7 @@ TEST_F(PoseTest, canonicalize_invalid) } /** - * @note test canonicalization with a valid constructed LaneletPose + * @note Test canonicalization with a valid constructed LaneletPose. */ TEST_F(PoseTest, canonicalize_valid) { @@ -110,7 +110,7 @@ TEST_F(PoseTest, canonicalize_valid) } /** - * @note test conversion to geometry_msgs::msg::Pose from CanonicalizedLaneletPose + * @note Test conversion to geometry_msgs::msg::Pose from CanonicalizedLaneletPose. */ TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) { @@ -119,13 +119,13 @@ TEST_F(PoseTest, toMapPose_CanonicalizedLaneletPose) const geometry_msgs::msg::Pose pose = makePose( makePoint(81585.1622, 50176.9202, 34.2595), - geometry_msgs::build().x(0).y(0).z(-0.6397).w(0.7686)); + geometry_msgs::build().x(0.0).y(0.0).z(-0.6397).w(0.7686)); EXPECT_POSE_NEAR(traffic_simulator::pose::toMapPose(canonicalized_pose), pose, 0.01); } /** - * @note test conversion to geometry_msgs::msg::Pose from LaneletPose with HdMapUtils pointer + * @note Test conversion to geometry_msgs::msg::Pose from LaneletPose with HdMapUtils pointer. */ TEST_F(PoseTest, toMapPose_LaneletPose) { @@ -139,7 +139,7 @@ TEST_F(PoseTest, toMapPose_LaneletPose) } /** - * @note test function behavior with a pose that can be canonicalized + * @note Test function behavior with a pose that can be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) { @@ -159,7 +159,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) } /** - * @note test function behavior with a pose that can not be canonicalized + * @note Test function behavior with a pose that can not be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) { @@ -170,7 +170,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) } /** - * @note test function behavior with a pose that can be canonicalized + * @note Test function behavior with a pose that can be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) { @@ -190,7 +190,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_valid) } /** - * @note test function behavior with a pose that can not be canonicalized, matching distance too large + * @note Test function behavior with a pose that can not be canonicalized, matching distance too large. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) { @@ -206,7 +206,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) } /** - * @note test function behavior with an empty unique_route_lanelets vector and a pose that can not be canonicalized + * @note Test function behavior with an empty unique_route_lanelets vector and a pose that can not be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) { @@ -214,12 +214,12 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), {}, true, 1.0, hdmap_utils), + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils), std::nullopt); } /** - * @note test function behavior with an empty unique_route_lanelets vector and a pose that can be canonicalized + * @note Test function behavior with an empty unique_route_lanelets vector and a pose that can be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) { @@ -239,7 +239,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) } /** - * @note test function behavior with a non empty unique_route_lanelets vector and a pose that can not be canonicalized + * @note Test function behavior with a non empty unique_route_lanelets vector and a pose that can not be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) { @@ -252,7 +252,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) } /** - * @note test function behavior with a non empty unique_route_lanelets vector and a pose that can be canonicalized + * @note Test function behavior with a non empty unique_route_lanelets vector and a pose that can be canonicalized. */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) { @@ -272,7 +272,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) } /** - * @note test calculation correctness with a non trivial example + * @note Test calculation correctness with a non trivial example. */ TEST(pose, transformRelativePoseToGlobal) { @@ -287,12 +287,12 @@ TEST(pose, transformRelativePoseToGlobal) } /** - * @note test calculation correctness when only one pose is zeroed, the goal is to obtain a pose equal to the second argument + * @note Test calculation correctness when only one pose is zeroed, the goal is to obtain a pose equal to the second argument. */ TEST(pose, relativePose_poses_zeros) { - const auto from = makePose(makePoint(0, 0, 0)); - const auto to = makePose(makePoint(10, 51, 2)); + const auto from = makePose(makePoint(0.0, 0.0, 0.0)); + const auto to = makePose(makePoint(10.0, 51.0, 2.0)); const auto relative = traffic_simulator::pose::relativePose(from, to); ASSERT_TRUE(relative.has_value()); @@ -300,12 +300,12 @@ TEST(pose, relativePose_poses_zeros) } /** - * @note test calculation correctness when both poses are zeroed + * @note Test calculation correctness when both poses are zeroed. */ TEST(pose, relativePose_poses_zero) { - const auto from = makePose(makePoint(0, 0, 0)); - const auto to = makePose(makePoint(0, 0, 0)); + const auto from = makePose(makePoint(0.0, 0.0, 0.0)); + const auto to = makePose(makePoint(0.0, 0.0, 0.0)); const auto relative = traffic_simulator::pose::relativePose(from, to); ASSERT_TRUE(relative.has_value()); @@ -313,7 +313,7 @@ TEST(pose, relativePose_poses_zero) } /** - * @note ttest calculation correctness with a non trivial example + * @note ttest calculation correctness with a non trivial example. */ TEST(pose, relativePose_poses_complex) { @@ -328,7 +328,7 @@ TEST(pose, relativePose_poses_complex) } /** - * @note test calculation correctness with the overload + * @note Test calculation correctness with the overload. */ TEST_F(PoseTest, relativePose_first) { @@ -348,7 +348,7 @@ TEST_F(PoseTest, relativePose_first) } /** - * @note test calculation correctness with the overload + * @note Test calculation correctness with the overload. */ TEST_F(PoseTest, relativePose_second) { @@ -367,54 +367,54 @@ TEST_F(PoseTest, relativePose_second) } /** - * @note test calculation correctness when bounding boxes are disjoint + * @note Test calculation correctness when bounding boxes are disjoint. */ TEST_F(PoseTest, boundingBoxRelativePose_disjoint) { const auto pose_relative = makePose(makePoint(0.1108, -20.5955, 0.0)); const auto from = makePose(makePoint(81600.3967, 50094.4298, 34.6759)); const auto to = makePose(makePoint(81604.5076, 50071.8343, 40.8482)); - const auto boundingBox = makeBoundingBox(); + const auto bounding_box = makeBoundingBox(); const auto relative = - traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); ASSERT_TRUE(relative.has_value()); EXPECT_POSE_NEAR(pose_relative, relative.value(), 0.01); } /** - * @note test calculation correctness when bounding boxes share an edge + * @note Test calculation correctness when bounding boxes share an edge. */ TEST_F(PoseTest, boundingBoxRelativePose_commonEdge) { const auto from = makePose(makePoint(81600, 50094, 34)); const auto to = makePose(makePoint(81601, 50094, 34)); - const auto boundingBox = makeSmallBoundingBox(); + const auto bounding_box = makeSmallBoundingBox(); const auto relative = - traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); EXPECT_FALSE(relative.has_value()); } /** - * @note test calculation correctness when bounding boxes intersect + * @note Test calculation correctness when bounding boxes intersect. */ TEST_F(PoseTest, boundingBoxRelativePose_intersect) { const auto from = makePose(makePoint(81600, 50094, 34)); const auto to = makePose(makePoint(81600.5, 50094, 34)); - const auto boundingBox = makeSmallBoundingBox(); + const auto bounding_box = makeSmallBoundingBox(); const auto relative = - traffic_simulator::pose::boundingBoxRelativePose(from, boundingBox, to, boundingBox); + traffic_simulator::pose::boundingBoxRelativePose(from, bounding_box, to, bounding_box); EXPECT_FALSE(relative.has_value()); } /** - * @note test s-value calculation correctness when longitudinal distance between the poses can not be calculated + * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated. */ TEST_F(PoseTest, relativeLaneletPose_s_invalid) { @@ -425,11 +425,11 @@ TEST_F(PoseTest, relativeLaneletPose_s_invalid) const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); - EXPECT_TRUE(isnan(relative.s)); + EXPECT_TRUE(std::isnan(relative.s)); } /** - * @note test s-value calculation correctness when longitudinal distance between the poses can be calculated + * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated. */ TEST_F(PoseTest, relativeLaneletPose_s_valid) { @@ -444,7 +444,7 @@ TEST_F(PoseTest, relativeLaneletPose_s_valid) } /** - * @note test offset-value calculation correctness when lateral distance between the poses can not be calculated + * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated. */ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) { @@ -455,11 +455,11 @@ TEST_F(PoseTest, relativeLaneletPose_offset_invalid) const auto relative = traffic_simulator::pose::relativeLaneletPose(from, to, false, hdmap_utils); - EXPECT_TRUE(isnan(relative.offset)); + EXPECT_TRUE(std::isnan(relative.offset)); } /** - * @note test offset-value calculation correctness when lateral distance between the poses can be calculated + * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated. */ TEST_F(PoseTest, relativeLaneletPose_offset_valid) { @@ -474,7 +474,7 @@ TEST_F(PoseTest, relativeLaneletPose_offset_valid) } /** - * @note test s-value calculation correctness when longitudinal distance between the poses can not be calculated + * @note Test s-value calculation correctness when longitudinal distance between the poses can not be calculated. */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) { @@ -482,16 +482,16 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_invalid) traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); - const auto boundingBox = makeBoundingBox(); + const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, boundingBox, to, boundingBox, false, hdmap_utils); + from, bounding_box, to, bounding_box, false, hdmap_utils); - EXPECT_TRUE(isnan(relative.s)); + EXPECT_TRUE(std::isnan(relative.s)); } /** - * @note test s-value calculation correctness when longitudinal distance between the poses can be calculated + * @note Test s-value calculation correctness when longitudinal distance between the poses can be calculated. */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) { @@ -499,16 +499,16 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_s_valid) traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 0.0, hdmap_utils); - const auto boundingBox = makeBoundingBox(); + const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, boundingBox, to, boundingBox, false, hdmap_utils); + from, bounding_box, to, bounding_box, false, hdmap_utils); EXPECT_NEAR(relative.s, 103.74, 0.01); } /** - * @note test offset-value calculation correctness when lateral distance between the poses can not be calculated + * @note Test offset-value calculation correctness when lateral distance between the poses can not be calculated. */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) { @@ -516,16 +516,16 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_invalid) traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(196, 0.0, 1.0, hdmap_utils); - const auto boundingBox = makeBoundingBox(); + const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, boundingBox, to, boundingBox, false, hdmap_utils); + from, bounding_box, to, bounding_box, false, hdmap_utils); - EXPECT_TRUE(isnan(relative.s)); + EXPECT_TRUE(std::isnan(relative.s)); } /** - * @note test offset-value calculation correctness when lateral distance between the poses can be calculated + * @note Test offset-value calculation correctness when lateral distance between the poses can be calculated. */ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) { @@ -533,16 +533,16 @@ TEST_F(PoseTest, boundingBoxRelativeLaneletPose_offset_valid) traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, -1.0, hdmap_utils); const auto to = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, 0.0, 1.0, hdmap_utils); - const auto boundingBox = makeBoundingBox(); + const auto bounding_box = makeBoundingBox(); const auto relative = traffic_simulator::pose::boundingBoxRelativeLaneletPose( - from, boundingBox, to, boundingBox, false, hdmap_utils); + from, bounding_box, to, bounding_box, false, hdmap_utils); EXPECT_EQ(relative.offset, 0.0); } /** - * @note test calculation correctness when the pose lies within the lanelet + * @note Test calculation correctness when the pose lies within the lanelet. */ TEST_F(PoseTest, isInLanelet_inside) { @@ -553,7 +553,7 @@ TEST_F(PoseTest, isInLanelet_inside) } /** - * @note test calculation correctness when the pose lies outside the front of the llanelet, distance greater than the tolerance + * @note Test calculation correctness when the pose lies outside the front of the lanelet, distance greater than the tolerance. */ TEST_F(PoseTest, isInLanelet_outsideFrontFar) { @@ -564,7 +564,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontFar) } /** - * @note test calculation correctness when the pose lies outside the front of the llanelet, distance smaller than the tolerance + * @note Test calculation correctness when the pose lies outside the front of the lanelet, distance smaller than the tolerance. */ TEST_F(PoseTest, isInLanelet_outsideFrontClose) { @@ -575,7 +575,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontClose) } /** - * @note test calculation correctness when the pose lies outside the back of the llanelet, distance greater than the tolerance + * @note Test calculation correctness when the pose lies outside the back of the lanelet, distance greater than the tolerance. */ TEST_F(PoseTest, isInLanelet_outsideBackFar) { @@ -586,7 +586,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackFar) } /** - * @note test calculation correctness when the pose lies outside the back of the lanelet, distance smaller than the tolerance + * @note Test calculation correctness when the pose lies outside the back of the lanelet, distance smaller than the tolerance. */ TEST_F(PoseTest, isInLanelet_outsideBackClose) { @@ -597,7 +597,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackClose) } /** - * @note test calculation correctness when there are no following lanelets and the pose lies within the lanelet + * @note Test calculation correctness when there are no following lanelets and the pose lies within the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) { @@ -608,7 +608,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_noFollowing_within) } /** - * @note test calculation correctness when there is a single following lanelet and the pose lies within the lanelet + * @note Test calculation correctness when there is a single following lanelet and the pose lies within the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) { @@ -619,7 +619,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) } /** - * @note test calculation correctness when there is a signle following lanelet and the pose lies after the end of the lanelet + * @note Test calculation correctness when there is a signle following lanelet and the pose lies after the end of the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) { @@ -630,7 +630,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) } /** - * @note test calculation correctness when there are multiple following lanelets and the pose lies within the lanelet + * @note Test calculation correctness when there are multiple following lanelets and the pose lies within the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) { @@ -641,7 +641,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_within) } /** - * @note test calculation correctness when there are multiple following lanelets and the pose lies after the end of the lanelet + * @note Test calculation correctness when there are multiple following lanelets and the pose lies after the end of the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) { @@ -652,7 +652,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_multipleFollowing_outside) } /** - * @note test function behavior when non existent lanelet::Id has been passed + * @note Test function behavior when non existent lanelet::Id has been passed. */ TEST_F(PoseTest, laneletLength_invalid) { @@ -661,7 +661,7 @@ TEST_F(PoseTest, laneletLength_invalid) } /** - * @note test calculation correctness when a correct lanelet::Id has been passed + * @note Test calculation correctness when a correct lanelet::Id has been passed. */ TEST_F(PoseTest, laneletLength_valid) { From ae824022e4fd0e1136b085f13f403326d0f04c19 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Tue, 10 Sep 2024 09:33:46 +0200 Subject: [PATCH 3/5] CR requested changes part 2 --- .../test/src/utils/test_pose.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index 5834df083b6..fcbe0acf606 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -163,7 +163,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_valid) */ TEST_F(PoseTest, toCanonicalizedLaneletPose_noBoundingBox_noRoute_invalid) { - const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose(pose, true, hdmap_utils), std::nullopt); @@ -210,7 +210,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_noRoute_invalid) */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyInvalid) { - const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose( @@ -229,7 +229,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), {}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{}, true, 1.0, hdmap_utils); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -243,7 +243,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_emptyValid) */ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyInvalid) { - const geometry_msgs::msg::Pose pose = makePose(makePoint(0, 0, 0)); + const geometry_msgs::msg::Pose pose = makePose(makePoint(0.0, 0.0, 0.0)); EXPECT_EQ( traffic_simulator::pose::toCanonicalizedLaneletPose( @@ -262,7 +262,7 @@ TEST_F(PoseTest, toCanonicalizedLaneletPose_BoundingBox_route_nonEmptyValid) const geometry_msgs::msg::Pose pose = static_cast(lanelet_pose); const auto canonicalized_pose = traffic_simulator::pose::toCanonicalizedLaneletPose( - pose, makeBoundingBox(), {195}, true, 1.0, hdmap_utils); + pose, makeBoundingBox(), lanelet::Ids{195}, true, 1.0, hdmap_utils); ASSERT_TRUE(canonicalized_pose.has_value()); EXPECT_POSE_NEAR(pose, static_cast(canonicalized_pose.value()), 0.01); @@ -549,7 +549,8 @@ TEST_F(PoseTest, isInLanelet_inside) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 0.0, 0.0, hdmap_utils); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 0, hdmap_utils)); + EXPECT_TRUE(traffic_simulator::pose::isInLanelet( + pose, 195, std::numeric_limits::epsilon(), hdmap_utils)); } /** @@ -560,7 +561,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontFar) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -10.0, 0.0, hdmap_utils); - EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1, hdmap_utils)); + EXPECT_FALSE(traffic_simulator::pose::isInLanelet(pose, 3002163, 1.0, hdmap_utils)); } /** @@ -571,7 +572,7 @@ TEST_F(PoseTest, isInLanelet_outsideFrontClose) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(3002163, -1.0, 0.0, hdmap_utils); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2, hdmap_utils)); + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 3002163, 2.0, hdmap_utils)); } /** @@ -593,7 +594,7 @@ TEST_F(PoseTest, isInLanelet_outsideBackClose) const auto pose = traffic_simulator::helper::constructCanonicalizedLaneletPose(195, 110.0, 0.0, hdmap_utils); - EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10, hdmap_utils)); + EXPECT_TRUE(traffic_simulator::pose::isInLanelet(pose, 195, 10.0, hdmap_utils)); } /** From 816603cd625965b12cb09395658a725ced4ddc00 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Wed, 11 Sep 2024 06:50:11 +0200 Subject: [PATCH 4/5] Fix typos in comments --- simulation/traffic_simulator/test/src/utils/test_pose.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index fcbe0acf606..843fd56d397 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -313,7 +313,7 @@ TEST(pose, relativePose_poses_zero) } /** - * @note ttest calculation correctness with a non trivial example. + * @note Test calculation correctness with a non trivial example. */ TEST(pose, relativePose_poses_complex) { @@ -620,7 +620,7 @@ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_within) } /** - * @note Test calculation correctness when there is a signle following lanelet and the pose lies after the end of the lanelet. + * @note Test calculation correctness when there is a single following lanelet and the pose lies after the end of the lanelet. */ TEST_F(PoseTest, isAtEndOfLanelets_singleFollowing_outside) { From 6d6e49a9a59ebf86011a16b1589a283c09ac495b Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Thu, 12 Sep 2024 12:17:57 +0200 Subject: [PATCH 5/5] Change names of relativePose tests --- simulation/traffic_simulator/test/src/utils/test_pose.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/simulation/traffic_simulator/test/src/utils/test_pose.cpp b/simulation/traffic_simulator/test/src/utils/test_pose.cpp index 843fd56d397..a7132a31c62 100644 --- a/simulation/traffic_simulator/test/src/utils/test_pose.cpp +++ b/simulation/traffic_simulator/test/src/utils/test_pose.cpp @@ -289,7 +289,7 @@ TEST(pose, transformRelativePoseToGlobal) /** * @note Test calculation correctness when only one pose is zeroed, the goal is to obtain a pose equal to the second argument. */ -TEST(pose, relativePose_poses_zeros) +TEST(pose, relativePose_poses_zero) { const auto from = makePose(makePoint(0.0, 0.0, 0.0)); const auto to = makePose(makePoint(10.0, 51.0, 2.0)); @@ -302,7 +302,7 @@ TEST(pose, relativePose_poses_zeros) /** * @note Test calculation correctness when both poses are zeroed. */ -TEST(pose, relativePose_poses_zero) +TEST(pose, relativePose_poses_zeros) { const auto from = makePose(makePoint(0.0, 0.0, 0.0)); const auto to = makePose(makePoint(0.0, 0.0, 0.0)); @@ -330,7 +330,7 @@ TEST(pose, relativePose_poses_complex) /** * @note Test calculation correctness with the overload. */ -TEST_F(PoseTest, relativePose_first) +TEST_F(PoseTest, relativePose_canonicalized_end_position) { const auto pose_relative = makePose( makePoint(9.9999, -0.0009, 0.0126), @@ -350,7 +350,7 @@ TEST_F(PoseTest, relativePose_first) /** * @note Test calculation correctness with the overload. */ -TEST_F(PoseTest, relativePose_second) +TEST_F(PoseTest, relativePose_canonicalized_start_position) { const auto pose_relative = makePose( makePoint(145244.7916, 786706.3326, 0.0127),