From ed55433c1eac9e86c7c943c8fbbb7e3b66d87471 Mon Sep 17 00:00:00 2001 From: Grzegorz Maj Date: Sun, 19 Jan 2025 17:46:44 +0100 Subject: [PATCH] Change sign of slope acceleration and check canonicalization status in acceleration calculation --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 4 ++-- .../include/traffic_simulator/data_type/entity_status.hpp | 4 ++-- simulation/traffic_simulator/src/data_type/entity_status.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp index b8c0fe8362f..730f8799235 100644 --- a/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp @@ -336,7 +336,7 @@ void EgoEntitySimulation::update( `AutowareUniverse::getVehicleCommand`, and at the same time change `acceleration` to a signed value. */ - input(0) = gear_sign * (acceleration + acceleration_by_slope); + input(0) = gear_sign * acceleration + acceleration_by_slope; input(1) = tire_angle; break; @@ -364,7 +364,7 @@ void EgoEntitySimulation::update( auto EgoEntitySimulation::calculateAccelerationBySlope() const -> double { - if (consider_acceleration_by_road_slope_) { + if (consider_acceleration_by_road_slope_ && status_.laneMatchingSucceed()) { constexpr double gravity_acceleration = -9.81; const double ego_pitch_angle = math::geometry::convertQuaternionToEulerAngle( diff --git a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp index b1d07af341f..f253448aad5 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp @@ -68,9 +68,9 @@ class CanonicalizedEntityStatus auto setLinearJerk(double) -> void; auto laneMatchingSucceed() const noexcept -> bool; - auto getLaneletId() const noexcept -> lanelet::Id; + auto getLaneletId() const -> lanelet::Id; auto getLaneletIds() const noexcept -> lanelet::Ids; - auto getLaneletPose() const noexcept -> const LaneletPose &; + auto getLaneletPose() const -> const LaneletPose &; auto getCanonicalizedLaneletPose() const noexcept -> const std::optional &; auto getName() const noexcept -> const std::string & { return entity_status_.name; } diff --git a/simulation/traffic_simulator/src/data_type/entity_status.cpp b/simulation/traffic_simulator/src/data_type/entity_status.cpp index 758d10b6abf..a62befb5e75 100644 --- a/simulation/traffic_simulator/src/data_type/entity_status.cpp +++ b/simulation/traffic_simulator/src/data_type/entity_status.cpp @@ -127,7 +127,7 @@ auto CanonicalizedEntityStatus::getAltitude() const -> double return entity_status_.pose.position.z; } -auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const LaneletPose & +auto CanonicalizedEntityStatus::getLaneletPose() const -> const LaneletPose & { if (canonicalized_lanelet_pose_) { return canonicalized_lanelet_pose_->getLaneletPose(); @@ -136,7 +136,7 @@ auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const Lanelet } } -auto CanonicalizedEntityStatus::getLaneletId() const noexcept -> lanelet::Id +auto CanonicalizedEntityStatus::getLaneletId() const -> lanelet::Id { return getLaneletPose().lanelet_id; }