174 lines
6.6 KiB
Python
174 lines
6.6 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
|
from launch.conditions import IfCondition
|
|
from launch.substitutions import LaunchConfiguration
|
|
from launch_ros.actions import ComposableNodeContainer, Node
|
|
from launch_ros.actions.composable_node_container import ComposableNode
|
|
from rbs_launch_utils.launch_common import load_yaml, load_yaml_abs
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
robot_description_content = LaunchConfiguration("robot_description")
|
|
robot_description_semantic_content = LaunchConfiguration(
|
|
"robot_description_semantic"
|
|
)
|
|
robot_description_kinematics_filepath = LaunchConfiguration(
|
|
"robot_description_kinematics"
|
|
)
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
use_moveit = LaunchConfiguration("use_moveit")
|
|
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
|
base_link_name = LaunchConfiguration("base_link_name").perform(context)
|
|
# with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
|
|
|
robot_description = {"robot_description": robot_description_content}
|
|
robot_description_semantic = {
|
|
"robot_description_semantic": robot_description_semantic_content
|
|
}
|
|
namespace = LaunchConfiguration("namespace")
|
|
|
|
# kinematics_yaml = load_yaml_abs(
|
|
# robot_description_kinematics_filepath.perform(context)
|
|
# )
|
|
# robot_description_kinematics = {"robot_description_kinematics": kinematics_yaml}
|
|
|
|
skills_container = ComposableNodeContainer(
|
|
name="skills",
|
|
namespace=namespace,
|
|
package="rclcpp_components",
|
|
# prefix=['kitty -e gdb -ex run --args'],
|
|
# prefix=['gdbserver localhost:1234'],
|
|
executable="component_container_mt",
|
|
composable_node_descriptions=[
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToJointStateActionServer",
|
|
name="mtjs_jtc",
|
|
parameters=[{"use_sim_time": use_sim_time}],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::CartesianMoveToPose",
|
|
name="mtp_cart",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToPose",
|
|
name="mtp_jtc",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
{"base_link": base_link_name},
|
|
{"ee_link": ee_link_name},
|
|
robot_description,
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveToPoseJtcCartesian",
|
|
name="mtp_jtc_cart",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"robot_name": namespace},
|
|
{"base_link": base_link_name},
|
|
{"ee_link": ee_link_name},
|
|
robot_description,
|
|
],
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveitMtp",
|
|
name="mtp_moveit",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
# robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
{"base_link": base_link_name},
|
|
{"ee_link": ee_link_name},
|
|
],
|
|
condition=IfCondition(use_moveit),
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveitMtpCart",
|
|
name="mtp_moveit_cart",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
# robot_description_kinematics,
|
|
{"use_sim_time": use_sim_time},
|
|
],
|
|
condition=IfCondition(use_moveit),
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::MoveitMtpOrientationConstraint",
|
|
name="mtp_moveit_orientation_constraint",
|
|
parameters=[
|
|
robot_description,
|
|
robot_description_semantic,
|
|
{"use_sim_time": use_sim_time},
|
|
{"base_link": base_link_name},
|
|
{"ee_link": ee_link_name},
|
|
],
|
|
condition=IfCondition(use_moveit),
|
|
),
|
|
ComposableNode(
|
|
package="rbs_skill_servers",
|
|
plugin="rbs_skill_actions::TwistCmdWithCondition",
|
|
name="twist_cmd_with_condition",
|
|
parameters=[
|
|
{"use_sim_time": use_sim_time},
|
|
{"condition_topic" : "/gz_ros2_vacuum_gripper_plugin/contact_info"},
|
|
# {"robot_name": namespace},
|
|
{"base_link": base_link_name},
|
|
# {"ee_link": ee_link_name},
|
|
# robot_description,
|
|
],
|
|
),
|
|
|
|
],
|
|
)
|
|
|
|
nodes_to_start = [
|
|
skills_container,
|
|
]
|
|
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="false")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("use_moveit", default_value="false")
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument("with_gripper_condition", default_value="")
|
|
)
|
|
declared_arguments.append(DeclareLaunchArgument("namespace", default_value=""))
|
|
declared_arguments.append(DeclareLaunchArgument("ee_link_name", default_value=""))
|
|
declared_arguments.append(DeclareLaunchArgument("base_link_name", default_value=""))
|
|
return LaunchDescription(
|
|
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
|
)
|