ros2_control seem to be working, testing the communication with Pico

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
 

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>

src/ezbot_robot/description/ros2_control.xacro

@@ -1,58 +1,57 @@
 <?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>
             <param name="left_wheel_name">roue_gauche_joint</param>
             <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>
             <param name="enc_counts_per_rev">3600</param>
         </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>

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
 
     ])

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(