Skip to content
Snippets Groups Projects

added distance dynamic dimension in speed action

Compare and
2 files
+ 97
38
Compare changes
  • Side-by-side
  • Inline
Files
2
@@ -87,7 +87,8 @@ void SpeedAction::StartAction()
// TODO: Add CUBIC once it's clear how the shape should look like and how the coefficients can be calculated
auto dynamics_dimension = speed_action_dynamics->GetDynamicsDimension();
if (dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::TIME ||
dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::RATE)
dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::RATE ||
dynamics_dimension == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::DISTANCE)
{
for (auto actor : actors_)
{
@@ -120,12 +121,12 @@ bool SpeedAction::HasControlStrategyGoalBeenReached(const std::string& actor)
NET_ASAM_OPENSCENARIO::v1_1::DynamicsShape::STEP ||
(speed_action_data_->GetSpeedActionDynamics()->GetDynamicsShape() ==
NET_ASAM_OPENSCENARIO::v1_1::DynamicsShape::LINEAR &&
environment_->HasControlStrategyGoalBeenReached(
entity.value().get().GetUniqueId(),
mantle_api::ControlStrategyType::kFollowVelocitySpline));
environment_->HasControlStrategyGoalBeenReached(entity.value().get().GetUniqueId(),
mantle_api::ControlStrategyType::kFollowVelocitySpline));
}
units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(NET_ASAM_OPENSCENARIO::v1_1::ISpeedActionTarget* speed_action_target)
units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(
NET_ASAM_OPENSCENARIO::v1_1::ISpeedActionTarget* speed_action_target)
{
units::velocity::meters_per_second_t target_speed{0};
auto absolute_target_speed = speed_action_target->GetAbsoluteTargetSpeed();
@@ -141,9 +142,9 @@ units::velocity::meters_per_second_t SpeedAction::GetTargetSpeed(NET_ASAM_OPENSC
if (!environment_->GetEntityRepository().Get(relative_entity_name).has_value())
{
throw std::runtime_error(
"Relative Entity with name \"" + relative_entity_name + "\" of SpeedAction does not exist. Please adjust the "
"scenario.");
throw std::runtime_error("Relative Entity with name \"" + relative_entity_name +
"\" of SpeedAction does not exist. Please adjust the "
"scenario.");
}
auto target_base = environment_->GetEntityRepository().Get(relative_entity_name).value().get().GetVelocity();
@@ -179,31 +180,45 @@ void SpeedAction::SetLinearVelocitySplineControlStrategy(
return;
}
const auto &entity = environment_->GetEntityRepository().Get(actor).value().get();
const auto& entity = environment_->GetEntityRepository().Get(actor).value().get();
mantle_api::SplineSection<units::velocity::meters_per_second> spline_section;
spline_section.start_time = mantle_api::Time(0);
if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::TIME)
{
spline_section.end_time = units::time::second_t(transition_dynamics->GetValue());
std::get<2>(spline_section.polynomial) = (target_speed - entity.GetVelocity().Length()) / units::time::second_t(transition_dynamics->GetValue());
std::get<2>(spline_section.polynomial) =
(target_speed - entity.GetVelocity().Length()) / units::time::second_t(transition_dynamics->GetValue());
}
else if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::RATE)
{
spline_section.end_time =
units::math::abs(target_speed - entity.GetVelocity().Length()) /
units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue());
spline_section.end_time = units::math::abs(target_speed - entity.GetVelocity().Length()) /
units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue());
std::get<2>(spline_section.polynomial) =
units::math::copysign(units::acceleration::meters_per_second_squared_t(transition_dynamics->GetValue()),
target_speed - entity.GetVelocity().Length());
}
else if (transition_dynamics->GetDynamicsDimension() == NET_ASAM_OPENSCENARIO::v1_1::DynamicsDimension::DISTANCE)
{
auto start_speed = entity.GetVelocity().Length().value();
// Distance has to be converted to time, since spline is expected over time, not distance
// distance = time(v_init + v_delta/2) = time(v_init + v_end)/2 => time = 2*distance/(v_init + v_end)
spline_section.end_time =
units::time::second_t(2 * transition_dynamics->GetValue() / (target_speed.value() - start_speed));
// v_end = v_init + acceleration*time, where time = 2*distance/(v_init + v_end) then acceleration = (v_end -
// v_init) * (v_end + v_init) / 2* distance
auto rate = units::acceleration::meters_per_second_squared_t((target_speed.value() - start_speed) *
(target_speed.value() + start_speed) / 2 *
transition_dynamics->GetValue());
std::get<2>(spline_section.polynomial) = units::math::copysign(
(units::acceleration::meters_per_second_squared_t(rate)), target_speed - entity.GetVelocity().Length());
}
std::get<3>(spline_section.polynomial) = entity.GetVelocity().Length();
auto control_strategy = std::make_shared<mantle_api::FollowVelocitySplineControlStrategy>();
control_strategy->default_value = target_speed;
control_strategy->velocity_splines.push_back(spline_section);
environment_->UpdateControlStrategies(entity.GetUniqueId(),
{control_strategy});
environment_->UpdateControlStrategies(entity.GetUniqueId(), {control_strategy});
}
} // namespace OPENSCENARIO
Loading