Merge pull request #1509 from tier4/RJD-1505/fix_slope_acceleration_sign

Author: HansRobo

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -318,26 +318,7 @@ void EgoEntitySimulation::update(
       case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
       case VehicleModelType::IDEAL_STEER_ACC:
       case VehicleModelType::IDEAL_STEER_ACC_GEARED:
-        /*
-           TODO FIX THIS!!!
-
-           THIS IS MAYBE INCORRECT.
-
-           SHOULD BE
-               gear_sign * acceleration + acceleration_by_slope
-           OR
-               signed_acceleration + acceleration_by_slope
-
-           Currently, acceleration is obtained as an unsigned value
-           (`acceleration`) and a signed value (`gear_sign`), but this is for
-           historical reasons and there is no longer any reason to do so.
-
-           Therefore, when resolving this TODO comment, the assignee should
-           remove `gear_sign` from the tuple returned by
-           `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;
 
@@ -365,7 +346,7 @@ void EgoEntitySimulation::update(
 
 auto EgoEntitySimulation::calculateAccelerationBySlope() const -> double
 {
-  if (consider_acceleration_by_road_slope_) {
+  if (consider_acceleration_by_road_slope_ && status_.isInLanelet()) {
     constexpr double gravity_acceleration = -9.81;
     /// @todo why there is a need to recalculate orientation using getLaneletPose?
     /// status_.getMapPose().orientation already contains the orientation

simulation/traffic_simulator/include/traffic_simulator/data_type/entity_status.hpp

@@ -66,9 +66,9 @@ class CanonicalizedEntityStatus
   auto setLinearJerk(double) -> void;
 
   auto isInLanelet() const noexcept -> bool;
-  auto getLaneletId() const noexcept -> lanelet::Id;
-  auto getLaneletIds() const noexcept -> lanelet::Ids;
-  auto getLaneletPose() const noexcept -> const LaneletPose &;
+  auto getLaneletId() const -> lanelet::Id;
+  auto getLaneletIds() const -> lanelet::Ids;
+  auto getLaneletPose() const -> const LaneletPose &;
   auto getCanonicalizedLaneletPose() const noexcept
     -> const std::optional<CanonicalizedLaneletPose> &;
   auto getName() const noexcept -> const std::string & { return entity_status_.name; }

simulation/traffic_simulator/src/data_type/entity_status.cpp

@@ -125,7 +125,7 @@ auto CanonicalizedEntityStatus::getAltitude() const -> double
                                      : 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();
@@ -134,12 +134,12 @@ auto CanonicalizedEntityStatus::getLaneletPose() const noexcept -> const Lanelet
   }
 }
 
-auto CanonicalizedEntityStatus::getLaneletId() const noexcept -> lanelet::Id
+auto CanonicalizedEntityStatus::getLaneletId() const -> lanelet::Id
 {
   return getLaneletPose().lanelet_id;
 }
 
-auto CanonicalizedEntityStatus::getLaneletIds() const noexcept -> lanelet::Ids
+auto CanonicalizedEntityStatus::getLaneletIds() const -> lanelet::Ids
 {
   return isInLanelet() ? lanelet::Ids{getLaneletId()} : lanelet::Ids{};
 }