Consider road slope in ego vehicle simulation

.github/pull_request_samples/example_detail.md

@@ -28,8 +28,8 @@ This PR fixes how the length of the curve is computed
 
 ## References
 
-- [determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed](https://gamedev.stackexchange.com/questions/14985/determine-arc-length-of-a-catmull-rom-spline-to-move-at-a-constant-speed)
-  - This link is an example and is not directly related to this sample.
+See also [this document.](https://tier4.github.io/scenario_simulator_v2-docs/)  
+This link is an example and is not directly related to this sample.
 
 # Destructive Changes
 

external/concealer/src/autoware_universe.cpp

@@ -156,6 +156,8 @@ auto AutowareUniverse::getGearCommand() const -> autoware_auto_vehicle_msgs::msg
 auto AutowareUniverse::getGearSign() const -> double
 {
   using autoware_auto_vehicle_msgs::msg::GearCommand;
+  // TODO: Add support for GearCommand::NONE to return 0.0
+  // ref: https://github.com/autowarefoundation/autoware.universe/blob/main/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp#L475
   return getGearCommand().command == GearCommand::REVERSE or
              getGearCommand().command == GearCommand::REVERSE_2
            ? -1.0

simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp

@@ -60,27 +60,37 @@ class EgoEntitySimulation
 
   traffic_simulator_msgs::msg::EntityStatus status_;
 
+  const bool consider_acceleration_by_road_slope_;
+
 public:
   const std::shared_ptr<hdmap_utils::HdMapUtils> hdmap_utils_ptr_;
+
   const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters;
 
 private:
-  auto getCurrentPose() const -> geometry_msgs::msg::Pose;
+  auto calculateEgoPitch() const -> double;
+
+  auto getCurrentPose(const double pitch_angle) const -> geometry_msgs::msg::Pose;
 
   auto getCurrentTwist() const -> geometry_msgs::msg::Twist;
 
   auto getCurrentAccel(const double step_time) const -> geometry_msgs::msg::Accel;
 
   auto getLinearJerk(double step_time) -> double;
 
+  auto getMatchedLaneletPoseFromEntityStatus(
+    const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const
+    -> std::optional<traffic_simulator_msgs::msg::LaneletPose>;
+
   auto updatePreviousValues() -> void;
 
 public:
   auto setAutowareStatus() -> void;
 
   explicit EgoEntitySimulation(
     const traffic_simulator_msgs::msg::VehicleParameters &, double,
-    const std::shared_ptr<hdmap_utils::HdMapUtils> &, const rclcpp::Parameter & use_sim_time);
+    const std::shared_ptr<hdmap_utils::HdMapUtils> &, const rclcpp::Parameter & use_sim_time,
+    const bool consider_acceleration_by_road_slope);
 
   auto update(double time, double step_time, bool npc_logic_started) -> void;
 

simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp

@@ -207,9 +207,16 @@ auto ScenarioSimulator::spawnVehicleEntity(
     ego_vehicles_.emplace_back(req.parameters());
     traffic_simulator_msgs::msg::VehicleParameters parameters;
     simulation_interface::toMsg(req.parameters(), parameters);
+    auto get_consider_acceleration_by_road_slope = [&]() {
+      if (!has_parameter("consider_acceleration_by_road_slope")) {
+        declare_parameter("consider_acceleration_by_road_slope", false);
+      }
+      return get_parameter("consider_acceleration_by_road_slope").as_bool();
+    };
     ego_entity_simulation_ = std::make_shared<vehicle_simulation::EgoEntitySimulation>(
       parameters, step_time_, hdmap_utils_,
-      get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)));
+      get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)),
+      get_consider_acceleration_by_road_slope());
     traffic_simulator_msgs::msg::EntityStatus initial_status;
     initial_status.name = parameters.name;
     simulation_interface::toMsg(req.pose(), initial_status.pose);

simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp

@@ -35,12 +35,13 @@ static auto getParameter(const std::string & name, T value = {})
 EgoEntitySimulation::EgoEntitySimulation(
   const traffic_simulator_msgs::msg::VehicleParameters & parameters, double step_time,
   const std::shared_ptr<hdmap_utils::HdMapUtils> & hdmap_utils,
-  const rclcpp::Parameter & use_sim_time)
+  const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope)
 : autoware(std::make_unique<concealer::AutowareUniverse>()),
   vehicle_model_type_(getVehicleModelType()),
   vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)),
   hdmap_utils_ptr_(hdmap_utils),
-  vehicle_parameters(parameters)
+  vehicle_parameters(parameters),
+  consider_acceleration_by_road_slope_(consider_acceleration_by_road_slope)
 {
   autoware->set_parameter(use_sim_time);
 }
