ROS2话题示例练习3-模拟IMU传感器
·

Hello!大家好,好久不见。今天继续带来一个 ROS2 话题的示例代码,模拟一个 IMU 传感器,帮助大家还有我自己进一步熟悉话题的使用方法。
顺便预告一下,后续大概还有两到三个话题相关的示例,会和大家一起更深入地了解 ROS2 系统。敬请期待!
1. 创建功能包
第一步还是和以前一样,创建一个功能包
cd
~/ros2_ws/srcros2 pkg create --build-type ament_cmake cpp_imu_simulator
--dependencies
rclcpp sensor_msgs
2. 编写发布节点代码
在 cpp_imu_simulator/src/ 目录下创建 imu_simulator.cpp:
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/imu.hpp>
#include <chrono>
#include <random>
#include <cmath>
using namespace std::chrono_literals;
class ImuSimulator : public rclcpp::Node
{
public:
ImuSimulator()
: Node("imu_simulator"),
frame_count_(0),
linear_accel_x_(0.0),
linear_accel_y_(0.0),
linear_accel_z_(9.8),
angular_vel_x_(0.0),
angular_vel_y_(0.0),
angular_vel_z_(0.0)
{
// 创建发布者
publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("imu/data", 10);
// 创建定时器,每20ms发布一次(50Hz)
timer_ = this->create_wall_timer(
20ms, std::bind(&ImuSimulator::publish_imu_data, this));
// 初始化随机数生成器(模拟测量噪声)
std::random_device rd;
gen_ = std::mt19937(rd());
noise_dist_ = std::normal_distribution<>(0.0, 0.05); // 标准差0.05
// 初始化运动参数
init_motion_parameters();
RCLCPP_INFO(this->get_logger(), "IMU Simulator started");
RCLCPP_INFO(this->get_logger(), "Publishing IMU data to topic: imu/data at 50Hz");
}
private:
void init_motion_parameters()
{
// 模拟简单的运动:围绕Z轴缓慢旋转,同时在Z轴方向有轻微摆动
motion_time_ = 0.0;
oscillation_amplitude_ = 0.5; // 摆动幅度
oscillation_frequency_ = 0.5; // 摆动频率(Hz)
rotation_speed_ = 0.2; // 旋转速度(rad/s)
}
void update_motion(double dt)
{
// 更新时间
motion_time_ += dt;
// 计算摆动运动(模拟载体晃动)
double oscillation = oscillation_amplitude_ * std::sin(2 * M_PI * oscillation_frequency_ * motion_time_);
// 更新加速度(模拟重力和运动加速度)
// 假设传感器坐标系相对于重力方向有小角度摆动
double roll = oscillation * 0.1; // 横滚角(小角度)
double pitch = oscillation * 0.15; // 俯仰角(小角度)
// 重力分量在传感器坐标系中的投影
linear_accel_x_ = 9.8 * std::sin(pitch);
linear_accel_y_ = 9.8 * std::sin(roll) * std::cos(pitch);
linear_accel_z_ = 9.8 * std::cos(roll) * std::cos(pitch);
// 添加运动加速度(模拟加减速)
double motion_accel = 0.5 * std::sin(2 * M_PI * 0.3 * motion_time_);
linear_accel_x_ += motion_accel;
// 更新角速度(模拟旋转)
angular_vel_z_ = rotation_speed_ + 0.1 * std::sin(M_PI * 0.5 * motion_time_);
angular_vel_x_ = 0.05 * std::sin(M_PI * 0.8 * motion_time_);
angular_vel_y_ = 0.03 * std::cos(M_PI * 0.6 * motion_time_);
}
void publish_imu_data()
{
// 更新时间步长(0.02秒)
update_motion(0.02);
auto msg = sensor_msgs::msg::Imu();
// 设置消息头
msg.header.stamp = this->now();
msg.header.frame_id = "imu_link";
// 设置方向(四元数)
// 这里简化为单位四元数(无旋转),实际应用中应该根据姿态计算
msg.orientation.x = 0.0;
msg.orientation.y = 0.0;
msg.orientation.z = 0.0;
msg.orientation.w = 1.0;
// 设置方向协方差(表示姿态估计的不确定性)
msg.orientation_covariance = {0.001, 0.0, 0.0,
0.0, 0.001, 0.0,
0.0, 0.0, 0.001};
// 设置角速度(添加噪声)
msg.angular_velocity.x = angular_vel_x_ + noise_dist_(gen_);
msg.angular_velocity.y = angular_vel_y_ + noise_dist_(gen_);
msg.angular_velocity.z = angular_vel_z_ + noise_dist_(gen_);
// 设置角速度协方差
msg.angular_velocity_covariance = {0.01, 0.0, 0.0,
0.0, 0.01, 0.0,
0.0, 0.0, 0.01};
// 设置线性加速度(添加噪声)
msg.linear_acceleration.x = linear_accel_x_ + noise_dist_(gen_);
msg.linear_acceleration.y = linear_accel_y_ + noise_dist_(gen_);
msg.linear_acceleration.z = linear_accel_z_ + noise_dist_(gen_);
// 设置线性加速度协方差
msg.linear_acceleration_covariance = {0.05, 0.0, 0.0,
0.0, 0.05, 0.0,
0.0, 0.0, 0.05};
// 发布消息
publisher_->publish(msg);
// 定期打印状态(每100帧打印一次,约2秒一次)
frame_count_++;
if (frame_count_ % 100 == 0) {
RCLCPP_INFO(this->get_logger(),
"IMU Data - Acc: [%.2f, %.2f, %.2f] m/s², Gyro: [%.3f, %.3f, %.3f] rad/s",
msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z,
msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z);
}
}
// ROS2成员变量
rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
rclcpp::TimerBase::SharedPtr timer_;
// 状态变量
size_t frame_count_;
double linear_accel_x_, linear_accel_y_, linear_accel_z_;
double angular_vel_x_, angular_vel_y_, angular_vel_z_;
// 运动参数
double motion_time_;
double oscillation_amplitude_;
double oscillation_frequency_;
double rotation_speed_;
// 随机数生成器(模拟噪声)
std::mt19937 gen_;
std::normal_distribution<> noise_dist_;
};
int main(int argc, char * argv[])
{
// 初始化ROS2
rclcpp::init(argc, argv);
// 创建节点并进入spin循环
auto node = std::make_shared<ImuSimulator>();
rclcpp::spin(node);
// 关闭ROS2
rclcpp::shutdown();
return 0;
}
3. 完整的CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(cpp_imu_simulator)
# 编译器警告选项
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# 查找依赖包
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(sensor_msgs REQUIRED)
# 添加可执行文件
add_executable(imu_simulator src/imu_simulator.cpp)
# 链接依赖库
ament_target_dependencies(imu_simulator
rclcpp
sensor_msgs
)
# 安装可执行文件
install(TARGETS
imu_simulator
DESTINATION lib/${PROJECT_NAME}
)
# 测试相关(可选)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
# 打包
ament_package()
4. 完整的package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>cpp_imu_simulator</name>
<version>0.0.1</version>
<description>
A ROS2 IMU simulator that publishes synthetic IMU data including acceleration and angular velocity
</description>
<maintainer email="your_email@example.com">Your Name</maintainer>
<license>Apache License 2.0</license>
<!-- 构建工具依赖 -->
<buildtool_depend>ament_cmake</buildtool_depend>
<!-- 构建依赖 -->
<build_depend>rclcpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<!-- 执行依赖 -->
<exec_depend>rclcpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<!-- 导出 -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
5. 编译和运行
# 进入工作空间
cd ~/ros2_ws
# 编译IMU模拟器包
colcon build --packages-select cpp_imu_simulator
# 设置环境变量
source install/setup.bash
# 运行IMU模拟器节点
ros2 run cpp_imu_simulator imu_simulator
6. 验证和测试
以下是常用的话题相关的指令,可以尝试使用,亲自看下会有什么效果
# 进入工作空间
cd ~/ros2_ws
# 编译IMU模拟器包
colcon build --packages-select cpp_imu_simulator
# 设置环境变量
source install/setup.bash
# 运行IMU模拟器节点
ros2 run cpp_imu_simulator imu_simulator
欢迎有问题的小伙伴在留言区提问。
更多推荐
所有评论(0)