From 261e18af83b50cca5d6d638581a12c2107868e0f Mon Sep 17 00:00:00 2001 From: Matt Spencer Date: Fri, 1 May 2026 10:57:05 +0000 Subject: [PATCH] Camera pan/tilt to sensible values --- .../raspbot_v2/camera_orientation_node.py | 70 ++++++++++--------- .../src/raspbot_v2/urdf/raspbot_v2.urdf.xacro | 4 +- .../src/components/CameraControls.jsx | 6 +- 3 files changed, 43 insertions(+), 37 deletions(-) diff --git a/robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py b/robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py index 524fccb..625483e 100644 --- a/robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py +++ b/robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py @@ -19,15 +19,17 @@ State : /joint_states (sensor_msgs/JointState) 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 +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 -60.0 – ROS lower limit for pan (°) +pan_max_deg float default 90.0 – ROS upper limit for pan (°) +pan_servo_center_deg float default 90.0 – hardware servo angle that equals ROS 0° +tilt_min_deg float default -90.0 – ROS lower limit for tilt (°) +tilt_max_deg float default 45.0 – ROS upper limit for tilt (°) +tilt_servo_center_deg float default 60.0 – hardware servo angle that equals ROS 0° +pan_center_deg float default 0.0 – startup position in ROS degrees +tilt_center_deg float default -15.0 – startup position in ROS degrees +state_rate_hz float default 10.0 – joint-state publish rate """ import math @@ -56,22 +58,26 @@ class CameraOrientationNode(Node): 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.declare_parameter('pan_servo_id', _PAN_SERVO_ID) + self.declare_parameter('tilt_servo_id', _TILT_SERVO_ID) + self.declare_parameter('pan_min_deg', -60.0) + self.declare_parameter('pan_max_deg', 90.0) + self.declare_parameter('pan_servo_center_deg', 90.0) + self.declare_parameter('tilt_min_deg', -90.0) + self.declare_parameter('tilt_max_deg', 45.0) + self.declare_parameter('tilt_servo_center_deg', 60.0) + self.declare_parameter('pan_center_deg', 0.0) + self.declare_parameter('tilt_center_deg', -15.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 + 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._pan_servo_center = self.get_parameter('pan_servo_center_deg').value + self._tilt_min = self.get_parameter('tilt_min_deg').value + self._tilt_max = self.get_parameter('tilt_max_deg').value + self._tilt_servo_center = self.get_parameter('tilt_servo_center_deg').value pan_center = self.get_parameter('pan_center_deg').value tilt_center = self.get_parameter('tilt_center_deg').value @@ -141,15 +147,15 @@ class CameraOrientationNode(Node): # 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)) + def _apply(self, pan_ros_deg: float, tilt_ros_deg: float): + """Convert ROS-convention degrees to servo degrees and drive hardware.""" + pan_servo = int(round(pan_ros_deg + self._pan_servo_center)) + tilt_servo = int(round(tilt_ros_deg + self._tilt_servo_center)) 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 + self._bot.Ctrl_Servo(self._pan_id, pan_servo) + self._bot.Ctrl_Servo(self._tilt_id, tilt_servo) + self._pan_deg = pan_ros_deg + self._tilt_deg = tilt_ros_deg except Exception as exc: self.get_logger().error(f'Servo write failed: {exc}') diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro index a0c7367..72e63d0 100644 --- a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro +++ b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro @@ -111,7 +111,7 @@ - + @@ -126,7 +126,7 @@ - + diff --git a/webui/frontend/src/components/CameraControls.jsx b/webui/frontend/src/components/CameraControls.jsx index a2d27aa..ba8a3f2 100644 --- a/webui/frontend/src/components/CameraControls.jsx +++ b/webui/frontend/src/components/CameraControls.jsx @@ -1,9 +1,9 @@ import { useState } from 'react' -const PAN_MIN = -Math.PI / 2 // -90° +const PAN_MIN = -Math.PI / 3 // -60° const PAN_MAX = Math.PI / 2 // +90° -const TILT_MIN = -0.524 // -30° -const TILT_MAX = 0.960 // +55° +const TILT_MIN = -Math.PI / 2 // -90° +const TILT_MAX = Math.PI / 4 // +45° const toDeg = (r) => Math.round(r * 180 / Math.PI)