ROS2 Humble(C++)新手求助：订阅话题并实现指定发布功能

核心思路 #

两个话题都是1Hz发布相同数值，需要先分别缓存两个话题的最新数据，等两者都就绪后，遍历数组找出所有两元素之和等于目标数值的索引对，最后把这些索引对发布出去。

步骤拆解与代码示例 #

1. 创建ROS2包 #

先建一个带C++支持的包，包名就叫sum_index_finder：

ros2 pkg create --build-type ament_cmake sum_index_finder --dependencies rclcpp std_msgs

2. 编写节点代码 #

在包的src目录下新建sum_index_finder_node.cpp，代码如下：

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int8_multi_array.hpp"
#include "std_msgs/msg/int8.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"

class SumIndexFinder : public rclcpp::Node
{
public:
    SumIndexFinder() : Node("sum_index_finder_node")
    {
        // 订阅Int8数组话题
        array_sub_ = this->create_subscription<std_msgs::msg::Int8MultiArray>(
            "int8_array_topic", 10,
            std::bind(&SumIndexFinder::array_callback, this, std::placeholders::_1));
        
        // 订阅目标数值话题
        target_sub_ = this->create_subscription<std_msgs::msg::Int8>(
            "int8_target_topic", 10,
            std::bind(&SumIndexFinder::target_callback, this, std::placeholders::_1));
        
        // 发布匹配到的索引对话题
        index_pub_ = this->create_publisher<std_msgs::msg::Int32MultiArray>(
            "matched_index_pairs", 10);
    }

private:
    void array_callback(const std_msgs::msg::Int8MultiArray::SharedPtr msg)
    {
        latest_array_ = msg->data;
        process_data();
    }

    void target_callback(const std_msgs::msg::Int8::SharedPtr msg)
    {
        latest_target_ = msg->data;
        has_target_ = true;
        process_data();
    }

    void process_data()
    {
        // 确保数组和目标数值都已获取到最新数据
        if (latest_array_.empty() || !has_target_)
        {
            return;
        }

        std_msgs::msg::Int32MultiArray index_msg;
        index_msg.data.clear();

        // 遍历数组找符合条件的索引对（i<j避免重复输出同一对）
        for (size_t i = 0; i < latest_array_.size(); ++i)
        {
            for (size_t j = i + 1; j < latest_array_.size(); ++j)
            {
                if (latest_array_[i] + latest_array_[j] == latest_target_)
                {
                    index_msg.data.push_back(static_cast<int32_t>(i));
                    index_msg.data.push_back(static_cast<int32_t>(j));
                }
            }
        }

        // 发布结果
        index_pub_->publish(index_msg);
    }

    rclcpp::Subscription<std_msgs::msg::Int8MultiArray>::SharedPtr array_sub_;
    rclcpp::Subscription<std_msgs::msg::Int8>::SharedPtr target_sub_;
    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr index_pub_;
    
    std::vector<int8_t> latest_array_;
    int8_t latest_target_;
    bool has_target_ = false;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<SumIndexFinder>());
    rclcpp::shutdown();
    return 0;
}

3. 修改CMakeLists.txt #

在包的CMakeLists.txt里添加编译配置：

add_executable(sum_index_finder_node src/sum_index_finder_node.cpp)
ament_target_dependencies(sum_index_finder_node rclcpp std_msgs)

install(TARGETS
  sum_index_finder_node
  DESTINATION lib/${PROJECT_NAME})

4. 编译与运行 #

• 在工作空间根目录编译包：

colcon build --packages-select sum_index_finder

• 激活环境：

source install/setup.bash

• 运行节点：

ros2 run sum_index_finder sum_index_finder_node

• 用以下命令测试发布数据：

# 发布数组话题（1Hz）
ros2 topic pub --rate 1 /int8_array_topic std_msgs/msg/Int8MultiArray "{data: [1, 2, 3, 4, 5]}"
# 发布目标数值话题（比如找和为5的索引对）
ros2 topic pub --rate 1 /int8_target_topic std_msgs/msg/Int8 "{data: 5}"
# 查看结果
ros2 topic echo /matched_index_pairs

关键注意点 #

• 数据同步：两个话题独立发布，必须等两者都有最新数据再处理，避免用旧数据匹配新数值。
• 索引去重：代码里用i<j的遍历逻辑，避免重复输出(0,3)和(3,0)这类同一对索引，如果任务允许重复，去掉j=i+1的限制即可。
• 消息类型：结果用std_msgs/Int32MultiArray，每两个元素代表一组索引对，方便接收端解析。

类似实现参考方向 #

可以参考ROS2官方的C++订阅发布示例，核心逻辑都是：

1. 继承rclcpp::Node自定义节点
2. 初始化订阅者、发布者
3. 回调函数缓存数据
4. 处理数据并发布结果

内容的提问来源于stack exchange，提问作者unknown