ROS2系列教程：话题Topic通信（下）自定义消息与周期

1
定义 .msg/.srv 文件（接口包）
2
→ colcon build 生成类型代码
3
→ 发布者/订阅者 include/import 使用
4
→ ros2 run 运行验证

1
# py_pkg/robot_publisher.py —— 发布自定义消息
2
import rclpy
3
from rclpy.node import Node
4
from my_interfaces.msg import RobotStatus
5
from builtin_interfaces.msg import Time
6

7
class RobotPublisher(Node):
8
    def __init__(self):
9
        super().__init__('robot_publisher')
10
        # 发布自定义消息类型
11
        self.pub = self.create_publisher(RobotStatus, 'robot_status', 10)
12
        self.timer = self.create_timer(1.0, self.publish_status)
13
        self.battery = 1.0
14

15
    def publish_status(self):
16
        msg = RobotStatus()
17
        msg.robot_name = 'turtle_01'
18
        self.battery -= 0.01
19
        msg.battery_level = self.battery
20
        msg.status_code = 1
21
        # 时间戳：取当前系统时间（秒+纳秒）
22
        msg.stamp = Time()
23
        now = self.get_clock().now()
24
        msg.stamp.sec = now.seconds_nanoseconds()[0]
25
        msg.stamp.nanosec = now.seconds_nanoseconds()[1]
26
        self.pub.publish(msg)
27
        self.get_logger().info(
28
            f'发布: {msg.robot_name} 电量={msg.battery_level:.2f}')
29

30
def main():
31
    rclpy.init()
32
    node = RobotPublisher()
33
    rclpy.spin(node)
34
    node.destroy_node()
35
    rclpy.shutdown()
36

37
if __name__ == '__main__':
38
    main()

1
// cpp_pkg/src/robot_subscriber.cpp —— 订阅自定义消息
2
#include "rclcpp/rclcpp.hpp"
3
#include "my_interfaces/msg/robot_status.hpp"
4

5
class RobotSubscriber : public rclcpp::Node
6
{
7
public:
8
  RobotSubscriber() : Node("robot_subscriber")
9
  {
10
    sub_ = this->create_subscription<my_interfaces::msg::RobotStatus>(
11
      "robot_status", 10,
12
      std::bind(&RobotSubscriber::on_status, this, std::placeholders::_1));
13
  }
14

15
private:
16
  void on_status(const my_interfaces::msg::RobotStatus::SharedPtr msg)
17
  {
18
    // 访问嵌套的 stamp 字段
19
    auto sec = msg->stamp.sec;
20
    RCLCPP_INFO(this->get_logger(),
21
      "收到: %s 电量=%.2f 状态=%d sec=%ld",
22
      msg->robot_name.c_str(), msg->battery_level,
23
      msg->status_code, sec);
24
  }
25

26
  rclcpp::Subscription<my_interfaces::msg::RobotStatus>::SharedPtr sub_;
27
};
28

29
int main(int argc, char** argv)
30
{
31
  rclcpp::init(argc, argv);
32
  rclcpp::spin(std::make_shared<RobotSubscriber>());
33
  rclcpp::shutdown();
34
  return 0;
35
}

1
<depend>my_interfaces</depend>

1
find_package(my_interfaces REQUIRED)
2
ament_target_dependencies(robot_subscriber my_interfaces)

1
sensor_msgs/msg/LaserScan      # 激光雷达：ranges 数组 + angle_min/max
2
sensor_msgs/msg/Image          # 图像：宽高 + 像素编码 + data
3
geometry_msgs/msg/Twist        # 速度指令：linear + angular
4
geometry_msgs/msg/PoseStamped  # 带时间戳位姿（导航目标）
5
nav_msgs/msg/Odometry          # 里程计：位姿 + 速度 + 协方差
6
std_msgs/msg/Header            # 通用头：时间戳 + frame_id

