m5p3nc3r/ros-raspbot-v2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Camera pan/tilt to sensible values

Browse Source
This commit is contained in:
Matt Spencer
2026-05-01 10:57:05 +00:00
parent 8e78d14086
commit 261e18af83
3 changed files with 43 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
robot/src/raspbot_v2/raspbot_v2/camera_orientation_node.py
+38 -32
View File
@@ -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}')
robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
+2 -2
View File
@@ -111,7 +111,7 @@
<child link="camera_pan_link"/>
<axis xyz="0 0 1"/>
<origin xyz="0 0 0.015" rpy="0 0 0"/>
<limit lower="-1.5708" upper="1.5708" effort="1.0" velocity="1.0"/>
<limit lower="-1.0472" upper="1.5708" effort="1.0" velocity="1.0"/>
</joint>
<!-- Tilt joint (pitch, around Y) -->
@@ -126,7 +126,7 @@
<child link="camera_link"/>
<axis xyz="0 1 0"/>
<origin xyz="0.02 0 0" rpy="0 0 0"/>
<limit lower="-0.5236" upper="0.9599" effort="1.0" velocity="1.0"/>
<limit lower="-1.5708" upper="0.7854" effort="1.0" velocity="1.0"/>
</joint>
</robot>