Merge pull request #1477 from tier4/fix/acc_by_slope

tier4 · Dec 9, 2024 · fcaf62b · fcaf62b
2 parents 6910edb + cee4c42
commit fcaf62b
Show file tree

Hide file tree

Showing 2 changed files with 57 additions and 38 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
@@ -344,8 +344,10 @@ auto EgoEntitySimulation::calculateAccelerationBySlope() const -> double
   if (consider_acceleration_by_road_slope_) {
     constexpr double gravity_acceleration = -9.81;
     const double ego_pitch_angle =
-      math::geometry::convertQuaternionToEulerAngle(status_.getMapPose().orientation).y;
-    return gravity_acceleration * std::sin(ego_pitch_angle);
+      math::geometry::convertQuaternionToEulerAngle(
+        hdmap_utils_ptr_->toMapPose(status_.getLaneletPose(), true).pose.orientation)
+        .y;
+    return -std::sin(ego_pitch_angle) * gravity_acceleration;
   } else {
     return 0.0;
   }

diff --git a/...lation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/...lation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp
@@ -40,19 +40,20 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope)
   // expected value in the lanelet(id:7)
   // first 25m: 1m up
   constexpr double expected_slope_acceleration_first_25m =
-    std::sin(std::atan(1. / 25.)) * gravity_acceleration;
+    -std::sin(-std::atan2(1., 25.)) * gravity_acceleration;
+  EXPECT_LE(expected_slope_acceleration_first_25m, 0.0);  // up -> negative slope acceleration
   // last 25m:  4m up
   constexpr double expected_slope_acceleration_last_25m =
-    std::sin(std::atan(4. / 25.)) * gravity_acceleration;
+    -std::sin(-std::atan2(4., 25.)) * gravity_acceleration;
+  EXPECT_LE(expected_slope_acceleration_last_25m, 0.0);  // up -> negative slope acceleration
 
   auto get_slope_acceleration_at =
     [&](const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, bool consider_slope) {
       traffic_simulator_msgs::msg::EntityStatus initial_status;
       initial_status.name = "ego";
-      initial_status.lanelet_pose_valid = true;
-      initial_status.lanelet_pose = lanelet_pose;
-      initial_status.pose =
-        traffic_simulator::pose::toMapPose(initial_status.lanelet_pose, hdmap_utils);
+      // use pitch-filled map pose
+      initial_status.lanelet_pose_valid = false;
+      initial_status.pose = hdmap_utils->toMapPose(lanelet_pose, true).pose;
 
       EgoEntitySimulation ego_entity_simulation(
         initial_status, traffic_simulator_msgs::msg::VehicleParameters(), 1.f / 30.f, hdmap_utils,
@@ -68,45 +69,61 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope)
   // it will not exactly match the ideal value, so we manually selected the smallest possible value specifically for this test.
   constexpr double compare_epsilon = 1e-7;
 
-  // first 25m, up, with considering slope
-  lanelet_pose.s = 12.5;
-  lanelet_pose.rpy.z = 0.0;
-  EXPECT_NEAR(
-    expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true),
-    compare_epsilon);
+  // first 25m, up
+  {
+    lanelet_pose.s = 12.5;
+    lanelet_pose.rpy.z = 0.0;
 
-  // first 25m, up, without considering slope
-  EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+    // with considering slope
+    EXPECT_NEAR(
+      expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true),
+      compare_epsilon);
+
+    // without considering slope
+    EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+  }
 
   // last 25m, up
-  lanelet_pose.s = 37.5;
-  lanelet_pose.rpy.z = 0.0;
-  EXPECT_NEAR(
-    expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true),
-    compare_epsilon);
+  {
+    lanelet_pose.s = 37.5;
+    lanelet_pose.rpy.z = 0.0;
+
+    // with considering slope
+    EXPECT_NEAR(
+      expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true),
+      compare_epsilon);
 
-  // last 25m, up, without considering slope
-  EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+    // without considering slope
+    EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+  }
 
   // first 25m, down
-  lanelet_pose.s = 12.5;
-  lanelet_pose.rpy.z = M_PI;
-  EXPECT_NEAR(
-    -expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true),
-    compare_epsilon);
+  {
+    lanelet_pose.s = 12.5;
+    lanelet_pose.rpy.z = M_PI;
+
+    // with considering slope
+    EXPECT_NEAR(
+      -expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true),
+      compare_epsilon);
 
-  // first 25m, down, without considering slope
-  EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+    // without considering slope
+    EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+  }
 
   // last 25m, down
-  lanelet_pose.s = 37.5;
-  lanelet_pose.rpy.z = M_PI;
-  EXPECT_NEAR(
-    -expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true),
-    compare_epsilon);
-
-  // last 25m, down, without considering slope
-  EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+  {
+    lanelet_pose.s = 37.5;
+    lanelet_pose.rpy.z = M_PI;
+
+    // with considering slope
+    EXPECT_NEAR(
+      -expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true),
+      compare_epsilon);
+
+    // without considering slope
+    EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false));
+  }
 
   rclcpp::shutdown();