From 86684b0bffb83dca13f56298f09d9d6d142741e9 Mon Sep 17 00:00:00 2001
From: David <ds0196@uah.edu>
Date: Fri, 10 Oct 2025 02:22:12 -0500
Subject: [PATCH] feat: add ros2_control to Core URDF

---
 .../config/ros2_controllers.yaml              | 35 ++++++++++++++++++
 .../launch/display.launch.py                  |  2 +-
 .../launch/robot_state_publisher.launch.py    |  4 +--
 ...rover_description.urdf => core_rover.urdf} |  8 ++---
 .../urdf/core_rover_description.xacro         |  8 +++++
 .../urdf/ros2_control.xacro                   | 36 +++++++++++++++++++
 6 files changed, 86 insertions(+), 7 deletions(-)
 create mode 100644 src/core_rover_description/config/ros2_controllers.yaml
 rename src/core_rover_description/urdf/{core_rover_description.urdf => core_rover.urdf} (95%)
 create mode 100644 src/core_rover_description/urdf/core_rover_description.xacro
 create mode 100644 src/core_rover_description/urdf/ros2_control.xacro

diff --git a/src/core_rover_description/config/ros2_controllers.yaml b/src/core_rover_description/config/ros2_controllers.yaml
new file mode 100644
index 0000000..c14fbc3
--- /dev/null
+++ b/src/core_rover_description/config/ros2_controllers.yaml
@@ -0,0 +1,35 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 10
+    use_sim_time: true
+
+    diff_controller:
+      type: "diff_drive_controller/DiffDriveController"
+
+    joint_broadcaster:
+      type: "joint_state_broadcaster/JointStateBroadcaster"
+
+diff_controller: 
+  ros__parameters:
+    left_wheel_names: ["bl_wheel_joint", "fl_wheel_joint"]
+    right_wheel_names: ["br_wheel_joint", "fr_wheel_joint"]
+
+    wheel_separation: 0.836
+    wheels_per_side: 2
+    wheel_radius: 0.171
+
+    base_frame_id: "base_link"
+    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
+
+    position_feedback: true
+    open_loop: false
+    enable_odom_tf: true
+
+    cmd_vel_timeout: 0.5
+    publish_limited_velocity: false
+
+    use_stamped_vel: false
+    publish_rate: 10.0
+
+    # TODO: add reasonable velocity and acceleration limits
diff --git a/src/core_rover_description/launch/display.launch.py b/src/core_rover_description/launch/display.launch.py
index 14a385a..b15f799 100644
--- a/src/core_rover_description/launch/display.launch.py
+++ b/src/core_rover_description/launch/display.launch.py
@@ -21,7 +21,7 @@ def generate_launch_description():
   default_rviz_config_path = os.path.join(pkg_share, 'config/rviz_basic_settings.rviz')
  
   # Set the path to the URDF file
-  default_urdf_model_path = os.path.join(pkg_share, 'urdf/core_rover_description.urdf')
+  default_urdf_model_path = os.path.join(pkg_share, 'urdf/core_rover_description.xacro')
  
   ########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############  
   # Launch configuration variables specific to simulation
diff --git a/src/core_rover_description/launch/robot_state_publisher.launch.py b/src/core_rover_description/launch/robot_state_publisher.launch.py
index ed3b87b..04bea01 100644
--- a/src/core_rover_description/launch/robot_state_publisher.launch.py
+++ b/src/core_rover_description/launch/robot_state_publisher.launch.py
@@ -115,7 +115,7 @@ def generate_launch_description():
     """
     # Define filenames
     urdf_package = 'core_rover_description'
-    urdf_filename = 'core_rover_description.urdf'
+    urdf_filename = 'core_rover_description.xacro'
     rviz_config_filename = 'rviz_basic_settings.rviz'
 
     # Set paths to important files
@@ -233,6 +233,6 @@ def generate_launch_description():
     ld.add_action(start_joint_state_publisher_cmd)
     ld.add_action(start_joint_state_publisher_gui_cmd)
     ld.add_action(start_robot_state_publisher_cmd)
-    ld.add_action(start_rviz_cmd)
+    # ld.add_action(start_rviz_cmd)
 
     return ld
diff --git a/src/core_rover_description/urdf/core_rover_description.urdf b/src/core_rover_description/urdf/core_rover.urdf
similarity index 95%
rename from src/core_rover_description/urdf/core_rover_description.urdf
rename to src/core_rover_description/urdf/core_rover.urdf
index bf37292..97f35a5 100644
--- a/src/core_rover_description/urdf/core_rover_description.urdf
+++ b/src/core_rover_description/urdf/core_rover.urdf
@@ -152,7 +152,7 @@
   </link>
 
   <joint
-    name="br_wheel_axis"
+    name="br_wheel_joint"
     type="continuous">
     <origin
       xyz="-0.41838 -0.17155 0.12984"
@@ -162,7 +162,7 @@
     <child
       link="br_wheel" />
     <axis
-      xyz="1 0 0" />
+      xyz="-1 0 0" />
   </joint>
 
   <link
@@ -217,7 +217,7 @@
     <child
       link="fr_wheel" />
     <axis
-      xyz="1 0 0" />
+      xyz="-1 0 0" />
   </joint>
 
   <link
@@ -262,7 +262,7 @@
   </link>
 
   <joint
-    name="averaging_bar_axis"
+    name="averaging_bar_joint"
     type="revolute">
     <origin
       xyz="0 -0.23362 -0.0508"
diff --git a/src/core_rover_description/urdf/core_rover_description.xacro b/src/core_rover_description/urdf/core_rover_description.xacro
new file mode 100644
index 0000000..2454c05
--- /dev/null
+++ b/src/core_rover_description/urdf/core_rover_description.xacro
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="core_rover">
+
+  <xacro:include filename="core_rover.urdf"/>
+
+  <xacro:include filename="ros2_control.xacro"/>
+
+</robot>
\ No newline at end of file
diff --git a/src/core_rover_description/urdf/ros2_control.xacro b/src/core_rover_description/urdf/ros2_control.xacro
new file mode 100644
index 0000000..7924e1f
--- /dev/null
+++ b/src/core_rover_description/urdf/ros2_control.xacro
@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+    <ros2_control name="GazeboSystem" type="system">
+        <hardware>
+            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
+        </hardware>
+        <joint name="bl_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="br_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="fl_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="fr_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
+
+    <gazebo>
+        <plugin filename="gz_ros2_control-system" name="gz_ros2_control::GazeboSimROS2ControlPlugin">
+            <parameters>$(find core_rover_description)/config/ros2_controllers.yaml</parameters>
+        </plugin>
+    </gazebo>
+
+</robot>
\ No newline at end of file