From 3e26cd7dcd17538b74a75c2000dd1409d1b7e933 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 8 Jan 2025 16:32:24 +0900 Subject: [PATCH 1/6] Add new struct template `concealer::NormalDistribution` Signed-off-by: yamacir-kit --- .../concealer/include/concealer/publisher.hpp | 26 ++++++++++++++++--- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index aeb323084e4..dfc5b743d7e 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -20,22 +20,40 @@ namespace concealer { -template +template +struct NormalDistribution +{ + template + explicit NormalDistribution(Ts &&...) + { + } + + template + auto operator()(T && x) const -> decltype(auto) + { + return std::forward(x); + } +}; + +template typename Randomizer = NormalDistribution> class Publisher { typename rclcpp::Publisher::SharedPtr publisher; + Randomizer randomize; + public: template explicit Publisher(const std::string & topic, Node & node) - : publisher(node.template create_publisher(topic, rclcpp::QoS(1).reliable())) + : publisher(node.template create_publisher(topic, rclcpp::QoS(1).reliable())), + randomize(topic) { } template - auto operator()(Ts &&... xs) const -> decltype(auto) + auto operator()(Ts &&... xs) -> decltype(auto) { - return publisher->publish(std::forward(xs)...); + return publisher->publish(randomize(std::forward(xs)...)); } }; } // namespace concealer From a0e554897df158e45ffb19126ba3643ca898ddb9 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 8 Jan 2025 17:35:59 +0900 Subject: [PATCH 2/6] Add template specialization `NormalDistribution` Signed-off-by: yamacir-kit --- external/concealer/CMakeLists.txt | 1 + .../concealer/include/concealer/publisher.hpp | 29 +++++- external/concealer/src/publisher.cpp | 98 +++++++++++++++++++ 3 files changed, 127 insertions(+), 1 deletion(-) create mode 100644 external/concealer/src/publisher.cpp diff --git a/external/concealer/CMakeLists.txt b/external/concealer/CMakeLists.txt index 0c05bfd2e30..c267d3dd5cf 100644 --- a/external/concealer/CMakeLists.txt +++ b/external/concealer/CMakeLists.txt @@ -18,6 +18,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/execute.cpp src/field_operator_application.cpp src/is_package_exists.cpp + src/publisher.cpp src/task_queue.cpp) target_link_libraries(${PROJECT_NAME} diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index dfc5b743d7e..91d108792b2 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -15,7 +15,8 @@ #ifndef CONCEALER__PUBLISHER_HPP_ #define CONCEALER__PUBLISHER_HPP_ -#include +#include +#include #include namespace concealer @@ -35,6 +36,32 @@ struct NormalDistribution } }; +template <> +struct NormalDistribution +{ + std::random_device::result_type seed; + + std::random_device device; + + std::default_random_engine engine; + + std::uniform_real_distribution position_x, position_y, position_z, orientation_r, + orientation_p, orientation_y, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z; + + template + auto getParameter(const std::string & name, T value = {}) + { + auto node = rclcpp::Node("normal_distribution", "simulation"); + node.declare_parameter(name, value); + node.get_parameter(name, value); + return value; + } + + explicit NormalDistribution(const std::string &); + + auto operator()(nav_msgs::msg::Odometry odometry) -> nav_msgs::msg::Odometry; +}; + template typename Randomizer = NormalDistribution> class Publisher { diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp new file mode 100644 index 00000000000..9f83e8fd6d8 --- /dev/null +++ b/external/concealer/src/publisher.cpp @@ -0,0 +1,98 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +namespace concealer +{ +NormalDistribution::NormalDistribution(const std::string & topic) +: seed(getParameter(topic + ".seed")), + engine(seed ? seed : device()), + position_x( + getParameter(topic + ".pose.pose.position.x.mean"), + getParameter(topic + ".pose.pose.position.x.standard_deviation")), + position_y( + getParameter(topic + ".pose.pose.position.y.mean"), + getParameter(topic + ".pose.pose.position.y.standard_deviation")), + position_z( + getParameter(topic + ".pose.pose.position.z.mean"), + getParameter(topic + ".pose.pose.position.z.standard_deviation")), + orientation_r( + getParameter(topic + ".pose.pose.orientation.r.mean"), + getParameter(topic + ".pose.pose.orientation.r.standard_deviation")), + orientation_p( + getParameter(topic + ".pose.pose.orientation.p.mean"), + getParameter(topic + ".pose.pose.orientation.p.standard_deviation")), + orientation_y( + getParameter(topic + ".pose.pose.orientation.y.mean"), + getParameter(topic + ".pose.pose.orientation.y.standard_deviation")), + linear_x( + getParameter(topic + ".twist.twist.linear.x.mean"), + getParameter(topic + ".twist.twist.linear.x.standard_deviation")), + linear_y( + getParameter(topic + ".twist.twist.linear.y.mean"), + getParameter(topic + ".twist.twist.linear.y.standard_deviation")), + linear_z( + getParameter(topic + ".twist.twist.linear.z.mean"), + getParameter(topic + ".twist.twist.linear.z.standard_deviation")), + angular_x( + getParameter(topic + ".twist.twist.angular.x.mean"), + getParameter(topic + ".twist.twist.angular.x.standard_deviation")), + angular_y( + getParameter(topic + ".twist.twist.angular.y.mean"), + getParameter(topic + ".twist.twist.angular.y.standard_deviation")), + angular_z( + getParameter(topic + ".twist.twist.angular.z.mean"), + getParameter(topic + ".twist.twist.angular.z.standard_deviation")) +{ +} + +auto NormalDistribution::operator()(nav_msgs::msg::Odometry odometry) + -> nav_msgs::msg::Odometry +{ + odometry.pose.pose.position.x += position_x(engine); + odometry.pose.pose.position.y += position_y(engine); + odometry.pose.pose.position.z += position_z(engine); + + Eigen::Vector3d euler = Eigen::Quaterniond( + odometry.pose.pose.orientation.w, odometry.pose.pose.orientation.x, + odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z) + .matrix() + .eulerAngles(0, 1, 2); + + euler.x() += orientation_r(engine); + euler.y() += orientation_p(engine); + euler.z() += orientation_y(engine); + + const Eigen::Quaterniond orientation = Eigen::AngleAxisd(euler.x(), Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(euler.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(euler.z(), Eigen::Vector3d::UnitZ()); + + odometry.pose.pose.orientation.x = orientation.x(); + odometry.pose.pose.orientation.y = orientation.y(); + odometry.pose.pose.orientation.z = orientation.z(); + odometry.pose.pose.orientation.w = orientation.w(); + + odometry.twist.twist.linear.x += linear_x(engine); + odometry.twist.twist.linear.y += linear_y(engine); + odometry.twist.twist.linear.z += linear_z(engine); + + odometry.twist.twist.angular.x += angular_x(engine); + odometry.twist.twist.angular.y += angular_y(engine); + odometry.twist.twist.angular.z += angular_z(engine); + + return odometry; +} +} // namespace concealer From abbcdc42424cc8c719538ce60ca236c806693f6d Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Wed, 8 Jan 2025 18:00:20 +0900 Subject: [PATCH 3/6] Add launch argument `parameter_file_path` to `scenario_test_runner` Signed-off-by: yamacir-kit --- .../launch/mock_test.launch.py | 4 +++ .../config/parameters.yaml | 25 +++++++++++++++++++ .../launch/scenario_test_runner.launch.py | 7 +++++- 3 files changed, 35 insertions(+), 1 deletion(-) create mode 100644 test_runner/scenario_test_runner/config/parameters.yaml diff --git a/mock/cpp_mock_scenarios/launch/mock_test.launch.py b/mock/cpp_mock_scenarios/launch/mock_test.launch.py index 0a1f08e45f4..e5bf9853dd8 100644 --- a/mock/cpp_mock_scenarios/launch/mock_test.launch.py +++ b/mock/cpp_mock_scenarios/launch/mock_test.launch.py @@ -115,6 +115,7 @@ def launch_setup(context, *args, **kwargs): launch_rviz = LaunchConfiguration("launch_rviz", default=False) launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True) output_directory = LaunchConfiguration("output_directory", default=Path("/tmp")) + parameter_file_path = LaunchConfiguration("parameter_file_path", default=Path(get_package_share_directory("scenario_test_runner")) / "config/parameters.yaml") port = LaunchConfiguration("port", default=5555) publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) record = LaunchConfiguration("record", default=False) @@ -140,6 +141,7 @@ def launch_setup(context, *args, **kwargs): 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"parameter_file_path := {parameter_file_path.perform(context)}") print(f"port := {port.perform(context)}") print(f"publish_empty_context := {publish_empty_context.perform(context)}") print(f"record := {record.perform(context)}") @@ -174,6 +176,7 @@ def make_parameters(): {"junit_path": junit_path}, ] parameters += make_vehicle_parameters() + parameters += [parameter_file_path.perform(context)] return parameters def make_vehicle_parameters(): @@ -218,6 +221,7 @@ def description(): DeclareLaunchArgument("global_timeout", default_value=global_timeout ), DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), + DeclareLaunchArgument("parameter_file_path", default_value=parameter_file_path ), DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), DeclareLaunchArgument("output_directory", default_value=output_directory ), DeclareLaunchArgument("rviz_config", default_value=rviz_config ), diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml new file mode 100644 index 00000000000..6ae2dc8d6b8 --- /dev/null +++ b/test_runner/scenario_test_runner/config/parameters.yaml @@ -0,0 +1,25 @@ +simulation: + normal_distribution: + ros__parameters: + /localization/kinematic_state: + seed: 0 # If 0 is specified, a random seed value will be generated for each run. + pose: + pose: + position: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 } + orientation: + r: { mean: 0.0, standard_deviation: 0.0 } + p: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + twist: + twist: + linear: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 } + angular: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 } 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 9ad03b6f027..a5fcd88ed6e 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 @@ -80,6 +80,7 @@ def launch_setup(context, *args, **kwargs): launch_rviz = LaunchConfiguration("launch_rviz", default=False) launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True) output_directory = LaunchConfiguration("output_directory", default=Path("/tmp")) + parameter_file_path = LaunchConfiguration("parameter_file_path", default=Path(get_package_share_directory("scenario_test_runner")) / "config/parameters.yaml") port = LaunchConfiguration("port", default=5555) publish_empty_context = LaunchConfiguration("publish_empty_context", default=False) record = LaunchConfiguration("record", default=True) @@ -106,6 +107,7 @@ def launch_setup(context, *args, **kwargs): 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"parameter_file_path := {parameter_file_path.perform(context)}") print(f"port := {port.perform(context)}") print(f"publish_empty_context := {publish_empty_context.perform(context)}") print(f"record := {record.perform(context)}") @@ -164,6 +166,8 @@ def collect_prefixed_parameters(): if (it := collect_prefixed_parameters()) != []: parameters += [{"autoware.": it}] + parameters += [parameter_file_path.perform(context)] + return parameters return [ @@ -180,13 +184,14 @@ def collect_prefixed_parameters(): DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ), DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ), DeclareLaunchArgument("output_directory", default_value=output_directory ), + DeclareLaunchArgument("parameter_file_path", default_value=parameter_file_path ), DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ), 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("simulate_localization", default_value=simulate_localization ), - DeclareLaunchArgument("speed_condition", default_value=speed_condition ), + DeclareLaunchArgument("speed_condition", default_value=speed_condition ), DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ), DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ), # fmt: on From 996bc389eb589a120eada1fee4c0941d049943d5 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 9 Jan 2025 11:57:56 +0900 Subject: [PATCH 4/6] Add new struct template `Identity` Signed-off-by: yamacir-kit --- .../include/concealer/autoware_universe.hpp | 16 ++++++++-------- .../concealer/include/concealer/publisher.hpp | 11 +++++++---- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/external/concealer/include/concealer/autoware_universe.hpp b/external/concealer/include/concealer/autoware_universe.hpp index 3e28ffe89c3..6279909f875 100644 --- a/external/concealer/include/concealer/autoware_universe.hpp +++ b/external/concealer/include/concealer/autoware_universe.hpp @@ -62,14 +62,14 @@ class AutowareUniverse : public rclcpp::Node, Subscriber getTurnIndicatorsCommand; Subscriber getPathWithLaneId; - Publisher setAcceleration; - Publisher setOdometry; - Publisher setPose; - Publisher setSteeringReport; - Publisher setGearReport; - Publisher setControlModeReport; - Publisher setVelocityReport; - Publisher setTurnIndicatorsReport; + Publisher setAcceleration; + Publisher setOdometry; + Publisher setPose; + Publisher setSteeringReport; + Publisher setGearReport; + Publisher setControlModeReport; + Publisher setVelocityReport; + Publisher setTurnIndicatorsReport; std::atomic current_acceleration; std::atomic current_pose; diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index 91d108792b2..9d43f3f7910 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -22,20 +22,23 @@ namespace concealer { template -struct NormalDistribution +struct Identity { template - explicit NormalDistribution(Ts &&...) + explicit constexpr Identity(Ts &&...) { } template - auto operator()(T && x) const -> decltype(auto) + constexpr auto operator()(T && x) const -> decltype(auto) { return std::forward(x); } }; +template +struct NormalDistribution; + template <> struct NormalDistribution { @@ -62,7 +65,7 @@ struct NormalDistribution auto operator()(nav_msgs::msg::Odometry odometry) -> nav_msgs::msg::Odometry; }; -template typename Randomizer = NormalDistribution> +template typename Randomizer = Identity> class Publisher { typename rclcpp::Publisher::SharedPtr publisher; From 0caa0ecc1d7c939884311b6bb105bb0b70dccfe2 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 9 Jan 2025 17:00:53 +0900 Subject: [PATCH 5/6] Update randomizer to receive `NodeParametersInterface` Signed-off-by: yamacir-kit --- .../concealer/include/concealer/publisher.hpp | 16 ++-- external/concealer/src/autoware_universe.cpp | 2 +- external/concealer/src/publisher.cpp | 82 +++++++++++++------ .../config/parameters.yaml | 5 +- 4 files changed, 66 insertions(+), 39 deletions(-) diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index 9d43f3f7910..95236fa6123 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -42,6 +42,8 @@ struct NormalDistribution; template <> struct NormalDistribution { + int version; + std::random_device::result_type seed; std::random_device device; @@ -51,16 +53,8 @@ struct NormalDistribution std::uniform_real_distribution position_x, position_y, position_z, orientation_r, orientation_p, orientation_y, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z; - template - auto getParameter(const std::string & name, T value = {}) - { - auto node = rclcpp::Node("normal_distribution", "simulation"); - node.declare_parameter(name, value); - node.get_parameter(name, value); - return value; - } - - explicit NormalDistribution(const std::string &); + explicit NormalDistribution( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &); auto operator()(nav_msgs::msg::Odometry odometry) -> nav_msgs::msg::Odometry; }; @@ -76,7 +70,7 @@ class Publisher template explicit Publisher(const std::string & topic, Node & node) : publisher(node.template create_publisher(topic, rclcpp::QoS(1).reliable())), - randomize(topic) + randomize(node.get_node_parameters_interface(), topic) { } diff --git a/external/concealer/src/autoware_universe.cpp b/external/concealer/src/autoware_universe.cpp index b67bdbe7fd1..f4ddac37c0d 100644 --- a/external/concealer/src/autoware_universe.cpp +++ b/external/concealer/src/autoware_universe.cpp @@ -17,7 +17,7 @@ namespace concealer { AutowareUniverse::AutowareUniverse(bool simulate_localization) -: rclcpp::Node("concealer", "simulation", rclcpp::NodeOptions().use_global_arguments(false)), +: rclcpp::Node("AutowareUniverse", "simulation"), getCommand("/control/command/control_cmd", rclcpp::QoS(1), *this), getGearCommand("/control/command/gear_cmd", rclcpp::QoS(1), *this), getTurnIndicatorsCommand("/control/command/turn_indicators_cmd", rclcpp::QoS(1), *this), diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index 9f83e8fd6d8..3ae8e677d15 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -14,49 +14,79 @@ #include #include +#include namespace concealer { -NormalDistribution::NormalDistribution(const std::string & topic) -: seed(getParameter(topic + ".seed")), +template +auto getParameter( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node, + const std::string & name, T value = {}) +{ + if (not node->has_parameter(name)) { + node->declare_parameter(name, rclcpp::ParameterValue(value)); + } + return node->get_parameter(name).get_value(); +} + +NormalDistribution::NormalDistribution( + const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node, + const std::string & topic) +: version([&]() { + switch (const auto version = getParameter(node, topic + ".type.version"); version) { + case 1: + return version; + default: + throw common::Error( + "An unexpected message version ", version, " was specified for topic ", + std::quoted(topic)); + } + }()), + seed(getParameter(node, topic + ".seed")), engine(seed ? seed : device()), position_x( - getParameter(topic + ".pose.pose.position.x.mean"), - getParameter(topic + ".pose.pose.position.x.standard_deviation")), + getParameter(node, topic + ".pose.pose.position.x.mean"), + getParameter(node, topic + ".pose.pose.position.x.standard_deviation")), position_y( - getParameter(topic + ".pose.pose.position.y.mean"), - getParameter(topic + ".pose.pose.position.y.standard_deviation")), + getParameter(node, topic + ".pose.pose.position.y.mean"), + getParameter(node, topic + ".pose.pose.position.y.standard_deviation")), position_z( - getParameter(topic + ".pose.pose.position.z.mean"), - getParameter(topic + ".pose.pose.position.z.standard_deviation")), + getParameter(node, topic + ".pose.pose.position.z.mean"), + getParameter(node, topic + ".pose.pose.position.z.standard_deviation")), orientation_r( - getParameter(topic + ".pose.pose.orientation.r.mean"), - getParameter(topic + ".pose.pose.orientation.r.standard_deviation")), + getParameter(node, topic + ".pose.pose.orientation.r.mean"), + getParameter(node, topic + ".pose.pose.orientation.r.standard_deviation")), orientation_p( - getParameter(topic + ".pose.pose.orientation.p.mean"), - getParameter(topic + ".pose.pose.orientation.p.standard_deviation")), + getParameter(node, topic + ".pose.pose.orientation.p.mean"), + getParameter(node, topic + ".pose.pose.orientation.p.standard_deviation")), orientation_y( - getParameter(topic + ".pose.pose.orientation.y.mean"), - getParameter(topic + ".pose.pose.orientation.y.standard_deviation")), + getParameter(node, topic + ".pose.pose.orientation.y.mean"), + getParameter(node, topic + ".pose.pose.orientation.y.standard_deviation")), linear_x( - getParameter(topic + ".twist.twist.linear.x.mean"), - getParameter(topic + ".twist.twist.linear.x.standard_deviation")), + getParameter(node, topic + ".twist.twist.linear.x.mean"), + getParameter(node, topic + ".twist.twist.linear.x.standard_deviation")), linear_y( - getParameter(topic + ".twist.twist.linear.y.mean"), - getParameter(topic + ".twist.twist.linear.y.standard_deviation")), + getParameter(node, topic + ".twist.twist.linear.y.mean"), + getParameter(node, topic + ".twist.twist.linear.y.standard_deviation")), linear_z( - getParameter(topic + ".twist.twist.linear.z.mean"), - getParameter(topic + ".twist.twist.linear.z.standard_deviation")), + getParameter(node, topic + ".twist.twist.linear.z.mean"), + getParameter(node, topic + ".twist.twist.linear.z.standard_deviation")), angular_x( - getParameter(topic + ".twist.twist.angular.x.mean"), - getParameter(topic + ".twist.twist.angular.x.standard_deviation")), + getParameter(node, topic + ".twist.twist.angular.x.mean"), + getParameter(node, topic + ".twist.twist.angular.x.standard_deviation")), angular_y( - getParameter(topic + ".twist.twist.angular.y.mean"), - getParameter(topic + ".twist.twist.angular.y.standard_deviation")), + getParameter(node, topic + ".twist.twist.angular.y.mean"), + getParameter(node, topic + ".twist.twist.angular.y.standard_deviation")), angular_z( - getParameter(topic + ".twist.twist.angular.z.mean"), - getParameter(topic + ".twist.twist.angular.z.standard_deviation")) + getParameter(node, topic + ".twist.twist.angular.z.mean"), + getParameter(node, topic + ".twist.twist.angular.z.standard_deviation")) { + if (const auto name = getParameter(node, topic + ".type.name"); + name != "nav_msgs::msg::Odometry") { + throw common::Error( + "An unexpected message type ", std::quoted(name), " was specified for topic ", + std::quoted(topic)); + } } auto NormalDistribution::operator()(nav_msgs::msg::Odometry odometry) diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml index 6ae2dc8d6b8..b81e5253d70 100644 --- a/test_runner/scenario_test_runner/config/parameters.yaml +++ b/test_runner/scenario_test_runner/config/parameters.yaml @@ -1,7 +1,10 @@ simulation: - normal_distribution: + AutowareUniverse: ros__parameters: /localization/kinematic_state: + type: + name: nav_msgs::msg::Odometry + version: 1 seed: 0 # If 0 is specified, a random seed value will be generated for each run. pose: pose: From 5ce7a0aafea0d09bc45c0dd18dcefb91c6ccdc48 Mon Sep 17 00:00:00 2001 From: yamacir-kit Date: Thu, 9 Jan 2025 18:58:06 +0900 Subject: [PATCH 6/6] Update parameter file format Signed-off-by: yamacir-kit --- .../concealer/include/concealer/publisher.hpp | 2 - external/concealer/src/publisher.cpp | 78 +++++++++---------- .../config/parameters.yaml | 41 +++++----- 3 files changed, 57 insertions(+), 64 deletions(-) diff --git a/external/concealer/include/concealer/publisher.hpp b/external/concealer/include/concealer/publisher.hpp index 95236fa6123..2b29648dc69 100644 --- a/external/concealer/include/concealer/publisher.hpp +++ b/external/concealer/include/concealer/publisher.hpp @@ -42,8 +42,6 @@ struct NormalDistribution; template <> struct NormalDistribution { - int version; - std::random_device::result_type seed; std::random_device device; diff --git a/external/concealer/src/publisher.cpp b/external/concealer/src/publisher.cpp index 3ae8e677d15..d6eebda88a0 100644 --- a/external/concealer/src/publisher.cpp +++ b/external/concealer/src/publisher.cpp @@ -32,61 +32,57 @@ auto getParameter( NormalDistribution::NormalDistribution( const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node, const std::string & topic) -: version([&]() { - switch (const auto version = getParameter(node, topic + ".type.version"); version) { - case 1: - return version; - default: - throw common::Error( - "An unexpected message version ", version, " was specified for topic ", - std::quoted(topic)); - } - }()), - seed(getParameter(node, topic + ".seed")), +: seed(getParameter(node, topic + ".seed")), engine(seed ? seed : device()), position_x( - getParameter(node, topic + ".pose.pose.position.x.mean"), - getParameter(node, topic + ".pose.pose.position.x.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.x.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.x.standard_deviation")), position_y( - getParameter(node, topic + ".pose.pose.position.y.mean"), - getParameter(node, topic + ".pose.pose.position.y.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.y.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.y.standard_deviation")), position_z( - getParameter(node, topic + ".pose.pose.position.z.mean"), - getParameter(node, topic + ".pose.pose.position.z.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.z.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.z.standard_deviation")), orientation_r( - getParameter(node, topic + ".pose.pose.orientation.r.mean"), - getParameter(node, topic + ".pose.pose.orientation.r.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.r.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.r.standard_deviation")), orientation_p( - getParameter(node, topic + ".pose.pose.orientation.p.mean"), - getParameter(node, topic + ".pose.pose.orientation.p.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.p.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.p.standard_deviation")), orientation_y( - getParameter(node, topic + ".pose.pose.orientation.y.mean"), - getParameter(node, topic + ".pose.pose.orientation.y.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.y.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.y.standard_deviation")), linear_x( - getParameter(node, topic + ".twist.twist.linear.x.mean"), - getParameter(node, topic + ".twist.twist.linear.x.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.x.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.x.standard_deviation")), linear_y( - getParameter(node, topic + ".twist.twist.linear.y.mean"), - getParameter(node, topic + ".twist.twist.linear.y.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.y.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.y.standard_deviation")), linear_z( - getParameter(node, topic + ".twist.twist.linear.z.mean"), - getParameter(node, topic + ".twist.twist.linear.z.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.z.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.z.standard_deviation")), angular_x( - getParameter(node, topic + ".twist.twist.angular.x.mean"), - getParameter(node, topic + ".twist.twist.angular.x.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.x.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.x.standard_deviation")), angular_y( - getParameter(node, topic + ".twist.twist.angular.y.mean"), - getParameter(node, topic + ".twist.twist.angular.y.standard_deviation")), + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.y.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.y.standard_deviation")), angular_z( - getParameter(node, topic + ".twist.twist.angular.z.mean"), - getParameter(node, topic + ".twist.twist.angular.z.standard_deviation")) + getParameter(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.z.mean"), + getParameter( + node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.z.standard_deviation")) { - if (const auto name = getParameter(node, topic + ".type.name"); - name != "nav_msgs::msg::Odometry") { - throw common::Error( - "An unexpected message type ", std::quoted(name), " was specified for topic ", - std::quoted(topic)); - } } auto NormalDistribution::operator()(nav_msgs::msg::Odometry odometry) diff --git a/test_runner/scenario_test_runner/config/parameters.yaml b/test_runner/scenario_test_runner/config/parameters.yaml index b81e5253d70..597930fb49b 100644 --- a/test_runner/scenario_test_runner/config/parameters.yaml +++ b/test_runner/scenario_test_runner/config/parameters.yaml @@ -2,27 +2,26 @@ simulation: AutowareUniverse: ros__parameters: /localization/kinematic_state: - type: - name: nav_msgs::msg::Odometry - version: 1 + version: 20240605 # architecture_type suffix (mandatory) seed: 0 # If 0 is specified, a random seed value will be generated for each run. - pose: + nav_msgs::msg::Odometry: pose: - position: - x: { mean: 0.0, standard_deviation: 0.0 } - y: { mean: 0.0, standard_deviation: 0.0 } - z: { mean: 0.0, standard_deviation: 0.0 } - orientation: - r: { mean: 0.0, standard_deviation: 0.0 } - p: { mean: 0.0, standard_deviation: 0.0 } - y: { mean: 0.0, standard_deviation: 0.0 } - twist: + pose: + position: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 } + orientation: + r: { mean: 0.0, standard_deviation: 0.0 } + p: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } twist: - linear: - x: { mean: 0.0, standard_deviation: 0.0 } - y: { mean: 0.0, standard_deviation: 0.0 } - z: { mean: 0.0, standard_deviation: 0.0 } - angular: - x: { mean: 0.0, standard_deviation: 0.0 } - y: { mean: 0.0, standard_deviation: 0.0 } - z: { mean: 0.0, standard_deviation: 0.0 } + twist: + linear: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 } + angular: + x: { mean: 0.0, standard_deviation: 0.0 } + y: { mean: 0.0, standard_deviation: 0.0 } + z: { mean: 0.0, standard_deviation: 0.0 }