From 71fcb7b3326dbb6c8dad20f059d83393f1d8d26b Mon Sep 17 00:00:00 2001 From: knorrrr <99851410+knorrrr@users.noreply.github.com> Date: Thu, 26 Sep 2024 20:38:21 +0900 Subject: [PATCH] feat: add sensor noise (#118) * add launch file template Signed-off-by: Takagi, Isamu * update control_mode_request Signed-off-by: Takagi, Isamu * add awsim relay Signed-off-by: Takagi, Isamu * fix Signed-off-by: Takagi, Isamu * feat: add awsim adapter Signed-off-by: Takagi, Isamu * wip * chune imu gnss * add steering * chore fix type * use thread * del relay topic * re-tune param * fix timestamp * fix merge diff Signed-off-by: Takagi, Isamu * Update aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * Update aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> * change gnss delay to 300ms * Revert "change gnss delay to 300ms" This reverts commit 1fd3a862d0b47aeded424e2690d0ed60eafee788. * del ekf additional sim delay --------- Signed-off-by: Takagi, Isamu Co-authored-by: Takagi, Isamu Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> --- .../launch/reference.launch.xml | 3 +- .../aichallenge_awsim_adapter/CMakeLists.txt | 7 + .../config/sensor_converter.param.yaml | 18 +++ .../aichallenge_awsim_adapter.launch.xml | 3 + .../aichallenge_awsim_adapter/package.xml | 4 +- .../src/sensor_converter.cpp | 134 ++++++++++++++++++ .../src/sensor_converter.hpp | 81 +++++++++++ .../launch/mode/awsim.launch.xml | 20 --- 8 files changed, 247 insertions(+), 23 deletions(-) create mode 100644 aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/config/sensor_converter.param.yaml create mode 100644 aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.cpp create mode 100644 aichallenge/workspace/src/aichallenge_system/aichallenge_awsim_adapter/src/sensor_converter.hpp 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 @@ - - - - - - - - - - - - - - - - - - - -