From ba90d0555dc00b04203357f6dac6ac3f68d7881b Mon Sep 17 00:00:00 2001
From: Jan Enno Maschke <jan.enno.maschke1@volkswagen.de>
Date: Fri, 20 Oct 2023 14:25:03 +0200
Subject: [PATCH] Add correct units in velocity and acceleration Get/Set
functions in Agent Adapter
---
.../opSimulation/modules/World_OSI/AgentAdapter.cpp | 8 ++++----
.../opSimulation/modules/World_OSI/AgentAdapter.h | 12 +++++++-----
2 files changed, 11 insertions(+), 9 deletions(-)
diff --git a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp
index bfd4614a5..f377c9821 100644
--- a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp
+++ b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.cpp
@@ -165,10 +165,10 @@ Common::Vector2d<units::acceleration::meters_per_second_squared_t> AgentAdapter:
0.0_mps_sq, 0.0_mps_sq}; //! acceleration from rotation of vehicle around reference point
if (longitudinal != 0_m || lateral != 0_m)
{
- rotationAcceleration.x = -lateral * GetYawAcceleration() - longitudinal * units::math::pow<2>(GetYawRate());
- rotationAcceleration.x /= 1_rad;
- rotationAcceleration.y = longitudinal * GetYawAcceleration() - lateral * units::math::pow<2>(GetYawRate());
- rotationAcceleration.y /= 1_rad;
+ rotationAcceleration.x
+ = (-lateral * GetYawAcceleration() - longitudinal * units::math::pow<2>(GetYawRate()) / 1_rad) / 1_rad;
+ rotationAcceleration.y
+ = (longitudinal * GetYawAcceleration() - lateral * units::math::pow<2>(GetYawRate()) / 1_rad) / 1_rad;
}
return {GetBaseTrafficObject().GetAbsAcceleration().ax + rotationAcceleration.x,
GetBaseTrafficObject().GetAbsAcceleration().ay + rotationAcceleration.y};
diff --git a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h
index 22335182c..358a7ded3 100644
--- a/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h
+++ b/sim/src/core/opSimulation/modules/World_OSI/AgentAdapter.h
@@ -176,8 +176,8 @@ public:
0.0_mps, 0.0_mps}; //! velocity from rotation of vehicle around reference point
if (longitudinal != 0.0_m || lateral != 0.0_m)
{
- rotation.x = -lateral * GetYawRate();
- rotation.y = longitudinal * GetYawRate();
+ rotation.x = -lateral * GetYawRate()/1_rad;
+ rotation.y = longitudinal * GetYawRate()/1_rad;
}
GetBaseTrafficObject().SetAbsVelocity({velocity.x + rotation.x, velocity.y + rotation.y, velocity.z});
@@ -197,12 +197,14 @@ public:
{
units::length::meter_t longitudinal = GetLongitudinal(ObjectPointPredefined::Center);
units::length::meter_t lateral = GetLateral(ObjectPointPredefined::Center);
- Common::Vector2d<units::velocity::meters_per_second_t> rotation{
+ Common::Vector2d<units::acceleration::meters_per_second_squared_t> rotation{
0.0_mps, 0.0_mps}; //! acceleration from rotation of vehicle around reference point
if (longitudinal != 0.0_m || lateral != 0.0_m)
{
- rotation.x = -lateral * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * longitudinal;
- rotation.y = longitudinal * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * lateral;
+ rotation.x
+ = (-lateral * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * longitudinal / 1_rad) / 1_rad;
+ rotation.y
+ = (longitudinal * GetYawAcceleration() - units::math::pow<2>(GetYawRate()) * lateral / 1_rad) / 1_rad;
}
GetBaseTrafficObject().SetAbsAcceleration(
{acceleration.x + rotation.x, acceleration.y + rotation.y, acceleration.z});
--
GitLab