Attached arm to rover, arm moving script added
This commit is contained in:
Marcin M
@@ -1,74 +1,94 @@
|
||||
import os
|
||||
import xacro
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
|
||||
from launch.actions import IncludeLaunchDescription, RegisterEventHandler, TimerAction
|
||||
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
|
||||
|
||||
# Absolute and relative path variables
|
||||
world_path = './src/world_description/corn_field.sdf'
|
||||
arm_urdf_path = '/home/marcin/arm_ws/robotarm_description/urdf/robot_arm.urdf'
|
||||
rover_sdf_path = '/home/marcin/arm_ws/src/world_description/harvester.sdf'
|
||||
|
||||
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
|
||||
# 1. Include the standard MoveIt 2 move_group launch file (Delayed for stability)
|
||||
move_group_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(moveit_config_pkg, 'launch', 'move_group.launch.py')
|
||||
)
|
||||
)
|
||||
|
||||
# 3. Launch Gazebo Sim (Modern Gazebo) empty world
|
||||
# 2. Launch Gazebo Sim (Modern Gazebo) loaded directly into your field 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
|
||||
launch_arguments={'gz_args': '-r ' + world_path }.items(),
|
||||
)
|
||||
|
||||
# 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(
|
||||
# 3. MODIFIED: Spawns the ENTIRE rover model from the master SDF file.
|
||||
# Because your SDF includes the arm internally, Gazebo builds the combined asset together.
|
||||
spawn_complete_rover = 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
|
||||
'-file', rover_sdf_path,
|
||||
'-name', 'harvester_robot', # Matches the model namespace inside your world bridges
|
||||
'-z', '0.1' # Clear the field ground mesh on entry
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# 5. Define ros2_control Spawners
|
||||
# Broadcaster for joint states (vital for MoveIt to know where the arm currently is)
|
||||
# 4. Define ros2_control Spawners
|
||||
joint_state_broadcaster_spawner = Node(
|
||||
package='controller_manager',
|
||||
executable='spawner',
|
||||
arguments=['joint_state_broadcaster', '--controller-manager', '/controller_manager'],
|
||||
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'],
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["arm_controller", "--controller-manager", "/controller_manager"],
|
||||
)
|
||||
|
||||
# 6. Bridge Gazebo Clock to ROS 2 (Crucial for simulation time syncing)
|
||||
# 5. Bridge Gazebo Clock and Data Channels to ROS 2 Jazzy
|
||||
bridge = Node(
|
||||
package='ros_gz_bridge',
|
||||
executable='parameter_bridge',
|
||||
arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
|
||||
arguments=[
|
||||
'/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock',
|
||||
'/world/corn_field_wide/model/harvester_robot/joint_state@sensor_msgs/msg/JointState[gz.msgs.Model',
|
||||
'/model/harvester_robot/joint/joint_1/cmd_pos@std_msgs/msg/Float64]gz.msgs.Double'
|
||||
],
|
||||
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
|
||||
# 6. Process the xacro file dynamically for the ROS 2 Kinematics Stack
|
||||
xacro_file = os.path.join(
|
||||
get_package_share_directory('robotarm_description'),
|
||||
'urdf',
|
||||
'robot_arm.urdf.xacro'
|
||||
)
|
||||
robot_description_raw = xacro.process_file(xacro_file).toxml()
|
||||
|
||||
# 7. Robot State Publisher (Feeds the mathematical link tree strictly to MoveIt 2)
|
||||
robot_state_publisher = Node(
|
||||
package='robot_state_publisher',
|
||||
executable='robot_state_publisher',
|
||||
output='both',
|
||||
parameters=[{'robot_description': robot_description_raw, 'use_sim_time': True}]
|
||||
)
|
||||
|
||||
# 8. Timing/Order Sequence Event Handlers
|
||||
# Waits for the COMPLETE rover to exist before spinning up hardware controller instances
|
||||
delay_joint_state_broadcaster = RegisterEventHandler(
|
||||
event_handler=OnProcessExit(
|
||||
target_action=spawn_robot,
|
||||
target_action=spawn_complete_rover,
|
||||
on_exit=[joint_state_broadcaster_spawner],
|
||||
)
|
||||
)
|
||||
@@ -79,41 +99,18 @@ def generate_launch_description():
|
||||
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,
|
||||
spawn_complete_rover, # Spawns the single, complete vehicle
|
||||
robot_state_publisher,
|
||||
bridge,
|
||||
delay_joint_state_broadcaster,
|
||||
delay_arm_controller,
|
||||
robot_state_publisher_node,
|
||||
delay_move_group_launch,
|
||||
])
|
||||
Reference in New Issue
Block a user