Feature: Add noise to Localization, Velocity status and IMU

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, NormalDistribution>       setOdometry;
-  Publisher<PoseWithCovarianceStamped>          setPose;
-  Publisher<SteeringReport>                     setSteeringReport;
-  Publisher<GearReport>                         setGearReport;
-  Publisher<ControlModeReport>                  setControlModeReport;
-  Publisher<VelocityReport, NormalDistribution> setVelocityReport;
-  Publisher<TurnIndicatorsReport>               setTurnIndicatorsReport;
+  Publisher<AccelWithCovarianceStamped>                    setAcceleration;
+  Publisher<Odometry, NormalDistribution>                  setOdometry;
+  Publisher<PoseWithCovarianceStamped, NormalDistribution> setPose;
+  Publisher<SteeringReport>                                setSteeringReport;
+  Publisher<GearReport>                                    setGearReport;
+  Publisher<ControlModeReport>                             setControlModeReport;
+  Publisher<VelocityReport, NormalDistribution>            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

@@ -16,10 +16,12 @@
 #define CONCEALER__PUBLISHER_HPP_
 
 #include <autoware_vehicle_msgs/msg/velocity_report.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <get_parameter/get_parameter.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <random>
 #include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/imu.hpp>
 
 namespace concealer
 {
@@ -128,6 +130,82 @@ struct NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport> : public R
     -> autoware_vehicle_msgs::msg::VelocityReport;
 };
 
+template <>
+struct NormalDistribution<geometry_msgs::msg::PoseWithCovarianceStamped> : public RandomNumberEngine
+{
+  // clang-format off
+  NormalDistributionError<double> position_local_x_error,
+                                  position_local_y_error,
+                                  position_local_z_error,
+                                  orientation_r_error,
+                                  orientation_p_error,
+                                  orientation_y_error,
+                                  covariance_diagonal_x_x_error,
+                                  covariance_diagonal_y_y_error,
+                                  covariance_diagonal_z_z_error,
+                                  covariance_diagonal_roll_roll_error,
+                                  covariance_diagonal_pitch_pitch_error,
+                                  covariance_diagonal_yaw_yaw_error;
+  // clang-format on
+
+  explicit NormalDistribution(
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &);
+
+  auto operator()(geometry_msgs::msg::PoseWithCovarianceStamped pose)
+    -> geometry_msgs::msg::PoseWithCovarianceStamped;
+};
+
+template <>
+struct NormalDistribution<sensor_msgs::msg::Imu> : public RandomNumberEngine
+{
+  // clang-format off
+  NormalDistributionError<double> orientation_r_error,
+                                  orientation_p_error,
+                                  orientation_y_error,
+                                  angular_velocity_x_error,
+                                  angular_velocity_y_error,
+                                  angular_velocity_z_error,
+                                  linear_acceleration_x_error,
+                                  linear_acceleration_y_error,
+                                  linear_acceleration_z_error,
+                                  orientation_covariance_diagonal_roll_roll_error,
+                                  orientation_covariance_diagonal_pitch_pitch_error,
+                                  orientation_covariance_diagonal_yaw_yaw_error,
+                                  angular_velocity_covariance_diagonal_x_x_error,
+                                  angular_velocity_covariance_diagonal_y_y_error,
+                                  angular_velocity_covariance_diagonal_z_z_error,
+                                  linear_acceleration_covariance_diagonal_x_x_error,
+                                  linear_acceleration_covariance_diagonal_y_y_error,
+                                  linear_acceleration_covariance_diagonal_z_z_error;
+  // clang-format on
+
+  /// @note set this to false to disable randomization
+  bool active{true};
+
+  explicit NormalDistribution(
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &);
+
+  auto operator()(sensor_msgs::msg::Imu imu) -> sensor_msgs::msg::Imu;
+};
+
+template <>
+struct NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport> : public RandomNumberEngine
+{
+  double speed_threshold;
+
+  // clang-format off
+  NormalDistributionError<float> longitudinal_velocity_error,
+                                 lateral_velocity_error,
+                                 heading_rate_error;
+  // clang-format on
+
+  explicit NormalDistribution(
+    const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr &, const std::string &);
+
+  auto operator()(autoware_vehicle_msgs::msg::VelocityReport velocity_report)
+    -> autoware_vehicle_msgs::msg::VelocityReport;
+};
+
 template <typename Message, template <typename> typename Randomizer = Identity>
 class Publisher
 {

external/concealer/package.xml

@@ -35,6 +35,8 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>traffic_simulator_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry</depend>
 
   <test_depend>ament_cmake_clang_format</test_depend>
   <test_depend>ament_cmake_copyright</test_depend>

external/concealer/src/autoware_universe.cpp

@@ -105,12 +105,6 @@ AutowareUniverse::AutowareUniverse(bool simulate_localization) try
         message.header.stamp = get_clock()->now();
         message.header.frame_id = "map";
         message.pose.pose = current_pose.load();
-        message.pose.covariance.at(6 * 0 + 0) = 0.0225;    // XYZRPY_COV_IDX::X_X
-        message.pose.covariance.at(6 * 1 + 1) = 0.0225;    // XYZRPY_COV_IDX::Y_Y
-        message.pose.covariance.at(6 * 2 + 2) = 0.0225;    // XYZRPY_COV_IDX::Z_Z
-        message.pose.covariance.at(6 * 3 + 3) = 0.000625;  // XYZRPY_COV_IDX::ROLL_ROLL
-        message.pose.covariance.at(6 * 4 + 4) = 0.000625;  // XYZRPY_COV_IDX::PITCH_PITCH
-        message.pose.covariance.at(6 * 5 + 5) = 0.000625;  // XYZRPY_COV_IDX::YAW_YAW
         return message;
       }());
 

