146 lines
5.1 KiB
Python
146 lines
5.1 KiB
Python
import os
|
|
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription
|
|
from launch.actions import (DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription,
|
|
Shutdown)
|
|
from launch.conditions import UnlessCondition
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
import yaml
|
|
|
|
|
|
def load_yaml(package_name, file_path):
|
|
package_path = get_package_share_directory(package_name)
|
|
absolute_file_path = os.path.join(package_path, file_path)
|
|
|
|
try:
|
|
with open(absolute_file_path, 'r') as file:
|
|
return yaml.safe_load(file)
|
|
except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available
|
|
return None
|
|
|
|
|
|
def generate_launch_description():
|
|
robot_ip_parameter_name = 'robot_ip'
|
|
use_fake_hardware_parameter_name = 'use_fake_hardware'
|
|
fake_sensor_commands_parameter_name = 'fake_sensor_commands'
|
|
|
|
robot_ip = LaunchConfiguration(robot_ip_parameter_name)
|
|
use_fake_hardware = LaunchConfiguration(use_fake_hardware_parameter_name)
|
|
fake_sensor_commands = LaunchConfiguration(fake_sensor_commands_parameter_name)
|
|
|
|
# planning_context
|
|
franka_xacro_file = os.path.join(get_package_share_directory('franka_description'), 'robots',
|
|
'panda_arm.urdf.xacro')
|
|
robot_description_config = Command(
|
|
[FindExecutable(name='xacro'), ' ', franka_xacro_file, ' hand:=true',
|
|
' robot_ip:=', robot_ip, ' use_fake_hardware:=', use_fake_hardware,
|
|
' fake_sensor_commands:=', fake_sensor_commands])
|
|
|
|
robot_description = {'robot_description': robot_description_config}
|
|
|
|
# Publish TF
|
|
robot_state_publisher = Node(
|
|
package='robot_state_publisher',
|
|
executable='robot_state_publisher',
|
|
name='robot_state_publisher',
|
|
output='both',
|
|
parameters=[robot_description],
|
|
)
|
|
|
|
ros2_controllers_path = os.path.join(
|
|
get_package_share_directory('franka_moveit_config'),
|
|
'config',
|
|
'panda_ros_controllers.yaml',
|
|
)
|
|
ros2_control_node = Node(
|
|
package='controller_manager',
|
|
executable='ros2_control_node',
|
|
parameters=[robot_description, ros2_controllers_path],
|
|
remappings=[('joint_states', 'franka/joint_states')],
|
|
output={
|
|
'stdout': 'screen',
|
|
'stderr': 'screen',
|
|
},
|
|
on_exit=Shutdown(),
|
|
)
|
|
|
|
load_controllers = []
|
|
for controller in ['panda_arm_controller', 'joint_state_broadcaster']:
|
|
load_controllers += [
|
|
ExecuteProcess(
|
|
cmd=['ros2 run controller_manager spawner {}'.format(controller)],
|
|
shell=True,
|
|
output='screen',
|
|
)
|
|
]
|
|
|
|
joint_state_publisher = Node(
|
|
package='joint_state_publisher',
|
|
executable='joint_state_publisher',
|
|
name='joint_state_publisher',
|
|
parameters=[
|
|
{'source_list': ['franka/joint_states', 'panda_gripper/joint_states'], 'rate': 30}],
|
|
)
|
|
|
|
franka_robot_state_broadcaster = Node(
|
|
package='controller_manager',
|
|
executable='spawner',
|
|
arguments=['franka_robot_state_broadcaster'],
|
|
output='screen',
|
|
condition=UnlessCondition(use_fake_hardware),
|
|
)
|
|
|
|
franka_grasp_demo = Node(
|
|
package='franka_logistics',
|
|
executable='franka_grasp_demo',
|
|
name='franka_grasp_demo',
|
|
output='screen',
|
|
)
|
|
|
|
franka_gui = Node(
|
|
package='franka_logistics',
|
|
executable='franka_position_run_gui',
|
|
name='franka_position_run_gui',
|
|
output='screen',
|
|
)
|
|
|
|
robot_arg = DeclareLaunchArgument(
|
|
robot_ip_parameter_name,
|
|
default_value='192.168.1.211',
|
|
description='Hostname or IP address of the robot.')
|
|
|
|
use_fake_hardware_arg = DeclareLaunchArgument(
|
|
use_fake_hardware_parameter_name,
|
|
default_value='false',
|
|
description='Use fake hardware')
|
|
fake_sensor_commands_arg = DeclareLaunchArgument(
|
|
fake_sensor_commands_parameter_name,
|
|
default_value='false',
|
|
description="Fake sensor commands. Only valid when '{}' is true".format(
|
|
use_fake_hardware_parameter_name))
|
|
|
|
gripper_launch_file = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource([PathJoinSubstitution(
|
|
[FindPackageShare('franka_gripper'), 'launch', 'gripper.launch.py'])]),
|
|
launch_arguments={'robot_ip': robot_ip,
|
|
use_fake_hardware_parameter_name: use_fake_hardware}.items(),
|
|
)
|
|
|
|
return LaunchDescription(
|
|
[
|
|
robot_arg,
|
|
use_fake_hardware_arg,
|
|
fake_sensor_commands_arg,
|
|
robot_state_publisher,
|
|
joint_state_publisher,
|
|
franka_gui,
|
|
franka_grasp_demo,
|
|
ros2_control_node,
|
|
franka_robot_state_broadcaster,
|
|
gripper_launch_file
|
|
] + load_controllers
|
|
) |