超声波（Ultrasonic）模块驱动，因为数据接口简单，低复杂度，在 ROS2 中常也是 以 Topic + 数据结构 的方式实现，也就是我们前面讲的ROS Wrapper方式。后面，我们将这种实现方式成为直接发送Topic 模式的驱动。

1. 什么是超声波？

超声波是一种高频声波，其频率超过了人类听觉的范围。
人类能够听到的声波振动频率大约在每秒 20 次（低沉的轰鸣声）到每秒 20,000 次（尖锐的哨声）之间。然而，超声波的频率超过 20,000 Hz，因此人类无法听到。

2. 什么是超声波传感器？

超声波传感器通常是一种通过测量运行时间（run-time measurement）来确定与物体之间距离的传感器。它无法判断该距离处是什么物体。
如图所示，超声波传感器会发射超声波，这也是其名称的由来。当超声波遇到物体时，会被反射回来。传感器上的振膜接收这些反射的超声波。
由于已知介质中声波的传播速度，传感器可以根据超声波从发射到接收的时间间隔来计算与物体的距离。下面会更具体地解释这个原理。

接下来，我们就以HC-SR04 传感器为例, 来说明超声波模块的ROS2 驱动的开发。

3. HC-SR04 传感器

HC-SR04 超声波测距传感器实际上由两个超声波换能器组成。超声波换能器和超声波传感器是用于产生或感知超声波能量的设备。它们大致可以分为三类：

• 发射器 (Transmitter)：将电信号转换为超声波。
• 接收器 (Receiver)：将超声波转换为电信号。
• 收发器 (Transceiver)：既可以发射也可以接收超声波。
  HC-SR04 传感器就是由一个发射器和一个接收器组成，如图所示。
  
  其中一个换能器作为发射器，将电信号转换为 40 kHz 的超声波脉冲；
  另一个换能器作为接收器，用于接收这些发射的脉冲。

当接收器接收到反射回来的脉冲时，会产生一个输出脉冲，其脉宽与前方物体的距离成正比。

该传感器可实现 2 cm 到 400 cm（约 13 英尺） 的非接触式距离测量，测量精度可达 3 mm。

3.1 技术规格（Technical Specifications）

项目	参数
工作电压（Operating Voltage）	DC 5V
工作电流（Operating Current）	15mA
工作频率（Operating Frequency）	40KHz
最大测量距离（Max Range）	4m
最小测量距离（Min Range）	2cm
测距精度（Ranging Accuracy）	3mm
测量角度（Measuring Angle）	15°
触发输入信号（Trigger Input Signal）	10µs TTL 脉冲
尺寸（Dimension）	45 × 20 × 15 mm

3.2 引脚说明（Pinout）

让我们来看一下 HC-SR04 的引脚定义。

引脚	名称	说明
VCC	电源输入	为 HC-SR04 提供工作电压。可直接连接到树莓派的 5V 电源输出引脚。
Trig（Trigger）	触发引脚	用于触发超声波脉冲。将此引脚拉高（HIGH）持续 10 微秒，传感器便会发出一次超声波脉冲。
Echo	回波引脚	当超声波脉冲发射后，该引脚会被置为高电平，并在接收到回波后恢复为低电平。通过测量 Echo 引脚保持高电平的时间，即可计算出与障碍物之间的距离。
GND	地线	接到host的 GND（地）引脚。

3.3 工作原理（Operating Principle）

整个过程从将 Trigger 引脚拉高 10 微秒 （10µs） 开始。
传感器收到触发信号后，会发出一组 8 个频率为 40kHz 的超声波脉冲。

这组 8 脉冲的波形经过特殊设计，用于让接收端能够区分“自己发出的脉冲”和“环境中的噪声”，从而提高测距精度和抗干扰能力。

随后，这些超声波脉冲会从发射器传播到空气中。与此同时，Echo 引脚被置为高电平，表示开始等待回波信号。

如果脉冲被障碍物反射回来，接收端会检测到回波信号，并立即将 Echo 引脚拉低；

如果在传感器的测量范围内没有检测到反射波，则 Echo 引脚会在 38 毫秒后超时自动拉低。

