from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy

# 场景1：传感器数据（允许丢帧，低延迟优先）
sensor_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,  # 丢几帧不影响
    durability=DurabilityPolicy.VOLATILE,        # 不需要历史数据
    history=HistoryPolicy.KEEP_LAST,
    depth=5                                       # 只保留最近5条
)

# 场景2：控制指令（不允许丢失，保证送达）
control_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,       # 每条指令必须送达
    durability=DurabilityPolicy.VOLATILE,
    history=HistoryPolicy.KEEP_LAST,
    depth=10                                      # 缓存10条指令
)

# 场景3：状态数据（晚加入也能获取最新状态）
status_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,  # 保存最新一条状态
    history=HistoryPolicy.KEEP_LAST,
    depth=1                                        # 只需要最新状态
)

# 查看话题的发布者和订阅者QoS
ros2 topic info /sensor/data --verbose

#!/usr/bin/env python3
"""传感器数据发布者 - 使用BEST_EFFORT QoS，30Hz频率"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Imu

class ImuPublisher(Node):
    def __init__(self):
        super().__init__('imu_publisher')

        # 传感器QoS：允许丢帧，低延迟
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )

        self.pub = self.create_publisher(Imu, 'imu/data', qos)
        self.timer = self.create_timer(0.033, self.publish_imu)  # 约30Hz
        self.count = 0

    def publish_imu(self):
        msg = Imu()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'imu_link'
        # 模拟加速度数据
        msg.linear_acceleration.x = 0.0
        msg.linear_acceleration.y = 0.0
        msg.linear_acceleration.z = 9.81
        self.pub.publish(msg)
        self.count += 1
        if self.count % 30 == 0:
            self.get_logger().info(f'已发布 {self.count} 帧IMU数据')

def main(args=None):
    rclpy.init(args=args)
    node = ImuPublisher()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

#!/usr/bin/env python3
"""IMU数据订阅者 - 匹配BEST_EFFORT QoS"""

import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile, ReliabilityPolicy, DurabilityPolicy, HistoryPolicy
from sensor_msgs.msg import Imu

class ImuSubscriber(Node):
    def __init__(self):
        super().__init__('imu_subscriber')

        # 匹配发布者的QoS配置
        qos = QoSProfile(
            reliability=ReliabilityPolicy.BEST_EFFORT,
            durability=DurabilityPolicy.VOLATILE,
            history=HistoryPolicy.KEEP_LAST,
            depth=5
        )

        self.sub = self.create_subscription(
            Imu, 'imu/data', self.imu_callback, qos
        )
        self.frame_count = 0

    def imu_callback(self, msg):
        self.frame_count += 1
        self.get_logger().info(
            f'收到IMU数据 #{self.frame_count}: '
            f'az={msg.linear_acceleration.z:.2f} m/s²'
        )

def main(args=None):
    rclpy.init(args=args)
    node = ImuSubscriber()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        pass
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()

// 控制指令发布者 - 使用RELIABLE QoS
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <rclcpp/qos.hpp>

class CmdVelPublisher : public rclcpp::Node {
public:
    CmdVelPublisher() : Node("cmd_vel_publisher") {
        // 控制指令QoS：保证送达
        auto qos = rclcpp::QoS(10)
            .reliable()
            .volatile_durability();

        pub_ = this->create_publisher<geometry_msgs::msg::Twist>(
            "cmd_vel", qos);

        // 10Hz发布控制指令
        timer_ = this->create_wall_timer(
            std::chrono::milliseconds(100),
            std::bind(&CmdVelPublisher::publish_cmd, this));
    }

private:
    void publish_cmd() {
        auto msg = geometry_msgs::msg::Twist();
        msg.linear.x = 0.5;   // 前进0.5m/s
        msg.angular.z = 0.1;  // 旋转0.1rad/s
        pub_->publish(msg);
        RCLCPP_INFO(this->get_logger(), "发布指令: vx=%.2f wz=%.2f",
                     msg.linear.x, msg.angular.z);
    }

    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

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

find_package(rclcpp REQUIRED)
find_package(geometry_msgs REQUIRED)

add_executable(cmd_vel_publisher src/cmd_vel_publisher.cpp)
ament_target_dependencies(cmd_vel_publisher rclcpp geometry_msgs)

from std_msgs.msg import Bool, Float64

# 发布开关状态
pub_switch = node.create_publisher(Bool, 'power_switch', 10)
msg = Bool()
msg.data = True
pub_switch.publish(msg)

# 发布温度值
pub_temp = node.create_publisher(Float64, 'temperature', 10)
msg = Float64()
msg.data = 36.5
pub_temp.publish(msg)

from geometry_msgs.msg import Twist, Pose

# 发布速度指令
pub_cmd = node.create_publisher(Twist, 'cmd_vel', 10)
msg = Twist()
msg.linear.x = 0.5    # 前进0.5m/s
msg.angular.z = 0.3   # 旋转0.3rad/s
pub_cmd.publish(msg)

# 发布位姿
pub_pose = node.create_publisher(Pose, 'robot_pose', 10)
msg = Pose()
msg.position.x = 1.0
msg.position.y = 2.0
msg.orientation.w = 1.0  # 无旋转
pub_pose.publish(msg)

from sensor_msgs.msg import LaserScan, JointState

# 订阅激光雷达数据
def scan_callback(msg):
    front_dist = msg.ranges[len(msg.ranges) // 2]  # 正前方距离
    node.get_logger().info(f'前方距离: {front_dist:.2f}m')

sub_scan = node.create_subscription(
    LaserScan, 'scan', scan_callback, 10)

# 发布关节状态
pub_joint = node.create_publisher(JointState, 'joint_states', 10)
msg = JointState()
msg.name = ['joint1', 'joint2']
msg.position = [0.5, 1.0]
pub_joint.publish(msg)

from nav_msgs.msg import Odometry, OccupancyGrid

# 订阅里程计
def odom_callback(msg):
    x = msg.pose.pose.position.x
    y = msg.pose.pose.position.y
    node.get_logger().info(f'当前位置: x={x:.2f}, y={y:.2f}')

sub_odom = node.create_subscription(
    Odometry, 'odom', odom_callback, 10)

# 订阅地图
def map_callback(msg):
    width = msg.info.width
    height = msg.info.height
    resolution = msg.info.resolution
    node.get_logger().info(
        f'地图: {width}x{height}, 分辨率: {resolution}m/cell')

sub_map = node.create_subscription(
    OccupancyGrid, 'map', map_callback,
    QoSProfile(durability=DurabilityPolicy.TRANSIENT_LOCAL,
               reliability=ReliabilityPolicy.RELIABLE,
               depth=1))

# 列出话题
ros2 topic list

# 同时显示消息类型
ros2 topic list -t

/cmd_vel [geometry_msgs/msg/Twist]
/imu/data [sensor_msgs/msg/Imu]
/odom [nav_msgs/msg/Odometry]
/scan [sensor_msgs/msg/LaserScan]

# 查看完整消息
ros2 topic echo /imu/data

# 只看特定字段
ros2 topic echo /imu/data --field linear_acceleration

# 只看一条
ros2 topic echo /imu/data --once

# 基本信息：发布者/订阅者数量
ros2 topic info /imu/data

# 详细信息：包含QoS配置
ros2 topic info /imu/data --verbose

ros2 topic hz /imu/data

average rate: 30.002
    min: 0.033s max: 0.034s std dev: 0.00033s window: 100

ros2 topic bw /camera/image_raw

Subscribed to [/camera/image_raw]
average: 28.15MB/s
    mean: 27.98 min: 26.50 max: 29.80 window: 100

# 发布一条速度指令
ros2 topic pub /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.5, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}"

# 持续发布，频率1Hz
ros2 topic pub -r 1 /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.5}, angular: {z: 0.3}}"

# 只发布一条
ros2 topic pub --once /cmd_vel geometry_msgs/msg/Twist \
  "{linear: {x: 0.0}, angular: {z: 0.0}}"

ros2 topic type /imu/data
# 输出: sensor_msgs/msg/Imu

# 查看消息定义
ros2 interface show sensor_msgs/msg/Imu

# 高频场景优化QoS
fast_qos = QoSProfile(
    reliability=ReliabilityPolicy.BEST_EFFORT,
    durability=DurabilityPolicy.VOLATILE,
    history=HistoryPolicy.KEEP_LAST,
    depth=1  # 只保留最新一条
)

# 发布者：持久化最新一条消息
pub_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
    history=HistoryPolicy.KEEP_LAST,
    depth=1
)
pub = node.create_publisher(Map, 'map', pub_qos)

# 订阅者：也需要TRANSIENT_LOCAL才能收到
sub_qos = QoSProfile(
    reliability=ReliabilityPolicy.RELIABLE,
    durability=DurabilityPolicy.TRANSIENT_LOCAL,
    history=HistoryPolicy.KEEP_LAST,
    depth=1
)
sub = node.create_subscription(Map, 'map', callback, sub_qos)