Add noise for pose

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/continuous_transform_broadcaster.hpp

@@ -28,6 +28,8 @@ namespace concealer
 template <typename Node>
 class ContinuousTransformBroadcaster
 {
+  const std::string child_frame_id;
+
   tf2_ros::Buffer transform_buffer;
 
   tf2_ros::TransformBroadcaster transform_broadcaster;
@@ -52,7 +54,7 @@ class ContinuousTransformBroadcaster
   {
     current_transform.header.stamp = static_cast<Node &>(*this).get_clock()->now();
     current_transform.header.frame_id = "map";
-    current_transform.child_frame_id = "base_link";
+    current_transform.child_frame_id = child_frame_id;
     current_transform.transform.translation.x = pose.position.x;
     current_transform.transform.translation.y = pose.position.y;
     current_transform.transform.translation.z = pose.position.z;
@@ -61,8 +63,9 @@ class ContinuousTransformBroadcaster
     return current_transform;
   }
 
-  explicit ContinuousTransformBroadcaster()
-  : transform_buffer(static_cast<Node &>(*this).get_clock()),
+  explicit ContinuousTransformBroadcaster(const std::string & child_frame_id)
+  : child_frame_id(child_frame_id),
+    transform_buffer(static_cast<Node &>(*this).get_clock()),
     transform_broadcaster(static_cast<Node *>(this)),
     timer(static_cast<Node &>(*this).create_wall_timer(
       std::chrono::milliseconds(5), [this]() { return updateTransform(); }))

external/concealer/include/concealer/publisher.hpp

@@ -16,6 +16,7 @@
 #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>
@@ -128,6 +129,31 @@ 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 <typename Message, template <typename> typename Randomizer = Identity>
 class Publisher
 {

external/concealer/src/autoware_universe.cpp

@@ -24,6 +24,7 @@ namespace concealer
 // clang-format off
 AutowareUniverse::AutowareUniverse(bool simulate_localization) try
 : rclcpp::Node("concealer", "simulation"),
+  ContinuousTransformBroadcaster<AutowareUniverse>(simulate_localization? "base_link" : "base_link_ground_truth"),
   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),
@@ -105,12 +106,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

@@ -12,9 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <tf2/LinearMath/Quaternion.h>
+
 #include <Eigen/Geometry>
 #include <concealer/publisher.hpp>
 #include <scenario_simulator_exception/exception.hpp>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 
 namespace concealer
 {
@@ -42,18 +45,20 @@ NormalDistribution<nav_msgs::msg::Odometry>::NormalDistribution(
 : RandomNumberEngine(node, topic),
   speed_threshold(
     common::getParameter<double>(node, topic + ".nav_msgs::msg::Odometry.speed_threshold")),
+  // clang-format off
   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")
+  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")
+// clang-format on
 {
 }
 
@@ -115,11 +120,11 @@ NormalDistribution<autoware_vehicle_msgs::msg::VelocityReport>::NormalDistributi
 : RandomNumberEngine(node, topic),
   speed_threshold(common::getParameter<double>(
     node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.speed_threshold")),
-  longitudinal_velocity_error(
-    node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.longitudinal_velocity.error"),
-  lateral_velocity_error(
-    node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.lateral_velocity.error"),
-  heading_rate_error(node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.heading_rate.error")
+  // clang-format off
+  longitudinal_velocity_error(node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.longitudinal_velocity.error"),
+  lateral_velocity_error(     node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.lateral_velocity.error"),
+  heading_rate_error(         node, topic + ".autoware_vehicle_msgs::msg::VelocityReport.heading_rate.error")
+// clang-format on
 {
 }
 
@@ -141,4 +146,77 @@ 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();
+
+  tf2::Quaternion original_orientation;
+  tf2::fromMsg(pose.orientation, original_orientation);
+
+  double roll_error = orientation_r_error.apply(engine, 0.0);
+  double pitch_error = orientation_p_error.apply(engine, 0.0);
+  double yaw_error = orientation_y_error.apply(engine, 0.0);
+
+  tf2::Quaternion error_quaternion;
+  error_quaternion.setRPY(roll_error, pitch_error, yaw_error);
+
+  tf2::Quaternion noised_orientation = original_orientation * error_quaternion;
+  noised_orientation.normalize();
+
+  pose.orientation = tf2::toMsg(noised_orientation);
+
+  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;
+}
+
 }  // namespace concealer