Add controls for the camera orientation
This commit is contained in:
@@ -24,6 +24,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
|
||||
ros-${ROS_DISTRO}-rclpy \
|
||||
ros-${ROS_DISTRO}-geometry-msgs \
|
||||
ros-${ROS_DISTRO}-std-msgs \
|
||||
ros-${ROS_DISTRO}-sensor-msgs \
|
||||
python3-smbus \
|
||||
&& rm -rf /var/lib/apt/lists/*
|
||||
|
||||
|
||||
@@ -1,11 +1,15 @@
|
||||
# my_robot — Motor Controller Node
|
||||
# my_robot
|
||||
|
||||
ROS 2 package for differential-drive motor control on the Yahboom Raspbot V2 platform.
|
||||
ROS 2 package for the Yahboom Raspbot V2 platform — differential-drive motor control and pan/tilt camera orientation.
|
||||
|
||||
---
|
||||
|
||||
## Architecture
|
||||
|
||||
Both nodes share the same I²C bus. The Linux kernel serialises individual transactions, so they can run as separate processes without additional locking.
|
||||
|
||||
### Motor controller
|
||||
|
||||
```
|
||||
┌───────────────────────────────────┐
|
||||
│ MotorControllerNode │
|
||||
@@ -28,7 +32,7 @@ ROS 2 package for differential-drive motor control on the Yahboom Raspbot V2 pla
|
||||
└───────────────────────────────────┘
|
||||
```
|
||||
|
||||
### Topics
|
||||
#### Topics
|
||||
|
||||
| Topic | Direction | Type | Description |
|
||||
|---|---|---|---|
|
||||
@@ -36,16 +40,63 @@ ROS 2 package for differential-drive motor control on the Yahboom Raspbot V2 pla
|
||||
| `/wheel_speeds` | Subscribed | `std_msgs/Float32MultiArray` | Direct per-wheel speed override `[FL, FR, RL, RR]` in library units (0–255) |
|
||||
| `/current_wheel_speeds` | Published | `std_msgs/Float32MultiArray` | Current wheel speeds read from hardware, published at 10 Hz |
|
||||
|
||||
### Parameters
|
||||
#### Parameters
|
||||
|
||||
| Parameter | Default | Description |
|
||||
|---|---|---|
|
||||
| `wheel_base` | `0.3` | Distance between left and right wheels in metres |
|
||||
| `max_speed` | `1.0` | Maximum motor speed in library units |
|
||||
|
||||
### Hardware interface
|
||||
---
|
||||
|
||||
The node drives the Yahboom Raspbot V2 motor controller over **I²C bus 1** (device address `0x2B`) using the bundled `raspbot_v2_interface` library. The only host device required is `/dev/i2c-1`.
|
||||
### Camera orientation controller
|
||||
|
||||
```
|
||||
┌──────────────────────────────────────┐
|
||||
│ CameraOrientationNode │
|
||||
│ │
|
||||
/joint_command ────────>│ JointState (names: pan, tilt) │
|
||||
(sensor_msgs/ │ position in radians │
|
||||
JointState) │ │
|
||||
│ pan → servo 1 (0°–180°) │
|
||||
│ tilt → servo 2 (0°–110°) │
|
||||
│ │
|
||||
│ ▼ │
|
||||
│ raspbot_v2_interface │
|
||||
│ I²C bus 1, addr 0x2B │
|
||||
│ ▼ │
|
||||
│ /dev/i2c-1 ──────> Pan/tilt servos │
|
||||
│ │
|
||||
/joint_states <────────│ current angles @ 10 Hz │
|
||||
(sensor_msgs/ │ position in radians │
|
||||
JointState) │ │
|
||||
└──────────────────────────────────────┘
|
||||
```
|
||||
|
||||
#### Topics
|
||||
|
||||
| Topic | Direction | Type | Description |
|
||||
|---|---|---|---|
|
||||
| `/joint_command` | Subscribed | `sensor_msgs/JointState` | Commanded pan/tilt angles. Joint names `"pan"` and `"tilt"`, positions in **radians**. Unknown joint names are ignored. |
|
||||
| `/joint_states` | Published | `sensor_msgs/JointState` | Current angles reflected from the last command, published at 10 Hz |
|
||||
|
||||
#### Parameters
|
||||
|
||||
| Parameter | Default | Description |
|
||||
|---|---|---|
|
||||
| `pan_servo_id` | `1` | Raspbot servo channel for pan |
|
||||
| `tilt_servo_id` | `2` | Raspbot servo channel for tilt |
|
||||
| `pan_min_deg` | `0.0` | Pan lower limit (degrees) |
|
||||
| `pan_max_deg` | `180.0` | Pan upper limit (degrees) |
|
||||
| `tilt_min_deg` | `0.0` | Tilt lower limit (degrees) |
|
||||
| `tilt_max_deg` | `110.0` | Tilt upper limit (degrees) — hardware cap |
|
||||
| `pan_center_deg` | `90.0` | Startup and shutdown park position for pan |
|
||||
| `tilt_center_deg` | `60.0` | Startup and shutdown park position for tilt |
|
||||
| `state_rate_hz` | `10.0` | `~/joint_states` publish rate |
|
||||
|
||||
#### Hardware interface
|
||||
|
||||
The node drives the pan and tilt servos over **I²C bus 1** (device address `0x2B`). The same `/dev/i2c-1` device used by the motor controller is sufficient — no additional device node is required.
|
||||
|
||||
---
|
||||
|
||||
@@ -126,7 +177,57 @@ docker run --rm \
|
||||
|
||||
If your board exposes the motor controller on a different bus (check with `ls /dev/i2c-*` on the host), substitute the correct device node (e.g. `--device /dev/i2c-0`).
|
||||
|
||||
### Overriding parameters at launch
|
||||
### Camera orientation node
|
||||
|
||||
Override the default `CMD` to run the camera orientation node instead:
|
||||
|
||||
```bash
|
||||
docker run --rm \
|
||||
--network=host \
|
||||
--device /dev/i2c-1 \
|
||||
--env ROS_DOMAIN_ID=0 \
|
||||
my_robot:latest \
|
||||
ros2 run my_robot camera_orientation
|
||||
```
|
||||
|
||||
#### Overriding parameters at launch
|
||||
|
||||
```bash
|
||||
docker run --rm \
|
||||
--network=host \
|
||||
--device /dev/i2c-1 \
|
||||
--env ROS_DOMAIN_ID=0 \
|
||||
my_robot:latest \
|
||||
ros2 run my_robot camera_orientation \
|
||||
--ros-args -p pan_center_deg:=90.0 -p tilt_center_deg:=45.0
|
||||
```
|
||||
|
||||
#### Commanding the camera from the host
|
||||
|
||||
Pan to centre (90°) and tilt to 30°:
|
||||
|
||||
```bash
|
||||
ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \
|
||||
"{name: ['pan', 'tilt'], position: [1.5708, 0.5236]}"
|
||||
```
|
||||
|
||||
You can command a single axis by omitting the other joint name:
|
||||
|
||||
```bash
|
||||
# Pan only
|
||||
ros2 topic pub --once /joint_command sensor_msgs/msg/JointState \
|
||||
"{name: ['pan'], position: [0.0]}"
|
||||
```
|
||||
|
||||
#### Verifying orientation state
|
||||
|
||||
```bash
|
||||
ros2 topic echo /joint_states
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
### Overriding parameters at launch (motor controller)
|
||||
|
||||
ROS 2 parameters can be passed through `--ros-args`:
|
||||
|
||||
@@ -172,11 +273,14 @@ ros2 topic echo /current_wheel_speeds
|
||||
.
|
||||
├── Dockerfile # Two-stage production image
|
||||
├── docker-entrypoint.sh # Sources ROS overlays before exec
|
||||
├── package.xml # ROS package manifest
|
||||
├── setup.py # ament_python build definition
|
||||
├── my_robot/
|
||||
│ ├── __init__.py
|
||||
│ └── motor_controller_node.py
|
||||
├── src/
|
||||
│ └── my_robot/
|
||||
│ ├── package.xml # ROS package manifest
|
||||
│ ├── setup.py # ament_python build definition
|
||||
│ └── my_robot/
|
||||
│ ├── __init__.py
|
||||
│ ├── motor_controller_node.py # Differential-drive motor control
|
||||
│ └── camera_orientation_node.py # Pan/tilt servo control
|
||||
└── raspbot_v2_interface/ # Vendored Yahboom hardware library
|
||||
└── Raspbot_Lib/
|
||||
└── Raspbot_Lib.py # I²C driver (smbus, bus 1, addr 0x2B)
|
||||
|
||||
@@ -0,0 +1,199 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Camera orientation controller node.
|
||||
|
||||
Subscribes to joint commands and drives the pan/tilt servo pair on the
|
||||
Raspbot via Ctrl_Servo. Publishes current joint state for downstream
|
||||
consumers (e.g. tf2, rviz2).
|
||||
|
||||
Standard interfaces
|
||||
-------------------
|
||||
Command : /joint_command (sensor_msgs/JointState)
|
||||
Joint names: "pan" – horizontal rotation (yaw)
|
||||
Joint names: "tilt" – vertical rotation (pitch)
|
||||
Positions in **radians** (ROS convention).
|
||||
|
||||
State : /joint_states (sensor_msgs/JointState)
|
||||
Same joint names; position in radians, reflecting the last
|
||||
commanded angle.
|
||||
|
||||
Parameters
|
||||
----------
|
||||
pan_servo_id int default 1 – Raspbot servo channel for pan
|
||||
tilt_servo_id int default 2 – Raspbot servo channel for tilt
|
||||
pan_min_deg float default 0.0 – hardware lower limit for pan (°)
|
||||
pan_max_deg float default 180.0
|
||||
tilt_min_deg float default 0.0 – hardware lower limit for tilt (°)
|
||||
tilt_max_deg float default 110.0 – library caps tilt at 110°
|
||||
pan_center_deg float default 90.0 – position commanded on startup
|
||||
tilt_center_deg float default 60.0
|
||||
state_rate_hz float default 10.0 – joint-state publish rate
|
||||
"""
|
||||
|
||||
import math
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
|
||||
# Servo IDs used by Ctrl_Servo
|
||||
_PAN_SERVO_ID = 1
|
||||
_TILT_SERVO_ID = 2
|
||||
|
||||
|
||||
def _deg_to_rad(deg: float) -> float:
|
||||
return deg * math.pi / 180.0
|
||||
|
||||
|
||||
def _rad_to_deg(rad: float) -> float:
|
||||
return rad * 180.0 / math.pi
|
||||
|
||||
|
||||
class CameraOrientationNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__('camera_orientation')
|
||||
|
||||
# --- Parameters ---
|
||||
self.declare_parameter('pan_servo_id', _PAN_SERVO_ID)
|
||||
self.declare_parameter('tilt_servo_id', _TILT_SERVO_ID)
|
||||
self.declare_parameter('pan_min_deg', 0.0)
|
||||
self.declare_parameter('pan_max_deg', 180.0)
|
||||
self.declare_parameter('tilt_min_deg', 0.0)
|
||||
self.declare_parameter('tilt_max_deg', 110.0)
|
||||
self.declare_parameter('pan_center_deg', 90.0)
|
||||
self.declare_parameter('tilt_center_deg', 60.0)
|
||||
self.declare_parameter('state_rate_hz', 10.0)
|
||||
|
||||
self._pan_id = self.get_parameter('pan_servo_id').value
|
||||
self._tilt_id = self.get_parameter('tilt_servo_id').value
|
||||
self._pan_min = self.get_parameter('pan_min_deg').value
|
||||
self._pan_max = self.get_parameter('pan_max_deg').value
|
||||
self._tilt_min = self.get_parameter('tilt_min_deg').value
|
||||
self._tilt_max = self.get_parameter('tilt_max_deg').value
|
||||
|
||||
pan_center = self.get_parameter('pan_center_deg').value
|
||||
tilt_center = self.get_parameter('tilt_center_deg').value
|
||||
rate_hz = self.get_parameter('state_rate_hz').value
|
||||
|
||||
# --- Hardware ---
|
||||
# Import here so the node can be unit-tested without hardware present
|
||||
from raspbot_v2_interface.Raspbot_Lib import Raspbot
|
||||
self._bot = Raspbot()
|
||||
|
||||
# --- State (degrees, internal representation) ---
|
||||
self._pan_deg = pan_center
|
||||
self._tilt_deg = tilt_center
|
||||
|
||||
# Drive servos to the startup centre position
|
||||
self._apply(self._pan_deg, self._tilt_deg)
|
||||
|
||||
# --- Subscriber ---
|
||||
self._cmd_sub = self.create_subscription(
|
||||
JointState,
|
||||
'joint_command',
|
||||
self._joint_command_cb,
|
||||
10,
|
||||
)
|
||||
|
||||
# --- Publisher ---
|
||||
self._state_pub = self.create_publisher(JointState, 'joint_states', 10)
|
||||
self._state_timer = self.create_timer(1.0 / rate_hz, self._publish_state)
|
||||
|
||||
self.get_logger().info(
|
||||
f'Camera orientation node started '
|
||||
f'(pan servo {self._pan_id}, tilt servo {self._tilt_id}). '
|
||||
f'Centred at pan={pan_center}° tilt={tilt_center}°.'
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Subscriber callback
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _joint_command_cb(self, msg: JointState):
|
||||
"""
|
||||
Accept a JointState command. Only joints named "pan" or "tilt"
|
||||
are acted on; other names are silently ignored so this node can
|
||||
coexist on a shared joint_command topic.
|
||||
|
||||
Positions are expected in radians (ROS convention).
|
||||
"""
|
||||
if len(msg.name) != len(msg.position):
|
||||
self.get_logger().warn(
|
||||
'joint_command message has mismatched name/position lengths; ignoring.'
|
||||
)
|
||||
return
|
||||
|
||||
pan_deg = self._pan_deg
|
||||
tilt_deg = self._tilt_deg
|
||||
|
||||
for name, pos_rad in zip(msg.name, msg.position):
|
||||
pos_deg = _rad_to_deg(pos_rad)
|
||||
if name == 'pan':
|
||||
pan_deg = self._clamp(pos_deg, self._pan_min, self._pan_max)
|
||||
elif name == 'tilt':
|
||||
tilt_deg = self._clamp(pos_deg, self._tilt_min, self._tilt_max)
|
||||
|
||||
self._apply(pan_deg, tilt_deg)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Helpers
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _apply(self, pan_deg: float, tilt_deg: float):
|
||||
"""Send angles to hardware and update internal state."""
|
||||
pan_int = int(round(pan_deg))
|
||||
tilt_int = int(round(tilt_deg))
|
||||
try:
|
||||
self._bot.Ctrl_Servo(self._pan_id, pan_int)
|
||||
self._bot.Ctrl_Servo(self._tilt_id, tilt_int)
|
||||
self._pan_deg = pan_deg
|
||||
self._tilt_deg = tilt_deg
|
||||
except Exception as exc:
|
||||
self.get_logger().error(f'Servo write failed: {exc}')
|
||||
|
||||
@staticmethod
|
||||
def _clamp(value: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, value))
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# State publisher
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def _publish_state(self):
|
||||
msg = JointState()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
msg.name = ['pan', 'tilt']
|
||||
msg.position = [_deg_to_rad(self._pan_deg), _deg_to_rad(self._tilt_deg)]
|
||||
self._state_pub.publish(msg)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# Shutdown
|
||||
# ------------------------------------------------------------------
|
||||
|
||||
def destroy_node(self):
|
||||
self.get_logger().info('Parking camera servos at centre...')
|
||||
pan_center = self.get_parameter('pan_center_deg').value
|
||||
tilt_center = self.get_parameter('tilt_center_deg').value
|
||||
try:
|
||||
self._apply(pan_center, tilt_center)
|
||||
except Exception:
|
||||
pass
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = CameraOrientationNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
@@ -9,6 +9,7 @@
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>geometry_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
|
||||
@@ -15,6 +15,7 @@ setup(
|
||||
entry_points={
|
||||
'console_scripts': [
|
||||
'motor_controller = my_robot.motor_controller_node:main',
|
||||
'camera_orientation = my_robot.camera_orientation_node:main',
|
||||
],
|
||||
},
|
||||
)
|
||||
Reference in New Issue
Block a user