第4章：ROS2 核心概念¶

#include <rclcpp/rclcpp.hpp>

// 继承 rclcpp::Node，创建你自己的节点类
class ArmorDetector : public rclcpp::Node
{
public:
    ArmorDetector() : Node("armor_detector")  // (1)!
    {
        RCLCPP_INFO(this->get_logger(), "自瞄检测器启动！");
    }
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);                    // (2)!
    auto node = std::make_shared<ArmorDetector>();
    rclcpp::spin(node);                           // (3)!
    rclcpp::shutdown();                           // (4)!
    return 0;
}

#include "rm_interfaces/msg/armors.hpp"  // 自定义消息类型

class ArmorDetector : public rclcpp::Node
{
public:
    ArmorDetector() : Node("armor_detector")
    {
        // 创建一个发布者，往 "/detector/armors" 话题上发消息
        publisher_ = this->create_publisher<rm_interfaces::msg::Armors>(
            "/detector/armors",  // (1)!
            10                    // (2)!
        );
    }

private:
    rclcpp::Publisher<rm_interfaces::msg::Armors>::SharedPtr publisher_;
};

// 在某个函数中（比如检测到装甲板之后）
auto msg = rm_interfaces::msg::Armors();
msg.header.stamp = this->now();
msg.header.frame_id = "camera_optical_frame";
// ... 填充装甲板数据 ...

publisher_->publish(msg);

class ArmorTracker : public rclcpp::Node
{
public:
    ArmorTracker() : Node("armor_tracker")
    {
        // 订阅 "/detector/armors" 话题
        subscription_ = this->create_subscription<rm_interfaces::msg::Armors>(
            "/detector/armors",  // (1)!
            10,                  // (2)!
            std::bind(&ArmorTracker::armors_callback, this, std::placeholders::_1)  // (3)!
        );
    }

private:
    // 每当收到新消息，这个函数就会被自动调用
    void armors_callback(const rm_interfaces::msg::Armors::SharedPtr msg)
    {
        RCLCPP_INFO(this->get_logger(), "收到 %zu 个装甲板", msg->armors.size());
        // 在这里做跟踪处理 ...
    }

    rclcpp::Subscription<rm_interfaces::msg::Armors>::SharedPtr subscription_;
};

// 创建一条 Armors 消息
auto armors_msg = rm_interfaces::msg::Armors();
armors_msg.header.stamp = this->now();
armors_msg.header.frame_id = "camera_optical_frame";

// 创建一个 Armor，填充数据
auto armor = rm_interfaces::msg::Armor();
armor.number = "3";
armor.color = "blue";
armor.position.x = 0.5;   // 相机坐标系下，单位：米
armor.position.y = -0.1;
armor.position.z = 1.2;
armor.yaw = 0.3;

// 放入数组
armors_msg.armors.push_back(armor);

// 发布
publisher_->publish(armors_msg);

// 在回调函数中接收消息
void armors_callback(const rm_interfaces::msg::Armors::SharedPtr msg)
{
    for (const auto & armor : msg->armors) {
        RCLCPP_INFO(this->get_logger(),
            "检测到 %s 色 %s 号装甲板，距离 %.2f 米",
            armor.color.c_str(),
            armor.number.c_str(),
            armor.position.z);
    }
}

#include "std_srvs/srv/trigger.hpp"

class ArmorTracker : public rclcpp::Node
{
public:
    ArmorTracker() : Node("armor_tracker")
    {
        // 创建服务：服务名 "/tracker/reset"，收到请求时调用 reset_callback
        reset_service_ = this->create_service<std_srvs::srv::Trigger>(
            "/tracker/reset",
            std::bind(&ArmorTracker::reset_callback,
                      this,
                      std::placeholders::_1,
                      std::placeholders::_2)
        );
    }

private:
    void reset_callback(
        const std_srvs::srv::Trigger::Request::SharedPtr request,   // (1)!
        std_srvs::srv::Trigger::Response::SharedPtr response)       // (2)!
    {
        (void)request;  // Trigger 服务的 Request 没有字段
        // 执行重置逻辑
        tracker_.reset();
        response->success = true;
        response->message = "跟踪器已重置";
        RCLCPP_INFO(this->get_logger(), "跟踪器已重置！");
    }

    rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
};

// 从另一个节点中调用服务
auto client = this->create_client<std_srvs::srv::Trigger>("/tracker/reset");

auto request = std::make_shared<std_srvs::srv::Trigger::Request>();

// 异步调用
auto result = client->async_send_request(request);

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"

class ArmorTracker : public rclcpp::Node
{
public:
    ArmorTracker() : Node("armor_tracker")
    {
        // 创建 TF2 缓冲区和监听器
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    }

private:
    void armors_callback(const rm_interfaces::msg::Armors::SharedPtr msg)
    {
        // 查询从 camera_optical_frame 到 odom 的变换
        geometry_msgs::msg::TransformStamped transform;
        try {
            transform = tf_buffer_->lookupTransform(
                "odom",                   // 目标坐标系
                "camera_optical_frame",   // 源坐标系
                msg->header.stamp,        // 使用消息的时间戳
                std::chrono::milliseconds(50)  // 最大等待时间
            );
        } catch (tf2::TransformException & ex) {
            RCLCPP_WARN(this->get_logger(), "TF2 变换失败: %s", ex.what());
            return;
        }

        // 现在可以用这个变换，把装甲板坐标从相机坐标系转换到 odom 坐标系
        // ...
    }

    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_components/register_node_macro.hpp"

class ArmorDetector : public rclcpp::Node
{
public:
    // 注意：构造函数必须接受 NodeOptions 参数
    explicit ArmorDetector(const rclcpp::NodeOptions & options)
    : Node("armor_detector", options)
    {
        // 和普通节点一样的初始化逻辑
    }
};

// 注册为 Component（替代 main 函数中的 rclcpp::init + spin）
RCLCPP_COMPONENTS_REGISTER_NODE(ArmorDetector)  // (1)!

// 图像话题 —— 使用 SensorDataQoS（BEST_EFFORT）
subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
    "/camera/image",
    rclcpp::SensorDataQoS(),  // (1)!
    callback
);

// 状态话题 —— 使用默认的 RELIABLE（不指定就是默认）
publisher_ = this->create_publisher<rm_interfaces::msg::Target>(
    "/tracker/target",
    10  // (2)!
);