Add controls for the camera orientation

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/*
 

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
+├── src/
-├── setup.py                    # ament_python build definition
+│   └── my_robot/
-├── my_robot/
+│       ├── package.xml         # ROS package manifest
-│   ├── __init__.py
+│       ├── setup.py            # ament_python build definition
-│   └── motor_controller_node.py
+│       └── 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)

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()

src/my_robot/package.xml

@@ -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>
 

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',
         ],
     },
 )