diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index de8ed46b214..11174493820 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 + // @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 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 e281439e26d..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 @@ -60,12 +60,17 @@ class EgoEntitySimulation traffic_simulator_msgs::msg::EntityStatus status_; + const bool consider_acceleration_by_road_slope_; + public: const std::shared_ptr 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; @@ -73,6 +78,10 @@ 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: @@ -80,7 +89,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 a184d40cc55..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 @@ -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, - 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) + 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("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); // @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 @@ -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 +{ + // @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; }(); + + 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 = 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); + + // @note 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; + }; + + 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); + + // @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; + + // @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); + 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 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(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 a9fb5ac3fc1..cf59bb9dfa4 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; + + // @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, 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 ae0b1e34b43..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 @@ -60,53 +60,56 @@ 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) - use_sim_time = LaunchConfiguration("use_sim_time", default=False) - 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) + use_sim_time = LaunchConfiguration("use_sim_time", default=False) + 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"use_sim_time := {use_sim_time.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"use_sim_time := {use_sim_time.perform(context)}") + print(f"vehicle_model := {vehicle_model.perform(context)}") + print(f"workflow := {workflow.perform(context)}") def make_parameters(): 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}, @@ -134,22 +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("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("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 ), # fmt: on Node( package="scenario_test_runner",