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