external/concealer/src/publisher.cpp

@@ -14,6 +14,8 @@
 
 #include <Eigen/Geometry>
 #include <concealer/publisher.hpp>
+#include <geometry/quaternion/euler_to_quaternion.hpp>
+#include <geometry/quaternion/quaternion_to_euler.hpp>
 #include <scenario_simulator_exception/exception.hpp>
 
 namespace concealer
@@ -141,4 +143,151 @@ auto NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport>::operator()(
   }
 }
 
+NormalDistribution<geometry_msgs::msg::PoseWithCovarianceStamped>::NormalDistribution(
+  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+  const std::string & topic)
+: RandomNumberEngine(node, topic),
+  // clang-format off
+  position_local_x_error(               node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.position.local_x.error"),
+  position_local_y_error(               node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.position.local_y.error"),
+  position_local_z_error(               node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.position.local_z.error"),
+  orientation_r_error(                  node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.orientation.r.error"),
+  orientation_p_error(                  node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.orientation.p.error"),
+  orientation_y_error(                  node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.pose.orientation.y.error"),
+  covariance_diagonal_x_x_error(        node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.x_x.error"),
+  covariance_diagonal_y_y_error(        node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.y_y.error"),
+  covariance_diagonal_z_z_error(        node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.z_z.error"),
+  covariance_diagonal_roll_roll_error(  node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.roll_roll.error"),
+  covariance_diagonal_pitch_pitch_error(node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.pitch_pitch.error"),
+  covariance_diagonal_yaw_yaw_error(    node, topic + ".geometry_msgs::msg::PoseWithCovarianceStamped.pose.covariance.yaw_yaw.error")
+// clang-format on
+{
+}
+
+auto NormalDistribution<geometry_msgs::msg::PoseWithCovarianceStamped>::operator()(
+  geometry_msgs::msg::PoseWithCovarianceStamped pose_with_covariance_stamped)
+  -> geometry_msgs::msg::PoseWithCovarianceStamped
+{
+  geometry_msgs::msg::Pose & pose = pose_with_covariance_stamped.pose.pose;
+
+  const Eigen::Quaterniond orientation = Eigen::Quaterniond(
+    pose.orientation.w, pose.orientation.x, pose.orientation.y, 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;
+
+  pose.position.x += world_position.x();
+  pose.position.y += world_position.y();
+  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());
+
+  pose.orientation.x = q.x();
+  pose.orientation.y = q.y();
+  pose.orientation.z = q.z();
+  pose.orientation.w = q.w();
+
+  pose_with_covariance_stamped.pose.covariance.at(6 * 0 + 0) =
+    covariance_diagonal_x_x_error.apply(engine, 0.0);
+  pose_with_covariance_stamped.pose.covariance.at(6 * 1 + 1) =
+    covariance_diagonal_y_y_error.apply(engine, 0.0);
+  pose_with_covariance_stamped.pose.covariance.at(6 * 2 + 2) =
+    covariance_diagonal_z_z_error.apply(engine, 0.0);
+  pose_with_covariance_stamped.pose.covariance.at(6 * 3 + 3) =
+    covariance_diagonal_roll_roll_error.apply(engine, 0.0);
+  pose_with_covariance_stamped.pose.covariance.at(6 * 4 + 4) =
+    covariance_diagonal_pitch_pitch_error.apply(engine, 0.0);
+  pose_with_covariance_stamped.pose.covariance.at(6 * 5 + 5) =
+    covariance_diagonal_yaw_yaw_error.apply(engine, 0.0);
+
+  return pose_with_covariance_stamped;
+}
+
+NormalDistribution<sensor_msgs::msg::Imu>::NormalDistribution(
+  const rclcpp::node_interfaces::NodeParametersInterface::SharedPtr & node,
+  const std::string & topic)
+: RandomNumberEngine(node, topic),
+  // clang-format off
+  orientation_r_error(                              node, topic + ".sensor_msgs::msg::Imu.orientation.r.error"),
+  orientation_p_error(                              node, topic + ".sensor_msgs::msg::Imu.orientation.p.error"),
+  orientation_y_error(                              node, topic + ".sensor_msgs::msg::Imu.orientation.y.error"),
+  angular_velocity_x_error(                         node, topic + ".sensor_msgs::msg::Imu.angular_velocity.x.error"),
+  angular_velocity_y_error(                         node, topic + ".sensor_msgs::msg::Imu.angular_velocity.y.error"),
+  angular_velocity_z_error(                         node, topic + ".sensor_msgs::msg::Imu.angular_velocity.z.error"),
+  linear_acceleration_x_error(                      node, topic + ".sensor_msgs::msg::Imu.linear_acceleration.x.error"),
+  linear_acceleration_y_error(                      node, topic + ".sensor_msgs::msg::Imu.linear_acceleration.y.error"),
+  linear_acceleration_z_error(                      node, topic + ".sensor_msgs::msg::Imu.linear_acceleration.z.error"),
+  orientation_covariance_diagonal_roll_roll_error(  node, topic + ".sensor_msgs::msg::Imu.orientation_covariance.roll_roll.error"),
+  orientation_covariance_diagonal_pitch_pitch_error(node, topic + ".sensor_msgs::msg::Imu.orientation_covariance.pitch_pitch.error"),
+  orientation_covariance_diagonal_yaw_yaw_error(    node, topic + ".sensor_msgs::msg::Imu.orientation_covariance.yaw_yaw.error"),
+  angular_velocity_covariance_diagonal_x_x_error(   node, topic + ".sensor_msgs::msg::Imu.angular_velocity_covariance.x_x.error"),
+  angular_velocity_covariance_diagonal_y_y_error(   node, topic + ".sensor_msgs::msg::Imu.angular_velocity_covariance.y_y.error"),
+  angular_velocity_covariance_diagonal_z_z_error(   node, topic + ".sensor_msgs::msg::Imu.angular_velocity_covariance.z_z.error"),
+  linear_acceleration_covariance_diagonal_x_x_error(node, topic + ".sensor_msgs::msg::Imu.linear_acceleration_covariance.x_x.error"),
+  linear_acceleration_covariance_diagonal_y_y_error(node, topic + ".sensor_msgs::msg::Imu.linear_acceleration_covariance.y_y.error"),
+  linear_acceleration_covariance_diagonal_z_z_error(node, topic + ".sensor_msgs::msg::Imu.linear_acceleration_covariance.z_z.error")
+// clang-format on
+{
+}
+
+auto NormalDistribution<sensor_msgs::msg::Imu>::operator()(sensor_msgs::msg::Imu imu)
+  -> sensor_msgs::msg::Imu
+{
+  if (not active) {
+    return imu;
+  }
+
+  imu.orientation = math::geometry::convertEulerAngleToQuaternion([this, &imu] {
+    auto rpy = math::geometry::convertQuaternionToEulerAngle(imu.orientation);
+
+    rpy.x = orientation_r_error.apply(engine, rpy.x);
+    rpy.y = orientation_p_error.apply(engine, rpy.y);
+    rpy.z = orientation_y_error.apply(engine, rpy.z);
+    return rpy;
+  }());
+
+  imu.angular_velocity.x = angular_velocity_x_error.apply(engine, imu.angular_velocity.x);
+  imu.angular_velocity.y = angular_velocity_y_error.apply(engine, imu.angular_velocity.y);
+  imu.angular_velocity.z = angular_velocity_z_error.apply(engine, imu.angular_velocity.z);
+
+  imu.linear_acceleration.x = linear_acceleration_x_error.apply(engine, imu.linear_acceleration.x);
+  imu.linear_acceleration.y = linear_acceleration_y_error.apply(engine, imu.linear_acceleration.y);
+  imu.linear_acceleration.z = linear_acceleration_z_error.apply(engine, imu.linear_acceleration.z);
+
+  imu.orientation_covariance.at(3 * 0 + 0) =
+    orientation_covariance_diagonal_roll_roll_error.apply(engine, 0.0);
+  imu.orientation_covariance.at(3 * 1 + 1) =
+    orientation_covariance_diagonal_pitch_pitch_error.apply(engine, 0.0);
+  imu.orientation_covariance.at(3 * 2 + 2) =
+    orientation_covariance_diagonal_yaw_yaw_error.apply(engine, 0.0);
+
+  imu.angular_velocity_covariance.at(3 * 0 + 0) =
+    angular_velocity_covariance_diagonal_x_x_error.apply(engine, 0.0);
+  imu.angular_velocity_covariance.at(3 * 1 + 1) =
+    angular_velocity_covariance_diagonal_y_y_error.apply(engine, 0.0);
+  imu.angular_velocity_covariance.at(3 * 2 + 2) =
+    angular_velocity_covariance_diagonal_z_z_error.apply(engine, 0.0);
+
+  imu.linear_acceleration_covariance.at(3 * 0 + 0) =
+    linear_acceleration_covariance_diagonal_x_x_error.apply(engine, 0.0);
+  imu.linear_acceleration_covariance.at(3 * 1 + 1) =
+    linear_acceleration_covariance_diagonal_y_y_error.apply(engine, 0.0);
+  imu.linear_acceleration_covariance.at(3 * 2 + 2) =
+    linear_acceleration_covariance_diagonal_z_z_error.apply(engine, 0.0);
+
+  return imu;
+}
 }  // namespace concealer