runtime/rasmt_support/launch/rasmt_control.launch.py

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
)