Feature/publisher with customizable randomizer

docs/developer_guide/.pages

@@ -1,21 +1,22 @@
+# cspell: ignore TIERIV
+
 nav:
     - About: About.md
     - AutowareAPI.md
+    - BehaviorPlugin.md
+    - CONTRIBUTING.md
+    - Communication.md
+    - ConfiguringLocalizationTopics.md
+    - ConfiguringPerceptionTopics.md
     - DistanceCalculation.md
+    - Lane Pose Calculation: lane_pose_calculation
+    - ManualOverrideWithFollowTrajectoryAction.md
     - NPCBehavior.md
-    - BehaviorPlugin.md
-    # - ErrorCategories.md
     - OpenSCENARIOSupport.md
     - SimpleSensorSimulator.md
+    - SimulationResultFormat.md
     - SystemArchitecture.md
+    - TIERIVScenarioFormatVersion2.md
     - TrafficSimulator.md
-    - ZeroMQ.md
     - VehicleDynamics.md
-    - SimulationResultFormat.md
-    - Lane Pose Calculation: lane_pose_calculation
-    # cspell: ignore TIERIV
-    - TIERIVScenarioFormatVersion2.md
-    - CONTRIBUTING.md
-    - Communication.md
-    - ConfiguringPerceptionTopics.md
-    - ManualOverrideWithFollowTrajectoryAction.md
+    - ZeroMQ.md

docs/developer_guide/ConfiguringLocalizationTopics.md

@@ -0,0 +1,26 @@
+# Configuring Localization Topics
+
+This section describes properties for configuring localization topics that
+`scenario_simulator_v2` publishes to Autoware.
+
+## Parameter file
+
+Localization topics are configurable from the ROS 2 parameter file given to the
+launch argument `parameter_file_path` of scenario_test_runner. The default
+value of `parameter_file_path` is the path to [a sample parameter
+file](https://github.com/tier4/scenario_simulator_v2/blob/f15bacd819abd2044c2d6c076530a2c4070ded3d/test_runner/scenario_test_runner/config/parameters.yaml).
+
+All parameters that can be specified and their default values are shown in the
+sample parameter file. In practice, it is not necessary to specify all
+parameters except for some that are mandatory. In that case, the simulator will
+behave as if similar default values had been specified.
+
+Most parameters should have their uses understood just by looking at the sample
+parameter file or by reading the comments in the file. Below we discuss some
+parameters that require additional detailed explanation.
+
+<!-- cspell: ignore YYYYMMDD -->
+
+| Name                   | Value                                   | Default              | Description                                                                                                                                                          |
+|------------------------|-----------------------------------------|----------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| `<topic-name>.version` | An `int` type value in YYYYMMDD format  | None (**mandatory**) | Suffix of `scenario_test_runner` launch argument `architecture_type`, used to maintain backward compatibility of the simulator when changing the Autoware interface. |

external/concealer/CMakeLists.txt

@@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
   src/field_operator_application.cpp
   src/is_package_exists.cpp
   src/path_with_lane_id.cpp
+  src/publisher.cpp
   src/task_queue.cpp)
 
 target_link_libraries(${PROJECT_NAME}
@@ -27,6 +28,14 @@ target_link_libraries(${PROJECT_NAME}
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  file(GLOB ${PROJECT_NAME}_TESTS ${CMAKE_CURRENT_SOURCE_DIR}/test/*.cpp)
+
+  foreach(EACH IN LISTS ${PROJECT_NAME}_TESTS)
+    get_filename_component(TEST_NAME ${EACH} NAME_WE)
+    ament_add_gtest(${TEST_NAME} ${EACH})
+    target_link_libraries(${TEST_NAME} ${PROJECT_NAME})
+  endforeach()
 endif()
 
 ament_auto_package()

external/concealer/include/concealer/autoware_universe.hpp

@@ -61,14 +61,14 @@ class AutowareUniverse : public rclcpp::Node,
   Subscriber<TurnIndicatorsCommand>    getTurnIndicatorsCommand;
   Subscriber<priority::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,102 @@
 #ifndef CONCEALER__PUBLISHER_HPP_
 #define CONCEALER__PUBLISHER_HPP_
 
-#include <memory>
+#include <concealer/get_parameter.hpp>
+#include <nav_msgs/msg/odometry.hpp>
+#include <random>
 #include <rclcpp/rclcpp.hpp>
 
 namespace concealer
 {
-template <typename Message>
+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/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