Add the initial (untested) gazebo plugin configuration.

2026-05-07 20:55:24 +00:00
parent 54ed5c6a82
commit d3c3d03ae7
7 changed files with 146 additions and 16 deletions
@@ -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
@@ -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,
    )
@@ -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,
@@ -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>
@@ -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>
@@ -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>
@@ -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>