因此，一个持续时间为 38ms 的高电平脉冲 表示在传感器量程范围内没有检测到障碍物。

如果脉冲被障碍物反射回来, 这就会在 Echo 引脚 上产生一个高电平脉冲，其脉宽取决于回波返回所需的时间。通常，该脉冲宽度会在 150 微秒（µs）到 25 毫秒（ms） 之间变化，对应的距离范围正是传感器的有效检测距离。也就是说：

• 物体越近 → 回波返回时间越短 → 脉宽越窄；
• 物体越远 → 回波返回时间越长 → 脉宽越宽。

3.4 计算距离

接收到的脉冲宽度可用于计算物体与传感器之间的距离。这个计算基于我们在中学学过的一个简单公式：

距离 = 速度 × 时间（Distance = Speed × Time）

我们给出一个例子，已知：
时间（Time） = 500 µs， 声速（Speed of Sound） = 340 m/s = 0.034 cm/µs
代入公式：

距离 = 0.034 cm/µs × 500 µs = 17 cm

但要注意，这个时间包含了 声波往返的时间（从发射到反射回来的总时间）， 所以，实际距离应为这个值的一半：

距离 = (0.034 cm/µs × 500 µs) / 2
距离 = 8.5 cm

3.5 测量物体运动的速度

假设HC-SR04 超声波模块是静止的， 一个物体正在向该模块运动过来。要计算速度，我们只需要在短时间间隔内进行两次距离测量：

distance2 - distance1 = 在该时间段内的位移

如果两次测量的时间间隔是 1 秒，那么速度的计算公式就是：

速度 (cm/s) = (distance2 - distance1) / 1

方向判断

• 当物体远离传感器时，第二次测得的距离比第一次大，速度为正值。
• 当物体靠近传感器时，第二次距离比第一次小，速度为负值。
  因此，显示的速度带有正负号，即可反映物体的运动方向。

至此，我们就讲清楚了这个HC-SR04 超声波模块的所有内容。实际硬件驱动部分，留给读者自行完善。

4. 写超声波模块的Driver

根据以上的讲解，我们知道该模块，主要提供 测量与物体的距离 和速度。由于该模块会持续工作，只要 GPIO 接口驱动它，就可以持续发射/接收超声波，因此，不需要向外提供start， stop接口。

所以，我们的驱动，最终需要将测量的距离 和速度以topic的形式，发布给ROS2 即可。

严格的讲， 模块驱动的topic 名称&消息类型 并不是 ROS 或 ROS 2 的“标准”，而是 驱动开发者自己定义的约定，只要文档清楚即可。

接下来，我们将确定我们的topic 的名称和类型：

• 距离：
  • topic名称：/ultrasonic/distance
  • 消息类型：驱动发布传感器测得的距离信息，通常使用 sensor_msgs/Range 消息类型，因为它是官方定义的标准消息类型，用于表示距离传感器数据。
• 速度：
  • topic名称：/ultrasonic/velocity
  • 消息类型：相对速度（或变化率），一般也可以用 sensor_msgs/Range，为了证明可以自由选择，我们使用Float32，当然，客户也可以自己定义。

4.1 sensor_msgs/msg/Range 详解（重点）

消息类型：sensor_msgs/msg/Range
定义内容：

std_msgs/Header header   # 时间戳和坐标系
uint8 RADIATION_ULTRASOUND=0 #超声波传感器
uint8 RADIATION_INFRARED=1 #红外传感器
uint8 radiation_type     # 超声或红外
float32 field_of_view    # 传感器视场角（弧度）
float32 min_range        # 最小测距
float32 max_range        # 最大测距
float32 range            # 实际测距值

• uint8 radiation_type:
  • 表示传感器类型（辐射类型）。
  • 必须填写
• float32 field_of_view：
  • 传感器的视场角（单位：弧度），表示传感器的测量范围角度大小。
• float32 min_range / float32 max_range：
  • 传感器可测量的最小和最大距离（单位：米）
• float32 range：
  • 传感器测得的实际距离值（单位：米）。
  • 必须填写

4.2 std_msgs/msg/Header 详解（重点）

