From 9eeca7dcdb07cad2e159c9ac9197190746639813 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Tue, 10 Oct 2023 18:10:29 +0900 Subject: [PATCH 01/43] consider lanalet slope Signed-off-by: Masaya Kataoka --- .../src/vehicle_simulation/ego_entity_simulation.cpp | 9 +++++++++ 1 file changed, 9 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 9b39a8f108a..d2651213eb4 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,6 +385,15 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( 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; + const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( + spline.getPose(s_value.value()).orientation); + const auto original_rpy = + quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); + status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(rpy.x).y(original_rpy.y).z(rpy.z)); + lanelet_pose->rpy = + quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( + spline.getPose(s_value.value()).orientation, status.pose.orientation)); } } From 160b51afd6cd6cf0da23811663ac4ff0b414ea5d Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 11 Oct 2023 16:50:17 +0900 Subject: [PATCH 02/43] add considerLaneletSlope() function Signed-off-by: Masaya Kataoka --- .../simple_sensor_simulator/simple_sensor_simulator.hpp | 3 +++ .../src/simple_sensor_simulator.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index 5edd84e8a29..aa1914fb2d7 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -136,6 +136,9 @@ class ScenarioSimulator : public rclcpp::Node -> simulation_api_schema::AttachPseudoTrafficLightDetectorResponse; int getSocketPort(); + + bool considerLaneletSlope(); + std::vector ego_vehicles_; std::vector vehicles_; std::vector pedestrians_; diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index aafab6ef288..b06bf56022e 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -76,6 +76,14 @@ int ScenarioSimulator::getSocketPort() return get_parameter("port").as_int(); } +bool ScenarioSimulator::considerLaneletSlope() +{ + if (!has_parameter("consider_lanelet_slope")) { + declare_parameter("consider_lanelet_slope", false); + } + return get_parameter("consider_lanelet_slope").as_bool(); +} + auto ScenarioSimulator::initialize(const simulation_api_schema::InitializeRequest & req) -> simulation_api_schema::InitializeResponse { From 77e319787b3e382696b8e8508c26815f26188d02 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 12 Oct 2023 19:48:33 +0900 Subject: [PATCH 03/43] remove warnings Signed-off-by: Masaya Kataoka --- .../sensor_simulation/lidar/lidar_sensor.hpp | 2 +- .../sensor_simulation/sensor_simulation.hpp | 2 +- .../simple_sensor_simulator.hpp | 3 +-- .../src/simple_sensor_simulator.cpp | 16 +++++++--------- 4 files changed, 10 insertions(+), 13 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp index f28d48ca608..ce33bdbeeea 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp @@ -48,7 +48,7 @@ class LidarSensorBase virtual ~LidarSensorBase() = default; virtual auto update( - const double current_simulation_time, const std::vector &, + const double /*current_simulation_time*/, const std::vector &, const rclcpp::Time & current_ros_time) -> void = 0; auto getDetectedObjects() const -> const std::vector & { return detected_objects_; } diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp index 7c9dfb300f9..6927a364d4f 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/sensor_simulation.hpp @@ -92,7 +92,7 @@ class SensorSimulation } auto attachPseudoTrafficLightsDetector( - const double current_simulation_time, + const double /*current_simulation_time*/, const simulation_api_schema::PseudoTrafficLightDetectorConfiguration & configuration, rclcpp::Node & node, std::shared_ptr hdmap_utils) -> void { diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index aa1914fb2d7..3849f93785a 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -137,8 +137,6 @@ class ScenarioSimulator : public rclcpp::Node int getSocketPort(); - bool considerLaneletSlope(); - std::vector ego_vehicles_; std::vector vehicles_; std::vector pedestrians_; @@ -156,6 +154,7 @@ class ScenarioSimulator : public rclcpp::Node geographic_msgs::msg::GeoPoint getOrigin(); std::shared_ptr hdmap_utils_; std::shared_ptr ego_entity_simulation_; + const bool consider_lanelet_slope_; bool isEgo(const std::string & name); bool isEntityExists(const std::string & name); diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index b06bf56022e..3906763a7cf 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -47,7 +47,13 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options) [this](auto &&... xs) { return followPolylineTrajectory(std::forward(xs)...); }, [this](auto &&... xs) { return attachPseudoTrafficLightDetector(std::forward(xs)...); - }) + }), + consider_lanelet_slope_([&]() { + if (!has_parameter("consider_lanelet_slope")) { + declare_parameter("consider_lanelet_slope", false); + } + return get_parameter("consider_lanelet_slope").as_bool(); + }()) { } @@ -76,14 +82,6 @@ int ScenarioSimulator::getSocketPort() return get_parameter("port").as_int(); } -bool ScenarioSimulator::considerLaneletSlope() -{ - if (!has_parameter("consider_lanelet_slope")) { - declare_parameter("consider_lanelet_slope", false); - } - return get_parameter("consider_lanelet_slope").as_bool(); -} - auto ScenarioSimulator::initialize(const simulation_api_schema::InitializeRequest & req) -> simulation_api_schema::InitializeResponse { From d2f5e2eea664f91e08e45ed2bdebeed4015a020c Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 12 Oct 2023 19:52:28 +0900 Subject: [PATCH 04/43] add consider_lanelet_slope member value Signed-off-by: Masaya Kataoka --- .../vehicle_simulation/ego_entity_simulation.hpp | 5 +++-- .../simple_sensor_simulator/src/simple_sensor_simulator.cpp | 2 +- .../src/vehicle_simulation/ego_entity_simulation.cpp | 5 +++-- 3 files changed, 7 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 e6cd5a4579f..88bea472442 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 @@ -43,7 +43,6 @@ class EgoEntitySimulation private: const VehicleModelType vehicle_model_type_; - const std::shared_ptr vehicle_model_ptr_; std::optional previous_linear_velocity_, previous_angular_velocity_; @@ -61,6 +60,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; + const bool consider_lanelet_slope; private: auto getCurrentPose() const -> geometry_msgs::msg::Pose; @@ -78,7 +78,8 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &); + const std::shared_ptr &, + const bool consider_lanelet_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 3906763a7cf..c529e067f47 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -204,7 +204,7 @@ auto ScenarioSimulator::spawnVehicleEntity( traffic_simulator_msgs::msg::VehicleParameters parameters; simulation_interface::toMsg(req.parameters(), parameters); ego_entity_simulation_ = std::make_shared( - parameters, step_time_, hdmap_utils_); + parameters, step_time_, hdmap_utils_, consider_lanelet_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 d2651213eb4..2fcf35fefb0 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 @@ -33,11 +33,12 @@ 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 std::shared_ptr & hdmap_utils, const bool consider_lanelet_slope) : 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_lanelet_slope(consider_lanelet_slope) { } From 9c2f1a04f11ea9cabe10bdcb1dd15d51ed8d92dd Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 12 Oct 2023 19:54:01 +0900 Subject: [PATCH 05/43] enable toggle setting Signed-off-by: Masaya Kataoka --- .../ego_entity_simulation.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 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 2fcf35fefb0..e88adfae705 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 @@ -386,15 +386,17 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( 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; - const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( - spline.getPose(s_value.value()).orientation); - const auto original_rpy = - quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); - status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( - geometry_msgs::build().x(rpy.x).y(original_rpy.y).z(rpy.z)); - lanelet_pose->rpy = - quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( - spline.getPose(s_value.value()).orientation, status.pose.orientation)); + if(consider_lanelet_slope){ + const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( + spline.getPose(s_value.value()).orientation); + const auto original_rpy = + quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); + status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(rpy.x).y(original_rpy.y).z(rpy.z)); + lanelet_pose->rpy = + quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( + spline.getPose(s_value.value()).orientation, status.pose.orientation)); + } } } From ccbb3f5b962095dcd2b5076776a5da4179324e42 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Thu, 12 Oct 2023 19:57:59 +0900 Subject: [PATCH 06/43] modify launch file --- .../launch/scenario_test_runner.launch.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) 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 aeae2f338d8..2f74415a479 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 @@ -63,6 +63,7 @@ def launch_setup(context, *args, **kwargs): 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_lanelet_slope = LaunchConfiguration("consider_lanelet_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) @@ -84,6 +85,7 @@ def launch_setup(context, *args, **kwargs): 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_lanelet_slope := {consider_lanelet_slope(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)}") @@ -134,6 +136,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_lanelet_slope", default_value=consider_lanelet_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 ), @@ -172,7 +175,9 @@ def description(): name="simple_sensor_simulator", output="screen", on_exit=ShutdownOnce(), - parameters=[{"port": port}]+make_vehicle_parameters(), + parameters=[ + {"port": port, "consider_lanelet_slope": consider_lanelet_slope} + ] + make_vehicle_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), LifecycleNode( From 8129d8f37c6845378fb1e44f2dab83a7c016b53d Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Mon, 16 Oct 2023 18:09:46 +0900 Subject: [PATCH 07/43] update slop calculation logic Signed-off-by: Masaya Kataoka --- common/math/geometry/src/polygon/line_segment.cpp | 6 ++++-- common/math/geometry/src/spline/hermite_curve.cpp | 4 ++-- .../sensor_simulation/lidar/lidar_sensor.hpp | 3 ++- .../vehicle_simulation/ego_entity_simulation.hpp | 3 +-- .../src/vehicle_simulation/ego_entity_simulation.cpp | 7 +++++-- .../traffic_simulator/src/hdmap_utils/hdmap_utils.cpp | 2 +- .../launch/scenario_test_runner.launch.py | 2 +- 7 files changed, 16 insertions(+), 11 deletions(-) diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index 9cf0022e7f3..a28a555e0e7 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -96,8 +96,10 @@ auto LineSegment::getPose(const double s, const bool denormalize_s) const .orientation([this]() -> geometry_msgs::msg::Quaternion { const auto tangent_vec = getVector(); return quaternion_operation::convertEulerAngleToQuaternion( - geometry_msgs::build().x(0.0).y(0.0).z( - std::atan2(tangent_vec.y, tangent_vec.x))); + geometry_msgs::build() + .x(0.0) + .y(std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y))) + .z(std::atan2(tangent_vec.y, tangent_vec.x))); }()); } diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp index c32d676fd10..29a97250fec 100644 --- a/common/math/geometry/src/spline/hermite_curve.cpp +++ b/common/math/geometry/src/spline/hermite_curve.cpp @@ -263,8 +263,8 @@ const geometry_msgs::msg::Pose HermiteCurve::getPose(double s, bool denormalize_ geometry_msgs::msg::Pose pose; geometry_msgs::msg::Vector3 tangent_vec = getTangentVector(s, false); geometry_msgs::msg::Vector3 rpy; - rpy.x = 0.0; - rpy.y = 0.0; + rpy.x = 0; + rpy.y = std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)); rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy); pose.position = getPoint(s); diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp index ce33bdbeeea..f09f54045ad 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp @@ -48,7 +48,8 @@ class LidarSensorBase virtual ~LidarSensorBase() = default; virtual auto update( - const double /*current_simulation_time*/, const std::vector &, + const double /*current_simulation_time*/, + const std::vector &, const rclcpp::Time & current_ros_time) -> void = 0; auto getDetectedObjects() const -> const std::vector & { return detected_objects_; } 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 88bea472442..4185a218c99 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,8 +78,7 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &, - const bool consider_lanelet_slope); + const std::shared_ptr &, const bool consider_lanelet_slope); auto update(double time, double step_time, bool npc_logic_started) -> void; 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 e88adfae705..7dee349c1b4 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 @@ -386,13 +386,16 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( 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; - if(consider_lanelet_slope){ + if (consider_lanelet_slope) { const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( spline.getPose(s_value.value()).orientation); const auto original_rpy = quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( - geometry_msgs::build().x(rpy.x).y(original_rpy.y).z(rpy.z)); + geometry_msgs::build() + .x(original_rpy.x) + .y(rpy.y) + .z(original_rpy.z)); lanelet_pose->rpy = quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( spline.getPose(s_value.value()).orientation, status.pose.orientation)); diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 94ab54ce7fd..3e3037dcd91 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1404,7 +1404,7 @@ auto HdMapUtils::toMapPose(const traffic_simulator_msgs::msg::LaneletPose & lane const auto tangent_vec = spline->getTangentVector(pose->s); geometry_msgs::msg::Vector3 rpy; rpy.x = 0.0; - rpy.y = 0.0; + rpy.y = std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)); rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); ret.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy) * quaternion_operation::convertEulerAngleToQuaternion(pose->rpy); 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 2f74415a479..f658f44518f 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 @@ -85,7 +85,7 @@ def launch_setup(context, *args, **kwargs): 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_lanelet_slope := {consider_lanelet_slope(context)}") + print(f"consider_lanelet_slope := {consider_lanelet_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)}") From 64ee030c8cb36704f2ac543a158f23551ae76dff Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 18 Oct 2023 13:56:27 +0900 Subject: [PATCH 08/43] fix test case Signed-off-by: Masaya Kataoka --- common/math/geometry/test/test_line_segment.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp index 5802ad3df9e..e3cc9ca04b5 100644 --- a/common/math/geometry/test/test_line_segment.cpp +++ b/common/math/geometry/test/test_line_segment.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include +#include #include #include @@ -84,7 +85,8 @@ TEST(LineSegment, GetPose) line.getPose(0, false), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(0)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); + .orientation(quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); // [Snippet_getPose_with_s_0] /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_0 @@ -95,7 +97,8 @@ TEST(LineSegment, GetPose) line.getPose(1, false), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(1)) - .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); + .orientation(quaternion_operation::convertEulerAngleToQuaternion( + geometry_msgs::build().x(0).y(-M_PI * 0.5).z(0)))); // [Snippet_getPose_with_s_1] /// @snippet test/test_line_segment.cpp Snippet_getPose_with_s_1 } From 281f9edb2d21541d0beedc159be54c541f12c4e6 Mon Sep 17 00:00:00 2001 From: Masaya Kataoka Date: Wed, 18 Oct 2023 14:13:40 +0900 Subject: [PATCH 09/43] change default value Signed-off-by: Masaya Kataoka --- .../scenario_test_runner/launch/scenario_test_runner.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f658f44518f..6337274cbb3 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 @@ -63,7 +63,7 @@ def launch_setup(context, *args, **kwargs): 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_lanelet_slope = LaunchConfiguration("consider_lanelet_slope", default=False) + consider_lanelet_slope = LaunchConfiguration("consider_lanelet_slope", default=True) 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) From 1caeecac61196e774ce680e3209e0a90a6c8d146 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 5 Feb 2024 17:48:39 +0900 Subject: [PATCH 10/43] refactor: use consider_pose_by_road_slope instead of consider_lanelet_pose Signed-off-by: Kotaro Yoshimoto --- .../simple_sensor_simulator.hpp | 2 +- .../ego_entity_simulation.hpp | 4 +- .../src/simple_sensor_simulator.cpp | 10 +-- .../ego_entity_simulation.cpp | 5 +- .../launch/scenario_test_runner.launch.py | 74 +++++++++---------- 5 files changed, 48 insertions(+), 47 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index 3849f93785a..f0440db78fa 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -154,7 +154,7 @@ class ScenarioSimulator : public rclcpp::Node geographic_msgs::msg::GeoPoint getOrigin(); std::shared_ptr hdmap_utils_; std::shared_ptr ego_entity_simulation_; - const bool consider_lanelet_slope_; + const bool consider_pose_by_road_slope_; bool isEgo(const std::string & name); bool isEntityExists(const std::string & name); 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 6b5ec01380b..43d4c78c28a 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 @@ -61,7 +61,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; - const bool consider_lanelet_slope; + const bool consider_pose_by_road_slope; private: auto getCurrentPose() const -> geometry_msgs::msg::Pose; @@ -79,7 +79,7 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &, const bool consider_lanelet_slope); + const std::shared_ptr &, const bool consider_pose_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 c529e067f47..044a22f4245 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -48,11 +48,11 @@ ScenarioSimulator::ScenarioSimulator(const rclcpp::NodeOptions & options) [this](auto &&... xs) { return attachPseudoTrafficLightDetector(std::forward(xs)...); }), - consider_lanelet_slope_([&]() { - if (!has_parameter("consider_lanelet_slope")) { - declare_parameter("consider_lanelet_slope", false); + consider_pose_by_road_slope_([&]() { + if (!has_parameter("consider_pose_by_road_slope")) { + declare_parameter("consider_pose_by_road_slope", false); } - return get_parameter("consider_lanelet_slope").as_bool(); + return get_parameter("consider_pose_by_road_slope").as_bool(); }()) { } @@ -204,7 +204,7 @@ auto ScenarioSimulator::spawnVehicleEntity( traffic_simulator_msgs::msg::VehicleParameters parameters; simulation_interface::toMsg(req.parameters(), parameters); ego_entity_simulation_ = std::make_shared( - parameters, step_time_, hdmap_utils_, consider_lanelet_slope_); + parameters, step_time_, hdmap_utils_, consider_pose_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 c70e81462dd..027775423c5 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 @@ -34,12 +34,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 bool consider_lanelet_slope) + const std::shared_ptr & hdmap_utils, + const bool consider_pose_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), - consider_lanelet_slope(consider_lanelet_slope) + consider_pose_by_road_slope(consider_pose_by_road_slope) { } 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 8108dc6288a..22f0dc914b6 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 @@ -63,7 +63,7 @@ def launch_setup(context, *args, **kwargs): 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_lanelet_slope = LaunchConfiguration("consider_lanelet_slope", default=True) + consider_pose_by_road_slope = LaunchConfiguration("consider_pose_by_road_slope", default=True) 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) @@ -82,25 +82,25 @@ def launch_setup(context, *args, **kwargs): 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"consider_lanelet_slope := {consider_lanelet_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)}") + 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_pose_by_road_slope := {consider_pose_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 = [ @@ -133,22 +133,22 @@ 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("consider_lanelet_slope", default_value=consider_lanelet_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("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_pose_by_road_slope",default_value=consider_pose_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("vehicle_model", default_value=vehicle_model ), + DeclareLaunchArgument("workflow", default_value=workflow ), # fmt: on Node( package="scenario_test_runner", @@ -175,7 +175,7 @@ def description(): output="screen", on_exit=ShutdownOnce(), parameters=[ - {"port": port, "consider_lanelet_slope": consider_lanelet_slope} + {"port": port, "consider_pose_by_road_slope": consider_pose_by_road_slope} ] + make_vehicle_parameters(), condition=IfCondition(launch_simple_sensor_simulator), ), From b612536d59b7947fb0e1642675765f4b2b2b2919 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Feb 2024 19:02:03 +0900 Subject: [PATCH 11/43] doc: add memo for OpenSCENARIO standard violation Signed-off-by: Kotaro Yoshimoto --- .../src/syntax/relative_distance_condition.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index e2669fd8dfe..fd424468325 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -68,6 +68,9 @@ auto RelativeDistanceCondition::distance< } } +/** + * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. + */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::longitudinal, true>( @@ -97,6 +100,9 @@ auto RelativeDistanceCondition::distance< } } +/** + * @note This implementation differs from the OpenSCENARIO standard. See the section "6.4. Distances" in the OpenSCENARIO User Guide. + */ template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::lateral, true>( From 48b490745e6c9261f59529f549bb78b168ae23fd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Feb 2024 19:03:09 +0900 Subject: [PATCH 12/43] fix(RelativeDistanceCondition): consider z-axis Signed-off-by: Kotaro Yoshimoto --- .../src/syntax/relative_distance_condition.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index fd424468325..41af2a7cbac 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -128,7 +128,8 @@ auto RelativeDistanceCondition::distance< global().entities->at(entity_ref).as().is_added) { return std::hypot( makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y); + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y, + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z); } else { return Double::nan(); } @@ -144,7 +145,8 @@ auto RelativeDistanceCondition::distance< global().entities->at(entity_ref).as().is_added) { return std::hypot( makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y); + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y, + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z); } else { return Double::nan(); } From f5078c92dc444f958dea54e53550189d15a1b92a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Feb 2024 19:10:28 +0900 Subject: [PATCH 13/43] fix(DistanceCondition): consider z-axis in euclidian distance Signed-off-by: Kotaro Yoshimoto --- .../src/syntax/distance_condition.cpp | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index c56a27791f8..fb4db1a0ca7 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -111,22 +111,26 @@ auto DistanceCondition::distance< [&](const WorldPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }), position); } @@ -141,22 +145,26 @@ auto DistanceCondition::distance< [&](const WorldPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(relative_world.position.x, relative_world.position.y); + return std::hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z); }), position); } From 32c0ca807d360f5ca486059ff551d1084a352c8d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Feb 2024 19:23:33 +0900 Subject: [PATCH 14/43] fix(ReachPositionCondition): consider z-axis in euclidian distance Signed-off-by: Kotaro Yoshimoto --- .../src/syntax/reach_position_condition.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index 6e37f90ca3f..7f2a611bc42 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -54,22 +54,22 @@ auto ReachPositionCondition::evaluate() -> Object [&](const WorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y); + return std::hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeWorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y); + return std::hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const RelativeObjectPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y); + return std::hypot(pose.position.x, pose.position.y, pose.position.z); }, [&](const LanePosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y); + return std::hypot(pose.position.x, pose.position.y, pose.position.z); }); results.clear(); From 5ec7e8ceaca43229b1b6c0b787edeb98204734e8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 7 Feb 2024 11:12:38 +0900 Subject: [PATCH 15/43] chore: format Signed-off-by: Kotaro Yoshimoto --- .../vehicle_simulation/ego_entity_simulation.hpp | 3 ++- .../simple_sensor_simulator/src/simple_sensor_simulator.cpp | 3 ++- .../src/vehicle_simulation/ego_entity_simulation.cpp | 3 +-- 3 files changed, 5 insertions(+), 4 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 544ba0e8f5f..8c82266c13b 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 @@ -81,7 +81,8 @@ class EgoEntitySimulation explicit EgoEntitySimulation( const traffic_simulator_msgs::msg::VehicleParameters &, double, - const std::shared_ptr &, const rclcpp::Parameter & use_sim_time, , const bool consider_pose_by_road_slope); + const std::shared_ptr &, const rclcpp::Parameter & use_sim_time, , + const bool consider_pose_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 8b83fc7a59c..0cb1e0ae1d1 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -215,7 +215,8 @@ auto ScenarioSimulator::spawnVehicleEntity( simulation_interface::toMsg(req.parameters(), parameters); ego_entity_simulation_ = std::make_shared( parameters, step_time_, hdmap_utils_, - get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)), consider_pose_by_road_slope_); + get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)), + consider_pose_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 07aa6545bab..ab4bf10561a 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,8 +35,7 @@ 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 bool consider_pose_by_road_slope) + const rclcpp::Parameter & use_sim_time, const bool consider_pose_by_road_slope) : autoware(std::make_unique()), vehicle_model_type_(getVehicleModelType()), vehicle_model_ptr_(makeSimulationModel(vehicle_model_type_, step_time, parameters)), From 31d606ed9847107758ba077fb5433309c6702597 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 7 Feb 2024 11:13:57 +0900 Subject: [PATCH 16/43] fix: build error 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 ab4bf10561a..4b1fdb6c182 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 @@ -411,7 +411,7 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( 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; - if (consider_lanelet_slope) { + if (consider_pose_by_road_slope) { const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( spline.getPose(s_value.value()).orientation); const auto original_rpy = From cae97a973919b57c34f76a2870740c9a5d7f1c50 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 8 Feb 2024 17:05:56 +0900 Subject: [PATCH 17/43] doc: add note for origin orientation of grid map Signed-off-by: Kotaro Yoshimoto --- .../occupancy_grid/occupancy_grid_sensor.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp index edf4e02418f..dfc071c789d 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp @@ -82,6 +82,13 @@ auto OccupancyGridSensor::getOccupancyGrid( throw SimulationRuntimeError("Failed to calculate ego pose with north up."); } simulation_interface::toMsg(ego->pose(), ego_pose_north_up); + /** + * NOTE: + * There is no problem with the yaw axis being north-up, but unless the pitch and roll axes are + * adjusted to the slope of the road surface, the grid map will not project the obstacles correctly. + * However, the current implementation of autoware.universe does not consider the roll and pitch axes, + * so it is not considered here either. + */ ego_pose_north_up.orientation = geometry_msgs::msg::Quaternion(); } From abb064ea803f7ee8e335e798741626267ec2be9c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 8 Feb 2024 17:58:31 +0900 Subject: [PATCH 18/43] chore: fix conflict resolving miss Signed-off-by: Kotaro Yoshimoto --- .../vehicle_simulation/ego_entity_simulation.hpp | 2 +- 1 file changed, 1 insertion(+), 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 8c82266c13b..d8e5c26262d 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 @@ -81,7 +81,7 @@ 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_pose_by_road_slope); auto update(double time, double step_time, bool npc_logic_started) -> void; From 82008be1e0ffe4c1660bf00130dd6d38ec65f664 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 9 Feb 2024 11:57:01 +0900 Subject: [PATCH 19/43] fix: use output orientation as of now in orientation test Signed-off-by: Kotaro Yoshimoto --- .../test/test_lanelet_utils.cpp | 9 +++++---- .../test/test_test_randomizer.cpp | 18 ++++++++++-------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/test_runner/random_test_runner/test/test_lanelet_utils.cpp b/test_runner/random_test_runner/test/test_lanelet_utils.cpp index 0171d21472c..0939c70baa1 100644 --- a/test_runner/random_test_runner/test/test_lanelet_utils.cpp +++ b/test_runner/random_test_runner/test/test_lanelet_utils.cpp @@ -202,6 +202,7 @@ TEST(LaneletUtils, toMapPose) { const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0); EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose)); + // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( getLaneletUtils().toMapPose(pose).pose, geometry_msgs::build() @@ -210,10 +211,10 @@ TEST(LaneletUtils, toMapPose) .y(73735.0699484234) .z(0.3034051453)) .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(-0.9719821398) - .w(0.2350547166)), + .x(-0.0091567745565503053) + .y(-0.0022143853886961245) + .z(-0.97193900717756765) + .w(0.23504428583514828)), EPS); } diff --git a/test_runner/random_test_runner/test/test_test_randomizer.cpp b/test_runner/random_test_runner/test/test_test_randomizer.cpp index 3110b21754b..8cbb2c43ae8 100644 --- a/test_runner/random_test_runner/test/test_test_randomizer.cpp +++ b/test_runner/random_test_runner/test/test_test_randomizer.cpp @@ -53,6 +53,7 @@ TEST(TestRandomizer, generate_10NPC) description.ego_start_position, makeLaneletPose(34705, 6.2940393113), EPS); EXPECT_LANELET_POSE_NEAR( description.ego_goal_position, makeLaneletPose(34642, 9.6945373700), EPS); + // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( description.ego_goal_pose, geometry_msgs::build() @@ -61,10 +62,10 @@ TEST(TestRandomizer, generate_10NPC) .y(73741.1526960728) .z(0.0698702227)) .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(-0.9854984261) - .w(0.1696845662)), + .x(-0.0040560888625029243) + .y(-0.00069838333685736189) + .z(-0.98549007915725362) + .w(0.16968312905673485)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(10)); @@ -117,6 +118,7 @@ TEST(TestRandomizer, generate_8NPC) description.ego_start_position, makeLaneletPose(34648, 11.3744011441), EPS); EXPECT_LANELET_POSE_NEAR( description.ego_goal_position, makeLaneletPose(34507, 55.3754803281), EPS); + // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( description.ego_goal_pose, geometry_msgs::build() @@ -125,10 +127,10 @@ TEST(TestRandomizer, generate_8NPC) .y(73815.1830249432) .z(-2.9975350010)) .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(0.2345315792) - .w(0.9721085013)), + .x(-0.002510549540877986) + .y(0.01040596135909698) + .z(0.23451814178855812) + .w(0.97205280426591933)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(8)); From 1f7086821893c2144512fe34188fc2abc37ad517 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 14 Feb 2024 10:20:02 +0900 Subject: [PATCH 20/43] chore: format Signed-off-by: Kotaro Yoshimoto --- .../vehicle_simulation/ego_entity_simulation.hpp | 1 + 1 file changed, 1 insertion(+) 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 14b49b88f5e..786896175a2 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 @@ -62,6 +62,7 @@ class EgoEntitySimulation public: const std::shared_ptr hdmap_utils_ptr_; + const traffic_simulator_msgs::msg::VehicleParameters vehicle_parameters; const bool consider_pose_by_road_slope; From 9d8ae2c392859188101659ff0db74ed061a833fb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 26 Feb 2024 17:54:07 +0900 Subject: [PATCH 21/43] fix(build) Signed-off-by: Kotaro Yoshimoto --- .../include/simple_sensor_simulator/simple_sensor_simulator.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp index db2b806c66a..598ca1b95e6 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/simple_sensor_simulator.hpp @@ -157,7 +157,6 @@ class ScenarioSimulator : public rclcpp::Node geographic_msgs::msg::GeoPoint getOrigin(); std::shared_ptr hdmap_utils_; std::shared_ptr ego_entity_simulation_; - const bool consider_pose_by_road_slope_; bool isEgo(const std::string & name); bool isEntityExists(const std::string & name); From 5bf44fc35d0aa66ebbf338b07e5237c9e73067ab Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 26 Feb 2024 19:12:43 +0900 Subject: [PATCH 22/43] chore: apply formatter Signed-off-by: Kotaro Yoshimoto --- .../simple_sensor_simulator/src/simple_sensor_simulator.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp index 223b28df405..d3997da7803 100644 --- a/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp +++ b/simulation/simple_sensor_simulator/src/simple_sensor_simulator.cpp @@ -222,8 +222,7 @@ auto ScenarioSimulator::spawnVehicleEntity( ego_entity_simulation_ = std::make_shared( parameters, step_time_, hdmap_utils_, get_parameter_or("use_sim_time", rclcpp::Parameter("use_sim_time", false)), - get_consider_acceleration_by_road_slope(), - get_consider_pose_by_road_slope()); + get_consider_acceleration_by_road_slope(), get_consider_pose_by_road_slope()); traffic_simulator_msgs::msg::EntityStatus initial_status; initial_status.name = parameters.name; simulation_interface::toMsg(req.pose(), initial_status.pose); From 928f1398edf2ef9be553f3487b627fd0b8a92939 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 26 Feb 2024 19:31:30 +0900 Subject: [PATCH 23/43] chore: update scenario parameter Signed-off-by: Kotaro Yoshimoto --- ...ondition.EntityCondition.DistanceConditionFreespace.yaml | 4 ++-- ....EntityCondition.RelativeDistanceConditionFreespace.yaml | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml index 0c63ace8c06..7819dc646c0 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml @@ -85,7 +85,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: euclidianDistance rule: equalTo - value: 6.2855887168861768 #6.28727 + value: 6.239365667688271 #6.28727 Position: &POSITION_1 LanePosition: roadId: "" @@ -111,7 +111,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: longitudinal rule: equalTo - value: 6.2583065215658280 #6.26 + value: 6.211880466908042 #6.26 Position: *POSITION_1 - name: "" delay: 0 diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml index 13bce0cd036..7dc89b05e47 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml @@ -97,7 +97,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: euclidianDistance rule: equalTo - value: 5.2310695926720685 + value: 5.230991883723311808296330127632 - name: "" delay: 0 conditionEdge: none @@ -113,7 +113,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: longitudinal rule: equalTo - value: 5.2283065215597162 + value: 5.228228771542489994317293167114 - name: "" delay: 0 conditionEdge: none @@ -129,7 +129,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: lateral rule: equalTo - value: 0.1700000000055297 + value: 0.169999999998253770172595977783 - name: "" delay: 0 conditionEdge: none From d49c88ca81b8208047ad2602613da7d141673c46 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 27 Feb 2024 11:09:14 +0900 Subject: [PATCH 24/43] doc: Signed-off-by: Kotaro Yoshimoto --- .../sensor_simulation/lidar/lidar_sensor.hpp | 3 +-- .../occupancy_grid/occupancy_grid_sensor.cpp | 2 +- ...ondition.EntityCondition.DistanceConditionFreespace.yaml | 6 +++--- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp index f09f54045ad..f28d48ca608 100644 --- a/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/include/simple_sensor_simulator/sensor_simulation/lidar/lidar_sensor.hpp @@ -48,8 +48,7 @@ class LidarSensorBase virtual ~LidarSensorBase() = default; virtual auto update( - const double /*current_simulation_time*/, - const std::vector &, + const double current_simulation_time, const std::vector &, const rclcpp::Time & current_ros_time) -> void = 0; auto getDetectedObjects() const -> const std::vector & { return detected_objects_; } diff --git a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp index dfc071c789d..5c1944a57cb 100644 --- a/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp +++ b/simulation/simple_sensor_simulator/src/sensor_simulation/occupancy_grid/occupancy_grid_sensor.cpp @@ -83,7 +83,7 @@ auto OccupancyGridSensor::getOccupancyGrid( } simulation_interface::toMsg(ego->pose(), ego_pose_north_up); /** - * NOTE: + * @note * There is no problem with the yaw axis being north-up, but unless the pitch and roll axes are * adjusted to the slope of the road surface, the grid map will not project the obstacles correctly. * However, the current implementation of autoware.universe does not consider the roll and pitch axes, diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml index 7819dc646c0..1769227253e 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml @@ -85,7 +85,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: euclidianDistance rule: equalTo - value: 6.239365667688271 #6.28727 + value: 6.239365667688271 Position: &POSITION_1 LanePosition: roadId: "" @@ -111,7 +111,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: longitudinal rule: equalTo - value: 6.211880466908042 #6.26 + value: 6.211880466908042 Position: *POSITION_1 - name: "" delay: 0 @@ -127,7 +127,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: lateral rule: equalTo - value: 0.5849999999772990 #0.585 + value: 0.5849999999772990 Position: *POSITION_1 - name: "" delay: 0 From 66310655fffd303743e8b80f5fd86265ca4ac412 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 27 Feb 2024 20:02:35 +0900 Subject: [PATCH 25/43] fix: fit WorldPosition to lanelet in spawn function Signed-off-by: Kotaro Yoshimoto --- .../include/traffic_simulator/entity/entity_manager.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 44fb81c32ee..822c0248d0a 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -518,8 +518,6 @@ class EntityManager entity_status.lanelet_pose = static_cast(pose); entity_status.lanelet_pose_valid = true; } else { - entity_status.pose = pose; - /// @note If the entity is pedestrian or misc object, we have to consider matching to crosswalk lanelet. if (const auto lanelet_pose = toLaneletPose( pose, parameters.bounding_box, @@ -541,8 +539,11 @@ class EntityManager lanelet_pose) { entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; + // @note fix z, roll and pitch to fitting to the lanelet + entity_status.pose = toMapPose(*lanelet_pose); } else { entity_status.lanelet_pose_valid = false; + entity_status.pose = pose; } } From bd8a5db89cb2a0a66bb8cc5782889ca00b7529e4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 27 Feb 2024 22:29:20 +0900 Subject: [PATCH 26/43] fix: build error Signed-off-by: Kotaro Yoshimoto --- .../include/traffic_simulator/entity/entity_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index 822c0248d0a..f8c4da0b00d 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -540,7 +540,7 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; // @note fix z, roll and pitch to fitting to the lanelet - entity_status.pose = toMapPose(*lanelet_pose); + entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; } else { entity_status.lanelet_pose_valid = false; entity_status.pose = pose; From 51058b3947945b2d4d018c7134de3956c99b4393 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 10:23:05 +0900 Subject: [PATCH 27/43] chore: Update Position for OpenSCENARIO 1.2 Signed-off-by: Kotaro Yoshimoto --- .../include/openscenario_interpreter/syntax/position.hpp | 8 +++++++- .../openscenario_interpreter/src/syntax/position.cpp | 4 +++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp index 7b76e000dfe..4141742d53d 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/position.hpp @@ -26,7 +26,7 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- Position --------------------------------------------------------------- +/* ---- Position 1.2 ----------------------------------------------------------- * * * @@ -38,6 +38,8 @@ inline namespace syntax * * * + * + * * * * @@ -61,6 +63,8 @@ DEFINE_LAZY_VISITOR( CASE(LanePosition), // CASE(RelativeLanePosition), // CASE(RoutePosition), + // CASE(GeoPosition), + // CASE(TrajectoryPosition), ); DEFINE_LAZY_VISITOR( @@ -73,6 +77,8 @@ DEFINE_LAZY_VISITOR( CASE(LanePosition), // CASE(RelativeLanePosition), // CASE(RoutePosition), + // CASE(GeoPosition), + // CASE(TrajectoryPosition), ); } // namespace syntax } // namespace openscenario_interpreter diff --git a/openscenario/openscenario_interpreter/src/syntax/position.cpp b/openscenario/openscenario_interpreter/src/syntax/position.cpp index 1d1c9067764..39764457b8c 100644 --- a/openscenario/openscenario_interpreter/src/syntax/position.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/position.cpp @@ -31,7 +31,9 @@ Position::Position(const pugi::xml_node & node, Scope & scope) std::make_pair( "RelativeRoadPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), std::make_pair( "LanePosition", [&](auto && node) { return make< LanePosition>(node, scope); }), std::make_pair( "RelativeLanePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), - std::make_pair( "RoutePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }))) + std::make_pair( "RoutePosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "GeoPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }), + std::make_pair( "TrajectoryPosition", [&](auto && node) { throw UNSUPPORTED_ELEMENT_SPECIFIED(node.name()); return unspecified; }))) // clang-format on { } From 0aa22548c9a8354a05dae40df0d53f6524d5cc9e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 10:23:52 +0900 Subject: [PATCH 28/43] chore: use NativeWorldPosition Signed-off-by: Kotaro Yoshimoto --- .../include/openscenario_interpreter/syntax/world_position.hpp | 2 +- .../src/syntax/acquire_position_action.cpp | 2 +- .../openscenario_interpreter/src/syntax/teleport_action.cpp | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp index 19dc86ca37e..0e99ca4317b 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/world_position.hpp @@ -27,7 +27,7 @@ namespace openscenario_interpreter { inline namespace syntax { -/* ---- WorldPosition ---------------------------------------------------------- +/* ---- WorldPosition 1.2 ------------------------------------------------------ * * * diff --git a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp index 2141bef7bbd..89f2f6a4232 100644 --- a/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/acquire_position_action.cpp @@ -30,7 +30,7 @@ auto AcquirePositionAction::start() -> void { const auto acquire_position = overload( [](const WorldPosition & position, auto && actor) { - return applyAcquirePositionAction(actor, static_cast(position)); + return applyAcquirePositionAction(actor, static_cast(position)); }, [](const RelativeWorldPosition & position, auto && actor) { return applyAcquirePositionAction(actor, static_cast(position)); diff --git a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp index c8314e69878..4e59e603519 100644 --- a/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/teleport_action.cpp @@ -56,7 +56,7 @@ auto TeleportAction::teleport(const EntityRef & entity_ref, const Position & pos { auto teleport = overload( [&](const WorldPosition & position) { - return applyTeleportAction(entity_ref, static_cast(position)); + return applyTeleportAction(entity_ref, static_cast(position)); }, [&](const RelativeWorldPosition & position) { return applyTeleportAction( From cdc32c472287ff3062d7c99f73d2d0e75cb49566 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 13:27:55 +0900 Subject: [PATCH 29/43] refactor: add fit_orientation_to_lanelet option to HermiteCurve::getPose Signed-off-by: Kotaro Yoshimoto --- .../geometry/include/geometry/spline/hermite_curve.hpp | 3 ++- common/math/geometry/src/spline/hermite_curve.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp index a1afdb4f0c7..8e93353626e 100644 --- a/common/math/geometry/include/geometry/spline/hermite_curve.hpp +++ b/common/math/geometry/include/geometry/spline/hermite_curve.hpp @@ -48,7 +48,8 @@ class HermiteCurve std::vector getTrajectory(size_t num_points = 30) const; const std::vector getTrajectory( double start_s, double end_s, double resolution, bool denormalize_s = false) const; - const geometry_msgs::msg::Pose getPose(double s, bool denormalize_s = false) const; + const geometry_msgs::msg::Pose getPose( + double s, bool denormalize_s = false, bool fit_orientation_to_lanelet = false) const; const geometry_msgs::msg::Point getPoint(double s, bool denormalize_s = false) const; const geometry_msgs::msg::Vector3 getTangentVector(double s, bool denormalize_s = false) const; const geometry_msgs::msg::Vector3 getNormalVector(double s, bool denormalize_s = false) const; diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp index 99079888466..10a01152e19 100644 --- a/common/math/geometry/src/spline/hermite_curve.cpp +++ b/common/math/geometry/src/spline/hermite_curve.cpp @@ -279,7 +279,8 @@ const geometry_msgs::msg::Vector3 HermiteCurve::getTangentVector(double s, bool return vec; } -const geometry_msgs::msg::Pose HermiteCurve::getPose(double s, bool denormalize_s) const +const geometry_msgs::msg::Pose HermiteCurve::getPose( + double s, bool denormalize_s, bool fit_orientation_to_lanelet) const { if (denormalize_s) { s = s / getLength(); @@ -287,8 +288,10 @@ const geometry_msgs::msg::Pose HermiteCurve::getPose(double s, bool denormalize_ geometry_msgs::msg::Pose pose; geometry_msgs::msg::Vector3 tangent_vec = getTangentVector(s, false); geometry_msgs::msg::Vector3 rpy; - rpy.x = 0; - rpy.y = std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)); + rpy.x = 0.0; + rpy.y = fit_orientation_to_lanelet + ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) + : 0.0; rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy); pose.position = getPoint(s); From 6547d8200cf744af92c5da25a87fe3d1ca8a0758 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 13:44:00 +0900 Subject: [PATCH 30/43] refactor: add fill_pitch option to LineSegment::getPose Signed-off-by: Kotaro Yoshimoto --- .../geometry/include/geometry/polygon/line_segment.hpp | 3 ++- common/math/geometry/src/polygon/line_segment.cpp | 9 ++++++--- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/include/geometry/polygon/line_segment.hpp b/common/math/geometry/include/geometry/polygon/line_segment.hpp index c160b1e3ec2..9ce009e2089 100644 --- a/common/math/geometry/include/geometry/polygon/line_segment.hpp +++ b/common/math/geometry/include/geometry/polygon/line_segment.hpp @@ -40,7 +40,8 @@ class LineSegment const geometry_msgs::msg::Point end_point; auto getPoint(const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Point; - auto getPose(const double s, const bool denormalize_s = false) const -> geometry_msgs::msg::Pose; + auto getPose(const double s, const bool denormalize_s = false, const bool fill_pitch = false) + const -> geometry_msgs::msg::Pose; auto isIntersect2D(const geometry_msgs::msg::Point & point) const -> bool; auto isIntersect2D(const LineSegment & l0) const -> bool; auto getIntersection2DSValue( diff --git a/common/math/geometry/src/polygon/line_segment.cpp b/common/math/geometry/src/polygon/line_segment.cpp index 882f806885a..827d529660c 100644 --- a/common/math/geometry/src/polygon/line_segment.cpp +++ b/common/math/geometry/src/polygon/line_segment.cpp @@ -93,19 +93,22 @@ auto LineSegment::getPoint(const double s, const bool denormalize_s) const * @brief Get pose on the line segment from s value. Orientation of thee return value was calculated from the vector of the line segment. * @param s Normalized s value in coordinate along line segment. * @param denormalize_s If true, s value should be normalized in range [0,1]. If false, s value is not normalized. + * @param fill_pitch If true, the pitch value of the orientation is filled. If false, the pitch value of the orientation is 0. + * This parameter is introduced for backward-compatibility. * @return geometry_msgs::msg::Pose */ -auto LineSegment::getPose(const double s, const bool denormalize_s) const +auto LineSegment::getPose(const double s, const bool denormalize_s, const bool fill_pitch) const -> geometry_msgs::msg::Pose { return geometry_msgs::build() .position(getPoint(s, denormalize_s)) - .orientation([this]() -> geometry_msgs::msg::Quaternion { + .orientation([this, fill_pitch]() -> geometry_msgs::msg::Quaternion { const auto tangent_vec = getVector(); return quaternion_operation::convertEulerAngleToQuaternion( geometry_msgs::build() .x(0.0) - .y(std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y))) + .y( + fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0) .z(std::atan2(tangent_vec.y, tangent_vec.x))); }()); } From d28baf6e638071745b4aa56e11d99d313991ad99 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 13:44:44 +0900 Subject: [PATCH 31/43] refactor: rename fit_orientation_to_lanelet to fill_pitch in HermiteCurve::getPose Signed-off-by: Kotaro Yoshimoto --- .../geometry/include/geometry/spline/hermite_curve.hpp | 2 +- common/math/geometry/src/spline/hermite_curve.cpp | 7 +++---- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/common/math/geometry/include/geometry/spline/hermite_curve.hpp b/common/math/geometry/include/geometry/spline/hermite_curve.hpp index 8e93353626e..734a9de26f7 100644 --- a/common/math/geometry/include/geometry/spline/hermite_curve.hpp +++ b/common/math/geometry/include/geometry/spline/hermite_curve.hpp @@ -49,7 +49,7 @@ class HermiteCurve const std::vector getTrajectory( double start_s, double end_s, double resolution, bool denormalize_s = false) const; const geometry_msgs::msg::Pose getPose( - double s, bool denormalize_s = false, bool fit_orientation_to_lanelet = false) const; + double s, bool denormalize_s = false, bool fill_pitch = false) const; const geometry_msgs::msg::Point getPoint(double s, bool denormalize_s = false) const; const geometry_msgs::msg::Vector3 getTangentVector(double s, bool denormalize_s = false) const; const geometry_msgs::msg::Vector3 getNormalVector(double s, bool denormalize_s = false) const; diff --git a/common/math/geometry/src/spline/hermite_curve.cpp b/common/math/geometry/src/spline/hermite_curve.cpp index 10a01152e19..ac72a9b6774 100644 --- a/common/math/geometry/src/spline/hermite_curve.cpp +++ b/common/math/geometry/src/spline/hermite_curve.cpp @@ -279,8 +279,9 @@ const geometry_msgs::msg::Vector3 HermiteCurve::getTangentVector(double s, bool return vec; } +// @note fill_pitch is introduced for backward-compatibility. const geometry_msgs::msg::Pose HermiteCurve::getPose( - double s, bool denormalize_s, bool fit_orientation_to_lanelet) const + double s, bool denormalize_s, bool fill_pitch) const { if (denormalize_s) { s = s / getLength(); @@ -289,9 +290,7 @@ const geometry_msgs::msg::Pose HermiteCurve::getPose( geometry_msgs::msg::Vector3 tangent_vec = getTangentVector(s, false); geometry_msgs::msg::Vector3 rpy; rpy.x = 0.0; - rpy.y = fit_orientation_to_lanelet - ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) - : 0.0; + rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy); pose.position = getPoint(s); From 519b2b657031f13ecf608a4d8b4c7a7161646550 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 28 Feb 2024 19:12:32 +0900 Subject: [PATCH 32/43] feat(openscenario_interpreter): add flag to switch considering z in distance conditions Signed-off-by: Kotaro Yoshimoto --- .../syntax/distance_condition.hpp | 2 + .../syntax/reach_position_condition.hpp | 2 + .../syntax/relative_distance_condition.hpp | 2 + .../src/syntax/distance_condition.cpp | 56 +++++++++++++------ .../src/syntax/reach_position_condition.cpp | 25 +++++++-- .../syntax/relative_distance_condition.cpp | 24 ++++++-- 6 files changed, 84 insertions(+), 27 deletions(-) diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp index 0a0945e0b6f..5c30020102f 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/distance_condition.hpp @@ -81,6 +81,8 @@ struct DistanceCondition : private Scope, private SimulatorCore::ConditionEvalua std::vector results; // for description + const bool consider_z; + explicit DistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> std::string; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp index 35e98272ce5..527614ad500 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/reach_position_condition.hpp @@ -49,6 +49,8 @@ struct ReachPositionCondition : private SimulatorCore::CoordinateSystemConversio std::vector results; // for description + const bool consider_z; + explicit ReachPositionCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; diff --git a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp index 6aa1d54cbda..73593152962 100644 --- a/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp +++ b/openscenario/openscenario_interpreter/include/openscenario_interpreter/syntax/relative_distance_condition.hpp @@ -83,6 +83,8 @@ struct RelativeDistanceCondition : private Scope, private SimulatorCore::Conditi std::vector results; // for description + const bool consider_z; + explicit RelativeDistanceCondition(const pugi::xml_node &, Scope &, const TriggeringEntities &); auto description() const -> String; diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index fb4db1a0ca7..6a595abac52 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include // NOTE: Ignore spell miss due to OpenSCENARIO standard. @@ -42,7 +43,11 @@ DistanceCondition::DistanceCondition( value(readAttribute("value", node, scope)), position(readElement("Position", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), Double::nan()), + consider_z([]() { + rclcpp::Node node{"get_parameter", "simulation"}; + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }()) { } @@ -101,6 +106,15 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d return std::numeric_limits::quiet_NaN(); } +static double hypot(const double x, const double y, const double z, const bool consider_z) +{ + if (consider_z) { + return std::hypot(x, y, z); + } else { + return std::hypot(x, y); + } +} + template <> auto DistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, false>( @@ -111,26 +125,30 @@ auto DistanceCondition::distance< [&](const WorldPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }), position); } @@ -145,26 +163,30 @@ auto DistanceCondition::distance< [&](const WorldPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const RelativeWorldPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const RelativeObjectPosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }, [&](const LanePosition & position) { const auto relative_world = makeNativeBoundingBoxRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot( - relative_world.position.x, relative_world.position.y, relative_world.position.z); + return hypot( + relative_world.position.x, relative_world.position.y, relative_world.position.z, + consider_z); }), position); } diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index 7f2a611bc42..6c4c1487372 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include namespace openscenario_interpreter @@ -30,7 +31,12 @@ ReachPositionCondition::ReachPositionCondition( position(readElement("Position", node, scope)), compare(Rule::lessThan), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), Double::nan()), + consider_z([]() { + rclcpp::Node node{"get_parameter", "simulation"}; + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + ; + }()) { } @@ -47,6 +53,15 @@ auto ReachPositionCondition::description() const -> String return description.str(); } +static double hypot(const double x, const double y, const double z, const bool consider_z) +{ + if (consider_z) { + return std::hypot(x, y, z); + } else { + return std::hypot(x, y); + } +} + auto ReachPositionCondition::evaluate() -> Object { // TODO USE DistanceCondition::distance @@ -54,22 +69,22 @@ auto ReachPositionCondition::evaluate() -> Object [&](const WorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y, pose.position.z); + return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); }, [&](const RelativeWorldPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y, pose.position.z); + return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); }, [&](const RelativeObjectPosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y, pose.position.z); + return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); }, [&](const LanePosition & position, auto && triggering_entity) { const auto pose = makeNativeRelativeWorldPosition( triggering_entity, static_cast(position)); - return std::hypot(pose.position.x, pose.position.y, pose.position.z); + return hypot(pose.position.x, pose.position.y, pose.position.z, consider_z); }); results.clear(); diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 41af2a7cbac..8cdf2b22876 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -36,7 +36,11 @@ RelativeDistanceCondition::RelativeDistanceCondition( rule(readAttribute("rule", node, scope)), value(readAttribute("value", node, scope)), triggering_entities(triggering_entities), - results(triggering_entities.entity_refs.size(), Double::nan()) + results(triggering_entities.entity_refs.size(), Double::nan()), + consider_z([]() { + rclcpp::Node node{"get_parameter", "simulation"}; + return node.get_parameter("consider_pose_by_road_slope").as_bool(); + }()) { } @@ -118,6 +122,15 @@ auto RelativeDistanceCondition::distance< } } +static double hypot(const double x, const double y, const double z, const bool consider_z) +{ + if (consider_z) { + return std::hypot(x, y, z); + } else { + return std::hypot(x, y); + } +} + template <> auto RelativeDistanceCondition::distance< CoordinateSystem::entity, RelativeDistanceType::euclidianDistance, true>( @@ -126,10 +139,11 @@ auto RelativeDistanceCondition::distance< if ( global().entities->at(triggering_entity).as().is_added and global().entities->at(entity_ref).as().is_added) { - return std::hypot( + return hypot( makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.x, makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z); + makeNativeBoundingBoxRelativeWorldPosition(triggering_entity, entity_ref).position.z, + consider_z); } else { return Double::nan(); } @@ -143,10 +157,10 @@ auto RelativeDistanceCondition::distance< if ( global().entities->at(triggering_entity).as().is_added and global().entities->at(entity_ref).as().is_added) { - return std::hypot( + return hypot( makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.x, makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.y, - makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z); + makeNativeRelativeWorldPosition(triggering_entity, entity_ref).position.z, consider_z); } else { return Double::nan(); } From dfeb79effc520ad96a8438fbe7382012462a1e45 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 29 Feb 2024 07:46:29 +0900 Subject: [PATCH 33/43] chore(geometry): add some tests for LineSegment::getPose Signed-off-by: Kotaro Yoshimoto --- common/math/geometry/test/test_line_segment.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/common/math/geometry/test/test_line_segment.cpp b/common/math/geometry/test/test_line_segment.cpp index fa58e271826..cb646135004 100644 --- a/common/math/geometry/test/test_line_segment.cpp +++ b/common/math/geometry/test/test_line_segment.cpp @@ -387,7 +387,12 @@ TEST(LineSegment, GetPose) /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. // [Snippet_getPose_with_s_0] EXPECT_POSE_EQ( - line.getPose(0, false), + line.getPose(0, false, false), + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(0)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); + EXPECT_POSE_EQ( + line.getPose(0, false, true), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(0)) .orientation(quaternion_operation::convertEulerAngleToQuaternion( @@ -399,7 +404,12 @@ TEST(LineSegment, GetPose) /// Orientation should be defined by the direction of the `line`, so the orientation should be parallel to the z axis. // [Snippet_getPose_with_s_1] EXPECT_POSE_EQ( - line.getPose(1, false), + line.getPose(1, false, false), + geometry_msgs::build() + .position(geometry_msgs::build().x(0).y(0).z(1)) + .orientation(geometry_msgs::build().x(0).y(0).z(0).w(1))); + EXPECT_POSE_EQ( + line.getPose(1, false, true), geometry_msgs::build() .position(geometry_msgs::build().x(0).y(0).z(1)) .orientation(quaternion_operation::convertEulerAngleToQuaternion( From 02b96323cd65281883782fae766efc4469ce74b1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 29 Feb 2024 11:34:34 +0900 Subject: [PATCH 34/43] fix: add parameter declaration for consider_pose_by_road_slope in conditions Signed-off-by: Kotaro Yoshimoto --- .../openscenario_interpreter/src/syntax/distance_condition.cpp | 2 ++ .../src/syntax/reach_position_condition.cpp | 3 ++- .../src/syntax/relative_distance_condition.cpp | 2 ++ 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp index 6a595abac52..e9d46df84fe 100644 --- a/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/distance_condition.cpp @@ -46,6 +46,7 @@ DistanceCondition::DistanceCondition( results(triggering_entities.entity_refs.size(), Double::nan()), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; + node.declare_parameter("consider_pose_by_road_slope", false); return node.get_parameter("consider_pose_by_road_slope").as_bool(); }()) { @@ -106,6 +107,7 @@ auto DistanceCondition::distance(const EntityRef & triggering_entity) const -> d return std::numeric_limits::quiet_NaN(); } +// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) static double hypot(const double x, const double y, const double z, const bool consider_z) { if (consider_z) { diff --git a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp index 6c4c1487372..17cfa7d8967 100644 --- a/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/reach_position_condition.cpp @@ -34,8 +34,8 @@ ReachPositionCondition::ReachPositionCondition( results(triggering_entities.entity_refs.size(), Double::nan()), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; + node.declare_parameter("consider_pose_by_road_slope", false); return node.get_parameter("consider_pose_by_road_slope").as_bool(); - ; }()) { } @@ -53,6 +53,7 @@ auto ReachPositionCondition::description() const -> String return description.str(); } +// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) static double hypot(const double x, const double y, const double z, const bool consider_z) { if (consider_z) { diff --git a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp index 8cdf2b22876..6d34fd75a73 100644 --- a/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp +++ b/openscenario/openscenario_interpreter/src/syntax/relative_distance_condition.cpp @@ -39,6 +39,7 @@ RelativeDistanceCondition::RelativeDistanceCondition( results(triggering_entities.entity_refs.size(), Double::nan()), consider_z([]() { rclcpp::Node node{"get_parameter", "simulation"}; + node.declare_parameter("consider_pose_by_road_slope", false); return node.get_parameter("consider_pose_by_road_slope").as_bool(); }()) { @@ -122,6 +123,7 @@ auto RelativeDistanceCondition::distance< } } +// @todo: after checking all the scenario work well with consider_z = true, remove this function and use std::hypot(x,y,z) static double hypot(const double x, const double y, const double z, const bool consider_z) { if (consider_z) { From 606c3d8bc33b027a8f64269057010e346a155848 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 1 Mar 2024 14:49:49 +0900 Subject: [PATCH 35/43] chore: revert scenario changes Signed-off-by: Kotaro Yoshimoto --- ...ondition.EntityCondition.DistanceConditionFreespace.yaml | 6 +++--- ....EntityCondition.RelativeDistanceConditionFreespace.yaml | 6 +++--- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml index 1769227253e..0c63ace8c06 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.DistanceConditionFreespace.yaml @@ -85,7 +85,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: euclidianDistance rule: equalTo - value: 6.239365667688271 + value: 6.2855887168861768 #6.28727 Position: &POSITION_1 LanePosition: roadId: "" @@ -111,7 +111,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: longitudinal rule: equalTo - value: 6.211880466908042 + value: 6.2583065215658280 #6.26 Position: *POSITION_1 - name: "" delay: 0 @@ -127,7 +127,7 @@ OpenSCENARIO: freespace: true relativeDistanceType: lateral rule: equalTo - value: 0.5849999999772990 + value: 0.5849999999772990 #0.585 Position: *POSITION_1 - name: "" delay: 0 diff --git a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml index 7dc89b05e47..13bce0cd036 100644 --- a/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml +++ b/test_runner/scenario_test_runner/scenario/ByEntityCondition.EntityCondition.RelativeDistanceConditionFreespace.yaml @@ -97,7 +97,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: euclidianDistance rule: equalTo - value: 5.230991883723311808296330127632 + value: 5.2310695926720685 - name: "" delay: 0 conditionEdge: none @@ -113,7 +113,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: longitudinal rule: equalTo - value: 5.228228771542489994317293167114 + value: 5.2283065215597162 - name: "" delay: 0 conditionEdge: none @@ -129,7 +129,7 @@ OpenSCENARIO: freespace: true # True: distance is measured between closest bounding box points. False: reference point distance is used. relativeDistanceType: lateral rule: equalTo - value: 0.169999999998253770172595977783 + value: 0.1700000000055297 - name: "" delay: 0 conditionEdge: none From 89dc17f97bd3c01ba624f297b822805336067dc5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 1 Mar 2024 14:57:55 +0900 Subject: [PATCH 36/43] feat: add fill_pitch option to HdMapUtils::toMapPose Signed-off-by: Kotaro Yoshimoto --- .../hdmap_utils/hdmap_utils.hpp | 4 ++-- .../src/hdmap_utils/hdmap_utils.cpp | 5 ++-- .../random_test_runner/lanelet_utils.hpp | 3 ++- .../random_test_runner/src/lanelet_utils.cpp | 6 ++--- .../src/test_randomizer.cpp | 2 +- .../test/test_lanelet_utils.cpp | 23 +++++++++++++++++-- 6 files changed, 32 insertions(+), 11 deletions(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp index 24375e4b211..d1c933d007e 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp @@ -298,8 +298,8 @@ class HdMapUtils auto toMapPoints(const lanelet::Id, const std::vector & s) const -> std::vector; - auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &) const - -> geometry_msgs::msg::PoseStamped; + auto toMapPose(const traffic_simulator_msgs::msg::LaneletPose &, const bool fill_pitch = false) + const -> geometry_msgs::msg::PoseStamped; private: /** @defgroup cache diff --git a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp index 460a86aea33..c56b7544b52 100644 --- a/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp +++ b/simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp @@ -1393,7 +1393,8 @@ auto HdMapUtils::toMapPoints(const lanelet::Id lanelet_id, const std::vector geometry_msgs::msg::PoseStamped { if ( @@ -1409,7 +1410,7 @@ auto HdMapUtils::toMapPose(const traffic_simulator_msgs::msg::LaneletPose & lane const auto tangent_vec = spline->getTangentVector(pose->s); geometry_msgs::msg::Vector3 rpy; rpy.x = 0.0; - rpy.y = std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)); + rpy.y = fill_pitch ? std::atan2(-tangent_vec.z, std::hypot(tangent_vec.x, tangent_vec.y)) : 0.0; rpy.z = std::atan2(tangent_vec.y, tangent_vec.x); ret.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion(rpy) * quaternion_operation::convertEulerAngleToQuaternion(pose->rpy); diff --git a/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp b/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp index 431225d09d3..5203d1068cd 100644 --- a/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp +++ b/test_runner/random_test_runner/include/random_test_runner/lanelet_utils.hpp @@ -59,7 +59,8 @@ class LaneletUtils double max_distance); std::vector getLaneletIds(); - geometry_msgs::msg::PoseStamped toMapPose(traffic_simulator_msgs::msg::LaneletPose lanelet_pose); + geometry_msgs::msg::PoseStamped toMapPose( + const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch); std::vector getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id); double getLaneletLength(int64_t lanelet_id); bool isInLanelet(int64_t lanelet_id, double s); diff --git a/test_runner/random_test_runner/src/lanelet_utils.cpp b/test_runner/random_test_runner/src/lanelet_utils.cpp index 13a0301c656..e8159857e68 100644 --- a/test_runner/random_test_runner/src/lanelet_utils.cpp +++ b/test_runner/random_test_runner/src/lanelet_utils.cpp @@ -49,9 +49,9 @@ LaneletUtils::LaneletUtils(const boost::filesystem::path & filename) std::vector LaneletUtils::getLaneletIds() { return hdmap_utils_ptr_->getLaneletIds(); } geometry_msgs::msg::PoseStamped LaneletUtils::toMapPose( - traffic_simulator_msgs::msg::LaneletPose lanelet_pose) + const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, const bool fill_pitch) { - return hdmap_utils_ptr_->toMapPose(lanelet_pose); + return hdmap_utils_ptr_->toMapPose(lanelet_pose, fill_pitch); } std::vector LaneletUtils::getRoute(int64_t from_lanelet_id, int64_t to_lanelet_id) @@ -118,7 +118,7 @@ std::optional LaneletUtils::getOpposit perpendicular_vector.z = 0.0; perpendicular_vector = math::geometry::normalize(perpendicular_vector); - geometry_msgs::msg::Point global_position_p = toMapPose(pose).pose.position; + geometry_msgs::msg::Point global_position_p = toMapPose(pose, false).pose.position; geometry_msgs::msg::Vector3 global_position; global_position.x = global_position_p.x; global_position.y = global_position_p.y; diff --git a/test_runner/random_test_runner/src/test_randomizer.cpp b/test_runner/random_test_runner/src/test_randomizer.cpp index 66ff94909aa..f47b7ab0746 100644 --- a/test_runner/random_test_runner/src/test_randomizer.cpp +++ b/test_runner/random_test_runner/src/test_randomizer.cpp @@ -51,7 +51,7 @@ TestDescription TestRandomizer::generate() test_suite_parameters_.ego_goal_lanelet_id, test_suite_parameters_.ego_goal_s, test_suite_parameters_.ego_goal_partial_randomization, test_suite_parameters_.ego_goal_partial_randomization_distance); - ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position).pose; + ret.ego_goal_pose = lanelet_utils_->toMapPose(ret.ego_goal_position, false).pose; std::vector lanelets_around_start = lanelet_utils_->getLanesWithinDistance( ret.ego_start_position, test_suite_parameters_.npc_min_spawn_distance_from_ego, diff --git a/test_runner/random_test_runner/test/test_lanelet_utils.cpp b/test_runner/random_test_runner/test/test_lanelet_utils.cpp index 0939c70baa1..37d23ac027a 100644 --- a/test_runner/random_test_runner/test/test_lanelet_utils.cpp +++ b/test_runner/random_test_runner/test/test_lanelet_utils.cpp @@ -201,10 +201,29 @@ TEST(LaneletUtils, getLaneletIds) TEST(LaneletUtils, toMapPose) { const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0); - EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose)); + EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose, false)); + EXPECT_POSE_NEAR( + getLaneletUtils().toMapPose(pose, false).pose, + geometry_msgs::build() + .position(geometry_msgs::build() + .x(3747.3511648127) + .y(73735.0699484234) + .z(0.3034051453)) + .orientation(geometry_msgs::build() + .x(0.0) + .y(0.0) + .z(-0.9719821398) + .w(0.2350547166)), + EPS); +} + +TEST(LaneletUtils, toMapPoseWithFillingPitch) +{ + const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0); + EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose, true)); // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( - getLaneletUtils().toMapPose(pose).pose, + getLaneletUtils().toMapPose(pose, true).pose, geometry_msgs::build() .position(geometry_msgs::build() .x(3747.3511648127) From 368ac8864a42502275cf5f11ac8a98e64da6ced9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 1 Mar 2024 15:14:35 +0900 Subject: [PATCH 37/43] fix: use consider_pose_by_road_slope flag in EntityManager::spawnEntity Signed-off-by: Kotaro Yoshimoto --- .../include/traffic_simulator/entity/entity_manager.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp index f8c4da0b00d..fd0b7526f03 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp @@ -540,7 +540,13 @@ class EntityManager entity_status.lanelet_pose = *lanelet_pose; entity_status.lanelet_pose_valid = true; // @note fix z, roll and pitch to fitting to the lanelet - entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; + static bool consider_pose_by_road_slope = + getParameter("consider_pose_by_road_slope", false); + if (consider_pose_by_road_slope) { + entity_status.pose = hdmap_utils_ptr_->toMapPose(*lanelet_pose).pose; + } else { + entity_status.pose = pose; + } } else { entity_status.lanelet_pose_valid = false; entity_status.pose = pose; From 531272a0b6d4954d91817b6e1d846724af59ae36 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 11:19:40 +0900 Subject: [PATCH 38/43] chore(random_test_runner): restore test value --- .../test/test_test_randomizer.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/test_runner/random_test_runner/test/test_test_randomizer.cpp b/test_runner/random_test_runner/test/test_test_randomizer.cpp index 8cbb2c43ae8..522b12b326d 100644 --- a/test_runner/random_test_runner/test/test_test_randomizer.cpp +++ b/test_runner/random_test_runner/test/test_test_randomizer.cpp @@ -53,7 +53,6 @@ TEST(TestRandomizer, generate_10NPC) description.ego_start_position, makeLaneletPose(34705, 6.2940393113), EPS); EXPECT_LANELET_POSE_NEAR( description.ego_goal_position, makeLaneletPose(34642, 9.6945373700), EPS); - // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( description.ego_goal_pose, geometry_msgs::build() @@ -62,10 +61,10 @@ TEST(TestRandomizer, generate_10NPC) .y(73741.1526960728) .z(0.0698702227)) .orientation(geometry_msgs::build() - .x(-0.0040560888625029243) - .y(-0.00069838333685736189) - .z(-0.98549007915725362) - .w(0.16968312905673485)), + .x(0.0) + .y(0.0) + .z(-0.9854984261) + .w(0.1696845662)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(10)); @@ -127,10 +126,10 @@ TEST(TestRandomizer, generate_8NPC) .y(73815.1830249432) .z(-2.9975350010)) .orientation(geometry_msgs::build() - .x(-0.002510549540877986) - .y(0.01040596135909698) - .z(0.23451814178855812) - .w(0.97205280426591933)), + .x(0.0) + .y(0.0) + .z(0.2345315792) + .w(0.9721085013)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(8)); From 7447eecdbd8e7f8660df526d5537da870b13d6fc Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 14:42:51 +0900 Subject: [PATCH 39/43] chore: apply formatter --- .../test/test_test_randomizer.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/test_runner/random_test_runner/test/test_test_randomizer.cpp b/test_runner/random_test_runner/test/test_test_randomizer.cpp index 522b12b326d..2adfed32e8e 100644 --- a/test_runner/random_test_runner/test/test_test_randomizer.cpp +++ b/test_runner/random_test_runner/test/test_test_randomizer.cpp @@ -61,10 +61,10 @@ TEST(TestRandomizer, generate_10NPC) .y(73741.1526960728) .z(0.0698702227)) .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(-0.9854984261) - .w(0.1696845662)), + .x(0.0) + .y(0.0) + .z(-0.9854984261) + .w(0.1696845662)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(10)); @@ -126,10 +126,10 @@ TEST(TestRandomizer, generate_8NPC) .y(73815.1830249432) .z(-2.9975350010)) .orientation(geometry_msgs::build() - .x(0.0) - .y(0.0) - .z(0.2345315792) - .w(0.9721085013)), + .x(0.0) + .y(0.0) + .z(0.2345315792) + .w(0.9721085013)), EPS); // npc data EXPECT_EQ(description.npcs_descriptions.size(), size_t(8)); From aca35d3de9cd549b03ef0efe0f2bafe8fcd3fbf9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 15:09:34 +0900 Subject: [PATCH 40/43] doc: fix comment --- test_runner/random_test_runner/test/test_lanelet_utils.cpp | 2 +- test_runner/random_test_runner/test/test_test_randomizer.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/test_runner/random_test_runner/test/test_lanelet_utils.cpp b/test_runner/random_test_runner/test/test_lanelet_utils.cpp index 37d23ac027a..f9246b7198c 100644 --- a/test_runner/random_test_runner/test/test_lanelet_utils.cpp +++ b/test_runner/random_test_runner/test/test_lanelet_utils.cpp @@ -221,7 +221,7 @@ TEST(LaneletUtils, toMapPoseWithFillingPitch) { const traffic_simulator_msgs::msg::LaneletPose pose = makeLaneletPose(34621, 10.0); EXPECT_NO_THROW(getLaneletUtils().toMapPose(pose, true)); - // NOTE: orientation data is output as of #1103 + // @note orientation data is output as of #1103 EXPECT_POSE_NEAR( getLaneletUtils().toMapPose(pose, true).pose, geometry_msgs::build() diff --git a/test_runner/random_test_runner/test/test_test_randomizer.cpp b/test_runner/random_test_runner/test/test_test_randomizer.cpp index 2adfed32e8e..3110b21754b 100644 --- a/test_runner/random_test_runner/test/test_test_randomizer.cpp +++ b/test_runner/random_test_runner/test/test_test_randomizer.cpp @@ -117,7 +117,6 @@ TEST(TestRandomizer, generate_8NPC) description.ego_start_position, makeLaneletPose(34648, 11.3744011441), EPS); EXPECT_LANELET_POSE_NEAR( description.ego_goal_position, makeLaneletPose(34507, 55.3754803281), EPS); - // NOTE: orientation data is output as of #1103 EXPECT_POSE_NEAR( description.ego_goal_pose, geometry_msgs::build() From 2ebffbc60dc650cbe9e65b77327784fe30283901 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 16:40:10 +0900 Subject: [PATCH 41/43] feat: add fill_pitch option to CatmullRomSpline::getPose --- .../geometry/spline/catmull_rom_spline.hpp | 2 +- .../src/spline/catmull_rom_spline.cpp | 6 ++--- .../geometry/test/test_catmull_rom_spline.cpp | 27 +++++++++++++++++++ 3 files changed, 31 insertions(+), 4 deletions(-) diff --git a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp index 0c9d47bec1d..afb858133a6 100644 --- a/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp +++ b/common/math/geometry/include/geometry/spline/catmull_rom_spline.hpp @@ -39,7 +39,7 @@ class CatmullRomSpline : public CatmullRomSplineInterface auto getPoint(const double s, const double offset) const -> geometry_msgs::msg::Point; auto getTangentVector(const double s) const -> geometry_msgs::msg::Vector3; auto getNormalVector(const double s) const -> geometry_msgs::msg::Vector3; - auto getPose(const double s) const -> geometry_msgs::msg::Pose; + auto getPose(const double s, const bool fill_pitch = false) const -> geometry_msgs::msg::Pose; auto getTrajectory( const double start_s, const double end_s, const double resolution, const double offset = 0.0) const -> std::vector; diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index baf7a66eaf9..65b922393bf 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -641,7 +641,7 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: } } -auto CatmullRomSpline::getPose(const double s) const -> geometry_msgs::msg::Pose +auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const -> geometry_msgs::msg::Pose { switch (control_points.size()) { case 0: @@ -664,10 +664,10 @@ auto CatmullRomSpline::getPose(const double s) const -> geometry_msgs::msg::Pose "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); } - return line_segments_[0].getPose(s, true); + return line_segments_[0].getPose(s, true, fill_pitch); default: const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getPose(index_and_s.second, true); + return curves_[index_and_s.first].getPose(index_and_s.second, true, fill_pitch); } } diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp index a3f2976855d..eb47848e646 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/test_catmull_rom_spline.cpp @@ -30,6 +30,14 @@ math::geometry::CatmullRomSpline makeLine() return math::geometry::CatmullRomSpline(points); } +/// @brief Helper function generating sloped line: p(0,0,0)-> p(1,3,1) -> p(2,6,2) +math::geometry::CatmullRomSpline makeSlopedLine() +{ + const std::vector points{ + makePoint(0.0, 0.0, 0.0), makePoint(1.0, 3.0, 1.0), makePoint(2.0, 6.0, 2.0)}; + return math::geometry::CatmullRomSpline(points); +} + /// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0) math::geometry::CatmullRomSpline makeCurve() { @@ -465,6 +473,16 @@ TEST(CatmullRomSpline, getPoseLine) quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, 0.0, std::atan(3.0))), EPS); } +TEST(CatmullRomSpline, getPoseLineWithPitch) +{ + const math::geometry::CatmullRomSpline spline = makeSlopedLine(); + const auto pose = spline.getPose(std::hypot(1.5, 4.5, 1.5), true); + EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5, 1.5), EPS); + EXPECT_QUATERNION_NEAR( + pose.orientation, + quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, std::atan2(-1.0, std::sqrt(1.0+3.0*3.0)), std::atan(3.0))), EPS); +} + TEST(CatmullRomSpline, getPoseCurve) { const math::geometry::CatmullRomSpline spline = makeCurve(); @@ -474,6 +492,15 @@ TEST(CatmullRomSpline, getPoseCurve) EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); } +TEST(CatmullRomSpline, getPoseCurveWithPitch) +{ + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = spline.getPose(1.5, true); + constexpr double eps = 0.02; + EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps); + EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); +} + TEST(CatmullRomSpline, getSValue1) { const std::vector points{ From c7012293492920deb31966d0544e51ecaef4c01d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 17:05:31 +0900 Subject: [PATCH 42/43] fix: use fill_pitch option in EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet --- .../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 94072a447fe..e69c6d0681e 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 @@ -510,7 +510,7 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( status.pose.position.z = spline.getPoint(s_value.value()).z; if (consider_pose_by_road_slope_) { const auto rpy = quaternion_operation::convertQuaternionToEulerAngle( - spline.getPose(s_value.value()).orientation); + spline.getPose(s_value.value(), true).orientation); const auto original_rpy = quaternion_operation::convertQuaternionToEulerAngle(status.pose.orientation); status.pose.orientation = quaternion_operation::convertEulerAngleToQuaternion( @@ -520,7 +520,7 @@ auto EgoEntitySimulation::fillLaneletDataAndSnapZToLanelet( .z(original_rpy.z)); lanelet_pose->rpy = quaternion_operation::convertQuaternionToEulerAngle(quaternion_operation::getRotation( - spline.getPose(s_value.value()).orientation, status.pose.orientation)); + spline.getPose(s_value.value(), true).orientation, status.pose.orientation)); } } status.lanelet_pose_valid = true; From eb7673fcd152dfe81a9fb2cf6acb7d9565c6589f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 4 Mar 2024 18:41:00 +0900 Subject: [PATCH 43/43] chore: apply formatter --- .../src/spline/catmull_rom_spline.cpp | 3 +- .../geometry/test/test_catmull_rom_spline.cpp | 30 ++++++++++--------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index 65b922393bf..11c2143373d 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -641,7 +641,8 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: } } -auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const -> geometry_msgs::msg::Pose +auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const + -> geometry_msgs::msg::Pose { switch (control_points.size()) { case 0: diff --git a/common/math/geometry/test/test_catmull_rom_spline.cpp b/common/math/geometry/test/test_catmull_rom_spline.cpp index eb47848e646..0ec17c5b42c 100644 --- a/common/math/geometry/test/test_catmull_rom_spline.cpp +++ b/common/math/geometry/test/test_catmull_rom_spline.cpp @@ -33,9 +33,9 @@ math::geometry::CatmullRomSpline makeLine() /// @brief Helper function generating sloped line: p(0,0,0)-> p(1,3,1) -> p(2,6,2) math::geometry::CatmullRomSpline makeSlopedLine() { - const std::vector points{ - makePoint(0.0, 0.0, 0.0), makePoint(1.0, 3.0, 1.0), makePoint(2.0, 6.0, 2.0)}; - return math::geometry::CatmullRomSpline(points); + const std::vector points{ + makePoint(0.0, 0.0, 0.0), makePoint(1.0, 3.0, 1.0), makePoint(2.0, 6.0, 2.0)}; + return math::geometry::CatmullRomSpline(points); } /// @brief Helper function generating curve: p(0,0)-> p(1,1)-> p(2,0) @@ -475,12 +475,14 @@ TEST(CatmullRomSpline, getPoseLine) TEST(CatmullRomSpline, getPoseLineWithPitch) { - const math::geometry::CatmullRomSpline spline = makeSlopedLine(); - const auto pose = spline.getPose(std::hypot(1.5, 4.5, 1.5), true); - EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5, 1.5), EPS); - EXPECT_QUATERNION_NEAR( - pose.orientation, - quaternion_operation::convertEulerAngleToQuaternion(makeVector(0.0, std::atan2(-1.0, std::sqrt(1.0+3.0*3.0)), std::atan(3.0))), EPS); + const math::geometry::CatmullRomSpline spline = makeSlopedLine(); + const auto pose = spline.getPose(std::hypot(1.5, 4.5, 1.5), true); + EXPECT_POINT_NEAR(pose.position, makePoint(1.5, 4.5, 1.5), EPS); + EXPECT_QUATERNION_NEAR( + pose.orientation, + quaternion_operation::convertEulerAngleToQuaternion( + makeVector(0.0, std::atan2(-1.0, std::sqrt(1.0 + 3.0 * 3.0)), std::atan(3.0))), + EPS); } TEST(CatmullRomSpline, getPoseCurve) @@ -494,11 +496,11 @@ TEST(CatmullRomSpline, getPoseCurve) TEST(CatmullRomSpline, getPoseCurveWithPitch) { - const math::geometry::CatmullRomSpline spline = makeCurve(); - const auto pose = spline.getPose(1.5, true); - constexpr double eps = 0.02; - EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps); - EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); + const math::geometry::CatmullRomSpline spline = makeCurve(); + const auto pose = spline.getPose(1.5, true); + constexpr double eps = 0.02; + EXPECT_POINT_NEAR(pose.position, makePoint(1.0, 1.0), eps); + EXPECT_QUATERNION_NEAR(pose.orientation, geometry_msgs::msg::Quaternion(), eps); } TEST(CatmullRomSpline, getSValue1)