@@ -108,8 +109,8 @@ auto EgoEntitySimulation::makeSimulationModel(
   const auto steer_time_delay           = getParameter<double>("steer_time_delay",            0.24);
   const auto vel_lim                    = getParameter<double>("vel_lim",                     parameters.performance.max_speed);  // 50.0
   const auto vel_rate_lim               = getParameter<double>("vel_rate_lim",                parameters.performance.max_acceleration);  // 7.0
-  const auto vel_time_constant          = getParameter<double>("vel_time_constant",           0.1);
-  const auto vel_time_delay             = getParameter<double>("vel_time_delay",              0.1);
+  const auto vel_time_constant          = getParameter<double>("vel_time_constant",           0.1);  // 0.5 is default value on simple_planning_simulator
+  const auto vel_time_delay             = getParameter<double>("vel_time_delay",              0.1);  // 0.25 is default value on simple_planning_simulator
   const auto wheel_base                 = getParameter<double>("wheel_base",                  parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x);
   // clang-format on
 
@@ -266,13 +267,26 @@ void EgoEntitySimulation::update(
     } else {
       auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU());
 
+      auto acceleration_by_slope = [this]() {
+        if (consider_acceleration_by_road_slope_) {
+          // calculate longitudinal acceleration by slope
+          constexpr double gravity_acceleration = -9.81;
+          const double ego_pitch_angle = calculateEgoPitch();
+          const double slope_angle = -ego_pitch_angle;
+          return gravity_acceleration * std::sin(slope_angle);
+        } else {
+          return 0.0;
+        }
+      }();
+
       switch (vehicle_model_type_) {
         case VehicleModelType::DELAY_STEER_ACC:
         case VehicleModelType::DELAY_STEER_ACC_GEARED:
         case VehicleModelType::DELAY_STEER_MAP_ACC_GEARED:
         case VehicleModelType::IDEAL_STEER_ACC:
         case VehicleModelType::IDEAL_STEER_ACC_GEARED:
-          input(0) = autoware->getGearSign() * autoware->getAcceleration();
+          input(0) =
+            autoware->getGearSign() * (autoware->getAcceleration() + acceleration_by_slope);
           input(1) = autoware->getSteeringAngle();
           break;
 
@@ -297,6 +311,90 @@ void EgoEntitySimulation::update(
   updatePreviousValues();
 }
 
+auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus(
+  const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const
+  -> std::optional<traffic_simulator_msgs::msg::LaneletPose>
+{
+  // the lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose
+  const auto unique_route_lanelets =
+    traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
+  const auto matching_length = [entity_width] { return entity_width * 0.5 + 1.0; }();
+
+  std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
+
+  if (unique_route_lanelets.empty()) {
+    lanelet_pose =
+      hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+  } else {
+    lanelet_pose =
+      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, matching_length);
+    if (!lanelet_pose) {
+      lanelet_pose =
+        hdmap_utils_ptr_->toLaneletPose(status.pose, status.bounding_box, false, matching_length);
+    }
+  }
+  return lanelet_pose;
+}
+
+auto EgoEntitySimulation::calculateEgoPitch() const -> double
+{
+  // calculate prev/next point of lanelet centerline nearest to ego pose.
+  auto ego_lanelet = getMatchedLaneletPoseFromEntityStatus(
+    status_, std::max(
+               vehicle_parameters.axles.front_axle.track_width,
+               vehicle_parameters.axles.rear_axle.track_width));
+  if (not ego_lanelet) {
+    return 0.0;
+  }
+
+  auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet.value().lanelet_id);
+
+  // copied from motion_util::findNearestSegmentIndex
+  auto find_nearest_segment_index = [](
+                                      const std::vector<geometry_msgs::msg::Point> & points,
+                                      const geometry_msgs::msg::Point & point) {
+    assert(not points.empty());
+
+    double min_dist = std::numeric_limits<double>::max();
+    size_t min_idx = 0;
+
+    for (size_t i = 0; i < points.size(); ++i) {
+      const auto dist = [](const auto point1, const auto point2) {
+        const auto dx = point1.x - point2.x;
+        const auto dy = point1.y - point2.y;
+        return dx * dx + dy * dy;
+      }(points.at(i), point);
+
+      if (dist < min_dist) {
+        min_dist = dist;
+        min_idx = i;
+      }
+    }
+    return min_idx;
+  };
+
+  geometry_msgs::msg::Point ego_point;
+  ego_point.x = vehicle_model_ptr_->getX();
+  ego_point.y = vehicle_model_ptr_->getY();
+  const size_t ego_seg_idx = find_nearest_segment_index(centerline_points, ego_point);
+
+  const auto & prev_point = centerline_points.at(ego_seg_idx);
+  const auto & next_point = centerline_points.at(ego_seg_idx + 1);
+
+  // calculate ego yaw angle on lanelet coordinates
+  const double lanelet_yaw = std::atan2(next_point.y - prev_point.y, next_point.x - prev_point.x);
+  const double ego_yaw_against_lanelet = vehicle_model_ptr_->getYaw() - lanelet_yaw;
+
+  // calculate ego pitch angle considering ego yaw.
+  const double diff_z = next_point.z - prev_point.z;
+  const double diff_xy = std::hypot(next_point.x - prev_point.x, next_point.y - prev_point.y) /
+                         std::cos(ego_yaw_against_lanelet);
+  const bool reverse_sign = std::cos(ego_yaw_against_lanelet) < 0.0;
+  const double ego_pitch_angle =
+    reverse_sign ? -std::atan2(-diff_z, -diff_xy) : -std::atan2(diff_z, diff_xy);
+  return ego_pitch_angle;
+}
+
 auto EgoEntitySimulation::getCurrentTwist() const -> geometry_msgs::msg::Twist
 {
   geometry_msgs::msg::Twist current_twist;
@@ -305,7 +403,8 @@ auto EgoEntitySimulation::getCurrentTwist() const -> geometry_msgs::msg::Twist
   return current_twist;
 }
 
-auto EgoEntitySimulation::getCurrentPose() const -> geometry_msgs::msg::Pose
+auto EgoEntitySimulation::getCurrentPose(const double pitch_angle = 0.) const
+  -> geometry_msgs::msg::Pose
 {
   Eigen::VectorXd relative_position(3);
   relative_position(0) = vehicle_model_ptr_->getX();
@@ -318,10 +417,10 @@ auto EgoEntitySimulation::getCurrentPose() const -> geometry_msgs::msg::Pose
   current_pose.position.x = initial_pose_.position.x + relative_position(0);
   current_pose.position.y = initial_pose_.position.y + relative_position(1);
   current_pose.position.z = initial_pose_.position.z + relative_position(2);
-  current_pose.orientation = [this]() {
+  current_pose.orientation = [&]() {
     geometry_msgs::msg::Vector3 rpy;
     rpy.x = 0;
-    rpy.y = 0;
+    rpy.y = pitch_angle;
     rpy.z = vehicle_model_ptr_->getYaw();
     return initial_pose_.orientation * quaternion_operation::convertEulerAngleToQuaternion(rpy);
   }();
@@ -394,40 +493,20 @@ auto EgoEntitySimulation::updateStatus(double current_scenario_time, double step
 auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet(
   traffic_simulator_msgs::msg::EntityStatus & status) -> void
 {
-  const auto unique_route_lanelets =
-    traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets());
-  std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
-
-  const auto get_matching_length = [&] {
-    return std::max(
-             vehicle_parameters.axles.front_axle.track_width,
-             vehicle_parameters.axles.rear_axle.track_width) *
-             0.5 +
-           1.0;
-  };
-
-  if (unique_route_lanelets.empty()) {
-    lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
-      status.pose, status.bounding_box, false, get_matching_length());
-  } else {
-    lanelet_pose =
-      hdmap_utils_ptr_->toLaneletPose(status.pose, unique_route_lanelets, get_matching_length());
-    if (!lanelet_pose) {
-      lanelet_pose = hdmap_utils_ptr_->toLaneletPose(
-        status.pose, status.bounding_box, false, get_matching_length());
-    }
-  }
-  if (lanelet_pose) {
+  if (
+    auto lanelet_pose = getMatchedLaneletPoseFromEntityStatus(
+      status, std::max(
+                vehicle_parameters.axles.front_axle.track_width,
+                vehicle_parameters.axles.rear_axle.track_width))) {
     math::geometry::CatmullRomSpline spline(
       hdmap_utils_ptr_->getCenterPoints(lanelet_pose->lanelet_id));
     if (const auto s_value = spline.getSValue(status.pose)) {
       status.pose.position.z = spline.getPoint(s_value.value()).z;
     }
-  }
-
-  status.lanelet_pose_valid = static_cast<bool>(lanelet_pose);
-  if (status.lanelet_pose_valid) {
+    status.lanelet_pose_valid = true;
     status.lanelet_pose = lanelet_pose.value();
+  } else {
+    status.lanelet_pose_valid = false;
   }
 }
 }  // namespace vehicle_simulation

simulation/traffic_simulator/src/entity/ego_entity.cpp

@@ -275,6 +275,8 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void
 {
   const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets());
   std::optional<traffic_simulator_msgs::msg::LaneletPose> lanelet_pose;
+
+  // the lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus
   const auto get_matching_length = [&] {
     return std::max(
              vehicle_parameters.axles.front_axle.track_width,