127 lines
4.2 KiB
Python
127 lines
4.2 KiB
Python
from collections import namedtuple
|
|
from os import name
|
|
from launch import LaunchDescription
|
|
from launch.actions import (
|
|
DeclareLaunchArgument,
|
|
OpaqueFunction
|
|
)
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import Node
|
|
from rbs_launch_utils.launch_common import load_yaml
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
robot_description_decl = LaunchConfiguration("robot_description")
|
|
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
|
|
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
|
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
|
|
robot_description = {"robot_description": robot_description_decl}
|
|
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
|
|
namespace = LaunchConfiguration("namespace")
|
|
|
|
points_params = load_yaml(
|
|
"rbs_skill_servers", "config/gripperPositions.yaml"
|
|
)
|
|
|
|
move_topose_action_server = Node(
|
|
package="rbs_skill_servers",
|
|
executable="move_topose_action_server",
|
|
namespace=namespace,
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
]
|
|
)
|
|
|
|
gripper_control_node = Node(
|
|
package="rbs_skill_servers",
|
|
executable="gripper_control_action_server",
|
|
namespace=namespace,
|
|
parameters= [
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
],
|
|
condition=IfCondition(with_gripper_condition)
|
|
)
|
|
|
|
move_cartesian_path_action_server = Node(
|
|
package="rbs_skill_servers",
|
|
executable="move_cartesian_path_action_server",
|
|
namespace=namespace,
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
]
|
|
)
|
|
|
|
# FIXME: The name of this node, "move_topose,"
|
|
# is intended to be different from the actual MoveToPose node.
|
|
move_joint_state_action_server = Node(
|
|
package="rbs_skill_servers",
|
|
executable="move_to_joint_states_action_server",
|
|
namespace=namespace,
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
]
|
|
)
|
|
|
|
grasp_pose_loader = Node(
|
|
package="rbs_skill_servers",
|
|
executable="pick_place_pose_loader_service_server",
|
|
namespace=namespace,
|
|
output="screen"
|
|
)
|
|
|
|
nodes_to_start =[
|
|
move_topose_action_server,
|
|
gripper_control_node,
|
|
move_cartesian_path_action_server,
|
|
move_joint_state_action_server,
|
|
# grasp_pose_loader
|
|
]
|
|
return nodes_to_start
|
|
|
|
def generate_launch_description():
|
|
|
|
declared_arguments = []
|
|
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description",
|
|
default_value="",
|
|
description="robot description string",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("robot_description_semantic", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("robot_description_kinematics", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("use_sim_time", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("with_gripper_condition", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("points_params_filepath", default_value="")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("namespace", default_value="")
|
|
)
|
|
|
|
|
|
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])
|