diff --git a/Dockerfile b/Dockerfile
index b3b010b..6d0ceb2 100644
--- a/Dockerfile
+++ b/Dockerfile
@@ -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/*
diff --git a/README.md b/README.md
index 86c1f88..f27dd58 100644
--- a/README.md
+++ b/README.md
@@ -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)
diff --git a/src/my_robot/my_robot/camera_orientation_node.py b/src/my_robot/my_robot/camera_orientation_node.py
new file mode 100644
index 0000000..524fccb
--- /dev/null
+++ b/src/my_robot/my_robot/camera_orientation_node.py
@@ -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()
diff --git a/src/my_robot/package.xml b/src/my_robot/package.xml
index e5fe654..28d89fb 100644
--- a/src/my_robot/package.xml
+++ b/src/my_robot/package.xml
@@ -9,6 +9,7 @@
rclpy
geometry_msgs
std_msgs
+ sensor_msgs
ament_python
diff --git a/src/my_robot/setup.py b/src/my_robot/setup.py
index 1934ed2..32c1aa4 100644
--- a/src/my_robot/setup.py
+++ b/src/my_robot/setup.py
@@ -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',
],
},
)
\ No newline at end of file