From b99edfd661f5f80109eb86a9eda04fcf40b2954c Mon Sep 17 00:00:00 2001
From: ilithebutterfly <iliana.decarufel@gmail.com>
Date: Tue, 18 Jun 2024 15:39:31 -0400
Subject: [PATCH 1/2] Cleaned up ros2 control xacro, added macros, fixed
 cmd_vel for diffdrive

---
 .../rove_controller_bluetooth.launch.py       |   4 +-
 .../launch/rove_controller_usb.launch.py      |   3 +
 .../urdf/rove.ros2_control.urdf.xacro         | 118 +++++-------------
 src/rove_description/urdf/rove.urdf.xacro     |   2 +-
 4 files changed, 41 insertions(+), 86 deletions(-)

diff --git a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
index 43dc67e..3c4a870 100644
--- a/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
+++ b/src/rove_bringup/launch/rove_controller_bluetooth.launch.py
@@ -25,7 +25,9 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
-                # ('/cmd_vel', '/model/rove/cmd_vel')
+                # Remap needs to happen here since the diff_drive_controller node
+                # is started via a spawner, thus remappings do not work on it
+                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped') 
             ],
         ),
         Node(
diff --git a/src/rove_bringup/launch/rove_controller_usb.launch.py b/src/rove_bringup/launch/rove_controller_usb.launch.py
index 61153ce..9d34d47 100644
--- a/src/rove_bringup/launch/rove_controller_usb.launch.py
+++ b/src/rove_bringup/launch/rove_controller_usb.launch.py
@@ -25,6 +25,9 @@ def generate_launch_description():
             parameters=[teleop_joy_params_file],
             remappings=[
                 ('/joy', '/joy'),
+                # Remap needs to happen here since the diff_drive_controller node
+                # is started via a spawner, thus remappings do not work on it
+                ('/cmd_vel', '/diff_drive_controller/cmd_vel_unstamped')
             ],
         ),
         Node(
diff --git a/src/rove_description/urdf/rove.ros2_control.urdf.xacro b/src/rove_description/urdf/rove.ros2_control.urdf.xacro
index 45461da..a061319 100644
--- a/src/rove_description/urdf/rove.ros2_control.urdf.xacro
+++ b/src/rove_description/urdf/rove.ros2_control.urdf.xacro
@@ -1,14 +1,33 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">    
-    <xacro:macro name="rove_odrive_motor" params="position suffix id">
+    <xacro:macro name="track_motor_control" params="suffix nodeid">
+        <joint name="track_${suffix}_j">
+            <param name="node_id">${nodeid}</param>
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="track_fake_motor_control" params="suffix">
+        <joint name="track_${suffix}_j">
+            <command_interface name="velocity"/>
+            <state_interface name="position"/>
+            <state_interface name="velocity"/>
+            <state_interface name="effort"/>
+        </joint>
+    </xacro:macro>
+
+    <xacro:macro name="rove_odrive_motor" params="suffix id">
         <xacro:unless value="${(id == -1)}">
-            <xacro:track_motor_control position="${position}" suffix="${suffix}" nodeid="${id}"/>
+            <xacro:track_motor_control suffix="${suffix}" nodeid="${id}"/>
         </xacro:unless>
     </xacro:macro>
 
-    <xacro:macro name="rove_fake_odrive_motor" params="position suffix id">
+    <xacro:macro name="rove_fake_odrive_motor" params="suffix id">
         <xacro:if value="${(id == -1)}">
-            <xacro:track_fake_motor_control position="${position}" suffix="${suffix}"/>
+            <xacro:track_fake_motor_control suffix="${suffix}"/>
         </xacro:if>
     </xacro:macro>
 
@@ -18,91 +37,22 @@
                 <plugin>odrive_ros2_control_plugin/ODriveHardwareInterface</plugin>
                 <param name="can">can0</param>
             </hardware>
-            <joint name="track_fr_j">
-                <param name="node_id">23</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="track_rr_j">
-                <param name="node_id">24</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-
-            <joint name="track_fl_j">
-                <param name="node_id">21</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="track_rl_j">
-                <param name="node_id">22</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-
-            <!-- <joint name="track_rl_j">
-                <param name="node_id">1</param>
-            </joint> -->
-            <!-- <xacro:rove_odrive_motor position="f" suffix="l" id="${fl_id}"/>
-            <xacro:rove_odrive_motor position="r" suffix="l" id="${rl_id}"/>
-            <xacro:rove_odrive_motor position="f" suffix="r" id="${fr_id}"/>
-            <xacro:rove_odrive_motor position="r" suffix="r" id="${rr_id}"/> -->
+            <xacro:rove_odrive_motor suffix="fl" id="${fl_id}"/>
+            <xacro:rove_odrive_motor suffix="rl" id="${rl_id}"/>
+            <xacro:rove_odrive_motor suffix="fr" id="${fr_id}"/>
+            <xacro:rove_odrive_motor suffix="rr" id="${rr_id}"/>
         </ros2_control>
-        <!-- <ros2_control name="fake_odrive_control" type="system">
+        
+        <ros2_control name="fake_odrive_control" type="system">
             <hardware>
                 <plugin>mock_components/GenericSystem</plugin>
-                <param name="calculate_dynamics">true</param>
             </hardware>
-            <<joint name="track_fl_j">
-                <param name="node_id">1</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="track_rl_j">
-                <param name="node_id">2</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="track_fr_j">
-                <param name="node_id">3</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-            <joint name="track_rr_j">
-                <param name="node_id">3</param>
-                <command_interface name="velocity"/>
-                <state_interface name="position"/>
-                <state_interface name="velocity"/>
-                <state_interface name="effort"/>
-            </joint>
-        </ros2_control> -->
-        <!-- <ros2_control name="fake_odrive_control" type="system">
-            <hardware>
-                <plugin>mock_components/GenericSystem</plugin>
-            </hardware>
-            <xacro:rove_fake_odrive_motor position="f" suffix="l" id="${fl_id}"/>
-            <xacro:rove_fake_odrive_motor position="r" suffix="l" id="${rl_id}"/>
-            <xacro:rove_fake_odrive_motor position="f" suffix="r" id="${fr_id}"/>
-            <xacro:rove_fake_odrive_motor position="r" suffix="r" id="${rr_id}"/>
-        </ros2_control> -->
+            <xacro:rove_fake_odrive_motor suffix="fl" id="${fl_id}"/>
+            <xacro:rove_fake_odrive_motor suffix="rl" id="${rl_id}"/>
+            <xacro:rove_fake_odrive_motor suffix="fr" id="${fr_id}"/>
+            <xacro:rove_fake_odrive_motor suffix="rr" id="${rr_id}"/>
+        </ros2_control>
     </xacro:macro>
 
 
-    <xacro:macro name="rove_ros2_control">
-        <xacro:odrive_control/>
-    </xacro:macro>
 </robot>
\ No newline at end of file
diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 3dae78e..077788b 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -42,7 +42,7 @@
 
 
     <!-- <xacro:rove_ros2_control/> -->
-    <xacro:odrive_control fl_id="0" rl_id="-1" fr_id="-1" rr_id="-1"/>
+    <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>
 
 
     <!-- Insert ovis on top of rove -->

From b4256e026453a9da2cc5cf1e3763c991886df620 Mon Sep 17 00:00:00 2001
From: ilithebutterfly <iliana.decarufel@gmail.com>
Date: Tue, 18 Jun 2024 15:54:59 -0400
Subject: [PATCH 2/2] Added comment to indicate how to mock odrives

---
 src/rove_description/urdf/rove.urdf.xacro | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/rove_description/urdf/rove.urdf.xacro b/src/rove_description/urdf/rove.urdf.xacro
index 077788b..82334f1 100644
--- a/src/rove_description/urdf/rove.urdf.xacro
+++ b/src/rove_description/urdf/rove.urdf.xacro
@@ -42,6 +42,7 @@
 
 
     <!-- <xacro:rove_ros2_control/> -->
+     <!-- Put these at -1 to mock the motors if working without the motors -->
     <xacro:odrive_control fl_id="21" rl_id="22" fr_id="23" rr_id="24"/>