feat: add sensor noise

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml

@@ -53,8 +53,7 @@
   </group>
 
     <!-- Localization -->
-  <let name="pose_additional_delay_var" value="0.0" if="$(var simulation)"/>
-  <let name="pose_additional_delay_var" value="0.5" unless="$(var simulation)"/>
+  <let name="pose_additional_delay_var" value="0.5" />
   <group>
     <push-ros-namespace namespace="localization"/>
     <include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt

@@ -6,13 +6,20 @@ autoware_package()
 
 ament_auto_add_library(${PROJECT_NAME} SHARED
   src/actuation_cmd_converter.cpp
+  src/sensor_converter.cpp
 )
 
 rclcpp_components_register_node(${PROJECT_NAME}
   PLUGIN "ActuationCmdConverter"
   EXECUTABLE actuation_cmd_converter
 )
 
+rclcpp_components_register_node(${PROJECT_NAME}
+  PLUGIN "SensorConverter"
+  EXECUTABLE sensor_converter
+)
+
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml

@@ -0,0 +1,18 @@
+/**:
+  ros__parameters:
+    gnss_pose_delay: 500  # [ms]
+    gnss_pose_cov_delay: 500  # [ms]
+    gnss_pose_mean: 0.0
+    gnss_pose_stddev: 0.005
+    gnss_pose_cov_mean: 0.0
+    gnss_pose_cov_stddev: 0.007
+
+    imu_acc_mean: 0.0
+    imu_acc_stddev: 0.001
+    imu_ang_mean: 0.0
+    imu_ang_stddev: 0.0025
+    imu_ori_mean: 0.0
+    imu_ori_stddev: 0.001
+
+    steering_angle_mean: 0.0
+    steering_angle_stddev: 0.0000075

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml

@@ -7,4 +7,7 @@
     <param name="csv_path_accel_map" value="$(var csv_path_accel_map)"/>
     <param name="csv_path_brake_map" value="$(var csv_path_accel_map)"/>
   </node>
+  <node pkg="aichallenge_awsim_adapter" exec="sensor_converter">
+    <param from="$(find-pkg-share aichallenge_awsim_adapter)/config/sensor_converter.param.yaml" />
+  </node>
 </launch>

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml

@@ -16,7 +16,9 @@
   <depend>raw_vehicle_cmd_converter</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
-
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
 

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp

@@ -0,0 +1,134 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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 "sensor_converter.hpp"
+
+SensorConverter::SensorConverter(const rclcpp::NodeOptions & node_options)
+: Node("sensor_converter", node_options)
+{
+  using std::placeholders::_1;
+
+  // Parameters
+  gnss_pose_delay_ = declare_parameter<int>("gnss_pose_delay");
+  gnss_pose_cov_delay_ = declare_parameter<int>("gnss_pose_cov_delay");
+  gnss_pose_mean_ = declare_parameter<double>("gnss_pose_mean");
+  gnss_pose_stddev_ = declare_parameter<double>("gnss_pose_stddev");
+  gnss_pose_cov_mean_ = declare_parameter<double>("gnss_pose_cov_mean");
+  gnss_pose_cov_stddev_ = declare_parameter<double>("gnss_pose_cov_stddev");
+  imu_acc_mean_ = declare_parameter<double>("imu_acc_mean");
+  imu_acc_stddev_ = declare_parameter<double>("imu_acc_stddev");
+  imu_ang_mean_ = declare_parameter<double>("imu_ang_mean");
+  imu_ang_stddev_ = declare_parameter<double>("imu_ang_stddev");
+  imu_ori_mean_ = declare_parameter<double>("imu_ori_mean");
+  imu_ori_stddev_ = declare_parameter<double>("imu_ori_stddev");
+  steering_angle_mean_ = declare_parameter<double>("steering_angle_mean");
+  steering_angle_stddev_ = declare_parameter<double>("steering_angle_stddev");
+
+
+  // Subscriptions
+  sub_gnss_pose_ = create_subscription<PoseStamped>(
+    "/awsim/gnss/pose", 1, std::bind(&SensorConverter::on_gnss_pose, this, _1));
+  sub_gnss_pose_cov_ = create_subscription<PoseWithCovarianceStamped>(
+    "/awsim/gnss/pose_with_covariance", 1, std::bind(&SensorConverter::on_gnss_pose_cov, this, _1));
+  sub_imu_ = create_subscription<Imu>(
+    "/awsim/imu", 1, std::bind(&SensorConverter::on_imu, this, _1));
+  sub_steering_report_ = create_subscription<SteeringReport>(
+    "/awsim/steering_status", 1, std::bind(&SensorConverter::on_steering_report, this, _1));
+
+  // Publishers
+  pub_gnss_pose_ = create_publisher<PoseStamped>("/sensing/gnss/pose", 1);
+  pub_gnss_pose_cov_ = create_publisher<PoseWithCovarianceStamped>("/sensing/gnss/pose_with_covariance", 1);
+  pub_imu_ = create_publisher<Imu>("/sensing/imu/imu_raw", 1);
+  pub_steering_report_ = create_publisher<SteeringReport>("/vehicle/status/steering_status", 1);
+
+  std::random_device rd;
+  generator_ = std::mt19937(rd());
+  pose_distribution_ = std::normal_distribution<double>(gnss_pose_mean_, gnss_pose_stddev_);
+  pose_cov_distribution_ = std::normal_distribution<double>(gnss_pose_mean_, gnss_pose_stddev_);
+  imu_acc_distribution_ = std::normal_distribution<double>(imu_acc_mean_, imu_acc_stddev_);
+  imu_ang_distribution_ = std::normal_distribution<double>(imu_ang_mean_, imu_ang_stddev_);
+  imu_ori_distribution_ = std::normal_distribution<double>(imu_ori_mean_, imu_ori_stddev_);
+  steering_angle_distribution_ = std::normal_distribution<double>(steering_angle_mean_, steering_angle_stddev_);
+}
+
+void SensorConverter::on_gnss_pose(const PoseStamped::ConstSharedPtr msg)
+{
+  auto process_and_publish_gnss = [this, msg]() {
+    rclcpp::sleep_for(std::chrono::milliseconds(gnss_pose_delay_));
+
+    auto pose = std::make_shared<PoseStamped>(*msg);
+    pose->header.stamp = now();
+    pose->pose.position.x += pose_distribution_(generator_);
+    pose->pose.position.y += pose_distribution_(generator_);
+    pose->pose.position.z += pose_distribution_(generator_);
+    pose->pose.orientation.x += pose_distribution_(generator_);
+    pose->pose.orientation.y += pose_distribution_(generator_);
+    pose->pose.orientation.z += pose_distribution_(generator_);
+    pose->pose.orientation.w += pose_distribution_(generator_);
+
+    pub_gnss_pose_->publish(*pose);
+  };
+
+  std::thread processing_thread(process_and_publish_gnss);
+  processing_thread.detach();
+}
+
+
+void SensorConverter::on_gnss_pose_cov(const PoseWithCovarianceStamped::ConstSharedPtr msg)
+{
+    auto process_and_publish_gnss_cov = [this, msg]() {
+    rclcpp::sleep_for(std::chrono::milliseconds(gnss_pose_cov_delay_));
+
+    auto pose_cov = std::make_shared<PoseWithCovarianceStamped>(*msg);
+    pose_cov->header.stamp = now();
+    pose_cov->pose.pose.position.x += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.position.y += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.position.z += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.orientation.x += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.orientation.y += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.orientation.z += pose_cov_distribution_(generator_);
+    pose_cov->pose.pose.orientation.w += pose_cov_distribution_(generator_);
+
+    pub_gnss_pose_cov_->publish(*pose_cov);
+  };
+
+  std::thread processing_thread(process_and_publish_gnss_cov);
+  processing_thread.detach();
+}
+
+void SensorConverter::on_imu(const Imu::ConstSharedPtr msg)
+{
+  imu_ = std::make_shared<Imu>(*msg);
+  imu_ -> orientation.x += imu_ori_distribution_(generator_);
+  imu_ -> orientation.y += imu_ori_distribution_(generator_);
+  imu_ -> orientation.z += imu_ori_distribution_(generator_);
+  imu_ -> orientation.w += imu_ori_distribution_(generator_);
+  imu_ -> angular_velocity.x += imu_ang_distribution_(generator_);
+  imu_ -> angular_velocity.y += imu_ang_distribution_(generator_);
+  imu_ -> angular_velocity.z += imu_ang_distribution_(generator_);
+  imu_ -> linear_acceleration.x += imu_acc_distribution_(generator_);
+  imu_ -> linear_acceleration.y += imu_acc_distribution_(generator_);
+  imu_ -> linear_acceleration.z += imu_acc_distribution_(generator_);
+  pub_imu_->publish(*imu_);
+}
+
+void SensorConverter::on_steering_report(const SteeringReport::ConstSharedPtr msg)
+{
+  steering_report_ = std::make_shared<SteeringReport>(*msg);
+  steering_report_->steering_tire_angle += steering_angle_distribution_(generator_);
+  pub_steering_report_->publish(*steering_report_);
+}
+
+#include <rclcpp_components/register_node_macro.hpp>
+RCLCPP_COMPONENTS_REGISTER_NODE(SensorConverter)

aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp

@@ -0,0 +1,81 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// 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.
+
+#ifndef AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
+#define AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_
+
+
+#include <random>
+#include <rclcpp/rclcpp.hpp>
+
+#include <sensor_msgs/msg/imu.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp"
+
+using geometry_msgs::msg::PoseStamped;
+using geometry_msgs::msg::PoseWithCovarianceStamped;
+using sensor_msgs::msg::Imu;
+using autoware_auto_vehicle_msgs::msg::SteeringReport;
+
+class SensorConverter : public rclcpp::Node
+{
+public:
+  explicit SensorConverter(const rclcpp::NodeOptions & node_options);
+
+private:
+  rclcpp::Subscription<PoseStamped>::SharedPtr sub_gnss_pose_;
+  rclcpp::Subscription<PoseWithCovarianceStamped>::SharedPtr sub_gnss_pose_cov_;
+  rclcpp::Subscription<Imu>::SharedPtr sub_imu_;
+  rclcpp::Subscription<SteeringReport>::SharedPtr sub_steering_report_;
+  rclcpp::Publisher<PoseStamped>::SharedPtr pub_gnss_pose_;
+  rclcpp::Publisher<Imu>::SharedPtr pub_imu_;
+  rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_gnss_pose_cov_;
+  rclcpp::Publisher<SteeringReport>::SharedPtr pub_steering_report_;
+
+
+  void on_gnss_pose(const PoseStamped::ConstSharedPtr msg);
+  void on_gnss_pose_cov(const PoseWithCovarianceStamped::ConstSharedPtr msg);
+  void on_imu(const Imu::ConstSharedPtr msg);
+  void on_steering_report(const SteeringReport::ConstSharedPtr msg);
+
+  PoseStamped::SharedPtr pose_;
+  PoseWithCovarianceStamped::SharedPtr pose_cov_;
+  Imu::SharedPtr imu_;
+  SteeringReport::SharedPtr steering_report_;
+  int gnss_pose_delay_;
+  int gnss_pose_cov_delay_;
+
+  std::mt19937 generator_;
+  std::normal_distribution<double> pose_distribution_;
+  std::normal_distribution<double> pose_cov_distribution_;
+  std::normal_distribution<double> imu_acc_distribution_;
+  std::normal_distribution<double> imu_ang_distribution_;
+  std::normal_distribution<double> imu_ori_distribution_;
+  std::normal_distribution<double> steering_angle_distribution_;
+  double gnss_pose_mean_;
+  double gnss_pose_stddev_;
+  double gnss_pose_cov_mean_;
+  double gnss_pose_cov_stddev_;
+  double imu_acc_mean_;
+  double imu_acc_stddev_;
+  double imu_ang_mean_;
+  double imu_ang_stddev_;
+  double imu_ori_mean_;
+  double imu_ori_stddev_;
+  double steering_angle_mean_;
+  double steering_angle_stddev_;
+};
+
+#endif  // AUTOWARE_EXTERNAL_CMD_CONVERTER__NODE_HPP_

aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml

@@ -8,26 +8,6 @@
         <include file="$(find-pkg-share aichallenge_awsim_adapter)/launch/aichallenge_awsim_adapter.launch.xml"/>
     </group>
 
-    <node pkg="topic_tools" exec="relay" name="relay_gnss_pose">
-        <param name="input_topic" value="/awsim/gnss/pose"/>
-        <param name="output_topic" value="/sensing/gnss/pose"/>
-    </node>
-
-    <node pkg="topic_tools" exec="relay" name="relay_gnss_pose_cov">
-        <param name="input_topic" value="/awsim/gnss/pose_with_covariance"/>
-        <param name="output_topic" value="/sensing/gnss/pose_with_covariance"/>
-    </node>
-
-    <node pkg="topic_tools" exec="relay" name="relay_imu">
-        <param name="input_topic" value="/awsim/imu"/>
-        <param name="output_topic" value="/sensing/imu/imu_raw"/>
-    </node>
-
-    <node pkg="topic_tools" exec="relay" name="relay_steering">
-        <param name="input_topic" value="/awsim/steering_status"/>
-        <param name="output_topic" value="/vehicle/status/steering_status"/>
-    </node>
-
     <node pkg="topic_tools" exec="relay" name="relay_velocity">
         <param name="input_topic" value="/awsim/velocity_status"/>
         <param name="output_topic" value="/vehicle/status/velocity_status"/>