Add initial

2026-07-15 14:10:00 +00:00
parent a81b99cd3c
commit 76701e81c0
10 changed files with 1281 additions and 34 deletions
@@ -0,0 +1,46 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 50  # Hz — matches ESP32 auto-report rate
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    mecanum_drive_controller:
+      type: mecanum_drive_controller/MecanumDriveController
+
+# ─────────────────────────────────────────────────────────────────────────────
+# Mecanum drive controller
+# ─────────────────────────────────────────────────────────────────────────────
+# NOTE: The URDF joint names do not match the physical wheel positions due to a
+# naming inconsistency in agv_pro.urdf.  The mapping between the controller's
+# logical positions and the URDF joint names is as follows:
+#
+#   Physical position │ URDF joint name
+#   ──────────────────┼────────────────────────────
+#   front-left  (FL)  │ left_front_wheel_joint   ✓
+#   front-right (FR)  │ left_rear_wheel_joint     ← physically front-right
+#   rear-left   (RL)  │ right_front_wheel_joint   ← physically rear-left
+#   rear-right  (RR)  │ right_rear_wheel_joint   ✓
+#
+# The same mapping is used by the hardware interface (front_left_joint /
+# front_right_joint / rear_left_joint / rear_right_joint params in the URDF
+# <ros2_control> block).
+mecanum_drive_controller:
+  ros__parameters:
+    front_left_wheel_command_joint_name:  left_front_wheel_joint
+    front_right_wheel_command_joint_name: left_rear_wheel_joint
+    rear_left_wheel_command_joint_name:   right_front_wheel_joint
+    rear_right_wheel_command_joint_name:  right_rear_wheel_joint
+
+    odom_frame_id:  odom
+    base_frame_id:  base_footprint
+    enable_odom_tf: true
+
+    kinematics:
+      # wheel_radius: height of wheel centre above ground from URDF joint origin
+      # (base_footprint→base_link = 0.020 m + wheel joint z-offset ≈ 0.052 m = 0.072 m)
+      wheels_radius: 0.072
+      # sum_of_robot_center_projection_on_X_Y_axis = lx + ly
+      # lx ≈ 0.172 m (half wheelbase from URDF joint x-origins)
+      # ly ≈ 0.180 m (half track from URDF joint y-origins)
+      sum_of_robot_center_projection_on_X_Y_axis: 0.352
@@ -1,10 +1,11 @@
 import os
 from launch import LaunchDescription
 from launch.conditions import IfCondition
-from launch_ros.actions import Node,PushRosNamespace
-from launch.actions import DeclareLaunchArgument,IncludeLaunchDescription
-from launch.substitutions import Command,LaunchConfiguration,PythonExpression
+from launch_ros.actions import Node, PushRosNamespace
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, TimerAction
+from launch.substitutions import Command, LaunchConfiguration, PythonExpression
 from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.parameter_descriptions import ParameterValue
 from ament_index_python.packages import get_package_share_directory

 def include_lidar(pkg_name, launch_file, enable_lidar, lidar_type, expected_type):
@@ -38,17 +39,24 @@ def generate_launch_description():
        'agv_pro.urdf'
    )

