diff --git a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
index ce75ec78..d671b0bb 100644
--- a/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml
@@ -53,8 +53,7 @@
-
-
+
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt
index 59873936..4cb36878 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/CMakeLists.txt
@@ -6,6 +6,7 @@ autoware_package()
ament_auto_add_library(${PROJECT_NAME} SHARED
src/actuation_cmd_converter.cpp
+ src/sensor_converter.cpp
)
rclcpp_components_register_node(${PROJECT_NAME}
@@ -13,6 +14,12 @@ rclcpp_components_register_node(${PROJECT_NAME}
EXECUTABLE actuation_cmd_converter
)
+rclcpp_components_register_node(${PROJECT_NAME}
+ PLUGIN "SensorConverter"
+ EXECUTABLE sensor_converter
+)
+
ament_auto_package(INSTALL_TO_SHARE
launch
+ config
)
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml
new file mode 100644
index 00000000..a8683735
--- /dev/null
+++ b/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
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
index 10981b55..090a0514 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/launch/aichallenge_awsim_adapter.launch.xml
@@ -7,4 +7,7 @@
+
+
+
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml
index ef465c86..fa568759 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/package.xml
@@ -16,7 +16,9 @@
raw_vehicle_cmd_converter
rclcpp
rclcpp_components
-
+ sensor_msgs
+ geometry_msgs
+
ament_lint_auto
autoware_lint_common
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp
new file mode 100644
index 00000000..526e3c6e
--- /dev/null
+++ b/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("gnss_pose_delay");
+ gnss_pose_cov_delay_ = declare_parameter("gnss_pose_cov_delay");
+ gnss_pose_mean_ = declare_parameter("gnss_pose_mean");
+ gnss_pose_stddev_ = declare_parameter("gnss_pose_stddev");
+ gnss_pose_cov_mean_ = declare_parameter("gnss_pose_cov_mean");
+ gnss_pose_cov_stddev_ = declare_parameter("gnss_pose_cov_stddev");
+ imu_acc_mean_ = declare_parameter("imu_acc_mean");
+ imu_acc_stddev_ = declare_parameter("imu_acc_stddev");
+ imu_ang_mean_ = declare_parameter("imu_ang_mean");
+ imu_ang_stddev_ = declare_parameter("imu_ang_stddev");
+ imu_ori_mean_ = declare_parameter("imu_ori_mean");
+ imu_ori_stddev_ = declare_parameter("imu_ori_stddev");
+ steering_angle_mean_ = declare_parameter("steering_angle_mean");
+ steering_angle_stddev_ = declare_parameter("steering_angle_stddev");
+
+
+ // Subscriptions
+ sub_gnss_pose_ = create_subscription(
+ "/awsim/gnss/pose", 1, std::bind(&SensorConverter::on_gnss_pose, this, _1));
+ sub_gnss_pose_cov_ = create_subscription(
+ "/awsim/gnss/pose_with_covariance", 1, std::bind(&SensorConverter::on_gnss_pose_cov, this, _1));
+ sub_imu_ = create_subscription(
+ "/awsim/imu", 1, std::bind(&SensorConverter::on_imu, this, _1));
+ sub_steering_report_ = create_subscription(
+ "/awsim/steering_status", 1, std::bind(&SensorConverter::on_steering_report, this, _1));
+
+ // Publishers
+ pub_gnss_pose_ = create_publisher("/sensing/gnss/pose", 1);
+ pub_gnss_pose_cov_ = create_publisher("/sensing/gnss/pose_with_covariance", 1);
+ pub_imu_ = create_publisher("/sensing/imu/imu_raw", 1);
+ pub_steering_report_ = create_publisher("/vehicle/status/steering_status", 1);
+
+ std::random_device rd;
+ generator_ = std::mt19937(rd());
+ pose_distribution_ = std::normal_distribution(gnss_pose_mean_, gnss_pose_stddev_);
+ pose_cov_distribution_ = std::normal_distribution(gnss_pose_mean_, gnss_pose_stddev_);
+ imu_acc_distribution_ = std::normal_distribution(imu_acc_mean_, imu_acc_stddev_);
+ imu_ang_distribution_ = std::normal_distribution(imu_ang_mean_, imu_ang_stddev_);
+ imu_ori_distribution_ = std::normal_distribution(imu_ori_mean_, imu_ori_stddev_);
+ steering_angle_distribution_ = std::normal_distribution(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(*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(*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(*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(*msg);
+ steering_report_->steering_tire_angle += steering_angle_distribution_(generator_);
+ pub_steering_report_->publish(*steering_report_);
+}
+
+#include
+RCLCPP_COMPONENTS_REGISTER_NODE(SensorConverter)
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp b/aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp
new file mode 100644
index 00000000..a029189b
--- /dev/null
+++ b/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
+#include
+
+#include
+#include
+#include
+#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::SharedPtr sub_gnss_pose_;
+ rclcpp::Subscription::SharedPtr sub_gnss_pose_cov_;
+ rclcpp::Subscription::SharedPtr sub_imu_;
+ rclcpp::Subscription::SharedPtr sub_steering_report_;
+ rclcpp::Publisher::SharedPtr pub_gnss_pose_;
+ rclcpp::Publisher::SharedPtr pub_imu_;
+ rclcpp::Publisher::SharedPtr pub_gnss_pose_cov_;
+ rclcpp::Publisher::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 pose_distribution_;
+ std::normal_distribution pose_cov_distribution_;
+ std::normal_distribution imu_acc_distribution_;
+ std::normal_distribution imu_ang_distribution_;
+ std::normal_distribution imu_ori_distribution_;
+ std::normal_distribution 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_
diff --git a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml
index 4d912de1..14889df6 100644
--- a/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml
+++ b/aichallenge/workspace/src/aichallenge_system/aichallenge_system_launch/launch/mode/awsim.launch.xml
@@ -8,26 +8,6 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-