Add ultrasonic sensor
This commit is contained in:
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
|
||||
#控制灯珠特效
|
||||
|
||||
@@ -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',
|
||||
),
|
||||
|
||||
])
|
||||
|
||||
@@ -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()
|
||||
@@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user