From 01f8de69cb70fa50ee4e99f98e573cdda36fbcd8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 5 Dec 2024 20:54:24 +0900 Subject: [PATCH 1/6] fix: fix pitch angle sign in ego entity simulation test --- .../src/vehicle_simulation/test_ego_entity_simulation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 00c86c4f71b..b7fd0e64674 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -40,10 +40,10 @@ 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::atan(1. / 25.)) * gravity_acceleration; // last 25m: 4m up constexpr double expected_slope_acceleration_last_25m = - std::sin(std::atan(4. / 25.)) * gravity_acceleration; + std::sin(-std::atan(4. / 25.)) * gravity_acceleration; auto get_slope_acceleration_at = [&](const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, bool consider_slope) { From db986337ee224a067900bea4d585c87ec8bc04c3 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 6 Dec 2024 01:00:32 +0900 Subject: [PATCH 2/6] fix: use pitch-filled map pose in EgoEntitySimulation::calculateAccelerationBySlope test --- .../src/vehicle_simulation/test_ego_entity_simulation.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index b7fd0e64674..001b4851948 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -49,10 +49,9 @@ TEST(EgoEntitySimulation, calculateAccelerationBySlope) [&](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, From 7c531b6e91d25a5f35a2e18ec3b6378d60342759 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 6 Dec 2024 01:01:53 +0900 Subject: [PATCH 3/6] chore: add test for calculated expected value in EgoEntitySimulation::calculateAccelerationBySlope unit test --- .../src/vehicle_simulation/test_ego_entity_simulation.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 001b4851948..4eebee03bff 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -40,10 +40,12 @@ 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::atan(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::atan(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) { From 7c86f1d9633a8d2de560be64b90599acd42970eb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 6 Dec 2024 01:02:24 +0900 Subject: [PATCH 4/6] refactor: EgoEntitySimulation::calculateAccelerationBySlope unit test --- .../test_ego_entity_simulation.cpp | 76 +++++++++++-------- 1 file changed, 46 insertions(+), 30 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index 4eebee03bff..d25ce007265 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -69,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(); From 4c7d962232016fff4011d6502905528464aa3fbd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 6 Dec 2024 10:28:16 +0900 Subject: [PATCH 5/6] fix: EgoEntitySimulation::calculateAccelerationBySlope() --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 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 f28111b6a1c..2452d66f732 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 @@ -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; } From cee4c4244f660aee30000126446b78133edf92ab Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 9 Dec 2024 14:45:09 +0900 Subject: [PATCH 6/6] refactor: use std::atan2 instead of std::atan --- .../src/vehicle_simulation/test_ego_entity_simulation.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp index d25ce007265..90c801d74da 100644 --- a/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp +++ b/simulation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp @@ -40,11 +40,11 @@ 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 =