Add ultrasonic sensor

README.md

@@ -100,6 +100,55 @@ The node drives the pan and tilt servos over **I²C bus 1** (device address `0x2
 
 ---
 
+### Ultrasonic range sensor
+
+```
+                        ┌──────────────────────────────────────┐
+                        │          UltrasonicNode              │
+                        │                                      │
+                        │  Sensor off when no subscribers      │
+                        │  Sensor on  when subscribers > 0     │
+                        │  1 s warm-up after power-on          │
+                        │                                      │
+                        │         ▼                            │
+                        │   raspbot_v2_interface               │
+                        │   I²C bus 1, addr 0x2B               │
+                        │         ▼                            │
+                        │   /dev/i2c-1 ──────> HC-SR04 sensor  │
+                        │                                      │
+ /ultrasonic/range <────│  Range @ configurable rate           │
+ (sensor_msgs/Range)    │  radiation_type = ULTRASOUND         │
+                        │  range in metres (REP-117)           │
+                        └──────────────────────────────────────┘
+```
+
+#### Topics
+
+| Topic | Direction | Type | Description |
+|---|---|---|---|
+| `/ultrasonic/range` | Published | `sensor_msgs/Range` | Distance in metres. `+inf` when beyond max range, `-inf` when closer than min range (REP-117). Only published while subscribers are connected. |
+
+#### Parameters
+
+| Parameter | Default | Description |
+|---|---|---|
+| `publish_rate_hz` | `10.0` | Sensor poll and publish rate |
+| `frame_id` | `'ultrasonic'` | `header.frame_id` on published messages |
+| `min_range_m` | `0.02` | Minimum valid range in metres |
+| `max_range_m` | `4.0` | Maximum valid range in metres |
+| `field_of_view` | `0.2618` | Sensor cone width in radians (~15°) |
+| `warmup_s` | `1.0` | Seconds to wait after powering the sensor on before publishing |
+
+#### Verifying range readings
+
+```bash
+ros2 topic echo /ultrasonic/range
+```
+
+The sensor will activate automatically when this command runs and deactivate when it is stopped.
+
+---
+
 ## Setting up the robot
 
 ### 1. Flash Raspberry Pi OS
@@ -199,6 +248,7 @@ Available launch arguments:
 | `max_speed` | `1.0` | Maximum motor speed in library units |
 | `pan_center_deg` | `90.0` | Pan angle at startup and shutdown (degrees) |
 | `tilt_center_deg` | `60.0` | Tilt angle at startup and shutdown (degrees) |
+| `ultrasonic_rate_hz` | `10.0` | Ultrasonic sensor publish rate (Hz) |
 
 ### Sending velocity commands from the host
 
@@ -262,7 +312,8 @@ ros2 topic echo /joint_states
 │       └── raspbot_v2/
 │           ├── __init__.py
 │           ├── motor_controller_node.py      # Differential-drive motor control
-│           └── camera_orientation_node.py    # Pan/tilt servo control
+│           ├── camera_orientation_node.py    # Pan/tilt servo control
+│           └── ultrasonic_node.py            # HC-SR04 range sensor
 └── raspbot_v2_interface/       # Vendored Yahboom hardware library
     └── Raspbot_Lib/
         └── Raspbot_Lib.py      # I²C driver (smbus, bus 1, addr 0x2B)

raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py

@@ -196,7 +196,15 @@ class Raspbot():
         except:
             print ('Ctrl_getDis_Switch I2C error')
 
-
+    def Read_Ultrasonic(self):
+        try:
+            diss_H = self.read_data_array(0x1b, 1)[0]
+            diss_L = self.read_data_array(0x1a, 1)[0]
+            dis = (diss_H << 8) | diss_L
+            return dis
+        except:
+            print('Read_Ultrasonic I2C error')
+            return None
 
 
 #控制灯珠特效

src/raspbot_v2/launch/robot.launch.py

@@ -13,6 +13,10 @@ def generate_launch_description():
         DeclareLaunchArgument('max_speed',  default_value='1.0',
                               description='Maximum motor speed in library units'),
 
+        # ── Ultrasonic sensor arguments ───────────────────────────────────
+        DeclareLaunchArgument('ultrasonic_rate_hz', default_value='10.0',
+                              description='Ultrasonic sensor publish rate (Hz)'),
+
         # ── Camera orientation arguments ──────────────────────────────────
         DeclareLaunchArgument('pan_center_deg',  default_value='90.0',
                               description='Pan angle at startup and shutdown (degrees)'),
@@ -42,4 +46,14 @@ def generate_launch_description():
             output='screen',
         ),
 
+        Node(
+            package='raspbot_v2',
+            executable='ultrasonic',
+            name='ultrasonic',
+            parameters=[{
+                'publish_rate_hz': LaunchConfiguration('ultrasonic_rate_hz'),
+            }],
+            output='screen',
+        ),
+
     ])

src/raspbot_v2/raspbot_v2/ultrasonic_node.py

@@ -0,0 +1,167 @@
+#!/usr/bin/env python3
+"""
+Ultrasonic range sensor node.
+
+Publishes distance readings from the Raspbot's HC-SR04-style sensor as a
+standard sensor_msgs/Range message.  The sensor is only powered on while at
+least one subscriber is active, and automatically powered off when the last
+subscriber disconnects.
+
+Standard interface
+------------------
+Published : /ultrasonic/range  (sensor_msgs/Range)
+            radiation_type = ULTRASOUND
+            range in metres; +inf when target is beyond max_range,
+            -inf when closer than min_range (REP-117 convention).
+
+Parameters
+----------
+publish_rate_hz  float  default 10.0   – sensor poll and publish rate
+frame_id         str    default 'ultrasonic' – header frame_id
+min_range_m      float  default 0.02   – minimum valid range (m)
+max_range_m      float  default 4.0    – maximum valid range (m)
+field_of_view    float  default 0.2618 – sensor cone width (rad, ~15°)
+warmup_s         float  default 1.0    – seconds to wait after powering the
+                                         sensor on before trusting readings
+"""
+
+import math
+import time
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Range
+
+
+class UltrasonicNode(Node):
+
+    def __init__(self):
+        super().__init__('ultrasonic')
+
+        # --- Parameters ---
+        self.declare_parameter('publish_rate_hz', 10.0)
+        self.declare_parameter('frame_id',        'ultrasonic')
+        self.declare_parameter('min_range_m',     0.02)
+        self.declare_parameter('max_range_m',     4.0)
+        self.declare_parameter('field_of_view',   0.2618)  # ~15 degrees
+        self.declare_parameter('warmup_s',        1.0)
+
+        self._frame_id    = self.get_parameter('frame_id').value
+        self._min_range   = self.get_parameter('min_range_m').value
+        self._max_range   = self.get_parameter('max_range_m').value
+        self._fov         = self.get_parameter('field_of_view').value
+        self._warmup_s    = self.get_parameter('warmup_s').value
+        rate_hz           = self.get_parameter('publish_rate_hz').value
+
+        # --- Hardware ---
+        from raspbot_v2_interface.Raspbot_Lib import Raspbot
+        self._bot = Raspbot()
+
+        # --- Sensor state ---
+        self._sensor_on    = False
+        self._enabled_at   = 0.0   # monotonic time when sensor was switched on
+
+        # --- Publisher ---
+        self._pub = self.create_publisher(Range, 'ultrasonic/range', 10)
+
+        # --- Timer ---
+        self._timer = self.create_timer(1.0 / rate_hz, self._tick)
+
+        self.get_logger().info(
+            f'Ultrasonic node started ({rate_hz} Hz, '
+            f'range {self._min_range}–{self._max_range} m). '
+            f'Sensor will activate when subscribers connect.'
+        )
+
+    # ------------------------------------------------------------------
+    # Timer callback
+    # ------------------------------------------------------------------
+
+    def _tick(self):
+        has_subscribers = self._pub.get_subscription_count() > 0
+
+        # Power the sensor on/off based on subscriber presence
+        if has_subscribers and not self._sensor_on:
+            self._set_sensor(True)
+            return  # skip this tick — let the sensor warm up
+
+        if not has_subscribers and self._sensor_on:
+            self._set_sensor(False)
+            return
+
+        if not self._sensor_on:
+            return
+
+        # Still warming up — do not publish unreliable readings
+        if time.monotonic() - self._enabled_at < self._warmup_s:
+            return
+
+        # Read and publish
+        raw_mm = self._bot.Read_Ultrasonic()
+        if raw_mm is None:
+            self.get_logger().warn('Ultrasonic read returned None', throttle_duration_sec=5.0)
+            return
+
+        self._pub.publish(self._build_msg(raw_mm))
+
+    # ------------------------------------------------------------------
+    # Helpers
+    # ------------------------------------------------------------------
+
+    def _set_sensor(self, on: bool):
+        try:
+            self._bot.Ctrl_Ulatist_Switch(1 if on else 0)
+            self._sensor_on  = on
+            self._enabled_at = time.monotonic()
+            self.get_logger().info(f'Ultrasonic sensor {"enabled" if on else "disabled"}')
+        except Exception as exc:
+            self.get_logger().error(f'Failed to set sensor state: {exc}')
+
+    def _build_msg(self, raw_mm: int) -> Range:
+        msg = Range()
+        msg.header.stamp    = self.get_clock().now().to_msg()
+        msg.header.frame_id = self._frame_id
+        msg.radiation_type  = Range.ULTRASOUND
+        msg.field_of_view   = self._fov
+        msg.min_range       = self._min_range
+        msg.max_range       = self._max_range
+
+        distance_m = raw_mm / 1000.0
+
+        # REP-117: report +inf / -inf for out-of-range readings
+        if distance_m > self._max_range:
+            msg.range = math.inf
+        elif distance_m < self._min_range:
+            msg.range = -math.inf
+        else:
+            msg.range = distance_m
+
+        return msg
+
+    # ------------------------------------------------------------------
+    # Shutdown
+    # ------------------------------------------------------------------
+
+    def destroy_node(self):
+        if self._sensor_on:
+            try:
+                self._bot.Ctrl_Ulatist_Switch(0)
+            except Exception:
+                pass
+        super().destroy_node()
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = UltrasonicNode()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/raspbot_v2/setup.py

@@ -17,6 +17,7 @@ setup(
         'console_scripts': [
             'motor_controller = raspbot_v2.motor_controller_node:main',
             'camera_orientation = raspbot_v2.camera_orientation_node:main',
+            'ultrasonic = raspbot_v2.ultrasonic_node:main',
         ],
     },
 )