138 lines
4.6 KiB
Python
138 lines
4.6 KiB
Python
from os.path import join
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch_ros.actions import Node, ComposableNodeContainer
|
|
from launch_ros.descriptions import ComposableNode
|
|
from launch.actions import ExecuteProcess
|
|
import yaml
|
|
|
|
def get_file(path):
|
|
with open(path, 'r') as f:
|
|
return f.read()
|
|
|
|
def get_yaml(path):
|
|
with open(path, 'r') as f:
|
|
return yaml.safe_load(f)
|
|
|
|
|
|
def generate_launch_description():
|
|
launch_root = get_package_share_directory('ar3_config')
|
|
description_root = get_package_share_directory('ar3_description')
|
|
config_root = join(launch_root, 'config')
|
|
|
|
robot_description = {'robot_description': get_file(join(description_root, 'ar3.urdf'))}
|
|
robot_description_semantic = {'robot_description_semantic': get_file(join(description_root, 'ar3.srdf'))}
|
|
robot_description_kinematics = get_yaml(join(config_root, 'kinematics.yaml'))
|
|
|
|
return LaunchDescription([
|
|
# RViz
|
|
Node(
|
|
package='rviz2',
|
|
executable='rviz2',
|
|
output='log',
|
|
arguments=['-d', join(launch_root, 'rviz', 'ar3.rviz')],
|
|
parameters=[robot_description, robot_description_semantic, robot_description_kinematics]
|
|
),
|
|
|
|
# Attatch robot to map
|
|
Node(
|
|
package='tf2_ros',
|
|
executable='static_transform_publisher',
|
|
output='screen',
|
|
arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
|
|
),
|
|
|
|
Node(
|
|
package='move_group_interface',
|
|
executable='move_group_interface_ndde',
|
|
output='screen',
|
|
),
|
|
Node(
|
|
package='move_group_interface',
|
|
executable='client_node',
|
|
output='screen',
|
|
),
|
|
Node(
|
|
package='joystickmove',
|
|
executable='move',
|
|
output='screen',
|
|
),
|
|
|
|
# Create joystick controller
|
|
ComposableNodeContainer(
|
|
name="joystick_container",
|
|
namespace="/",
|
|
package="rclcpp_components",
|
|
executable="component_container",
|
|
composable_node_descriptions=[
|
|
ComposableNode(
|
|
package="moveit_servo",
|
|
plugin="moveit_servo::ServoServer",
|
|
name="servo_server",
|
|
parameters=[
|
|
{'moveit_servo': get_yaml(join(config_root, 'ar3_joy.yaml'))},
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
],
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
),
|
|
ComposableNode(
|
|
package="joy",
|
|
plugin="joy::Joy",
|
|
name="joy_node",
|
|
extra_arguments=[{"use_intra_process_comms": True}],
|
|
)
|
|
],
|
|
output="screen",
|
|
),
|
|
|
|
# Controller
|
|
Node(
|
|
package='controller_manager',
|
|
executable='ros2_control_node',
|
|
output='screen',
|
|
parameters=[robot_description, join(config_root, 'controller_manager.yaml')]
|
|
),
|
|
|
|
# Start controllers
|
|
ExecuteProcess(
|
|
cmd=['ros2 run controller_manager spawner.py joint_trajectory_controller'],
|
|
shell=True,
|
|
output='screen'),
|
|
ExecuteProcess(
|
|
cmd=['ros2 run controller_manager spawner.py joint_state_broadcaster'],
|
|
shell=True,
|
|
output='screen'),
|
|
|
|
# Run platform
|
|
Node(
|
|
package='cobot_platform',
|
|
executable='move',
|
|
output='screen',
|
|
),
|
|
|
|
# Start the actual move_group node/action server
|
|
Node(
|
|
package="moveit_ros_move_group",
|
|
executable="move_group",
|
|
output="screen",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
get_yaml(join(config_root, 'ompl_planning.yaml')),
|
|
get_yaml(join(config_root, 'trajectory.yaml')),
|
|
get_yaml(join(config_root, 'planning_scene.yaml')),
|
|
get_yaml(join(config_root, "ar3_controllers.yaml")),
|
|
],
|
|
),
|
|
|
|
# Robot state publisher
|
|
Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
output='screen',
|
|
parameters=[robot_description]
|
|
),
|
|
])
|