Code owners
Assign users and groups as approvers for specific file changes. Learn more.
SpeedActionTest.cpp 26.23 KiB
/********************************************************************************
* Copyright (c) 2021-2023 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
* Copyright (c) 2023 Ansys, Inc.
*
* This program and the accompanying materials are made available under the
* terms of the Eclipse Public License 2.0 which is available at
* http://www.eclipse.org/legal/epl-2.0.
*
* SPDX-License-Identifier: EPL-2.0
********************************************************************************/
#include <MantleAPI/Test/test_utils.h>
#include <gtest/gtest.h>
#include "Conversion/OscToMantle/ConvertScenarioSpeedActionTarget.h"
#include "Storyboard/MotionControlAction/SpeedAction_impl.h"
#include "gmock/gmock.h"
using namespace mantle_api;
using namespace units::literals;
using testing::_;
using testing::Return;
using testing::SizeIs;
namespace OpenScenarioEngine::v1_2
{
class SpeedActionWithJerkLimitsTestFixture : public testing::Test
{
protected:
void Init(const Vec3<units::velocity::meters_per_second_t>& initial_velocity,
units::velocity::meters_per_second_t target_speed,
units::acceleration::meters_per_second_squared_t expected_acceleration,
std::optional<units::jerk::meters_per_second_cubed_t> max_acceleration_rate = std::nullopt,
std::optional<units::jerk::meters_per_second_cubed_t> max_deceleration_rate = std::nullopt)
{
vehicle_properties_.performance = {
.max_acceleration_rate = max_acceleration_rate.value_or(units::jerk::meters_per_second_cubed_t(std::numeric_limits<double>::infinity())),
.max_deceleration_rate = max_deceleration_rate.value_or(units::jerk::meters_per_second_cubed_t(std::numeric_limits<double>::infinity()))};
EXPECT_CALL(*mock_environment_, UpdateControlStrategies(_, _)).WillOnce(testing::SaveArg<1>(&control_strategies_));
EXPECT_CALL(mock_vehicle_, GetPropertiesImpl()).WillOnce(Return(&vehicle_properties_));
EXPECT_CALL(mock_vehicle_, GetVelocity()).WillOnce(Return(initial_velocity));
const mantle_api::TransitionDynamics transition_dynamics{Dimension::kRate, Shape::kLinear, expected_acceleration()};
const TransitionDynamics speed_action_dynamics{transition_dynamics, FollowingMode::kFollow};
const SpeedActionBase::Values values{std::vector<std::string>{"Vehicle1"},
speed_action_dynamics,
[target_speed]() { return target_speed; }};
const SpeedActionBase::Interfaces interfaces{mock_environment_};
speed_action_ = std::make_unique<SpeedAction>(values, interfaces);
}
void SetAndCheckControlStrategies()
{
EXPECT_NO_THROW(speed_action_->SetControlStrategy());
EXPECT_THAT(control_strategies_, SizeIs(1));
EXPECT_EQ(control_strategies_.front()->type, ControlStrategyType::kFollowVelocitySpline);
}
std::unique_ptr<SpeedAction> speed_action_{nullptr};
std::shared_ptr<MockEnvironment> mock_environment_{std::make_shared<MockEnvironment>()};
MockVehicle& mock_vehicle_{dynamic_cast<MockVehicle&>(mock_environment_->GetEntityRepository().GetHost())};
VehicleProperties vehicle_properties_{};
std::vector<std::shared_ptr<ControlStrategy>> control_strategies_{};
};
TEST(SpeedAction, GivenMatchingControlStrategyGoalIsReached_ReturnTrue){
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
ON_CALL(*mockEnvironment,
HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kFollowVelocitySpline))
.WillByDefault(Return(true));
TransitionDynamics speedActionDynamics;
speedActionDynamics.transitionDynamics.shape = mantle_api::Shape::kLinear;
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 0_mps; }},
{mockEnvironment});
ASSERT_TRUE(speedAction.HasControlStrategyGoalBeenReached("Vehicle1"));
}
TEST(SpeedAction, GivenUnmatchingControlStrategyGoalIsReached_ReturnsFalse)
{
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
ON_CALL(*mockEnvironment,
HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kUndefined))
.WillByDefault(Return(false));
TransitionDynamics speedActionDynamics;
speedActionDynamics.transitionDynamics.shape = mantle_api::Shape::kLinear;
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 0_mps; }},
{mockEnvironment});
ASSERT_FALSE(speedAction.HasControlStrategyGoalBeenReached("Vehicle1"));
}
TEST(SpeedAction, GivenkStepShapeForSpeedActionDynamics_ReturnTrue)
{
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
ON_CALL(*mockEnvironment,
HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kUndefined))
.WillByDefault(Return(true));
TransitionDynamics speedActionDynamics;
speedActionDynamics.transitionDynamics.shape = mantle_api::Shape::kStep;
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 0_mps; }},
{mockEnvironment});
ASSERT_TRUE(speedAction.HasControlStrategyGoalBeenReached("Vehicle1"));
}
TEST(SpeedAction, GivenkUndefinedShapeForSpeedActionDynamics_ReturnFalse)
{
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
ON_CALL(*mockEnvironment,
HasControlStrategyGoalBeenReached(_, mantle_api::ControlStrategyType::kUndefined))
.WillByDefault(Return(false));
TransitionDynamics speedActionDynamics;
speedActionDynamics.transitionDynamics.shape = mantle_api::Shape::kUndefined;
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 0_mps; }},
{mockEnvironment});
ASSERT_FALSE(speedAction.HasControlStrategyGoalBeenReached("Vehicle1"));
}
TEST(SpeedAction, GivenIsStepShapeForSpeedActionDynamics_SetControlStrategy){
TransitionDynamics speedActionDynamics;
speedActionDynamics.transitionDynamics.shape = mantle_api::Shape::kStep;
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 0_mps; }},
{std::make_shared<mantle_api::MockEnvironment>()});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
}
TEST(SpeedAction, GivenLinearShapeAndkUndefinedDimensionForSpeedActionDynamics_ThrowError)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kUndefined, mantle_api::Shape::kLinear, {}};
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[]() -> units::velocity::meters_per_second_t { return 123_mps; }},
{std::make_shared<mantle_api::MockEnvironment>()});
EXPECT_THROW(speedAction.SetControlStrategy(), std::runtime_error);
}
TEST(SpeedAction, GivenLinearShapeAndkTimeDimensionForSpeedActionDynamics_SetControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kTime, mantle_api::Shape::kLinear, 3.6};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment,
UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, testing::SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(3600));
}
TEST(SpeedAction, GivenLinearShapeAndkRateDimensionForSpeedActionDynamics_SetControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kRate, mantle_api::Shape::kLinear, 1.2};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment,
UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, testing::SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget); EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(1000));
}
TEST(SpeedAction, GivenLinearShapeAndkDistanceDimensionForSpeedActionDynamics_SetControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kDistance, mantle_api::Shape::kLinear, 0.6};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment,
UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, testing::SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(1000));
}
TEST(SpeedAction, WithCubicShapeAndRate_SetsControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kRate, mantle_api::Shape::kCubic, 1.2};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment, UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget);
EXPECT_THAT(control_strategy->velocity_splines, SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(1000));
}
TEST(SpeedAction, WithCubicShapeAndTime_SetsControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kTime, mantle_api::Shape::kCubic, 1.2};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment, UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget);
EXPECT_THAT(control_strategy->velocity_splines, SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(1200));
}
TEST(SpeedAction, WithCubicShapeAndDistance_SetsControlStrategy)
{
TransitionDynamics speedActionDynamics{mantle_api::Dimension::kDistance, mantle_api::Shape::kCubic, 0.6};
SpeedActionTarget speedActionTarget = 1.2_mps;
auto mockEnvironment = std::make_shared<mantle_api::MockEnvironment>();
std::vector<std::shared_ptr<mantle_api::ControlStrategy>> control_strategies{};
EXPECT_CALL(*mockEnvironment,
UpdateControlStrategies(_, _))
.WillOnce(testing::SaveArg<1>(&control_strategies));
SpeedAction speedAction({std::vector<std::string>{"Vehicle1"},
speedActionDynamics,
[t = speedActionTarget]() -> units::velocity::meters_per_second_t { return t; }},
{mockEnvironment});
EXPECT_NO_THROW(speedAction.SetControlStrategy());
EXPECT_THAT(control_strategies, testing::SizeIs(1));
EXPECT_EQ(control_strategies.front()->type, mantle_api::ControlStrategyType::kFollowVelocitySpline);
auto control_strategy = dynamic_cast<mantle_api::FollowVelocitySplineControlStrategy*>(control_strategies[0].get());
EXPECT_EQ(control_strategy->default_value, speedActionTarget);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(control_strategy->velocity_splines[0].start_time, mantle_api::Time(0));
EXPECT_EQ(control_strategy->velocity_splines[0].end_time, mantle_api::Time(1000));
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenDecreasingSpeedAndJerkLimits_WhenExpectedRatesAreTooHigh_ThenSplineHasAccelerationAndDecelerationJerkLimits)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{123.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{0.0_mps};
const auto acceleration{-123.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(3));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), -0.5 * max_deceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[2].polynomial), 0.5 * max_acceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenDecreasingSpeedAndJerkLimitsAndPositiveSpeedTarget_WhenExpectedRateAreTooHigh_ThenSplineHasAccelerationAndDecelerationJerkLimitsAndInitialSpeed)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{456.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{123.0_mps};
const auto acceleration{-333.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed); EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(3));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), -0.5 * max_deceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[2].polynomial), 0.5 * max_acceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenDecreasingSpeedAndAccelerationRateLimit_WhenExpectedAccelerationRateIsTooHigh_ThenSplineHasAccelerationRateLimit)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{123.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{0.0_mps};
const auto acceleration{-123.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(2));
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[0].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[1].polynomial), 0.5 * max_acceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenDecreasingSpeedAndDecelerationRateLimit_WhenExpectedDecelerationRateIsTooHigh_ThenSplineHasDecelerationRateLimit)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{123.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{0.0_mps};
const auto acceleration{-123.0_mps_sq};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, std::nullopt, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(2));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), -0.5 * max_deceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenDecreasingSpeedAndNoJerkLimits_WhenExpectedDecelerationRateIsTooHigh_ThenSplineHasNoJerkLimits)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{123.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{0.0_mps};
const auto acceleration{-123.0_mps_sq};
Init(initial_velocity, target_speed, acceleration);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[0].polynomial), acceleration);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenIncreasingSpeedAndJerkLimits_WhenExpectedRatesAreTooHigh_ThenSplineHasAccelerationAndDecelerationJerkLimits)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{0.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{123.0_mps};
const auto acceleration{123.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(3));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), 0.5 * max_deceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[2].polynomial), -0.5 * max_acceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenIncreasingSpeedAndJerkLimitsAndInitialSpeed_WhenExpectedRateAreTooHigh_ThenSplineHasAccelerationAndDecelerationJerkLimitsAndInitialSpeed)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{123.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{456.0_mps};
const auto acceleration{333.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(3));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), 0.5 * max_deceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[2].polynomial), -0.5 * max_acceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenIncreasingSpeedAndAccelerationRateLimit_WhenExpectedAccelerationRateIsTooHigh_ThenSplineHasAccelerationRateLimit)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{0.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{123.0_mps};
const auto acceleration{123.0_mps_sq};
const auto max_acceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, max_acceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(2));
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[0].polynomial), 0.5 * max_acceleration_rate);
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[1].polynomial), acceleration);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenIncreasingSpeedAndDecelerationRateLimit_WhenExpectedDecelerationRateIsTooHigh_ThenSplineHasDecelerationRateLimit)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{0.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{123.0_mps};
const auto acceleration{123.0_mps_sq};
const auto max_deceleration_rate{30.0_mps_cu};
Init(initial_velocity, target_speed, acceleration, std::nullopt, max_deceleration_rate);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(2));
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[0].polynomial), acceleration);
EXPECT_EQ(std::get<1>(control_strategy->velocity_splines[1].polynomial), -0.5 * max_deceleration_rate);
}
TEST_F(SpeedActionWithJerkLimitsTestFixture, GivenIncreasingSpeedAndNoJerkLimits_WhenExpectedAccelerationRateIsTooHigh_ThenSplineHasNoJerkLimits)
{
const Vec3<units::velocity::meters_per_second_t> initial_velocity{0.0_mps, 0.0_mps, 0.0_mps};
const auto target_speed{123.0_mps};
const auto acceleration{123.0_mps_sq};
Init(initial_velocity, target_speed, acceleration);
SetAndCheckControlStrategies();
auto control_strategy{dynamic_cast<FollowVelocitySplineControlStrategy*>(control_strategies_.front().get())};
EXPECT_EQ(control_strategy->default_value, target_speed);
EXPECT_THAT(control_strategy->velocity_splines, testing::SizeIs(1));
EXPECT_EQ(std::get<3>(control_strategy->velocity_splines[0].polynomial), initial_velocity.Length());
EXPECT_EQ(std::get<2>(control_strategy->velocity_splines[0].polynomial), acceleration);
}
} // namespace OpenScenarioEngine::v1_2