1
# 给消息填 Header 的标准姿势
2
msg.header.stamp = self.get_clock().now().to_msg()
3
msg.header.frame_id = 'map'

1
# 10Hz 发布（0.1 秒周期）
2
self.timer = self.create_timer(0.1, self.publish_cb)

1
# 2Hz 发布：定时器 10Hz 跑，每 5 次发布一次
2
class ThrottledNode(Node):
3
    def __init__(self):
4
        super().__init__('throttled_node')
5
        self.pub = self.create_publisher(String, 'throttled', 10)
6
        self.timer = self.create_timer(0.1, self.tick)  # 10Hz
7
        self.counter = 0
8

9
    def tick(self):
10
        self.counter += 1
11
        if self.counter % 5 == 0:      # 每 5 拍 → 2Hz
12
            msg = String()
13
            msg.data = 'every 0.5s'
14
            self.pub.publish(msg)

1
# dynamic_rate.py —— 动态调整发布周期
2
import rclpy
3
from rclpy.node import Node
4
from std_msgs.msg import Float64
5

6
class DynamicRateNode(Node):
7
    def __init__(self):
8
        super().__init__('dynamic_rate_node')
9
        self.pub = self.create_publisher(Float64, 'speed', 10)
10
        self.rate = 1.0                # 当前周期（秒）
11
        self.timer = self.create_timer(self.rate, self.tick)
12
        self.count = 0
13

14
    def tick(self):
15
        self.count += 1
16
        msg = Float64()
17
        msg.data = float(self.count)
18
        self.pub.publish(msg)
19
        # 每 5 拍把周期在 0.5~2.0 秒之间循环
20
        if self.count % 5 == 0:
21
            self.rate = 2.0 if self.rate < 1.5 else 0.5
22
            self.timer.cancel()
23
            self.timer = self.create_timer(self.rate, self.tick)
24
            self.get_logger().info(f'周期调整为 {self.rate}s')
25

26
def main():
27
    rclpy.init()
28
    node = DynamicRateNode()
29
    rclpy.spin(node)
30
    node.destroy_node()
31
    rclpy.shutdown()
32

33
if __name__ == '__main__':
34
    main()

1
# py_pkg/high_freq_odom.py —— 100Hz 里程计仿真
2
import rclpy
3
from rclpy.node import Node
4
from nav_msgs.msg import Odometry
5
from geometry_msgs.msg import Point, Quaternion
6

7
class HighFreqOdom(Node):
8
    def __init__(self):
9
        super().__init__('high_freq_odom')
10
        # 100Hz：0.01 秒周期
11
        self.pub = self.create_publisher(Odometry, 'odom', 50)  # 队列深一点
12
        self.timer = self.create_timer(0.01, self.publish_odom)
13
        self.x = 0.0
14
        self.t = 0
15

16
    def publish_odom(self):
17
        self.t += 1
18
        self.x += 0.001   # 每拍前进 1mm
19
        msg = Odometry()
20
        msg.header.stamp = self.get_clock().now().to_msg()
21
        msg.header.frame_id = 'odom'
22
        msg.child_frame_id = 'base_link'
23
        msg.pose.pose.position = Point(x=self.x, y=0.0, z=0.0)
24
        msg.pose.pose.orientation = Quaternion(x=0., y=0., z=0., w=1.)
25
        # 简单协方差（第 1 个元素为位置方差）
26
        msg.pose.covariance[0] = 0.0001
27
        self.pub.publish(msg)
28

29
def main():
30
    rclpy.init()
31
    node = HighFreqOdom()
32
    rclpy.spin(node)
33
    node.destroy_node()
34
    rclpy.shutdown()
35

36
if __name__ == '__main__':
37
    main()

1
sensor_node(100Hz 发 /odom + 10Hz 发 /scan)
2
    ↓
3
logger_node(订阅两者，校验时间戳，周期打印)
4
    ↓
5
ros2 bag record（录制全程）
6
    ↓
7
回放验证