From 6c9b621d68cad8bd1f8a5a31cf1b00b1445b833a Mon Sep 17 00:00:00 2001
From: Vincent Belpois <vincent.belpois@gmail.com>
Date: Mon, 22 Apr 2024 22:57:04 +0200
Subject: [PATCH] ros2_control seem to be working, testing the communication
 with Pico

---
 src/ezbot_robot/config/controllers.yaml       | 17 ++--
 src/ezbot_robot/description/robot.urdf.xacro  |  5 +-
 .../description/ros2_control.xacro            | 27 +++---
 src/ezbot_robot/launch/real_robot.launch.py   | 84 +++++++++++++++----
 src/ezbot_robot/launch/rsp.launch.py          |  3 +-
 5 files changed, 96 insertions(+), 40 deletions(-)

diff --git a/src/ezbot_robot/config/controllers.yaml b/src/ezbot_robot/config/controllers.yaml
index e460e37..f65595a 100644
--- a/src/ezbot_robot/config/controllers.yaml
+++ b/src/ezbot_robot/config/controllers.yaml
@@ -7,19 +7,22 @@ controller_manager:
       type: joint_state_broadcaster/JointStateBroadcaster
 
     omnidirectional_controller:
-      type: omnidirectional_controllers/OmnidirectionalController
+      type: diff_drive_controller/DiffDriveController
 
 omnidirectional_controller:
   ros__parameters:
     joints:
-      - roue_avant_joint1
+      - roue_avant_joint
+      - roue_gauche_joint
+      - roue_arriere_joint
+      - roue_droite_joint
 
     wheel_names: 
-      - roue_avant_joint1
-      - roue_gauche_joint2
-      - roue_arriere_joint3
-      - roue_droite_joint4
-
+      - roue_avant_joint
+      - roue_gauche_joint
+      - roue_arriere_joint
+      - roue_droite_joint
+      
     robot_radius: 0.25
     wheel_radius: 0.03
 
diff --git a/src/ezbot_robot/description/robot.urdf.xacro b/src/ezbot_robot/description/robot.urdf.xacro
index c03007a..3a1fae4 100644
--- a/src/ezbot_robot/description/robot.urdf.xacro
+++ b/src/ezbot_robot/description/robot.urdf.xacro
@@ -3,7 +3,10 @@
 
     <xacro:include filename="robot_core.xacro" />
     <xacro:include filename="lidar.xacro"/>
-    <xacro:include filename="gazebo_control.xacro" />
+    <xacro:include filename="ros2_control.xacro" />
+    <!--
+        <xacro:include filename="gazebo_control.xacro" />
+    -->
     <xacro:include filename="camera.xacro" />
     <xacro:include filename="imu.xacro" />
 </robot>
\ No newline at end of file
diff --git a/src/ezbot_robot/description/ros2_control.xacro b/src/ezbot_robot/description/ros2_control.xacro
index ff6d26b..db70423 100644
--- a/src/ezbot_robot/description/ros2_control.xacro
+++ b/src/ezbot_robot/description/ros2_control.xacro
@@ -1,8 +1,9 @@
 <?xml version="1.0"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-    <ros2_control name="ros2_control_wheel_joint" type="system">
+    <ros2_control name="hellothere" type="system">
     
+      
         <hardware>
             <plugin>omnidrive_rppico/OmniDriveRpPicoHardware</plugin>
             <param name="front_wheel_name">roue_avant_joint</param>
@@ -10,8 +11,8 @@
             <param name="back_wheel_name">roue_arriere_joint</param>
             <param name="right_wheel_name">roue_droite_joint</param>
 
-            <param name="loop_rate">30</param>
-            <param name="device">/dev/pts/9</param>
+            <param name="loop_rate">30.0</param>
+            <param name="device">/dev/pts/7</param>
             <param name="baudrate">115200</param>
 
             <param name="timeout_ms">10000</param>
@@ -19,40 +20,38 @@
         </hardware>
   
 
-        <joint name="roue_avant_joint1">
+        <joint name="roue_avant_joint">
             <command_interface name="velocity"> 
                 <param name="min">-10</param>
-                <param name="max">10</param>
+                <param name="max">10</param> 
             </command_interface>
-            <state_interface name="velocity"/>
             <state_interface name="position"/>
+            <state_interface name="velocity"/>
         </joint>
-        <joint name="roue_gauche_joint2">
+        <joint name="roue_gauche_joint">
             <command_interface name="velocity">
                 <param name="min">-10</param>
                 <param name="max">10</param>
             </command_interface>
-            <state_interface name="velocity"/>
             <state_interface name="position"/>
+            <state_interface name="velocity"/>
         </joint>
-        <joint name="roue_arriere_joint3">
+        <joint name="roue_arriere_joint">
             <command_interface name="velocity">
                 <param name="min">-10</param>
                 <param name="max">10</param>
             </command_interface>