std_msgs/msg/Header.msg 的定义如下：

# Standard metadata for higher-level stamped data types.
uint32 seq          # 序列号（可选，用于追踪消息顺序）
builtin_interfaces/Time stamp  # 消息的时间戳
string frame_id     # 坐标系名称，例如 "base_link"、"laser_frame"

说明如下：

字段	类型	说明
seq	uint32	消息序列号（可选），每发送一条消息可递增，用于追踪消息顺序
stamp	builtin_interfaces/Time	**必须填写。**消息生成的时间戳，通常为传感器采样时间或处理时间
frame_id	string	强烈推荐填写。 数据所属的坐标系名称（通常是 TF 坐标系），如 "base_link"、"laser"、"ultrasonic"

4.3 std_msgs/Float32 详解（重点）

消息定义如下：

# std_msgs/Float32
float32 data

• data： 核心字段，表示一个单精度浮点数。
• 没有 header、时间戳或 frame_id，纯粹就是一个浮点值。

std_msgs/Float32 本身是一个非常简单的消息类型，它 没有 header，也就没有时间戳和坐标系信息。在很多场景下这是完全可以的，尤其是：

• 数据更新频率很高，时间戳不是关键；
• 只是传递一个单一的数值（如温度、速度、占空比等）；
• 消费者节点不依赖消息的时间顺序或同步。

但是，如果你的应用需要：

• 知道数据采集的时间（比如传感器数据要和其他传感器同步）；
• 关联坐标系（比如距离、位姿等）；
• 多传感器融合或记录日志（需要时间戳对齐），

那么就不推荐用 Float32，而应该用带 std_msgs/Header 的消息类型，例如 sensor_msgs/Range、sensor_msgs/Imu、geometry_msgs/PointStamped 等。

4.4 最终实现

最终代码如下：

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Range
from std_msgs.msg import Float32
from ultrasonic_hc_sr04 import UltrasonicHCSR04  # 你的驱动模块

class UltrasonicHCSR04Wrapper(Node):
    def __init__(self):
        super().__init__('ultrasonic_driver')

        # --- 参数加载 ---
        self.min_range = self.declare_parameter('minimum_range', 0.03).value  # m
        self.max_range = self.declare_parameter('maximum_range', 4.0).value   # m
        fov_deg = self.declare_parameter('field_of_view', 15.0).value         # degree
        self.fov = fov_deg * 0.0174532925199433  # degree → radian

        trigger_pin = self.declare_parameter('trigger_pin', 18).value
        echo_pin = self.declare_parameter('echo_pin', 24).value
        self.rate_hz = self.declare_parameter('publish_rate', 10.0).value

        # --- 驱动实例化 ---
        self.sensor = UltrasonicHCSR04(trigger_pin, echo_pin)

        # --- 发布者 ---
        self.distance_pub = self.create_publisher(Range, '/ultrasonic/distance', 10)
        self.velocity_pub = self.create_publisher(Float32, '/ultrasonic/velocity', 10)

        # --- 定时器（按频率触发）---
        self.create_timer(1.0 / self.rate_hz, self.publish_distance)
        self.create_timer(1.0 / self.rate_hz, self.publish_velocity)

        self.get_logger().info('Ultrasonic driver initialized.')

    def publish_distance(self):
        distance = self.sensor.distance() / 100.0  # cm → m

        msg = Range()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = 'ultrasonic_sensor'
        msg.radiation_type = Range.ULTRASOUND
        msg.field_of_view = self.fov
        msg.min_range = self.min_range
        msg.max_range = self.max_range
        msg.range = distance

        self.distance_pub.publish(msg)
        self.get_logger().debug(f'Distance: {distance:.2f} m')

    def publish_velocity(self):
        velocity = self.sensor.speed()  # 假设 m/s
        msg = Float32()
        msg.data = velocity
        self.velocity_pub.publish(msg)
        self.get_logger().debug(f'Velocity: {velocity:.2f} m/s')

    def destroy_node(self):
        self.get_logger().info('Shutting down ultrasonic driver...')
        super().destroy_node()


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


if __name__ == '__main__':
    main()