Working arm

2026-06-04 21:48:44 +02:00
commit 5b46c41031
34 changed files with 1073 additions and 0 deletions
@@ -0,0 +1,119 @@
+import os
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, RegisterEventHandler
+from launch.event_handlers import OnProcessExit
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.actions import Node
+from launch.actions import IncludeLaunchDescription, TimerAction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+
+def generate_launch_description():
+    # 1. Define package directories (Adjust names if yours differ)
+    moveit_config_pkg = get_package_share_directory('arm_moveit_config')
+    
+    # 2. Include the standard MoveIt 2 move_group launch file
+    # This automatically loads the robot description (URDF/XACRO) and handles robot_state_publisher
+    move_group_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(moveit_config_pkg, 'launch', 'move_group.launch.py')
+        )
+    )
+
+    # 3. Launch Gazebo Sim (Modern Gazebo) empty world
+    gazebo_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            os.path.join(get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')
+        ),
+        launch_arguments={'gz_args': '-r empty.sdf'}.items(), # '-r' runs simulation immediately
+    )
+
+    # 4. Spawn the robot entity inside the Gazebo world
+    # It listens to the /robot_description topic published by MoveIt's robot_state_publisher
+    spawn_robot = Node(
+        package='ros_gz_sim',
+        executable='create',
+        arguments=[
+            '-topic', 'robot_description',
+            '-name', 'six_axis_arm',
+            '-z', '0.0' # Adjust if your base needs to sit higher
+        ],
+        output='screen',
+    )
+
+    # 5. Define ros2_control Spawners
+    # Broadcaster for joint states (vital for MoveIt to know where the arm currently is)
+    joint_state_broadcaster_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
+    )
+
+    # Trajectory controller for sending trajectory paths to Gazebo
+    arm_controller_spawner = Node(
+        package='controller_manager',
+        executable='spawner',
+        arguments=['arm_controller', '--controller-manager', '/controller_manager'],
+    )
+
+    # 6. Bridge Gazebo Clock to ROS 2 (Crucial for simulation time syncing)
+    bridge = Node(
+        package='ros_gz_bridge',
+        executable='parameter_bridge',
+        arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
+        output='screen'
+    )
+
+    # 7. Delay controller spawning until AFTER the robot is spawned in Gazebo
+    # This prevents ros2_control from crashing due to Gazebo not being ready yet
+    delay_joint_state_broadcaster = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=spawn_robot,
+            on_exit=[joint_state_broadcaster_spawner],
+        )
+    )
+
+    delay_arm_controller = RegisterEventHandler(
+        event_handler=OnProcessExit(
+            target_action=joint_state_broadcaster_spawner,
+            on_exit=[arm_controller_spawner],
+        )
+    )
+    delay_move_group_launch = TimerAction(
+        period=5.0,
+        actions=[move_group_launch]
+    )
+
+    import xacro
+
+    # ... inside generate_launch_description() ...
+
+    # 1. Path to your actual Xacro file
+    xacro_file = os.path.join(
+        get_package_share_directory('robotarm_description'), # Change to your description pkg
+        'urdf',
+        'robot_arm.urdf.xacro' # Change to your actual file name
+    )
+
+    # 2. Process the xacro file into raw XML text
+    robot_description_raw = xacro.process_file(xacro_file).toxml()
+
+    # 3. Add the explicit Robot State Publisher Node
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description': robot_description_raw, 'use_sim_time': True}]
+    )
+
+
+    return LaunchDescription([
+        gazebo_launch,
+        delay_move_group_launch,
+        #move_group_launch,
+        spawn_robot,
+        bridge,
+        delay_joint_state_broadcaster,
+        delay_arm_controller,
+        robot_state_publisher_node,
+    ])