-            <state_interface name="velocity"/>
             <state_interface name="position"/>
+            <state_interface name="velocity"/>
         </joint>
-        <joint name="roue_droite_joint4">
+        <joint name="roue_droite_joint">
             <command_interface name="velocity">
                 <param name="min">-10</param>
                 <param name="max">10</param>
             </command_interface>
-            <state_interface name="velocity"/>
             <state_interface name="position"/>
+            <state_interface name="velocity"/>
         </joint>
     </ros2_control>
 
-
-
 </robot>
\ No newline at end of file
diff --git a/src/ezbot_robot/launch/real_robot.launch.py b/src/ezbot_robot/launch/real_robot.launch.py
index 3f33da8..386de8f 100644
--- a/src/ezbot_robot/launch/real_robot.launch.py
+++ b/src/ezbot_robot/launch/real_robot.launch.py
@@ -11,10 +11,12 @@
 from launch.actions import DeclareLaunchArgument
 import launch_ros.actions
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions import Command
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
 from launch_ros.actions import Node
 from launch.substitutions import Command
+import xacro
 
 
 
@@ -24,10 +26,9 @@ def generate_launch_description():
     # check if log_level is set
 
     logger = LaunchConfiguration('log_level')  
-    # if not set, set to info
+    # if not set, set to debug
     if logger is None:
-        logger = 'info'
-        
+        logger = 'debug'
 
 
 
@@ -40,26 +41,77 @@ def generate_launch_description():
                 PythonLaunchDescriptionSource([os.path.join(
                     get_package_share_directory(package_name),'launch','rsp.launch.py'
                 )]), launch_arguments={'use_sim_time': 'false'}.items()
+
     )
 
-    joystick = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource([os.path.join(
-            get_package_share_directory("teleop_twist_joy"),'launch','teleop-launch.py'
-        )]), launch_arguments={'config_filepath': '/home/vincent/joystick.yaml','joy_vel':'/omnidirectional_controller/cmd_vel_unstamped'}.items(),
-        
+    #robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])
+    controller_params_file = os.path.join(get_package_share_directory('ezbot_robot'), 'config', 'controllers.yaml')
+    
+    pkg_path = os.path.join(get_package_share_directory('ezbot_robot'))
+    xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
+    robot_description = {"robot_description": xacro.process_file(xacro_file).toxml()}
+
+
+    controller_manager = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[controller_params_file],
+        remappings = [('/controller_manager/robot_description', '/robot_description')],
+    )
+
+
+
+
+
+    delayed_controller_manager = TimerAction(
+        period=2.0,
+        actions=[controller_manager],
+    )
+
+
+    omnidrive_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['omnidirectional_controller'],
+    )
+    delayed_omnidrive_spawner = TimerAction(
+        period=1.0,
+        actions=[omnidrive_spawner],
     )
 
-    homemade_controller = Node(
-        package='ezbot_robot',
-        executable='homemade_controller',
-        name='homemade_controller',
-        output='screen',
-        parameters=[{'log_level': logger}]
+    joint_broad_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster'],
     )
+    delayed_joint_broad_spawner = TimerAction(
+        period=1.0,
+        actions=[joint_broad_spawner],
+    )   
+
+
+
+    #joystick = IncludeLaunchDescription(
+    #    PythonLaunchDescriptionSource([os.path.join(
+    #        get_package_share_directory("teleop_twist_joy"),'launch','teleop-launch.py'
+    #    )]), launch_arguments={'config_filepath': '/home/vincent/joystick.yaml','joy_vel':'/omnidirectional_controller/cmd_vel_unstamped'}.items(),
+    #    
+    #)
+
+    #homemade_controller = Node(
+    #    package='ezbot_robot',
+    #    executable='homemade_controller',
+    #    name='homemade_controller',
+    #    output='screen',
+    #    parameters=[{'log_level': logger}]
+    #)
 
     return LaunchDescription([
         rsp,
-        joystick,
-        homemade_controller
+        delayed_controller_manager,
+        delayed_omnidrive_spawner,
+        delayed_joint_broad_spawner
+        #joystick,
+        #homemade_controller
         
     ])
\ No newline at end of file
diff --git a/src/ezbot_robot/launch/rsp.launch.py b/src/ezbot_robot/launch/rsp.launch.py
index f57d7a1..2d0cdc3 100644
--- a/src/ezbot_robot/launch/rsp.launch.py
+++ b/src/ezbot_robot/launch/rsp.launch.py
@@ -27,10 +27,9 @@ def generate_launch_description():
         package='robot_state_publisher',
         executable='robot_state_publisher',
         output='screen',
-        parameters=[params]
+        parameters=[params],
     )
 
-
     # Launch!
     return LaunchDescription([
         DeclareLaunchArgument(