-    robot_description_content = Command([
-        'xacro ',
-        urdf_file,
-        ' namespace:=',
-        PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
-    ])
+    # Pass both namespace and port_name into the xacro processor so that the
+    # <ros2_control> block in the URDF picks up the correct serial port.
+    robot_description_content = ParameterValue(
+        Command([
+            'xacro ',
+            urdf_file,
+            ' namespace:=',
+            PythonExpression(['"', namespace, '" + "/" if "', namespace, '" != "" else ""']),
+            ' port_name:=',
+            port_name_arg,
+        ]),
+        value_type=str
+    )

    declare_port_name_arg = DeclareLaunchArgument(
-        'port_name', 
+        'port_name',
        default_value='/dev/agvpro_controller',
-        description='port name, e.g. /dev/ttyACM0'
+        description='Serial port for the AGV Pro base controller'
    )

    declare_namespace_arg = DeclareLaunchArgument(
@@ -71,22 +79,22 @@ def generate_launch_description():

    ns_action = PushRosNamespace(namespace)

-    agv_pro_node = Node(
-        package='agv_pro_base',
-        executable='agv_pro_node',
-        name='agv_pro_node',
-        output='screen',
-        parameters=[{
-            'port_name': port_name_arg,
-            'namespace': namespace,             
-        }],
-        remappings=[('cmd_vel', '/cmd_vel')]
+    ros2_controllers_yaml = os.path.join(
+        get_package_share_directory('agv_pro_bringup'),
+        'config',
+        'ros2_controllers.yaml'
    )

-    joint_state_pub = Node(
-        package='joint_state_publisher',
-        executable='joint_state_publisher',
-        name='joint_state_publisher'
+    # controller_manager loads the hardware plugin (serial comms, wheel states)
+    # and manages the controllers.
+    controller_manager = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[
+            {'robot_description': robot_description_content},
+            ros2_controllers_yaml,
+        ],
+        output='screen',
    )

    robot_state_pub = Node(
@@ -97,9 +105,57 @@ def generate_launch_description():
        output='screen'
    )

+    # joint_state_broadcaster publishes /joint_states from the hardware interface.
+    # Replaces the old static joint_state_publisher.
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=[
+            'joint_state_broadcaster',
+            '--controller-manager', 'controller_manager',
+        ],
+        output='screen',
+    )
+
+    # mecanum_drive_controller subscribes to ~/reference (TwistStamped).
+    # --controller-ros-args remaps it to /cmd_vel so nav2 and teleop work
+    # without extra flags (teleop still needs stamped:=true).
+    # Delayed slightly so controller_manager is ready before spawning.
+    mecanum_drive_controller_spawner = TimerAction(
+        period=2.0,
+        actions=[
+            Node(
+                package='controller_manager',
+                executable='spawner',
+                arguments=[
+                    'mecanum_drive_controller',
+                    '--controller-manager', 'controller_manager',
+                    '--controller-ros-args', '-r reference:=/cmd_vel',
+                ],
+                output='screen',
+            )
+        ]
+    )
+
+    # Relay /cmd_vel (TwistStamped) → /mecanum_drive_controller/reference.
+    # This bridges the standard nav2/teleop topic to the controller's
+    # internal subscription, which ros2_control does not remap at load time.
+    # lazy:=false ensures the subscription exists before any publisher appears.
+    cmd_vel_relay = Node(
+        package='topic_tools',
+        executable='relay',
+        name='cmd_vel_relay',
+        parameters=[{
+            'input_topic':  '/cmd_vel',
+            'output_topic': '/mecanum_drive_controller/reference',
+            'lazy': False,
+        }],
+        output='screen',
+    )
+
    lidar_launchs = [
        include_lidar('lslidar_driver', 'lsn10p_launch.py', enable_lidar, lidar_type, 'n10p'),
-        include_lidar('agv_pro_bringup', 'MID360_launch.py',enable_lidar, lidar_type, 'mid360'),
+        include_lidar('agv_pro_bringup', 'MID360_launch.py', enable_lidar, lidar_type, 'mid360'),
        include_lidar('agv_pro_bringup', 'unitree_l2_launch.py', enable_lidar, lidar_type, 'l2'),
    ]

@@ -110,9 +166,11 @@ def generate_launch_description():
            declare_enable_lidar_arg,
            declare_lidar_type_arg,
            ns_action,
-            agv_pro_node,
-            joint_state_pub,
+            controller_manager,
            robot_state_pub,
+            joint_state_broadcaster_spawner,
+            mecanum_drive_controller_spawner,
+            cmd_vel_relay,
            *lidar_launchs,
        ]
    )
@@ -9,13 +9,15 @@

  <buildtool_depend>ament_cmake</buildtool_depend>
  <exec_depend>robot_state_publisher</exec_depend>
-  <exec_depend>joint_state_publisher</exec_depend>
-  <exec_depend>rviz2</exec_depend>
+  <exec_depend>controller_manager</exec_depend>
+  <exec_depend>mecanum_drive_controller</exec_depend>
+  <exec_depend>joint_state_broadcaster</exec_depend>
+  <exec_depend>topic_tools</exec_depend>
+  <exec_depend>agv_pro_hardware</exec_depend>
  <exec_depend>agv_pro_description</exec_depend>
-  <exec_depend>agv_pro_base</exec_depend>
+  <exec_depend>rviz2</exec_depend>
  <exec_depend>cartographer_ros</exec_depend>
  <exec_depend>livox_ros_driver2</exec_depend>
-  <exec_depend>unitree_lidar_ros2</exec_depend>
  <exec_depend>lslidar_driver</exec_depend>

  <test_depend>ament_lint_auto</test_depend>