图片

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

欢迎有问题的小伙伴在留言区提问。

Logo

聚焦RDK与具身智能,汇集机器人开发教程、开源案例和技术交流

更多推荐