Migrate to a new physical robot

Uses the mecanum controller properly across physical and virtual There is a timing issue with i2c which is why the control update is limited to 10hz The sonar and LED's arent yet working, this will come soon.
2026-05-27 13:25:20 +00:00
parent c1319a6357
commit ec554bcf2c
96 changed files with 2874 additions and 3730 deletions
@@ -0,0 +1,119 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, RegisterEventHandler,IncludeLaunchDescription
+from launch.conditions import IfCondition,UnlessCondition
+from launch.event_handlers import OnProcessExit
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+
+from launch_ros.actions import Node, PushRosNamespace
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    use_sim_time  = LaunchConfiguration('use_sim_time')
+    use_hardware  = LaunchConfiguration('use_hardware')
+    namespace     = LaunchConfiguration('namespace')
+    enable_camera = LaunchConfiguration('enable_camera')
+
+    declared_args = [
+        DeclareLaunchArgument(
+            'namespace', default_value='',
+            description='ROS namespace for all robot nodes (e.g. "robot1").'
+        ),
+        DeclareLaunchArgument(
+            'use_sim_time', default_value='false',
+            description='Use simulation time.'
+        ),
+        DeclareLaunchArgument(
+            'use_hardware', default_value='false',
+            description='Load the real I2C hardware interface plugin (set true on physical robot).'
+        ),
+        DeclareLaunchArgument(
+            'enable_camera', default_value='false',
+            description='Enable the camera node.'
+        ),
+    ]
+
+    
+    robot_controllers = PathJoinSubstitution([
+        FindPackageShare('raspbot_v2_control'), 'config', 'controllers.yaml'
+    ])
+    
+    robot_description_content = Command([
+        PathJoinSubstitution([FindExecutable(name='xacro')]),
+        ' ',
+        PathJoinSubstitution([
+            FindPackageShare('raspbot_v2_description'), 'urdf', 'raspbot_v2.urdf.xacro'
+        ]),
+        ' use_sim_time:=', use_sim_time,
+        ' use_hardware:=', use_hardware,
+        ' controller_config:=', robot_controllers,
+    ])
+    robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)}
+        
+    control_node = Node(
+        package='controller_manager',
+        executable='ros2_control_node',
+        parameters=[robot_description, robot_controllers],
+        output='both',
+        remappings=[
+            # ("~/robot_description", "robot_description"),
+            ("mecanum_drive_controller/reference", "/cmd_vel"),
+            ("mecanum_drive_controller/odometry", "/odom"),
+            ("mecanum_drive_controller/tf_odometry", "/tf"),
+        ],
+        condition=UnlessCondition(use_sim_time),
+    )
+    
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='both',
+        parameters=[robot_description],
+    )
+    
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ],
+    )
+
+    robot_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['mecanum_drive_controller', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ]
+    )
+    
+    # Delay starting the robot controller until the joint state broadcaster has finished starting up.
+    delay_robot_controller_after_joint_state_broadcaster = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[robot_controller_spawner],
+        ),
+    )
+    
+    pan_tilt_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['pan_tilt_controller', '--controller-manager',
+                   PathJoinSubstitution(["/", namespace, "controller_manager"]),
+                   '--controller-manager-timeout', '30',
+        ],
+        condition=IfCondition(enable_camera),
+    )
+    
+    return LaunchDescription([
+        *declared_args,
+        PushRosNamespace(namespace),
+        control_node,
+        robot_state_publisher_node,
+        joint_state_broadcaster_spawner,
+        delay_robot_controller_after_joint_state_broadcaster,
+        pan_tilt_controller_spawner,
+    ])