From 4b8230231e016c8666ee9f9f4047e1e1978d3504 Mon Sep 17 00:00:00 2001
From: JesusSilvaUtrera <jsilvautrera@gmail.com>
Date: Thu, 9 May 2024 16:47:20 +0200
Subject: [PATCH 1/3] Hardware configuration is now being loaded from YAML
 file, updated README and updated launch files to pass xacro arguments

Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com>
---
 andino_description/README.md                  | 10 ++-
 .../config/andino/hardware.yaml               |  6 ++
 .../launch/andino_description.launch.py       | 11 +--
 .../launch/view_andino.launch.py              |  9 +--
 andino_description/urdf/andino.urdf.xacro     |  5 ++
 .../urdf/include/andino_control.urdf.xacro    | 67 +++++++++++--------
 6 files changed, 68 insertions(+), 40 deletions(-)
 create mode 100644 andino_description/config/andino/hardware.yaml

diff --git a/andino_description/README.md b/andino_description/README.md
index 73d299e2..dd132b0e 100644
--- a/andino_description/README.md
+++ b/andino_description/README.md
@@ -5,6 +5,14 @@ This package holds the urdf description of the robot.
 
 <img src="docs/robot_rviz.png">
 
+In the `urdf` folder you have the URDF files that contain the description of the robot, divided in different modules and merged into `andino.urdf.xacro` file.
+
+## Configuration
+
+In case you want to change the physical properties of some of the components of the robot, you can do it modifying the default YAML files inside the `config/andino` folder.
+
+You can even add your own configuration files in another directory in the `config` folder, and passing this directory to the main file using `yaml_config_dir` xacro argument inside the launch files.
+
 ## Launch Files
 
 For launching robot state publisher for filling up static tf information and serving the description of the robot. Typically used during robot bringup.
@@ -16,5 +24,3 @@ For launching the robot state publisher and providing some visualization with rv
 ```
 ros2 launch andino_description view_andino.launch.py
 ```
-
-
diff --git a/andino_description/config/andino/hardware.yaml b/andino_description/config/andino/hardware.yaml
new file mode 100644
index 00000000..cc162915
--- /dev/null
+++ b/andino_description/config/andino/hardware.yaml
@@ -0,0 +1,6 @@
+left_wheel_name: left_wheel_joint
+right_wheel_name: right_wheel_joint
+serial_device: /dev/ttyUSB_ARDUINO
+baud_rate: 57600
+timeout: 1000
+enc_ticks_per_rev: 585
diff --git a/andino_description/launch/andino_description.launch.py b/andino_description/launch/andino_description.launch.py
index 78122fa2..52093f22 100644
--- a/andino_description/launch/andino_description.launch.py
+++ b/andino_description/launch/andino_description.launch.py
@@ -31,7 +31,7 @@
 import os
 
 from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
+from launch import LaunchDescription, LaunchContext
 from launch.actions import DeclareLaunchArgument
 from launch.conditions import IfCondition
 from launch.substitutions import LaunchConfiguration
@@ -41,15 +41,16 @@
 
 def generate_launch_description():
 
+    # Obtains andino_description's share directory path.
+    pkg_andino_description = get_package_share_directory('andino_description')
+
     # Arguments
     rsp_argument = DeclareLaunchArgument('rsp', default_value='true',
                           description='Run robot state publisher node.')
 
-    # Obtains andino_description's share directory path.
-    pkg_andino_description = get_package_share_directory('andino_description')
-
     # Obtain urdf from xacro files.
-    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'))
+    arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
+    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
     robot_desc = doc.toprettyxml(indent='  ')
     params = {'robot_description': robot_desc,
               'publish_frequency': 30.0}
diff --git a/andino_description/launch/view_andino.launch.py b/andino_description/launch/view_andino.launch.py
index 33f93169..3c3ba484 100644
--- a/andino_description/launch/view_andino.launch.py
+++ b/andino_description/launch/view_andino.launch.py
@@ -41,6 +41,9 @@
 
 def generate_launch_description():
 
+    # Obtains andino_description's share directory path.
+    pkg_andino_description = get_package_share_directory('andino_description')
+
     # Arguments
     rviz_argument = DeclareLaunchArgument('rviz', default_value='true',
                           description='Open RViz.')
@@ -49,11 +52,9 @@ def generate_launch_description():
     jsp_argument = DeclareLaunchArgument('jsp', default_value='true',
                           description='Run joint state publisher node.')
 
-    # Obtains andino_description's share directory path.
-    pkg_andino_description = get_package_share_directory('andino_description')
-
     # Obtain urdf from xacro files.
-    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'))
+    arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
+    doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
     robot_desc = doc.toprettyxml(indent='  ')
     params = {'robot_description': robot_desc,
               'publish_frequency': 30.0}
