runtime/rbs_skill_servers/launch/skills.launch.py
Bill Finger c4c13d55b1 enhance skill server parameter handling and assembly service
- Unified `base_link` and `ee_link` parameter usage across launch files and skill server components.
- Improved synchronization in `mtjs_jtc` and `mtp_jtc` with a timeout mechanism for joint position updates, adding error handling when data is unavailable.
- Extended `AssemblyConfigService` to broadcast TF transforms for relative parts during initialization.
- Enhanced YAML parsing for `AssemblyConfigService` to handle missing orientations with default values.
- Updated `mtp_jtc_cart` to align parameter names with other skill server components.
- Added single-threaded executor to `AssemblyConfigService` for better lifecycle management.
2024-11-30 19:32:37 +03:00

153 lines
5.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=['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},
],
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),
),
],
)
assembly_config = Node(
package="rbs_utils",
executable="assembly_config_service.py",
namespace=namespace,
parameters=[{"use_sim_time": use_sim_time}],
output="screen",
)
nodes_to_start = [
assembly_config,
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)]
)