Skip to content

Commit

Permalink
feat: add sensor noise (#118)
Browse files Browse the repository at this point in the history
* add launch file template

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* update control_mode_request

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* add awsim relay

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* fix

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* feat: add awsim adapter

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>

* 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 <isamu.takagi@tier4.jp>

* 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 1fd3a86.

* del ekf  additional sim delay

---------

Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Takagi, Isamu <isamu.takagi@tier4.jp>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
  • Loading branch information
3 people authored Sep 26, 2024
1 parent 572bdf3 commit 71fcb7b
Show file tree
Hide file tree
Showing 8 changed files with 247 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -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">
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down
Original file line number Diff line number Diff line change
@@ -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)
Original file line number Diff line number Diff line change
@@ -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_
Original file line number Diff line number Diff line change
Expand Up @@ -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"/>
Expand Down

0 comments on commit 71fcb7b

Please sign in to comment.