diff --git a/andino_description/urdf/andino.urdf.xacro b/andino_description/urdf/andino.urdf.xacro
index 099b5716..1db4ed31 100644
--- a/andino_description/urdf/andino.urdf.xacro
+++ b/andino_description/urdf/andino.urdf.xacro
@@ -30,6 +30,10 @@
   <xacro:property name="sensor_yaml" value="$(arg yaml_config_dir)/sensors.yaml" />
   <xacro:property name="sensor_prop" value="${xacro.load_yaml(sensor_yaml)}"/>
 
+  <xacro:property name="hardware_yaml" value="$(arg yaml_config_dir)/hardware.yaml" />
+  <xacro:property name="hardware_props" value="${xacro.load_yaml(hardware_yaml)}"/>
+
+
   <!-- Footprint link -->
   <xacro:footprint wheel_props="${wheel_props}" />
 
@@ -89,6 +93,7 @@
   <!-- ros2_control -->
   <xacro:if value="$(arg use_real_ros_control)">
     <xacro:include filename="$(find ${package_name})/urdf/include/andino_control.urdf.xacro" />
+    <xacro:control hardware_props="${hardware_props}" />
   </xacro:if>
 
 
diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro
index 8dc8949a..ddedf601 100644
--- a/andino_description/urdf/include/andino_control.urdf.xacro
+++ b/andino_description/urdf/include/andino_control.urdf.xacro
@@ -1,34 +1,43 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-  <!-- TODO(francocipollone): This xacro should be part of andino_control package instead.-->
-  <ros2_control name="RealRobot" type="system">
-    <hardware>
-      <plugin>andino_base/DiffDriveAndino</plugin>
-      <!-- TODO(francocipollone): Parameters like loop_rate, device, baud_rate, etc should be loaded from a file or passed via the launch file as xacro args -->
-      <param name="left_wheel_name">left_wheel_joint</param>
-      <param name="right_wheel_name">right_wheel_joint</param>
-      <param name="serial_device">/dev/ttyUSB_ARDUINO</param>
-      <param name="baud_rate">57600</param>
-      <param name="timeout">1000</param>
-      <param name="enc_ticks_per_rev">585</param>
-    </hardware>
-    <joint name="left_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-5</param>
-        <param name="max">5</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-    <joint name="right_wheel_joint">
-      <command_interface name="velocity">
-        <param name="min">-5</param>
-        <param name="max">5</param>
-      </command_interface>
-      <state_interface name="velocity"/>
-      <state_interface name="position"/>
-    </joint>
-  </ros2_control>
+<!-- ===================== Control xacro =========================================
+
+  params:
+  - hardware_props [dictionary]: hardware properties loaded from the YAML file.
+-->
+
+  <xacro:macro name="control" params="hardware_props" >
+
+    <!-- TODO(francocipollone): This xacro should be part of andino_control package instead.-->
+    <ros2_control name="RealRobot" type="system">
+      <hardware>
+        <plugin>andino_base/DiffDriveAndino</plugin>
+        <param name="left_wheel_name">left_wheel_joint</param>
+        <param name="right_wheel_name">right_wheel_joint</param>
+        <param name="serial_device">/dev/ttyUSB_ARDUINO</param>
+        <param name="baud_rate">57600</param>
+        <param name="timeout">1000</param>
+        <param name="enc_ticks_per_rev">585</param>
+      </hardware>
+      <joint name="left_wheel_joint">
+        <command_interface name="velocity">
+          <param name="min">-5</param>
+          <param name="max">5</param>
+        </command_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+      </joint>
+      <joint name="right_wheel_joint">
+        <command_interface name="velocity">
+          <param name="min">-5</param>
+          <param name="max">5</param>
+        </command_interface>
+        <state_interface name="velocity"/>
+        <state_interface name="position"/>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
 
 </robot>

From 20302d562c6fbedd6bb432fe3721069eab9b2ccb Mon Sep 17 00:00:00 2001
From: JesusSilvaUtrera <jsilvautrera@gmail.com>
Date: Tue, 14 May 2024 08:38:28 +0200
Subject: [PATCH 2/3] Added hardware_props in andino_control.xacro

Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com>
---
 .../urdf/include/andino_control.urdf.xacro           | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro
index ddedf601..20699616 100644
--- a/andino_description/urdf/include/andino_control.urdf.xacro
+++ b/andino_description/urdf/include/andino_control.urdf.xacro
@@ -13,12 +13,12 @@
     <ros2_control name="RealRobot" type="system">
       <hardware>
         <plugin>andino_base/DiffDriveAndino</plugin>
