- 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.
153 lines
5.6 KiB
Python
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)]
|
|
)
|