Vins-Fusion之ROS2（节点创建、订阅者、发布者）(一)

步骤一、创建ROS节点

cpp 复制代码

auto n = rclcpp::Node::make_shared("vins_estimator");

步骤二、创建订阅者

cpp 复制代码

 rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub_imu = NULL;
    if(USE_IMU)
    {
        sub_imu = n->create_subscription<sensor_msgs::msg::Imu>(IMU_TOPIC, imu_qos, imu_callback);
        RCLCPP_INFO(n->get_logger(), "Subscribed to IMU topic: %s with QoS (SystemDefaults + RELIABLE/VOLATILE, depth=2000)", IMU_TOPIC.c_str());
    }
    auto sub_feature = n->create_subscription<sensor_msgs::msg::PointCloud>("/feature_tracker/feature", rclcpp::QoS(rclcpp::KeepLast(2000)), feature_callback);
    auto sub_img0 = n->create_subscription<sensor_msgs::msg::Image>(IMAGE0_TOPIC, image_qos, img0_callback);
    RCLCPP_INFO(n->get_logger(), "Subscribed to IMAGE0 topic: %s with QoS (SystemDefaults + RELIABLE/TRANSIENT_LOCAL, depth=100)", IMAGE0_TOPIC.c_str());
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_img1 = NULL;
    if(STEREO)
    {
        sub_img1 = n->create_subscription<sensor_msgs::msg::Image>(IMAGE1_TOPIC, image_qos, img1_callback);
        RCLCPP_INFO(n->get_logger(), "Subscribed to IMAGE1 topic: %s with QoS (SystemDefaults + RELIABLE/TRANSIENT_LOCAL, depth=100)", IMAGE1_TOPIC.c_str());
    }
    auto sub_restart = n->create_subscription<std_msgs::msg::Bool>("/vins_restart", rclcpp::QoS(rclcpp::KeepLast(100)), restart_callback);
    auto sub_imu_switch = n->create_subscription<std_msgs::msg::Bool>("/vins_imu_switch", rclcpp::QoS(rclcpp::KeepLast(100)), imu_switch_callback);
    auto sub_cam_switch = n->create_subscription<std_msgs::msg::Bool>("/vins_cam_switch", rclcpp::QoS(rclcpp::KeepLast(100)), cam_switch_callback);

解释：

函数原型：

cpp 复制代码

template<typename MessageT>
rclcpp::Subscription<MessageT>::SharedPtr 
create_subscription(
    const std::string &topic_name,
    const rclcpp::QoS &qos,
    std::function<void(typename MessageT::SharedPtr)> callback
)

第一个参数：IMU_TOPIC（话题名称）
类型：const std::string &
作用：要订阅的话题名称
示例："/imu0" 或 "/imu/data"
说明：
话题名会加上节点命名空间
完整话题名：/vins_estimator/imu0（如果节点名是 vins_estimator）

第二个参数：imu_qos（QoS 配置）
类型：const rclcpp::QoS &
作用：定义消息传递的质量策略

第三个参数：imu_callback（回调函数）
类型：std::function<void(typename MessageT::SharedPtr)>
作用：收到消息时调用的回调函数

步骤三、创建发布者

cpp 复制代码

void registerPub(rclcpp::Node::SharedPtr n)
{
    pub_latest_odometry = n->create_publisher<nav_msgs::msg::Odometry>("imu_propagate", 1000);
    pub_path = n->create_publisher<nav_msgs::msg::Path>("path", 1000);
    pub_odometry = n->create_publisher<nav_msgs::msg::Odometry>("odometry", 1000);
    pub_point_cloud = n->create_publisher<sensor_msgs::msg::PointCloud>("point_cloud", 1000);
    pub_margin_cloud = n->create_publisher<sensor_msgs::msg::PointCloud>("margin_cloud", 1000);
    pub_key_poses = n->create_publisher<visualization_msgs::msg::Marker>("key_poses", 1000);
    pub_camera_pose = n->create_publisher<nav_msgs::msg::Odometry>("camera_pose", 1000);
    pub_camera_pose_visual = n->create_publisher<visualization_msgs::msg::MarkerArray>("camera_pose_visual", 1000);
    pub_keyframe_pose = n->create_publisher<nav_msgs::msg::Odometry>("keyframe_pose", 1000);
    pub_keyframe_point = n->create_publisher<sensor_msgs::msg::PointCloud>("keyframe_point", 1000);
    pub_extrinsic = n->create_publisher<nav_msgs::msg::Odometry>("extrinsic", 1000);
    pub_image_track = n->create_publisher<sensor_msgs::msg::Image>("image_track", 1000);

    cameraposevisual.setScale(0.1);
    cameraposevisual.setLineWidth(0.01);
}

整体关系图：

cpp 复制代码

rclcpp::init(argc, argv)
    ↓
auto n = rclcpp::Node::make_shared("vins_estimator")
    ↓
    ┌─────────────────────────────────────┐
    │  节点对象 n (vins_estimator)         │
    │                                      │
    │  ┌──────────────────────────────┐  │
    │  │  发布者 (通过 n 创建)          │  │
    │  │  - pub_odometry               │  │
    │  │  - pub_path                    │  │
    │  │  - pub_point_cloud             │  │
    │  │  - pub_camera_pose             │  │
    │  │  - ... (共12个发布者)          │  │
    │  └──────────────────────────────┘  │
    │                                      │
    │  ┌──────────────────────────────┐  │
    │  │  订阅者 (通过 n 创建)          │  │
    │  │  - sub_imu                    │  │
    │  │  - sub_feature                │  │
    │  │  - sub_img0                   │  │
    │  │  - sub_img1                   │  │
    │  │  - sub_restart                │  │
    │  │  - sub_imu_switch             │  │
    │  │  - sub_cam_switch             │  │
    │  └──────────────────────────────┘  │
    └─────────────────────────────────────┘
    ↓
rclcpp::spin(n)  // 启动节点，开始接收消息和发布消息