From 1810bf67fe30647d767a4bb0f20b817201201351 Mon Sep 17 00:00:00 2001 From: kekeliu Date: Fri, 2 Sep 2022 20:48:59 +0800 Subject: [PATCH 1/3] support imu data simulation --- config/simulation/imu_simulator.yaml | 14 +++ src/ImuSimulator.cpp | 136 +++++++++++++++++++++++++++ 2 files changed, 150 insertions(+) create mode 100644 config/simulation/imu_simulator.yaml create mode 100644 src/ImuSimulator.cpp diff --git a/config/simulation/imu_simulator.yaml b/config/simulation/imu_simulator.yaml new file mode 100644 index 0000000..e05d258 --- /dev/null +++ b/config/simulation/imu_simulator.yaml @@ -0,0 +1,14 @@ +#Accelerometer +accelerometer_noise_density: 0.0025019929573561175 +accelerometer_random_walk: 6.972435158192731e-05 +accelerometer_bias: 0.007 + +#Gyroscope +gyroscope_noise_density: 0.0001888339269965301 +gyroscope_random_walk: 2.5565313322052523e-06 +gyroscope_bias: 0.006 + +rostopic: '/sensors/imu' +update_rate: 400.0 + +sequence_time: 11000 diff --git a/src/ImuSimulator.cpp b/src/ImuSimulator.cpp new file mode 100644 index 0000000..bec6681 --- /dev/null +++ b/src/ImuSimulator.cpp @@ -0,0 +1,136 @@ +/** + * @file ImuImulator.cpp + * @brief Tool to simulate imu data, ref: + * https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model. + * @author Rick Liu + */ + +// std, eigen and boost +#include +#include +#include +#include +#include +#include + +// ROS +#include +#include +#include + +#include "allan_variance_ros/yaml_parsers.hpp" + +using Vec3d = Eigen::Vector3d; + +Vec3d RandomNormalDistributionVector(double sigma) { + static boost::mt19937 rng; + static boost::normal_distribution<> nd(0, 1); + return {sigma * nd(rng), sigma * nd(rng), sigma * nd(rng)}; +} + +template void FillROSVector3d(const S &from, T &to) { + to.x = from.x(); + to.y = from.y(); + to.z = from.z(); +} + +class ImuSimulator { +public: + ImuSimulator(std::string config_file, std::string output_path) + : bag_output_(output_path, rosbag::bagmode::Write) { + auto yaml_config = loadYamlFile(config_file); + + get(yaml_config, "accelerometer_noise_density", + accelerometer_noise_density_); + get(yaml_config, "accelerometer_random_walk", accelerometer_random_walk_); + get(yaml_config, "accelerometer_bias_init", accelerometer_bias_init_); + + get(yaml_config, "gyroscope_noise_density", gyroscope_noise_density_); + get(yaml_config, "gyroscope_random_walk", gyroscope_random_walk_); + get(yaml_config, "gyroscope_bias_init", gyroscope_bias_init_); + + get(yaml_config, "rostopic", rostopic_); + get(yaml_config, "update_rate", update_rate_); + get(yaml_config, "sequence_time", sequence_time_); + } + + virtual ~ImuSimulator() { bag_output_.close(); } + + void run() { + ROS_INFO_STREAM("Generating imu data ..."); + + double dt = 1 / update_rate_; + + // clang-format off + ros::Time start_time = ros::Time::now(); + Vec3d accelerometer_bias = Vec3d::Constant(accelerometer_bias_init_); + Vec3d gyroscope_bias = Vec3d::Constant(gyroscope_bias_init_); + Vec3d accelerometer_real = Vec3d::Zero(); + Vec3d gyroscope_real = Vec3d::Zero(); + + + for (int64_t i = 0; i < sequence_time_ * update_rate_; ++i) { + // Reference: https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model + accelerometer_bias += RandomNormalDistributionVector(accelerometer_random_walk_) * sqrt(dt); + gyroscope_bias += RandomNormalDistributionVector(gyroscope_random_walk_) * sqrt(dt); + + Vec3d acc_measure = accelerometer_real + accelerometer_bias + RandomNormalDistributionVector(accelerometer_noise_density_) / sqrt(dt); + Vec3d gyro_measure = gyroscope_real + gyroscope_bias + RandomNormalDistributionVector(gyroscope_noise_density_) / sqrt(dt); + + sensor_msgs::Imu msg; + msg.header.stamp = start_time + ros::Duration(1, 0) * (i / update_rate_); + msg.header.seq = i; + FillROSVector3d(acc_measure, msg.linear_acceleration); + FillROSVector3d(gyro_measure, msg.angular_velocity); + + bag_output_.write(rostopic_, msg.header.stamp, msg); + } + // clang-format on + + ROS_INFO_STREAM("Finished generating data. "); + } + +private: + // ROS + rosbag::Bag bag_output_; + +private: + double accelerometer_noise_density_; + double accelerometer_random_walk_; + double accelerometer_bias_init_; + + double gyroscope_noise_density_; + double gyroscope_random_walk_; + double gyroscope_bias_init_; + + std::string rostopic_; + double update_rate_; + + double sequence_time_; +}; + +int main(int argc, char **argv) { + ros::init(argc, argv, "allan_variance_ros"); + ros::NodeHandle nh; + std::string rosbag_filename; + std::string config_file; + + if (argc >= 2) { + rosbag_filename = argv[1]; + config_file = argv[2]; + ROS_INFO_STREAM("Bag filename = " << rosbag_filename); + ROS_INFO_STREAM("Config File = " << config_file); + } else { + ROS_WARN("Rosbag filename and/or config file not provided!"); + } + + auto start = std::clock(); + + ImuSimulator simulator(config_file, rosbag_filename); + ROS_INFO_STREAM("Imu simulator constructed"); + simulator.run(); + + double durationTime = (std::clock() - start) / (double)CLOCKS_PER_SEC; + ROS_INFO("Total computation time: %f s", durationTime); + return 0; +} From 706cf1de37e403b4c558984bc5548a6162a8be97 Mon Sep 17 00:00:00 2001 From: kekeliu Date: Sat, 3 Sep 2022 20:28:53 +0800 Subject: [PATCH 2/3] fix configuration typos --- config/simulation/imu_simulator.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/simulation/imu_simulator.yaml b/config/simulation/imu_simulator.yaml index e05d258..c1fbc7e 100644 --- a/config/simulation/imu_simulator.yaml +++ b/config/simulation/imu_simulator.yaml @@ -1,12 +1,12 @@ #Accelerometer accelerometer_noise_density: 0.0025019929573561175 accelerometer_random_walk: 6.972435158192731e-05 -accelerometer_bias: 0.007 +accelerometer_bias_init: 0.007 #Gyroscope gyroscope_noise_density: 0.0001888339269965301 gyroscope_random_walk: 2.5565313322052523e-06 -gyroscope_bias: 0.006 +gyroscope_bias_init: 0.006 rostopic: '/sensors/imu' update_rate: 400.0 From e379350b98f1815c89db3fdeecec735ff462978f Mon Sep 17 00:00:00 2001 From: kekeliu Date: Tue, 18 Oct 2022 21:06:18 +0800 Subject: [PATCH 3/3] add executable imu_simulator to CMakeLists.txt --- CMakeLists.txt | 4 ++++ src/ImuSimulator.cpp | 5 +++-- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 869f713..8804922 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -43,3 +43,7 @@ add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EX add_executable(allan_variance src/allan_variance.cpp) target_link_libraries(allan_variance ${PROJECT_NAME} ) + +add_executable(imu_simulator src/ImuSimulator.cpp) + +target_link_libraries(imu_simulator ${PROJECT_NAME} ) diff --git a/src/ImuSimulator.cpp b/src/ImuSimulator.cpp index bec6681..8fb3b91 100644 --- a/src/ImuSimulator.cpp +++ b/src/ImuSimulator.cpp @@ -1,5 +1,5 @@ /** - * @file ImuImulator.cpp + * @file ImuSimulator.cpp * @brief Tool to simulate imu data, ref: * https://github.com/ethz-asl/kalibr/wiki/IMU-Noise-Model. * @author Rick Liu @@ -121,7 +121,8 @@ int main(int argc, char **argv) { ROS_INFO_STREAM("Bag filename = " << rosbag_filename); ROS_INFO_STREAM("Config File = " << config_file); } else { - ROS_WARN("Rosbag filename and/or config file not provided!"); + ROS_WARN("Usage: ./imu_simulator /path/to/output/bag_filename /path/to/simulation/config_filename"); + return 1; } auto start = std::clock();