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)  // 启动节点,开始接收消息和发布消息
相关推荐
AI算力2 小时前
让安全驾驶有“AI”相伴|腾视科技DMS视频监控一体机,守护每一次出行
人工智能·科技·ai·车载终端·ai算力·车载智能终端·车载dvr
泯泷2 小时前
超越基准:Claude Opus 4.5 的 4 个惊人洞察
人工智能
果果软件3 小时前
2026 后台管理系统选型:若依、JeecgBoot、芋道、NocoBase 和 GewuMind Boot 怎么选?
java·人工智能·后端·ai·开源
Csvn3 小时前
刚开始用 WorkBuddy,第一步该做什么?
人工智能
一水鉴天3 小时前
同一与唯一:AppBus 外壳与 DevOps 内核 之1 20261121(元宝)
人工智能·机器学习
Wellijohn3 小时前
一个 Android 老兵的十年开源路:从 167 star 的滑动控件,到本地优先的 AI Agent
android·人工智能·开源
ZONGYUAN-ROOT3 小时前
【API配额】API 配额监控告警不生效?用 Python 监控读数变化量,抓住静态阈值漏掉的「假重置」
人工智能
Wellijohn4 小时前
我把公司内部的 12 个 CRUD 接口交给了 AI Agent,然后省出了两周
人工智能·python·mcp
伞伞悦读5 小时前
【第92期】逻辑回归的分数概率与阈值的实验与排查
人工智能·python·深度学习·机器学习