From d3c3d03ae717f0c153da5e3cac3c86ffd713e086 Mon Sep 17 00:00:00 2001
From: Matt Spencer <matthew@thespencers.me.uk>
Date: Thu, 7 May 2026 20:55:24 +0000
Subject: [PATCH] Add the initial (untested) gazebo plugin configuration.

---
 robot/src/raspbot_v2/config/controllers.yaml  | 38 +++++++++++++++
 robot/src/raspbot_v2/launch/robot.launch.py   |  5 ++
 robot/src/raspbot_v2/setup.py                 |  1 +
 .../urdf/raspbot_v2.ros2_control.xacro        | 33 +++++++++++++
 .../src/raspbot_v2/urdf/raspbot_v2.urdf.xacro |  8 ++++
 robot/src/raspbot_v2/urdf/robot_base.xacro    | 30 ++++++++++--
 robot/src/raspbot_v2/urdf/sonar.xacro         | 47 ++++++++++++++-----
 7 files changed, 146 insertions(+), 16 deletions(-)
 create mode 100644 robot/src/raspbot_v2/config/controllers.yaml
 create mode 100644 robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro

diff --git a/robot/src/raspbot_v2/config/controllers.yaml b/robot/src/raspbot_v2/config/controllers.yaml
new file mode 100644
index 0000000..e78d744
--- /dev/null
+++ b/robot/src/raspbot_v2/config/controllers.yaml
@@ -0,0 +1,38 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz — must be >= the fastest controller's publish rate
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    pan_tilt_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+# ─── pan_tilt_controller ─────────────────────────────────────────────────────
+# Accepts goals on:
+#   /pan_tilt_controller/follow_joint_trajectory  (action, JointTrajectory)
+#
+# allow_partial_joints_goal: true lets you command only pan or only tilt,
+# matching the behaviour of the real camera_orientation_node which handles
+# the joints independently.
+pan_tilt_controller:
+  ros__parameters:
+    joints:
+      - pan
+      - tilt
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    allow_partial_joints_goal: true
+    open_loop_control: false
+    goal_time: 5.0         # seconds — trajectory must complete within this window
+    stopped_velocity_tolerance: 0.01  # rad/s
+    constraints:
+      goal_time: 0.5       # seconds — tolerance after goal_time expires
+      stopped_velocity_tolerance: 0.01
+      pan:
+        goal: 0.05         # rad — acceptable position error at goal
+      tilt:
+        goal: 0.05         # rad
diff --git a/robot/src/raspbot_v2/launch/robot.launch.py b/robot/src/raspbot_v2/launch/robot.launch.py
index a54f189..2ee0801 100644
--- a/robot/src/raspbot_v2/launch/robot.launch.py
+++ b/robot/src/raspbot_v2/launch/robot.launch.py
@@ -12,10 +12,15 @@ def generate_launch_description():
     urdf_file = os.path.join(
         get_package_share_directory('raspbot_v2'), 'urdf', 'raspbot_v2.urdf.xacro'
     )
+    controllers_config = os.path.join(
+        get_package_share_directory('raspbot_v2'), 'config', 'controllers.yaml'
+    )
+
     robot_description = ParameterValue(
         Command([
             'xacro ', urdf_file,
             ' wheel_separation:=', LaunchConfiguration('wheel_base'),
+            ' controllers_config:=', controllers_config,
         ]),
         value_type=str,
     )
