84 lines
2.4 KiB
Python
84 lines
2.4 KiB
Python
from numpy import load
|
|
from launch.launch_description import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, ExecuteProcess
|
|
from launch.conditions import UnlessCondition, IfCondition
|
|
from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
def generate_launch_description():
|
|
|
|
# Launch arguments
|
|
launch_args = []
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="robot_description",
|
|
description="Robot description XML file."
|
|
)
|
|
)
|
|
|
|
launch_args.append(
|
|
DeclareLaunchArgument(
|
|
name="sim",
|
|
default_value="true",
|
|
description="Launch robot in simulation or on real setup."
|
|
)
|
|
)
|
|
|
|
# Configure robot_description
|
|
robot_description = {"robot_description": LaunchConfiguration("robot_description")}
|
|
# Load controllers from YAML configuration file
|
|
controller_configurations = PathJoinSubstitution([
|
|
FindPackageShare("rasmt_support"),
|
|
"config",
|
|
"rasmt_position_velocity_controllers.yaml"
|
|
])
|
|
|
|
# Prepare controller manager and other required nodes
|
|
# ros2_control_node = Node(
|
|
# package="controller_manager",
|
|
# executable="ros2_control_node",
|
|
# parameters=[robot_description, controller_configurations],
|
|
# output={
|
|
# "stdout": "screen",
|
|
# "stderr": "screen",
|
|
# },
|
|
# )
|
|
|
|
robot_state_publisher = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
output="screen",
|
|
parameters=[robot_description]
|
|
)
|
|
|
|
# action_server_controller_hand_node = Node(
|
|
# package="robossembler_servers",
|
|
# executable="gripper_cmd_node"
|
|
# )
|
|
|
|
|
|
# Load controllers
|
|
load_controllers = []
|
|
for controller in [
|
|
"rasmt_arm_controller",
|
|
"rasmt_hand_controller",
|
|
"joint_state_broadcaster",
|
|
]:
|
|
load_controllers += [
|
|
ExecuteProcess(
|
|
cmd=["ros2 control load_controller --set-state start {}".format(controller)],
|
|
shell=True,
|
|
output="screen",
|
|
)
|
|
]
|
|
|
|
|
|
return LaunchDescription(
|
|
[
|
|
robot_state_publisher
|
|
]
|
|
+ load_controllers
|
|
+ launch_args
|
|
)
|