From 1d49e45240f72ebe04594dddeb9e2c3e090fa0bd Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Sun, 19 Apr 2026 09:08:35 +0000 Subject: [PATCH] Add ultrasonic sensor --- README.md | 53 +++++- .../Raspbot_Lib/Raspbot_Lib.py | 10 +- src/raspbot_v2/launch/robot.launch.py | 14 ++ src/raspbot_v2/raspbot_v2/ultrasonic_node.py | 167 ++++++++++++++++++ src/raspbot_v2/setup.py | 1 + 5 files changed, 243 insertions(+), 2 deletions(-) create mode 100644 src/raspbot_v2/raspbot_v2/ultrasonic_node.py diff --git a/README.md b/README.md index bd8eb34..91d95a0 100644 --- a/README.md +++ b/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) diff --git a/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py b/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py index 4b101ce..1ed5dee 100644 --- a/raspbot_v2_interface/Raspbot_Lib/Raspbot_Lib.py +++ b/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 #控制灯珠特效 diff --git a/src/raspbot_v2/launch/robot.launch.py b/src/raspbot_v2/launch/robot.launch.py index cfc8dc3..fa65af5 100644 --- a/src/raspbot_v2/launch/robot.launch.py +++ b/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', + ), + ]) diff --git a/src/raspbot_v2/raspbot_v2/ultrasonic_node.py b/src/raspbot_v2/raspbot_v2/ultrasonic_node.py new file mode 100644 index 0000000..6eed1b1 --- /dev/null +++ b/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() diff --git a/src/raspbot_v2/setup.py b/src/raspbot_v2/setup.py index c053e3d..3e520db 100644 --- a/src/raspbot_v2/setup.py +++ b/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', ], }, ) \ No newline at end of file