From 66e9e7710803ac898d2ab2d424e511223cc7f97f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 31 Jan 2024 15:17:46 +0900 Subject: [PATCH 01/21] doc: add memos to code Signed-off-by: Kotaro Yoshimoto --- external/concealer/src/autoware_universe.cpp | 2 ++ .../src/vehicle_simulation/ego_entity_simulation.cpp | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index de8ed46b214..dce5f359825 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/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 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 428ca770686..35097e1fca8 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 @@ -105,8 +105,8 @@ auto EgoEntitySimulation::makeSimulationModel( const auto steer_time_delay = getParameter("steer_time_delay", 0.24); const auto vel_lim = getParameter("vel_lim", parameters.performance.max_speed); // 50.0 const auto vel_rate_lim = getParameter("vel_rate_lim", parameters.performance.max_acceleration); // 7.0 - const auto vel_time_constant = getParameter("vel_time_constant", 0.1); - const auto vel_time_delay = getParameter("vel_time_delay", 0.1); + const auto vel_time_constant = getParameter("vel_time_constant", 0.1); // 0.5 is default value on simple_planning_simulator + const auto vel_time_delay = getParameter("vel_time_delay", 0.1); // 0.25 is default value on simple_planning_simulator const auto wheel_base = getParameter("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); // clang-format on From defed9e8da4fb7fe2f310fe6bbdbbb18c3465e3d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 31 Jan 2024 17:50:00 +0900 Subject: [PATCH 02/21] feat(ego_entity_simulation): consider slope in ego entity simulation Signed-off-by: Kotaro Yoshimoto --- .../ego_entity_simulation.hpp | 4 +- .../ego_entity_simulation.cpp | 81 ++++++++++++++++++- 2 files changed, 80 insertions(+), 5 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 3d306a67f29..625a595ebd7 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -64,7 +64,9 @@ class EgoEntitySimulation const std::shared_ptr hdmap_utils_ptr_; 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; 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 35097e1fca8..3743e5002da 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 @@ -263,13 +263,22 @@ void EgoEntitySimulation::update( } else { auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); + auto acceleration_by_slope = not consider_road_slope_ ? 0.0 : [this]() { + // 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); + }(); + 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; @@ -294,6 +303,69 @@ void EgoEntitySimulation::update( updatePreviousValues(); } +auto EgoEntitySimulation::calculateEgoPitch() const -> double +{ + const double ego_x = vehicle_model_ptr_->getX(); + const double ego_y = vehicle_model_ptr_->getY(); + const double ego_yaw = vehicle_model_ptr_->getYaw(); + + geometry_msgs::msg::Pose ego_pose; + ego_pose.position.x = ego_x; + ego_pose.position.y = ego_y; + ego_pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + traffic_simulator::helper::constructRPY(0., 0., ego_yaw)); + + // calculate prev/next point of lanelet centerline nearest to ego pose. + auto ego_lanelet_id = hdmap_utils_ptr_->getClosestLaneletId(ego_pose, 2.0); + if (not ego_lanelet_id) { + return 0.0; + } + + auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet_id.value()); + + // copied from motion_util::findNearestSegmentIndex + auto find_nearest_segment_index = []( + const std::vector & points, + const geometry_msgs::msg::Point & point) { + assert(not points.empty()); + + double min_dist = std::numeric_limits::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; + }; + + const size_t ego_seg_idx = find_nearest_segment_index(centerline_points, ego_pose.position); + + 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 = ego_yaw - 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; @@ -302,7 +374,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(); @@ -315,10 +388,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); }(); From ada5f90b047e06cba1eac0899fef069a9caf41fa Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 31 Jan 2024 17:50:30 +0900 Subject: [PATCH 03/21] feat(ego_entity_simulation): add flog for considering slope in ego entity simulation Signed-off-by: Kotaro Yoshimoto --- .../vehicle_simulation/ego_entity_simulation.hpp | 2 ++ .../src/vehicle_simulation/ego_entity_simulation.cpp | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 625a595ebd7..4cc63686d8c 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -60,6 +60,8 @@ class EgoEntitySimulation traffic_simulator_msgs::msg::EntityStatus status_; + bool consider_road_slope_ = false; + public: const std::shared_ptr hdmap_utils_ptr_; 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 3743e5002da..628416da5a9 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 @@ -38,7 +38,8 @@ EgoEntitySimulation::EgoEntitySimulation( : autoware(std::make_unique()), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), - hdmap_utils_ptr_(hdmap_utils) + hdmap_utils_ptr_(hdmap_utils), + consider_road_slope_(getParameter("consider_road_slope", false)) { } From a4ef2cdada30a6a2eca3bf08bb88d2a725fb000e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 2 Feb 2024 15:02:51 +0900 Subject: [PATCH 04/21] refactor(ego_entity_simulation): rename flag name for considering slope in ego entity simulation Signed-off-by: Kotaro Yoshimoto --- .../vehicle_simulation/ego_entity_simulation.hpp | 2 +- .../src/vehicle_simulation/ego_entity_simulation.cpp | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 4cc63686d8c..2c53e1369a9 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -60,7 +60,7 @@ class EgoEntitySimulation traffic_simulator_msgs::msg::EntityStatus status_; - bool consider_road_slope_ = false; + const bool consider_acceleration_by_road_slope_; public: const std::shared_ptr hdmap_utils_ptr_; 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 628416da5a9..fc92eade932 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 @@ -39,7 +39,8 @@ EgoEntitySimulation::EgoEntitySimulation( vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), hdmap_utils_ptr_(hdmap_utils), - consider_road_slope_(getParameter("consider_road_slope", false)) + consider_acceleration_by_road_slope_( + getParameter("consider_acceleration_by_road_slope", false)) { } @@ -264,7 +265,7 @@ void EgoEntitySimulation::update( } else { auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); - auto acceleration_by_slope = not consider_road_slope_ ? 0.0 : [this]() { + auto acceleration_by_slope = not consider_acceleration_by_road_slope_ ? 0.0 : [this]() { // calculate longitudinal acceleration by slope constexpr double gravity_acceleration = -9.81; const double ego_pitch_angle = calculateEgoPitch(); From f390e2219aa0df907c84666b8ee7320863fdbb88 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 2 Feb 2024 15:11:33 +0900 Subject: [PATCH 05/21] feat(scenario_test_runner): add consider_acceleration_by_road_slope for simple_sensor_simulator Signed-off-by: Kotaro Yoshimoto --- .../launch/scenario_test_runner.launch.py | 117 +++++++++--------- 1 file changed, 60 insertions(+), 57 deletions(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 047b9515896..36d6a688fbb 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -60,45 +60,47 @@ def default_autoware_launch_file_of(architecture_type): def launch_setup(context, *args, **kwargs): # fmt: off - architecture_type = LaunchConfiguration("architecture_type", default="awf/universe") - autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) - autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) - global_frame_rate = LaunchConfiguration("global_frame_rate", default=30.0) - global_real_time_factor = LaunchConfiguration("global_real_time_factor", default=1.0) - global_timeout = LaunchConfiguration("global_timeout", default=180) - initialize_duration = LaunchConfiguration("initialize_duration", default=30) - launch_autoware = LaunchConfiguration("launch_autoware", default=True) - launch_rviz = LaunchConfiguration("launch_rviz", default=False) - launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True) - output_directory = LaunchConfiguration("output_directory", default=Path("/tmp")) - port = LaunchConfiguration("port", default=5555) - record = LaunchConfiguration("record", default=True) - rviz_config = LaunchConfiguration("rviz_config", default="") - scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) - sensor_model = LaunchConfiguration("sensor_model", default="") - sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) - vehicle_model = LaunchConfiguration("vehicle_model", default="") - workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) + architecture_type = LaunchConfiguration("architecture_type", default="awf/universe") + autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context))) + autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context))) + consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False) + global_frame_rate = LaunchConfiguration("global_frame_rate", default=30.0) + global_real_time_factor = LaunchConfiguration("global_real_time_factor", default=1.0) + global_timeout = LaunchConfiguration("global_timeout", default=180) + initialize_duration = LaunchConfiguration("initialize_duration", default=30) + launch_autoware = LaunchConfiguration("launch_autoware", default=True) + launch_rviz = LaunchConfiguration("launch_rviz", default=False) + launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True) + output_directory = LaunchConfiguration("output_directory", default=Path("/tmp")) + port = LaunchConfiguration("port", default=5555) + record = LaunchConfiguration("record", default=True) + rviz_config = LaunchConfiguration("rviz_config", default="") + scenario = LaunchConfiguration("scenario", default=Path("/dev/null")) + sensor_model = LaunchConfiguration("sensor_model", default="") + sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8) + vehicle_model = LaunchConfiguration("vehicle_model", default="") + workflow = LaunchConfiguration("workflow", default=Path("/dev/null")) # fmt: on - print(f"architecture_type := {architecture_type.perform(context)}") - print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") - print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") - print(f"global_frame_rate := {global_frame_rate.perform(context)}") - print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") - print(f"global_timeout := {global_timeout.perform(context)}") - print(f"initialize_duration := {initialize_duration.perform(context)}") - print(f"launch_autoware := {launch_autoware.perform(context)}") - print(f"launch_rviz := {launch_rviz.perform(context)}") - print(f"output_directory := {output_directory.perform(context)}") - print(f"port := {port.perform(context)}") - print(f"record := {record.perform(context)}") - print(f"rviz_config := {rviz_config.perform(context)}") - print(f"scenario := {scenario.perform(context)}") - print(f"sensor_model := {sensor_model.perform(context)}") - print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") - print(f"vehicle_model := {vehicle_model.perform(context)}") - print(f"workflow := {workflow.perform(context)}") + print(f"architecture_type := {architecture_type.perform(context)}") + print(f"autoware_launch_file := {autoware_launch_file.perform(context)}") + print(f"autoware_launch_package := {autoware_launch_package.perform(context)}") + print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}") + print(f"global_frame_rate := {global_frame_rate.perform(context)}") + print(f"global_real_time_factor := {global_real_time_factor.perform(context)}") + print(f"global_timeout := {global_timeout.perform(context)}") + print(f"initialize_duration := {initialize_duration.perform(context)}") + print(f"launch_autoware := {launch_autoware.perform(context)}") + print(f"launch_rviz := {launch_rviz.perform(context)}") + print(f"output_directory := {output_directory.perform(context)}") + print(f"port := {port.perform(context)}") + print(f"record := {record.perform(context)}") + print(f"rviz_config := {rviz_config.perform(context)}") + print(f"scenario := {scenario.perform(context)}") + print(f"sensor_model := {sensor_model.perform(context)}") + print(f"sigterm_timeout := {sigterm_timeout.perform(context)}") + print(f"vehicle_model := {vehicle_model.perform(context)}") + print(f"workflow := {workflow.perform(context)}") def make_parameters(): parameters = [ @@ -115,7 +117,7 @@ def make_parameters(): ] parameters += make_vehicle_parameters() return parameters - + def make_vehicle_parameters(): parameters = [] @@ -131,21 +133,21 @@ def description(): return [ # fmt: off - DeclareLaunchArgument("architecture_type", default_value=architecture_type ), - DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), + DeclareLaunchArgument("architecture_type", default_value=architecture_type), + DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file), DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package), - DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), + DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate), DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor), - DeclareLaunchArgument("global_timeout", default_value=global_timeout ), - DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), - DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), - DeclareLaunchArgument("output_directory", default_value=output_directory ), - DeclareLaunchArgument("rviz_config", default_value=rviz_config ), - DeclareLaunchArgument("scenario", default_value=scenario ), - DeclareLaunchArgument("sensor_model", default_value=sensor_model ), - DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), - DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), - DeclareLaunchArgument("workflow", default_value=workflow ), + DeclareLaunchArgument("global_timeout", default_value=global_timeout), + DeclareLaunchArgument("launch_autoware", default_value=launch_autoware), + DeclareLaunchArgument("launch_rviz", default_value=launch_rviz), + DeclareLaunchArgument("output_directory", default_value=output_directory), + DeclareLaunchArgument("rviz_config", default_value=rviz_config), + DeclareLaunchArgument("scenario", default_value=scenario), + DeclareLaunchArgument("sensor_model", default_value=sensor_model), + DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout), + DeclareLaunchArgument("vehicle_model", default_value=vehicle_model), + DeclareLaunchArgument("workflow", default_value=workflow), # fmt: on Node( package="scenario_test_runner", @@ -156,12 +158,12 @@ def description(): on_exit=ShutdownOnce(), arguments=[ # fmt: off - "--global-frame-rate", global_frame_rate, + "--global-frame-rate", global_frame_rate, "--global-real-time-factor", global_real_time_factor, - "--global-timeout", global_timeout, - "--output-directory", output_directory, - "--scenario", scenario, - "--workflow", workflow, + "--global-timeout", global_timeout, + "--output-directory", output_directory, + "--scenario", scenario, + "--workflow", workflow, # fmt: on ], ), @@ -171,7 +173,8 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=[{"port": port}, {"consider_acceleration_by_road_slope", + consider_acceleration_by_road_slope}] + make_vehicle_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. From 184ee8e46e4b9024a9f68206b800d307851a8864 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 15 Feb 2024 14:51:00 +0900 Subject: [PATCH 06/21] chore: format Signed-off-by: Kotaro Yoshimoto --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 a608735fbba..4083b616e55 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 @@ -42,7 +42,7 @@ EgoEntitySimulation::EgoEntitySimulation( hdmap_utils_ptr_(hdmap_utils), vehicle_parameters(parameters), consider_acceleration_by_road_slope_( - getParameter("consider_acceleration_by_road_slope", false)) + getParameter("consider_acceleration_by_road_slope", false)) { autoware->set_parameter(use_sim_time); } From 3871e2068058e04e9a2b9eae337f200f9683ae55 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 19 Feb 2024 10:20:34 +0900 Subject: [PATCH 07/21] chore: format Signed-off-by: Kotaro Yoshimoto --- .../launch/scenario_test_runner.launch.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index eb2b069adfa..69dc42ddaaf 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -136,22 +136,22 @@ def description(): return [ # fmt: off - DeclareLaunchArgument("architecture_type", default_value=architecture_type), - DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file), + DeclareLaunchArgument("architecture_type", default_value=architecture_type ), + DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package), - DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate), + DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor), - DeclareLaunchArgument("global_timeout", default_value=global_timeout), - DeclareLaunchArgument("launch_autoware", default_value=launch_autoware), - DeclareLaunchArgument("launch_rviz", default_value=launch_rviz), - DeclareLaunchArgument("output_directory", default_value=output_directory), - DeclareLaunchArgument("rviz_config", default_value=rviz_config), - DeclareLaunchArgument("scenario", default_value=scenario), - DeclareLaunchArgument("sensor_model", default_value=sensor_model), - DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout), + DeclareLaunchArgument("global_timeout", default_value=global_timeout ), + DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), + DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), + DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("rviz_config", default_value=rviz_config ), + DeclareLaunchArgument("scenario", default_value=scenario ), + DeclareLaunchArgument("sensor_model", default_value=sensor_model ), + DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), - DeclareLaunchArgument("vehicle_model", default_value=vehicle_model), - DeclareLaunchArgument("workflow", default_value=workflow), + DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), + DeclareLaunchArgument("workflow", default_value=workflow ), # fmt: on Node( package="scenario_test_runner", From 4e7f42feb646a6af1d3e3cac5d39c4f6ab2cdbf3 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 19 Feb 2024 10:27:14 +0900 Subject: [PATCH 08/21] doc: add notification to duplicated lane matching algorithm Signed-off-by: Kotaro Yoshimoto --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 1 + simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 ++ 2 files changed, 3 insertions(+) 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 4083b616e55..3d09f35dcca 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 @@ -473,6 +473,7 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( traffic_simulator::helper::getUniqueValues(autoware->getRouteLanelets()); std::optional lanelet_pose; + // the lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose const auto get_matching_length = [&] { return std::max( vehicle_parameters.axles.front_axle.track_width, diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index a9fb5ac3fc1..aea0e89b002 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/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 lanelet_pose; + + // the lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet const auto get_matching_length = [&] { return std::max( vehicle_parameters.axles.front_axle.track_width, From 54faa275e75e01f24527492c430c631bd6e309de Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 16 Feb 2024 19:12:55 +0900 Subject: [PATCH 09/21] change example Signed-off-by: Masaya Kataoka Signed-off-by: Kotaro Yoshimoto --- .github/pull_request_samples/example_detail.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/pull_request_samples/example_detail.md b/.github/pull_request_samples/example_detail.md index c31fe9672cd..a9beca2f5d2 100644 --- a/.github/pull_request_samples/example_detail.md +++ b/.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. +Please check [this document about spline curve.](https://people.computing.clemson.edu/~dhouse/courses/405/notes/splines.pdf) +This link is an example and is not directly related to this sample. # Destructive Changes From 2bf107c7df60faef60de2665e586ab0c5138bc71 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Fri, 16 Feb 2024 19:16:42 +0900 Subject: [PATCH 10/21] change link Signed-off-by: Masaya Kataoka --- .github/pull_request_samples/example_detail.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/pull_request_samples/example_detail.md b/.github/pull_request_samples/example_detail.md index a9beca2f5d2..caea007de38 100644 --- a/.github/pull_request_samples/example_detail.md +++ b/.github/pull_request_samples/example_detail.md @@ -28,7 +28,7 @@ This PR fixes how the length of the curve is computed ## References -Please check [this document about spline curve.](https://people.computing.clemson.edu/~dhouse/courses/405/notes/splines.pdf) +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 From 8e18decbbd14ebe418583a9e7846f38e14210d1a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 20 Feb 2024 21:21:02 +0900 Subject: [PATCH 11/21] fix: pass consider_acceleration_by_road_slope to inside of EgoEntitySimulator --- .../vehicle_simulation/ego_entity_simulation.hpp | 4 +++- .../src/simple_sensor_simulator.cpp | 9 ++++++++- .../src/vehicle_simulation/ego_entity_simulation.cpp | 5 ++--- .../launch/scenario_test_runner.launch.py | 5 +++-- 4 files changed, 16 insertions(+), 7 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 47a0c644f9b..c55c0b82f34 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -64,6 +64,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; private: @@ -84,7 +85,8 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &, const rclcpp::Parameter & use_sim_time); + const std::shared_ptr &, 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; diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 798c9871a25..9a5253c5304 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/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( 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); 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 3d09f35dcca..197723ba3af 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 @@ -35,14 +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, - const rclcpp::Parameter & use_sim_time) + const rclcpp::Parameter & use_sim_time, const bool consider_acceleration_by_road_slope) : autoware(std::make_unique()), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), hdmap_utils_ptr_(hdmap_utils), vehicle_parameters(parameters), - consider_acceleration_by_road_slope_( - getParameter("consider_acceleration_by_road_slope", false)) + consider_acceleration_by_road_slope_(consider_acceleration_by_road_slope) { autoware->set_parameter(use_sim_time); } diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 69dc42ddaaf..60350e296cb 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -109,6 +109,7 @@ def make_parameters(): {"architecture_type": architecture_type}, {"autoware_launch_file": autoware_launch_file}, {"autoware_launch_package": autoware_launch_package}, + {"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope}, {"initialize_duration": initialize_duration}, {"launch_autoware": launch_autoware}, {"port": port}, @@ -139,6 +140,7 @@ def description(): DeclareLaunchArgument("architecture_type", default_value=architecture_type ), DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package), + DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope), DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor), DeclareLaunchArgument("global_timeout", default_value=global_timeout ), @@ -177,8 +179,7 @@ def description(): namespace="simulation", output="screen", on_exit=ShutdownOnce(), - parameters=make_parameters() + [{"use_sim_time": True}, {"consider_acceleration_by_road_slope", - consider_acceleration_by_road_slope}], + parameters=make_parameters() + [{"use_sim_time": True}], condition=IfCondition(launch_simple_sensor_simulator), ), # The `name` keyword overrides the name for all created nodes, so duplicated nodes appear. From 5bf7dc9df3abd69f35fc7c9474b373a087abf652 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 20 Feb 2024 21:23:21 +0900 Subject: [PATCH 12/21] refactor(EgoEntitySimulation): convert lane pose matching processing to getMatchedLaneletPoseFromEntityStatus function --- .../ego_entity_simulation.hpp | 2 + .../ego_entity_simulation.cpp | 84 +++++++++---------- .../src/entity/ego_entity.cpp | 2 +- 3 files changed, 45 insertions(+), 43 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index c55c0b82f34..9fcd5d40197 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -78,6 +78,8 @@ class EgoEntitySimulation auto getLinearJerk(double step_time) -> double; + auto getMatchedLaneletPoseFromEntityStatus(const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const -> std::optional; + auto updatePreviousValues() -> void; public: 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 197723ba3af..3e9c6fa6131 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 @@ -307,21 +307,39 @@ void EgoEntitySimulation::update( updatePreviousValues(); } -auto EgoEntitySimulation::calculateEgoPitch() const -> double +auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const + -> std::optional { - const double ego_x = vehicle_model_ptr_->getX(); - const double ego_y = vehicle_model_ptr_->getY(); - const double ego_yaw = vehicle_model_ptr_->getYaw(); + // 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; }(); - geometry_msgs::msg::Pose ego_pose; - ego_pose.position.x = ego_x; - ego_pose.position.y = ego_y; - ego_pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( - traffic_simulator::helper::constructRPY(0., 0., ego_yaw)); + std::optional 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_id = hdmap_utils_ptr_->getClosestLaneletId(ego_pose, 2.0); - if (not ego_lanelet_id) { + 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; } @@ -351,14 +369,17 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double return min_idx; }; - const size_t ego_seg_idx = find_nearest_segment_index(centerline_points, ego_pose.position); + 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 = ego_yaw - lanelet_yaw; + 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; @@ -468,41 +489,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 lanelet_pose; - - // the lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose - 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(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 diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index aea0e89b002..523c37a61dd 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -276,7 +276,7 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); std::optional lanelet_pose; - // the lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet + // 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, From 1bbc4860f3e6bffd9b8cab3245adbb5148faffd7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 20 Feb 2024 21:24:36 +0900 Subject: [PATCH 13/21] fix(EgoEntitySimulation) --- .../ego_entity_simulation.cpp | 18 +++++++++++------- 1 file changed, 11 insertions(+), 7 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 3e9c6fa6131..e9c7c182a48 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 @@ -267,12 +267,16 @@ void EgoEntitySimulation::update( } else { auto input = Eigen::VectorXd(vehicle_model_ptr_->getDimU()); - auto acceleration_by_slope = not consider_acceleration_by_road_slope_ ? 0.0 : [this]() { - // 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); + 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_) { @@ -343,7 +347,7 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double return 0.0; } - auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet_id.value()); + auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet.value().lanelet_id); // copied from motion_util::findNearestSegmentIndex auto find_nearest_segment_index = []( From 881fc3a2879acc53fc7ab2ecc87bd28dfe000757 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 20 Feb 2024 21:28:45 +0900 Subject: [PATCH 14/21] chore: format --- .../ego_entity_simulation.hpp | 4 ++- .../launch/scenario_test_runner.launch.py | 32 +++++++++---------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp index 9fcd5d40197..bd0b234b291 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp @@ -78,7 +78,9 @@ class EgoEntitySimulation auto getLinearJerk(double step_time) -> double; - auto getMatchedLaneletPoseFromEntityStatus(const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const -> std::optional; + auto getMatchedLaneletPoseFromEntityStatus( + const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const + -> std::optional; auto updatePreviousValues() -> void; diff --git a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py index 60350e296cb..674f01d8666 100755 --- a/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py +++ b/test_runner/scenario_test_runner/launch/scenario_test_runner.launch.py @@ -137,23 +137,23 @@ def description(): return [ # fmt: off - DeclareLaunchArgument("architecture_type", default_value=architecture_type ), - DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), - DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package), + DeclareLaunchArgument("architecture_type", default_value=architecture_type ), + DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ), + DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package ), DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope), - DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), - DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor), - DeclareLaunchArgument("global_timeout", default_value=global_timeout ), - DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), - DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), - DeclareLaunchArgument("output_directory", default_value=output_directory ), - DeclareLaunchArgument("rviz_config", default_value=rviz_config ), - DeclareLaunchArgument("scenario", default_value=scenario ), - DeclareLaunchArgument("sensor_model", default_value=sensor_model ), - DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), - DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), - DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), - DeclareLaunchArgument("workflow", default_value=workflow ), + DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ), + DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor ), + DeclareLaunchArgument("global_timeout", default_value=global_timeout ), + DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), + DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), + DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("rviz_config", default_value=rviz_config ), + DeclareLaunchArgument("scenario", default_value=scenario ), + DeclareLaunchArgument("sensor_model", default_value=sensor_model ), + DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ), + DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), + DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), + DeclareLaunchArgument("workflow", default_value=workflow ), # fmt: on Node( package="scenario_test_runner", From ecec2419a23ef83b5d0591e5bb8bfb49c807c44b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:14:25 +0900 Subject: [PATCH 15/21] Update external/concealer/src/autoware_universe.cpp Co-authored-by: Masaya Kataoka --- external/concealer/src/autoware_universe.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index dce5f359825..11174493820 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -156,8 +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 + // @todo Add support for GearCommand::NONE to return 0.0 + // @sa 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 From f16fddab2db7677fb0be12742dad61565b1adb5f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:23:35 +0900 Subject: [PATCH 16/21] Update simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp Co-authored-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e9c7c182a48..897afb3a2ff 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 @@ -349,7 +349,7 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double auto centerline_points = hdmap_utils_ptr_->getCenterPoints(ego_lanelet.value().lanelet_id); - // copied from motion_util::findNearestSegmentIndex + // @note Copied from motion_util::findNearestSegmentIndex auto find_nearest_segment_index = []( const std::vector & points, const geometry_msgs::msg::Point & point) { From cfed6252419da41d47cbcb201c89edd3cd251640 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:23:52 +0900 Subject: [PATCH 17/21] Update simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp Co-authored-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 4 ++-- 1 file changed, 2 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 897afb3a2ff..ef538dcac78 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 @@ -109,8 +109,8 @@ auto EgoEntitySimulation::makeSimulationModel( const auto steer_time_delay = getParameter("steer_time_delay", 0.24); const auto vel_lim = getParameter("vel_lim", parameters.performance.max_speed); // 50.0 const auto vel_rate_lim = getParameter("vel_rate_lim", parameters.performance.max_acceleration); // 7.0 - const auto vel_time_constant = getParameter("vel_time_constant", 0.1); // 0.5 is default value on simple_planning_simulator - const auto vel_time_delay = getParameter("vel_time_delay", 0.1); // 0.25 is default value on simple_planning_simulator + const auto vel_time_constant = getParameter("vel_time_constant", 0.1); // @note 0.5 is default value on simple_planning_simulator + const auto vel_time_delay = getParameter("vel_time_delay", 0.1); // @note 0.25 is default value on simple_planning_simulator const auto wheel_base = getParameter("wheel_base", parameters.axles.front_axle.position_x - parameters.axles.rear_axle.position_x); // clang-format on From 445030b6f8bd7e5d3a34233b4f479c60970c869f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:23:57 +0900 Subject: [PATCH 18/21] Update simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp Co-authored-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ef538dcac78..e0ae1aca47c 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 @@ -315,7 +315,7 @@ auto EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus( const traffic_simulator_msgs::msg::EntityStatus & status, const double entity_width) const -> std::optional { - // the lanelet matching algorithm should be equivalent to the one used in EgoEntity::setMapPose + // @note 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; }(); From fd9659312425d506694cf49f2f1b3a730f2c435a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:24:05 +0900 Subject: [PATCH 19/21] Update simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp Co-authored-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 e0ae1aca47c..168ec3321a3 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 @@ -381,7 +381,7 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double 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 + // @note 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; From bc70e40d47400ff41ad5d1b404ca3af6dfafca8c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:24:12 +0900 Subject: [PATCH 20/21] Update simulation/simple_sensor_simulator/src/vehicle_simulation/ego_entity_simulation.cpp Co-authored-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 168ec3321a3..c88290c5975 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 @@ -385,7 +385,7 @@ auto EgoEntitySimulation::calculateEgoPitch() const -> double 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. + // @note 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); From 1ad085ef11dffcd9a87e1b35177698b439dad5d1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 21 Feb 2024 16:24:43 +0900 Subject: [PATCH 21/21] Update simulation/traffic_simulator/src/entity/ego_entity.cpp Co-authored-by: Masaya Kataoka --- simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index 523c37a61dd..cf59bb9dfa4 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -276,7 +276,7 @@ auto EgoEntity::setMapPose(const geometry_msgs::msg::Pose & map_pose) -> void const auto unique_route_lanelets = traffic_simulator::helper::getUniqueValues(getRouteLanelets()); std::optional lanelet_pose; - // the lanelet matching algorithm should be equivalent to the one used in EgoEntitySimulation::getMatchedLaneletPoseFromEntityStatus + // @note 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,