Feature/publisher with customizable randomizer

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}

external/concealer/include/concealer/autoware_universe.hpp

@@ -62,14 +62,14 @@ class AutowareUniverse : public rclcpp::Node,
   Subscriber<TurnIndicatorsCommand> getTurnIndicatorsCommand;
   Subscriber<PathWithLaneId>        getPathWithLaneId;
 
-  Publisher<AccelWithCovarianceStamped> setAcceleration;
-  Publisher<Odometry>                   setOdometry;
-  Publisher<PoseWithCovarianceStamped>  setPose;
-  Publisher<SteeringReport>             setSteeringReport;
-  Publisher<GearReport>                 setGearReport;
-  Publisher<ControlModeReport>          setControlModeReport;
-  Publisher<VelocityReport>             setVelocityReport;
-  Publisher<TurnIndicatorsReport>       setTurnIndicatorsReport;
+  Publisher<AccelWithCovarianceStamped>   setAcceleration;
+  Publisher<Odometry, NormalDistribution> setOdometry;
+  Publisher<PoseWithCovarianceStamped>    setPose;
+  Publisher<SteeringReport>               setSteeringReport;
+  Publisher<GearReport>                   setGearReport;
+  Publisher<ControlModeReport>            setControlModeReport;
+  Publisher<VelocityReport>               setVelocityReport;
+  Publisher<TurnIndicatorsReport>         setTurnIndicatorsReport;
 
   std::atomic<geometry_msgs::msg::Accel> current_acceleration;
   std::atomic<geometry_msgs::msg::Pose>  current_pose;

external/concealer/include/concealer/publisher.hpp

@@ -15,27 +15,67 @@
 #ifndef CONCEALER__PUBLISHER_HPP_
 #define CONCEALER__PUBLISHER_HPP_
 
-#include <memory>
+#include <nav_msgs/msg/odometry.hpp>
+#include <random>
 #include <rclcpp/rclcpp.hpp>
 
 namespace concealer
 {
-template <typename Message>
+template <typename>
+struct Identity
+{
+  template <typename... Ts>
+  explicit constexpr Identity(Ts &&...)
+  {
+  }
+
+  template <typename T>
+  constexpr auto operator()(T && x) const -> decltype(auto)
+  {
+    return std::forward<decltype(x)>(x);
+  }
+};
+
+template <typename>
+struct NormalDistribution;
+
+template <>
+struct NormalDistribution<nav_msgs::msg::Odometry>
+{
+  std::random_device::result_type seed;
+
+  std::random_device device;
+
+  std::default_random_engine engine;
+
+  std::uniform_real_distribution<double> position_x, position_y, position_z, orientation_r,
+    orientation_p, orientation_y, linear_x, linear_y, linear_z, angular_x, angular_y, angular_z;
+
+  explicit NormalDistribution(
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &);
+
+  auto operator()(nav_msgs::msg::Odometry odometry) -> nav_msgs::msg::Odometry;
+};
+
+template <typename Message, template <typename> typename Randomizer = Identity>
 class Publisher
 {
   typename rclcpp::Publisher<Message>::SharedPtr publisher;
 
+  Randomizer<Message> randomize;
+
 public:
   template <typename Node>
   explicit Publisher(const std::string & topic, Node & node)
-  : publisher(node.template create_publisher<Message>(topic, rclcpp::QoS(1).reliable()))
+  : publisher(node.template create_publisher<Message>(topic, rclcpp::QoS(1).reliable())),
+    randomize(node.get_node_parameters_interface(), topic)
   {
   }
 
   template <typename... Ts>
-  auto operator()(Ts &&... xs) const -> decltype(auto)
+  auto operator()(Ts &&... xs) -> decltype(auto)
   {
-    return publisher->publish(std::forward<decltype(xs)>(xs)...);
+    return publisher->publish(randomize(std::forward<decltype(xs)>(xs)...));
   }
 };
 }  // namespace concealer

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),

external/concealer/src/publisher.cpp

@@ -0,0 +1,124 @@
+// 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 <Eigen/Geometry>
+#include <concealer/publisher.hpp>
+#include <scenario_simulator_exception/exception.hpp>
+
+namespace concealer
+{
+template <typename T>
+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<T>();
+}
+
+NormalDistribution<nav_msgs::msg::Odometry>::NormalDistribution(
+  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+  const std::string & topic)
+: seed(getParameter<int>(node, topic + ".seed")),
+  engine(seed ? seed : device()),
+  position_x(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.x.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.x.standard_deviation")),
+  position_y(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.y.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.y.standard_deviation")),
+  position_z(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.z.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.z.standard_deviation")),
+  orientation_r(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.r.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.r.standard_deviation")),
+  orientation_p(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.p.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.p.standard_deviation")),
+  orientation_y(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.y.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.y.standard_deviation")),
+  linear_x(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.x.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.x.standard_deviation")),
+  linear_y(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.y.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.y.standard_deviation")),
+  linear_z(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.z.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.z.standard_deviation")),
+  angular_x(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.x.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.x.standard_deviation")),
+  angular_y(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.y.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.y.standard_deviation")),
+  angular_z(
+    getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.z.mean"),
+    getParameter<double>(
+      node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.z.standard_deviation"))
+{
+}
+
+auto NormalDistribution<nav_msgs::msg::Odometry>::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

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                        ),

test_runner/scenario_test_runner/config/parameters.yaml

@@ -0,0 +1,27 @@
+simulation:
+  AutowareUniverse:
+    ros__parameters:
+      /localization/kinematic_state:
+        version: 20240605 # architecture_type suffix (mandatory)
+        seed: 0 # If 0 is specified, a random seed value will be generated for each run.
+        nav_msgs::msg::Odometry:
+          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 }

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