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,112 @@
 #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 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>();
+}
+
+template <typename>
+struct Identity
+{
+  explicit constexpr Identity(
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &)
+  {
+  }
+
+  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::mt19937_64 engine;
+
+  struct Error
+  {
+    std::normal_distribution<double> additive, multiplicative;
+
+    explicit Error(
+      const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+      const std::string & prefix)
+    : additive(
+        getParameter<double>(node, prefix + ".additive.mean"),
+        getParameter<double>(node, prefix + ".additive.standard_deviation")),
+      multiplicative(
+        getParameter<double>(node, prefix + ".multiplicative.mean"),
+        getParameter<double>(node, prefix + ".multiplicative.standard_deviation"))
+    {
+    }
+
+    auto apply(std::mt19937_64 & engine, double value) -> decltype(auto)
+    {
+      return value * (multiplicative(engine) + 1.0) + additive(engine);
+    }
+  };
+
+  // clang-format off
+  Error position_local_x_error,
+        position_local_y_error,
+        position_local_z_error,
+        orientation_r_error,
+        orientation_p_error,
+        orientation_y_error,
+        linear_x_error,
+        linear_y_error,
+        linear_z_error,
+        angular_x_error,
+        angular_y_error,
+        angular_z_error;
+  // clang-format on
+
+  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,95 @@
+// 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
+{
+NormalDistribution<nav_msgs::msg::Odometry>::NormalDistribution(
+  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+  const std::string & topic)
+: seed([&]() {
+    if (const auto value = getParameter<int>(node, topic + ".seed");
+        std::random_device::min() <= value and value <= std::random_device::max()) {
+      return value;
+    } else {
+      throw common::scenario_simulator_exception::Error(
+        "The value of parameter ", std::quoted(topic + ".seed"),
+        " must be greater than or equal to ", std::random_device::min(),
+        " and less than or equal to ", std::random_device::max());
+    }
+  }()),
+  engine(seed ? seed : device()),
+  position_local_x_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_x.error"),
+  position_local_y_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_y.error"),
+  position_local_z_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.position.local_z.error"),
+  orientation_r_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.r.error"),
+  orientation_p_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.p.error"),
+  orientation_y_error(node, topic + ".nav_msgs::msg::Odometry.pose.pose.orientation.y.error"),
+  linear_x_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.x.error"),
+  linear_y_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.y.error"),
+  linear_z_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.linear.z.error"),
+  angular_x_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.x.error"),
+  angular_y_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.y.error"),
+  angular_z_error(node, topic + ".nav_msgs::msg::Odometry.twist.twist.angular.z.error")
+{
+}
+
+auto NormalDistribution<nav_msgs::msg::Odometry>::operator()(nav_msgs::msg::Odometry odometry)
+  -> nav_msgs::msg::Odometry
+{
+  const Eigen::Quaterniond orientation = Eigen::Quaterniond(
+    odometry.pose.pose.orientation.w, odometry.pose.pose.orientation.x,
+    odometry.pose.pose.orientation.y, odometry.pose.pose.orientation.z);
+
+  Eigen::Vector3d local_position = Eigen::Vector3d(0.0, 0.0, 0.0);
+
+  local_position.x() = position_local_x_error.apply(engine, local_position.x());
+  local_position.y() = position_local_y_error.apply(engine, local_position.y());
+  local_position.z() = position_local_z_error.apply(engine, local_position.z());
+
+  const Eigen::Vector3d world_position = orientation.toRotationMatrix() * local_position;
+
+  odometry.pose.pose.position.x += world_position.x();
+  odometry.pose.pose.position.y += world_position.y();
+  odometry.pose.pose.position.z += world_position.z();
+
+  Eigen::Vector3d euler = orientation.matrix().eulerAngles(0, 1, 2);
+
+  euler.x() = orientation_r_error.apply(engine, euler.x());
+  euler.y() = orientation_p_error.apply(engine, euler.y());
+  euler.z() = orientation_y_error.apply(engine, euler.z());
+
+  const Eigen::Quaterniond q = 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 = q.x();
+  odometry.pose.pose.orientation.y = q.y();
+  odometry.pose.pose.orientation.z = q.z();
+  odometry.pose.pose.orientation.w = q.w();
+
+  odometry.twist.twist.linear.x = linear_x_error.apply(engine, odometry.twist.twist.linear.x);
+  odometry.twist.twist.linear.y = linear_y_error.apply(engine, odometry.twist.twist.linear.y);
+  odometry.twist.twist.linear.z = linear_z_error.apply(engine, odometry.twist.twist.linear.z);
+
+  odometry.twist.twist.angular.x = angular_x_error.apply(engine, odometry.twist.twist.angular.x);
+  odometry.twist.twist.angular.y = angular_y_error.apply(engine, odometry.twist.twist.angular.y);
+  odometry.twist.twist.angular.z = angular_z_error.apply(engine, odometry.twist.twist.angular.z);
+
+  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                        ),