From d4475cf55dbe9e2c2b5f02ec4d7f75990563e230 Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 9 May 2024 16:26:27 +0200 Subject: [PATCH 1/4] Revert "remove issues section and leave only passing tests" This reverts commit 843fa93075e966ad48be05d2a0afb43141d95599. --- .../test/src/entity/test_entity_base.cpp | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 5d76d3ec8fc..8b6a814b05c 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -774,6 +774,105 @@ TEST(EntityBase, getDistanceToLeftLaneBound) EXPECT_NEAR(distance_result, distance_actual, 0.1); } +TEST(EntityBase, getDistanceToRightLaneBound) +{ + // see issue 1 at the end of this file + const double lane_width = 3.0; + const double entity_center_offset = -0.5; + lanelet::Id id_previous = 34666; + lanelet::Id id = 120659; + lanelet::Id id_next = 120660; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(entity_center_offset); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setRouteLanelets({id_previous, id, id_next}); + + auto distance_result = dummy.getDistanceToRightLaneBound(); + double distance_actual = (lane_width - bbox.dimensions.y) / 2 + entity_center_offset; + EXPECT_NEAR(distance_result, distance_actual, 0.1); +} + +TEST(EntityBase, getDistanceToLaneBound) +{ + // see issue 1 at the end of this file + const double lane_width = 3.0; + const double entity_center_offset = -0.5; + lanelet::Id id_previous = 34666; + lanelet::Id id = 120659; + lanelet::Id id_next = 120660; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(entity_center_offset); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setRouteLanelets({id_previous, id, id_next}); + + auto distance_result = dummy.getDistanceToLaneBound(); + double distance_actual = std::min( + (lane_width - bbox.dimensions.y) / 2 - entity_center_offset, + (lane_width - bbox.dimensions.y) / 2 + entity_center_offset); + EXPECT_NEAR(distance_result, distance_actual, 0.1); +} + +TEST(EntityBase, getDistanceToLeftLaneBound_empty) +{ + // fix is to be implemented, 2nd issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + auto ids = std::vector{}; + + // auto left = dummy.getDistanceToLeftLaneBound(ids); + + throw std::runtime_error("Fix not implemented"); +} + +TEST(EntityBase, getDistanceToRightLaneBound_empty) +{ + // fix is to be implemented, 2nd issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + auto ids = std::vector{}; + + // auto right = dummy.getDistanceToRightLaneBound(ids); + throw std::runtime_error("Fix not implemented"); +} + +TEST(EntityBase, getDistanceToLaneBound_empty) +{ + // fix is to be implemented, 2nd issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + auto ids = std::vector{}; + + // auto distance = dummy.getDistanceToLaneBound(ids); + + throw std::runtime_error("Fix not implemented"); +} + TEST(EntityBase, getDistanceToLeftLaneBound_many) { const double entity_center_offset = -0.5; @@ -793,6 +892,46 @@ TEST(EntityBase, getDistanceToLeftLaneBound_many) EXPECT_NEAR(distance_result, distance_actual, 0.1); } +TEST(EntityBase, getDistanceToRightLaneBound_many) +{ + // typo, 1st issue + const double entity_center_offset = -0.5; + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto bbox = makeBoundingBox(entity_center_offset); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + lanelet::Ids ids{id_0, id_1}; + auto distance_result = dummy.getDistanceToRightLaneBound(ids); + auto distance_actual = dummy.getDistanceToRightLaneBound(id_0); + EXPECT_NEAR(distance_result, distance_actual, 0.1); +} + +TEST(EntityBase, getDistanceToLaneBound_many) +{ + // typo, 1st issue + const double entity_center_offset = -0.5; + lanelet::Id id_0 = 120659; + lanelet::Id id_1 = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto bbox = makeBoundingBox(entity_center_offset); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + lanelet::Ids ids{id_0, id_1}; + auto distance_result = dummy.getDistanceToLaneBound(ids); + auto distance_actual = dummy.getDistanceToLaneBound(id_0); + EXPECT_NEAR(distance_result, distance_actual, 0.1); +} + TEST(EntityBase, getLaneletPose_notOnRoadAndCrosswalkPedestrian) { auto hdmap_utils = makeHdMapUtilsSharedPointer(); @@ -938,3 +1077,25 @@ TEST(EntityBase, getMapPoseFromRelativePose_relative) auto ref_pose = makeCanonicalizedLaneletPose(hdmap_utils, id, s); EXPECT_POSE_NEAR(result_pose, static_cast(ref_pose), eps); } + +/* +ISSUES: +1: 182: "getDistanceToRightLaneBound" typo +2: 183: sigsegv on empty "distances" vector +3: 595, 581: removal of these lines might be considered. + It looks like a job to change the "target_speed" is added after + the "target_speed" is already set for the desired value. + The job will do nothing, if continuous == false and the target_speed is already set +4: 644: this line should be moved down, just like in 604, 630, 587. + "target_speed" often will not be set because of this line. +5: 385, 437, 512, 545: it is unclear, why is a request to change the speed being issued, + and then, immediately, the speed is forcefully changed. + The job assigned to change the target speed will return early anyway, + after checking "isTargetSpeedReached" (if continuous == false) +6: 551, 618: "requestSpeedChange" throws on invalid entity name if-and-only-if continuous is false. + Order of checking in the if statement might be swapped to fix this. +7: 49, 59: it should be considered to make the "cancelRequest" and "appendDebugMarker" functions + purely virtual, or make them throw. +8: 565: "requestSpeedChange" throws if "continuous" == true with "RelativeTargetSpeed", + whereas it does not when called with absolute ratget speed, in line 456. +*/ From f6d408bae3f616ac86fdc130ec056663e22255ac Mon Sep 17 00:00:00 2001 From: robomic Date: Thu, 9 May 2024 16:26:47 +0200 Subject: [PATCH 2/4] Revert "remove requestspeedchange tests" This reverts commit 19f21fba386f7e418b953619fdf6837658ac54ec. --- .../test/src/entity/test_entity_base.cpp | 1035 +++++++++++++++++ 1 file changed, 1035 insertions(+) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 8b6a814b05c..f43fe14984a 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -1078,6 +1078,1041 @@ TEST(EntityBase, getMapPoseFromRelativePose_relative) EXPECT_POSE_NEAR(result_pose, static_cast(ref_pose), eps); } +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteNotContinuous) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + const double target_speed = 3.0; + const bool continuous = false; + + dummy.requestSpeedChange(target_speed, continuous); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteContinuous) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + const double target_speed = 3.0; + const bool continuous = true; + + dummy.requestSpeedChange(target_speed, continuous); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteReached) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + const double target_speed = 3.0; + const bool continuous = false; + + dummy.setLinearVelocity(target_speed); + dummy.requestSpeedChange(target_speed, continuous); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuousInvalidTarget) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "invalid_name", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + bool continuous = false; + + EXPECT_THROW(dummy.requestSpeedChange(relative_taget_speed, continuous), common::Error); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuous) +{ + // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + bool continuous = false; + EXPECT_NO_THROW(dummy.requestSpeedChange(relative_taget_speed, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(dummy._getTargetSpeed().value(), target_speed); + + dummy.setLinearVelocity(target_speed); + + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuousInvalidTarget) +{ + // "requestSpeedChange" will not throw when "continuous" == true, explained in the 6th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "invalid_name", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + bool continuous = true; + + EXPECT_THROW(dummy.requestSpeedChange(relative_taget_speed, continuous), common::Error); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuous) +{ + // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + bool continuous = true; + + EXPECT_NO_THROW(dummy.requestSpeedChange(relative_taget_speed, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNoneReached) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + dummy.setLinearVelocity(target_speed); + + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNone) +{ + // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionStep) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 0.0); + auto transition = traffic_simulator::speed_change::Transition::STEP; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeContinuous) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, 10.0); + auto transition = traffic_simulator::speed_change::Transition::STEP; + bool continuous = true; + + EXPECT_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous), + common::Error); + + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeReached) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + dummy.setLinearVelocity(target_speed); + + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 10.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionStep) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + const double initial_speed = 25.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 10.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::STEP; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionNotStepNoTime) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 0.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNoneReached) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double target_speed = 20.0; + dummy.setLinearVelocity(target_speed); + + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNone) +{ + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double target_speed = 20.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionStep) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + + const double target_speed = 20.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 0.0); + auto transition = traffic_simulator::speed_change::Transition::STEP; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionStep) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + const double initial_speed = 25.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 10.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::STEP; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionNotStepNoTime) +{ + // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 0.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionLinear) +{ + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 5.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST( + EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoAccelerate) +{ + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 5.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST( + EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoDecelerate) +{ + lanelet::Id id = 120659; + + const double initial_speed = 30.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 5.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionLinear) +{ + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 0.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST( + EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoAccelerate) +{ + // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 0.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST( + EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoDecelerate) +{ + // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue + lanelet::Id id = 120659; + + const double initial_speed = 30.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 0.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + EXPECT_EQ(dummy.getCurrentTwist().linear.x, initial_speed); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionLinear) +{ + lanelet::Id id = 120659; + + const double initial_speed = 25.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 10.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionAuto) +{ + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double target_speed = 20.0; + const double constraint_value = 10.0; + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW(dummy.requestSpeedChange(target_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionLinear) +{ + lanelet::Id id = 120659; + + const double initial_speed = 25.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 10.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::LINEAR; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + +TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionAuto) +{ + lanelet::Id id = 120659; + + const double initial_speed = 10.0; + auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto bbox = makeBoundingBox(); + auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + dummy.setLinearVelocity(initial_speed); + + const double bob_speed = 17.0; + const double delta_speed = 3.0; + const double target_speed = bob_speed + delta_speed; + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + std::unordered_map + others; + others.emplace("bob_entity", bob_status); + dummy.setOtherStatus(others); + + const double constraint_value = 10.0; + traffic_simulator::speed_change::RelativeTargetSpeed relative_taget_speed( + "bob_entity", traffic_simulator::speed_change::RelativeTargetSpeed::Type::DELTA, delta_speed); + traffic_simulator::speed_change::Constraint constraint( + traffic_simulator::speed_change::Constraint::Type::TIME, constraint_value); + auto transition = traffic_simulator::speed_change::Transition::AUTO; + bool continuous = false; + + EXPECT_NO_THROW( + dummy.requestSpeedChange(relative_taget_speed, transition, constraint, continuous)); + + const double current_time = 5.0; + const double step_time = 7.0; + dummy.onPostUpdate(current_time, step_time); + EXPECT_TRUE(dummy._getTargetSpeed().has_value()); + EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); + + dummy.setLinearVelocity(target_speed); + dummy.onPostUpdate(current_time, step_time); + EXPECT_FALSE(dummy._getTargetSpeed().has_value()); + + auto default_constraints = dummy.getDefaultDynamicConstraints(); + auto current_constraints = dummy.getDynamicConstraints(); + EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); +} + /* ISSUES: 1: 182: "getDistanceToRightLaneBound" typo From 0f29d942451dd0d7572813eabc01e41c37e46ae0 Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 23 May 2024 14:08:47 +0200 Subject: [PATCH 3/4] Fix literals Signed-off-by: Mateusz Palczuk --- .../test/src/entity/test_entity_base.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 08401ea5e43..95dd0c1204c 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -625,10 +625,10 @@ TEST(EntityBase, getDistanceToRightLaneBound) auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - dummy.setRouteLanelets({id_previous, id, id_next}); + dummy._setRouteLanelets({id_previous, id, id_next}); auto distance_result = dummy.getDistanceToRightLaneBound(); - double distance_actual = (lane_width - bbox.dimensions.y) / 2 + entity_center_offset; + double distance_actual = (lane_width - bbox.dimensions.y) / 2.0 + entity_center_offset; EXPECT_NEAR(distance_result, distance_actual, 0.1); } @@ -647,12 +647,12 @@ TEST(EntityBase, getDistanceToLaneBound) auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - dummy.setRouteLanelets({id_previous, id, id_next}); + dummy._setRouteLanelets({id_previous, id, id_next}); auto distance_result = dummy.getDistanceToLaneBound(); double distance_actual = std::min( - (lane_width - bbox.dimensions.y) / 2 - entity_center_offset, - (lane_width - bbox.dimensions.y) / 2 + entity_center_offset); + (lane_width - bbox.dimensions.y) / 2.0 - entity_center_offset, + (lane_width - bbox.dimensions.y) / 2.0 + entity_center_offset); EXPECT_NEAR(distance_result, distance_actual, 0.1); } From 8ed561e699be3b79ba460a5070e6de5ea2bd0f7d Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 23 May 2024 14:48:53 +0200 Subject: [PATCH 4/4] Use test fixtures Signed-off-by: Mateusz Palczuk --- .../test/src/entity/test_entity_base.cpp | 382 ++++-------------- 1 file changed, 72 insertions(+), 310 deletions(-) diff --git a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp index 95dd0c1204c..ad244c90ef6 100644 --- a/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp +++ b/simulation/traffic_simulator/test/src/entity/test_entity_base.cpp @@ -619,12 +619,12 @@ TEST(EntityBase, getDistanceToRightLaneBound) lanelet::Id id = 120659; lanelet::Id id_next = 120660; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id); auto bbox = makeBoundingBox(entity_center_offset); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + DummyEntity dummy("dummy_entity", status, hdmap_utils); dummy._setRouteLanelets({id_previous, id, id_next}); auto distance_result = dummy.getDistanceToRightLaneBound(); @@ -641,12 +641,12 @@ TEST(EntityBase, getDistanceToLaneBound) lanelet::Id id = 120659; lanelet::Id id_next = 120660; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id); auto bbox = makeBoundingBox(entity_center_offset); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + DummyEntity dummy("dummy_entity", status, hdmap_utils); dummy._setRouteLanelets({id_previous, id, id_next}); auto distance_result = dummy.getDistanceToLaneBound(); @@ -656,17 +656,9 @@ TEST(EntityBase, getDistanceToLaneBound) EXPECT_NEAR(distance_result, distance_actual, 0.1); } -TEST(EntityBase, getDistanceToLeftLaneBound_empty) +TEST_F(EntityBaseTest, getDistanceToLeftLaneBound_empty) { // fix is to be implemented, 2nd issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); auto ids = std::vector{}; // auto left = dummy.getDistanceToLeftLaneBound(ids); @@ -674,34 +666,18 @@ TEST(EntityBase, getDistanceToLeftLaneBound_empty) throw std::runtime_error("Fix not implemented"); } -TEST(EntityBase, getDistanceToRightLaneBound_empty) +TEST_F(EntityBaseTest, getDistanceToRightLaneBound_empty) { // fix is to be implemented, 2nd issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); auto ids = std::vector{}; // auto right = dummy.getDistanceToRightLaneBound(ids); throw std::runtime_error("Fix not implemented"); } -TEST(EntityBase, getDistanceToLaneBound_empty) +TEST_F(EntityBaseTest, getDistanceToLaneBound_empty) { // fix is to be implemented, 2nd issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); auto ids = std::vector{}; // auto distance = dummy.getDistanceToLaneBound(ids); @@ -735,12 +711,12 @@ TEST(EntityBase, getDistanceToRightLaneBound_many) lanelet::Id id_0 = 120659; lanelet::Id id_1 = 120659; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id_0); auto bbox = makeBoundingBox(entity_center_offset); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + DummyEntity dummy("dummy_entity", status, hdmap_utils); lanelet::Ids ids{id_0, id_1}; auto distance_result = dummy.getDistanceToRightLaneBound(ids); @@ -755,12 +731,12 @@ TEST(EntityBase, getDistanceToLaneBound_many) lanelet::Id id_0 = 120659; lanelet::Id id_1 = 120659; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id_0); + auto hdmap_utils = makeHdMapUtilsSharedPointer(); + auto pose = makeCanonicalizedLaneletPose(hdmap_utils, id_0); auto bbox = makeBoundingBox(entity_center_offset); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); + auto status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); + DummyEntity dummy("dummy_entity", status, hdmap_utils); lanelet::Ids ids{id_0, id_1}; auto distance_result = dummy.getDistanceToLaneBound(ids); @@ -906,16 +882,8 @@ TEST_F(EntityBaseTest, getMapPoseFromRelativePose_relative) EXPECT_POSE_NEAR(result_pose, static_cast(ref_pose), eps); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteNotContinuous) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteNotContinuous) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); EXPECT_FALSE(dummy._getTargetSpeed().has_value()); const double target_speed = 3.0; @@ -937,16 +905,8 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteNotContinuous) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteContinuous) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteContinuous) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); EXPECT_FALSE(dummy._getTargetSpeed().has_value()); const double target_speed = 3.0; @@ -969,16 +929,8 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteContinuous) EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteReached) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteReached) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); EXPECT_FALSE(dummy._getTargetSpeed().has_value()); const double target_speed = 3.0; @@ -989,19 +941,11 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteReached) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuousInvalidTarget) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeNotContinuousInvalidTarget) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1020,21 +964,13 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuousInvalidTarge EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuous) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeNotContinuous) { // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1058,20 +994,12 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeNotContinuous) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuousInvalidTarget) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeContinuousInvalidTarget) { // "requestSpeedChange" will not throw when "continuous" == true, explained in the 6th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1090,21 +1018,13 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuousInvalidTarget) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuous) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeContinuous) { // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1129,22 +1049,14 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeContinuous) EXPECT_EQ(target_speed, dummy._getTargetSpeed().value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNoneReached) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintNoneReached) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; dummy.setLinearVelocity(target_speed); - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1166,21 +1078,13 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNoneReached) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNone) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintNone) { // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1207,21 +1111,13 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintNone) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionStep) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionStep) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1240,19 +1136,11 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTra EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeContinuous) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintTimeContinuous) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1272,22 +1160,14 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeContinuous) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeReached) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeReached) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; dummy.setLinearVelocity(target_speed); - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1310,23 +1190,16 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeReached) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionStep) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionStep) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - const double initial_speed = 25.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1346,23 +1219,16 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionS EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionNotStepNoTime) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionNotStepNoTime) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1382,16 +1248,8 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionN EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNoneReached) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintNoneReached) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double target_speed = 20.0; dummy.setLinearVelocity(target_speed); @@ -1408,16 +1266,8 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNoneReached) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNone) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintNone) { - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double target_speed = 20.0; traffic_simulator::speed_change::Constraint constraint( traffic_simulator::speed_change::Constraint::Type::NONE, 0.0); @@ -1437,17 +1287,9 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintNone) EXPECT_FALSE(dummy._getTargetSpeed().has_value()); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionStep) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionStep) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); - const double target_speed = 20.0; traffic_simulator::speed_change::Constraint constraint( traffic_simulator::speed_change::Constraint::Type::LONGITUDINAL_ACCELERATION, 0.0); @@ -1459,17 +1301,10 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTra EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionStep) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionStep) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - const double initial_speed = 25.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1484,17 +1319,10 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionS EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionNotStepNoTime) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionNotStepNoTime) { // internal call of "requestSpeedChange" is being issued with manual and forceful speed change, 5th issue - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1509,16 +1337,9 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionN EXPECT_EQ(dummy.getCurrentTwist().linear.x, target_speed); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionLinear) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionLinear) { - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1547,17 +1368,11 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTra EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST( - EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoAccelerate) +TEST_F( + EntityBaseTest, + requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoAccelerate) { - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1586,17 +1401,11 @@ TEST( EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST( - EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoDecelerate) +TEST_F( + EntityBaseTest, + requestSpeedChange_targetSpeedAbsoluteConstraintAccelerationTransitionAutoDecelerate) { - lanelet::Id id = 120659; - const double initial_speed = 30.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1625,22 +1434,15 @@ TEST( EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionLinear) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionLinear) { - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1673,24 +1475,18 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTra EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST( - EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoAccelerate) +TEST_F( + EntityBaseTest, + requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoAccelerate) { // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1724,24 +1520,18 @@ TEST( EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST( - EntityBase, requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoDecelerate) +TEST_F( + EntityBaseTest, + requestSpeedChange_targetSpeedRelativeConstraintAccelerationTransitionAutoDecelerate) { // "requestSpeedChange" is unable to change the "target_speed", explained in the 4th issue - lanelet::Id id = 120659; - const double initial_speed = 30.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1775,16 +1565,9 @@ TEST( EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionLinear) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionLinear) { - lanelet::Id id = 120659; - const double initial_speed = 25.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1811,16 +1594,9 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionL EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionAuto) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionAuto) { - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double target_speed = 20.0; @@ -1847,22 +1623,15 @@ TEST(EntityBase, requestSpeedChange_targetSpeedAbsoluteConstraintTimeTransitionA EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionLinear) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionLinear) { - lanelet::Id id = 120659; - const double initial_speed = 25.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status); @@ -1894,22 +1663,15 @@ TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionL EXPECT_DYNAMIC_CONSTRAINTS_EQ(default_constraints, current_constraints); } -TEST(EntityBase, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionAuto) +TEST_F(EntityBaseTest, requestSpeedChange_targetSpeedRelativeConstraintTimeTransitionAuto) { - lanelet::Id id = 120659; - const double initial_speed = 10.0; - auto hdmap_utils_ptr = makeHdMapUtilsSharedPointer(); - auto pose = makeCanonicalizedLaneletPose(hdmap_utils_ptr, id); - auto bbox = makeBoundingBox(); - auto status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox); - DummyEntity dummy("dummy_entity", status, hdmap_utils_ptr); dummy.setLinearVelocity(initial_speed); const double bob_speed = 17.0; const double delta_speed = 3.0; const double target_speed = bob_speed + delta_speed; - auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils_ptr, pose, bbox, bob_speed, "bob"); + auto bob_status = makeCanonicalizedEntityStatus(hdmap_utils, pose, bbox, bob_speed, "bob"); std::unordered_map others; others.emplace("bob_entity", bob_status);