-        <param name="left_wheel_name">left_wheel_joint</param>
-        <param name="right_wheel_name">right_wheel_joint</param>
-        <param name="serial_device">/dev/ttyUSB_ARDUINO</param>
-        <param name="baud_rate">57600</param>
-        <param name="timeout">1000</param>
-        <param name="enc_ticks_per_rev">585</param>
+        <param name="left_wheel_name">${hardware_props['left_wheel_name']}</param>
+        <param name="right_wheel_name">${hardware_props['right_wheel_name']}</param>
+        <param name="serial_device">${hardware_props['serial_device']}</param>
+        <param name="baud_rate">${hardware_props['baud_rate']}</param>
+        <param name="timeout">1${hardware_props['timeout']}</param>
+        <param name="enc_ticks_per_rev">${hardware_props['enc_ticks_per_rev']}</param>
       </hardware>
       <joint name="left_wheel_joint">
         <command_interface name="velocity">

From fd451a2f4b208933d4d6082f64edb2ffd7cbe2e6 Mon Sep 17 00:00:00 2001
From: JesusSilvaUtrera <jsilvautrera@gmail.com>
Date: Wed, 15 May 2024 12:15:18 +0200
Subject: [PATCH 3/3] Addressed Javi comments in the PR

Signed-off-by: JesusSilvaUtrera <jsilvautrera@gmail.com>
---
 andino_description/README.md                              | 2 +-
 andino_description/launch/andino_description.launch.py    | 6 +++---
 andino_description/launch/view_andino.launch.py           | 6 +++---
 andino_description/urdf/include/andino_control.urdf.xacro | 2 +-
 4 files changed, 8 insertions(+), 8 deletions(-)

diff --git a/andino_description/README.md b/andino_description/README.md
index dd132b0e..8e59ab99 100644
--- a/andino_description/README.md
+++ b/andino_description/README.md
@@ -11,7 +11,7 @@ In the `urdf` folder you have the URDF files that contain the description of the
 
 In case you want to change the physical properties of some of the components of the robot, you can do it modifying the default YAML files inside the `config/andino` folder.
 
-You can even add your own configuration files in another directory in the `config` folder, and passing this directory to the main file using `yaml_config_dir` xacro argument inside the launch files.
+You can even add your own configuration files in another directory in the `config` folder, and pass this directory to the main file using the `yaml_config_dir` xacro argument on the launch files.
 
 ## Launch Files
 
diff --git a/andino_description/launch/andino_description.launch.py b/andino_description/launch/andino_description.launch.py
index 52093f22..396059d7 100644
--- a/andino_description/launch/andino_description.launch.py
+++ b/andino_description/launch/andino_description.launch.py
@@ -41,13 +41,13 @@
 
 def generate_launch_description():
 
-    # Obtains andino_description's share directory path.
-    pkg_andino_description = get_package_share_directory('andino_description')
-
     # Arguments
     rsp_argument = DeclareLaunchArgument('rsp', default_value='true',
                           description='Run robot state publisher node.')
 
+    # Obtains andino_description's share directory path.
+    pkg_andino_description = get_package_share_directory('andino_description')
+
     # Obtain urdf from xacro files.
     arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
     doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
diff --git a/andino_description/launch/view_andino.launch.py b/andino_description/launch/view_andino.launch.py
index 3c3ba484..396b8a16 100644
--- a/andino_description/launch/view_andino.launch.py
+++ b/andino_description/launch/view_andino.launch.py
@@ -41,9 +41,6 @@
 
 def generate_launch_description():
 
-    # Obtains andino_description's share directory path.
-    pkg_andino_description = get_package_share_directory('andino_description')
-
     # Arguments
     rviz_argument = DeclareLaunchArgument('rviz', default_value='true',
                           description='Open RViz.')
@@ -52,6 +49,9 @@ def generate_launch_description():
     jsp_argument = DeclareLaunchArgument('jsp', default_value='true',
                           description='Run joint state publisher node.')
 
+    # Obtains andino_description's share directory path.
+    pkg_andino_description = get_package_share_directory('andino_description')
+
     # Obtain urdf from xacro files.
     arguments = {'yaml_config_dir': os.path.join(pkg_andino_description, 'config', 'andino')}
     doc = xacro.process_file(os.path.join(pkg_andino_description, 'urdf', 'andino.urdf.xacro'), mappings = arguments)
diff --git a/andino_description/urdf/include/andino_control.urdf.xacro b/andino_description/urdf/include/andino_control.urdf.xacro
index 20699616..9aee0f29 100644
--- a/andino_description/urdf/include/andino_control.urdf.xacro
+++ b/andino_description/urdf/include/andino_control.urdf.xacro
@@ -17,7 +17,7 @@
         <param name="right_wheel_name">${hardware_props['right_wheel_name']}</param>
         <param name="serial_device">${hardware_props['serial_device']}</param>
         <param name="baud_rate">${hardware_props['baud_rate']}</param>
-        <param name="timeout">1${hardware_props['timeout']}</param>
+        <param name="timeout">${hardware_props['timeout']}</param>
         <param name="enc_ticks_per_rev">${hardware_props['enc_ticks_per_rev']}</param>
       </hardware>
       <joint name="left_wheel_joint">