diff --git a/robot/src/raspbot_v2/setup.py b/robot/src/raspbot_v2/setup.py
index 61221cf..0db3946 100644
--- a/robot/src/raspbot_v2/setup.py
+++ b/robot/src/raspbot_v2/setup.py
@@ -13,6 +13,7 @@ setup(
         ('share/' + package_name, ['package.xml']),
         ('share/' + package_name + '/launch', ['launch/robot.launch.py']),
         ('share/' + package_name + '/urdf',   glob.glob('urdf/*.xacro')),
+        ('share/' + package_name + '/config', glob.glob('config/*.yaml')),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro
new file mode 100644
index 0000000..3db0a29
--- /dev/null
+++ b/robot/src/raspbot_v2/urdf/raspbot_v2.ros2_control.xacro
@@ -0,0 +1,33 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- ═══════════════════════════════════════════════════════════════════════
+       gz_ros2_control — Gazebo Harmonic system plugin
+
+       Wires the <ros2_control> hardware block in pan_tilt.xacro to gz-sim
+       so that the controller_manager can load and run controllers against
+       simulated pan and tilt joints.
+
+       Lifecycle in simulation:
+         1. gz-sim loads this plugin when the robot is spawned
+         2. The plugin reads robot_description from robot_state_publisher
+         3. controller_manager starts; it loads controllers.yaml
+         4. Spawning joint_state_broadcaster + pan_tilt_controller (via launch)
+            drives /joint_states and accepts trajectory commands
+       ═══════════════════════════════════════════════════════════════════════ -->
+
+  <gazebo>
+    <plugin filename="gz-sim-ros2-control-system"
+            name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+      <robot_param>robot_description</robot_param>
+      <robot_param_node>robot_state_publisher</robot_param_node>
+      <!-- Path injected at xacro-processing time by the launch file:
+           xacro ... controllers_config:=<path>/controllers.yaml
+           Empty by default so the file can be previewed without a built workspace. -->
+      <xacro:if value="${controllers_config != ''}">
+        <parameters>${controllers_config}</parameters>
+      </xacro:if>
+    </plugin>
+  </gazebo>
+
+</robot>
diff --git a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
index 8d3e511..69bb155 100644
--- a/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
+++ b/robot/src/raspbot_v2/urdf/raspbot_v2.urdf.xacro
@@ -53,6 +53,11 @@
   <xacro:arg name="sonar_min_range" default="0.02"/>  <!-- m (HC-SR04 spec) -->
   <xacro:arg name="sonar_max_range" default="4.0"/>   <!-- m (HC-SR04 spec) -->
 
+  <!-- ── ros2_control ────────────────────────────────────────────────────── -->
+  <!-- Passed by the launch file as an absolute path to controllers.yaml.
+       Empty by default so xacro can process the file without a built workspace. -->
+  <xacro:arg name="controllers_config" default=""/>
+
   <!-- ═══════════════════════════════════════════════════════════════════════
        Resolve args → properties so sub-files can use ${name} syntax
        ═══════════════════════════════════════════════════════════════════════ -->
@@ -86,6 +91,8 @@
   <xacro:property name="sonar_min_range" value="$(arg sonar_min_range)"/>
   <xacro:property name="sonar_max_range" value="$(arg sonar_max_range)"/>
 
+  <xacro:property name="controllers_config" value="$(arg controllers_config)"/>
+
   <!-- ═══════════════════════════════════════════════════════════════════════
        Sub-file includes  (order matters — base must come first)
        ═══════════════════════════════════════════════════════════════════════ -->
@@ -95,5 +102,6 @@
   <xacro:include filename="camera.xacro"/>
   <xacro:include filename="lidar.xacro"/>
   <xacro:include filename="sonar.xacro"/>
+  <xacro:include filename="raspbot_v2.ros2_control.xacro"/>
 
 </robot>
diff --git a/robot/src/raspbot_v2/urdf/robot_base.xacro b/robot/src/raspbot_v2/urdf/robot_base.xacro
index 2022983..12ee135 100644
--- a/robot/src/raspbot_v2/urdf/robot_base.xacro
+++ b/robot/src/raspbot_v2/urdf/robot_base.xacro
@@ -110,12 +110,15 @@
 
   <!-- ─────────────────────────────────────────────────────────────────────
        Gazebo — differential drive plugin
-       Bridges to /cmd_vel and publishes /odom.
+       Subscribes to /cmd_vel (gz topic), publishes /odom and the
+       odom → base_footprint TF.  Bridge gz topics to ROS 2 via ros_gz_bridge:
+         /cmd_vel    (ROS 2) → /cmd_vel    (gz)
+         /odom       (gz)    → /odom       (ROS 2, nav_msgs/Odometry)
+         /tf         (gz)    → /tf         (ROS 2, tf2_msgs/TFMessage)
        ───────────────────────────────────────────────────────────────────── -->
   <gazebo>
     <plugin filename="gz-sim-diff-drive-system"
             name="gz::sim::systems::DiffDrive">
-      <!-- Use averaged front/rear pairs for each side -->
       <left_joint>wheel_front_left_joint</left_joint>
       <left_joint>wheel_rear_left_joint</left_joint>
       <right_joint>wheel_front_right_joint</right_joint>
@@ -123,11 +126,30 @@
       <wheel_separation>${wheel_separation}</wheel_separation>
       <wheel_radius>${wheel_radius}</wheel_radius>
       <odom_publish_frequency>10</odom_publish_frequency>
-      <topic>cmd_vel</topic>
-      <odom_topic>odom</odom_topic>
+      <topic>/cmd_vel</topic>
+      <odom_topic>/odom</odom_topic>
+      <tf_topic>/tf</tf_topic>
       <frame_id>odom</frame_id>
       <child_frame_id>base_footprint</child_frame_id>
     </plugin>
   </gazebo>
 
+  <!-- ─────────────────────────────────────────────────────────────────────
+       Gazebo — joint state publisher for wheel joints
+       Publishes wheel positions on /gz/joint_states so robot_state_publisher
+       can update the wheel TF links.  Bridge to ROS 2 via ros_gz_bridge:
+         /gz/joint_states (gz) → /gz/joint_states (ROS 2, sensor_msgs/JointState)
+       Then merge with the ros2_control /joint_states in the launch file.
+       ───────────────────────────────────────────────────────────────────── -->
+  <gazebo>
+    <plugin filename="gz-sim-joint-state-publisher-system"
+            name="gz::sim::systems::JointStatePublisher">
+      <topic>/gz/joint_states</topic>
+      <joint_name>wheel_front_left_joint</joint_name>
+      <joint_name>wheel_front_right_joint</joint_name>
+      <joint_name>wheel_rear_left_joint</joint_name>
+      <joint_name>wheel_rear_right_joint</joint_name>
+    </plugin>
+  </gazebo>
+
 </robot>
diff --git a/robot/src/raspbot_v2/urdf/sonar.xacro b/robot/src/raspbot_v2/urdf/sonar.xacro
index b6fd96e..092c56f 100644
--- a/robot/src/raspbot_v2/urdf/sonar.xacro
+++ b/robot/src/raspbot_v2/urdf/sonar.xacro
@@ -34,24 +34,47 @@
   <!-- ─────────────────────────────────────────────────────────────────────
        Gazebo sonar sensor  (Gazebo Harmonic / gz-sim)
 
-       Simulated as a narrow-cone ray sensor. Data is published on gz topic
-       /ultrasonic/range and bridged to ROS 2 via ros_gz_bridge:
-         /ultrasonic/range  (gz) → /ultrasonic/range  (ROS 2, sensor_msgs/Range)
+       gz-sim has no native sonar sensor type.  The HC-SR04 is approximated
+       as a narrow-cone gpu_lidar — a 5×5 ray grid spanning ±7.5° (≈15° FOV)
+       in both axes, which matches the HC-SR04's beam pattern.
+
+       The sensor publishes a gz LaserScan on /ultrasonic/scan.  Bridge this
+       to ROS 2 via ros_gz_bridge, then convert to sensor_msgs/Range with a
+       small relay node (or just consume the LaserScan directly):
+         /ultrasonic/scan  (gz LaserScan) → /ultrasonic/scan  (ROS 2)
        ───────────────────────────────────────────────────────────────────── -->
   <gazebo reference="ultrasonic">
-    <sensor name="sonar" type="sonar">
+    <sensor name="sonar" type="gpu_lidar">
       <always_on>1</always_on>
       <update_rate>10</update_rate>
       <visualize>true</visualize>
-      <topic>/ultrasonic/range</topic>
+      <topic>/ultrasonic/scan</topic>
       <gz_frame_id>ultrasonic</gz_frame_id>
-      <sonar>
-        <!-- HC-SR04 datasheet values -->
-        <min>${sonar_min_range}</min>
-        <max>${sonar_max_range}</max>
-        <!-- Half-angle of the cone in radians (~15° full FOV → 0.13 rad half-angle) -->
-        <radius>0.13</radius>
-      </sonar>
+      <lidar>
+        <scan>
+          <horizontal>
+            <samples>5</samples>
+            <resolution>1</resolution>
+            <min_angle>-0.1309</min_angle>  <!-- −7.5° -->
+            <max_angle> 0.1309</max_angle>  <!-- +7.5° -->
+          </horizontal>
+          <vertical>
+            <samples>5</samples>
+            <min_angle>-0.1309</min_angle>
+            <max_angle> 0.1309</max_angle>
+          </vertical>
+        </scan>
+        <range>
+          <min>${sonar_min_range}</min>
+          <max>${sonar_max_range}</max>
+          <resolution>0.01</resolution>
+        </range>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.005</stddev>
+        </noise>
+      </lidar>
     </sensor>
   </gazebo>