410 lines
13 KiB
Python
410 lines
13 KiB
Python
import os
|
|
|
|
import xacro
|
|
from ament_index_python.packages import get_package_share_directory
|
|
from launch import LaunchDescription, condition
|
|
from launch.actions import (
|
|
DeclareLaunchArgument,
|
|
IncludeLaunchDescription,
|
|
OpaqueFunction,
|
|
)
|
|
from launch.conditions import IfCondition
|
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
|
from launch.substitutions import (
|
|
Command,
|
|
FindExecutable,
|
|
LaunchConfiguration,
|
|
PathJoinSubstitution,
|
|
)
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def launch_setup(context, *args, **kwargs):
|
|
robot_type = LaunchConfiguration("robot_type")
|
|
with_gripper_condition = LaunchConfiguration("with_gripper")
|
|
description_package = LaunchConfiguration("description_package")
|
|
description_file = LaunchConfiguration("description_file")
|
|
robot_name = LaunchConfiguration("robot_name")
|
|
use_moveit = LaunchConfiguration("use_moveit")
|
|
moveit_config_package = LaunchConfiguration("moveit_config_package")
|
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
|
use_controllers = LaunchConfiguration("use_controllers")
|
|
use_skills = LaunchConfiguration("use_skills")
|
|
namespace = LaunchConfiguration("namespace")
|
|
multi_robot = LaunchConfiguration("multi_robot")
|
|
robot_name = robot_name.perform(context)
|
|
namespace = namespace.perform(context)
|
|
robot_type = robot_type.perform(context)
|
|
description_package = description_package.perform(context)
|
|
description_file = description_file.perform(context)
|
|
multi_robot = multi_robot.perform(context)
|
|
robot_description_content = LaunchConfiguration("robot_description")
|
|
robot_description_semantic_content = LaunchConfiguration(
|
|
"robot_description_semantic"
|
|
)
|
|
control_space = LaunchConfiguration("control_space")
|
|
control_strategy = LaunchConfiguration("control_strategy")
|
|
ee_link_name = LaunchConfiguration("ee_link_name").perform(context)
|
|
base_link_name = LaunchConfiguration("base_link_name").perform(context)
|
|
|
|
use_rbs_utils = LaunchConfiguration("use_sim_time")
|
|
assembly_config_name = LaunchConfiguration("assembly_config_name")
|
|
|
|
remappings = []
|
|
if multi_robot == "true":
|
|
remappings.append([("/tf", "tf"), ("/tf_static", "tf_static")])
|
|
|
|
robot_description = {
|
|
"robot_description": robot_description_content.perform(context)
|
|
}
|
|
|
|
# robot_description_kinematics_filepath = os.path.join(
|
|
# get_package_share_directory(moveit_config_package.perform(context)),
|
|
# "moveit",
|
|
# "kinematics.yaml",
|
|
# )
|
|
|
|
# robot_description_kinematics = {
|
|
# "robot_description_kinematics": robot_description_kinematics
|
|
# }
|
|
|
|
robot_state_publisher = Node(
|
|
package="robot_state_publisher",
|
|
executable="robot_state_publisher",
|
|
namespace=namespace,
|
|
output="both",
|
|
remappings=remappings,
|
|
parameters=[{"use_sim_time": use_sim_time}, robot_description],
|
|
)
|
|
|
|
control = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("rbs_bringup"),
|
|
"launch",
|
|
"control.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"control_space": control_space,
|
|
"control_strategy": control_strategy,
|
|
"has_gripper": with_gripper_condition,
|
|
"namespace": namespace,
|
|
}.items(),
|
|
condition=IfCondition(use_controllers),
|
|
)
|
|
|
|
moveit = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare(moveit_config_package),
|
|
"launch",
|
|
"moveit.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"moveit_config_package": moveit_config_package,
|
|
# "moveit_config_file": moveit_config_file,
|
|
"use_sim_time": use_sim_time,
|
|
"tf_prefix": robot_name,
|
|
"with_gripper": with_gripper_condition,
|
|
"robot_description": robot_description_content,
|
|
# "robot_description_semantic": robot_description_semantic_content,
|
|
# "robot_description_kinematics": robot_description_kinematics_filepath,
|
|
"namespace": namespace,
|
|
}.items(),
|
|
condition=IfCondition(use_moveit),
|
|
)
|
|
|
|
skills = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("rbs_skill_servers"),
|
|
"launch",
|
|
"skills.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"robot_description": robot_description_content,
|
|
"robot_description_semantic": robot_description_semantic_content,
|
|
# "robot_description_kinematics": robot_description_kinematics_filepath,
|
|
"use_sim_time": use_sim_time,
|
|
"with_gripper_condition": with_gripper_condition,
|
|
"namespace": namespace,
|
|
"use_moveit": use_moveit,
|
|
"ee_link_name": ee_link_name,
|
|
"base_link_name": base_link_name,
|
|
}.items(),
|
|
condition=IfCondition(use_skills),
|
|
)
|
|
|
|
|
|
# assembly config loader
|
|
utils = IncludeLaunchDescription(
|
|
PythonLaunchDescriptionSource(
|
|
[
|
|
PathJoinSubstitution(
|
|
[
|
|
FindPackageShare("rbs_utils"),
|
|
"launch",
|
|
"utils.launch.py",
|
|
]
|
|
)
|
|
]
|
|
),
|
|
launch_arguments={
|
|
"use_sim_time": use_sim_time,
|
|
"assembly_config_name": assembly_config_name
|
|
}.items(),
|
|
condition=IfCondition(use_rbs_utils),
|
|
)
|
|
|
|
nodes_to_start = [
|
|
robot_state_publisher,
|
|
control,
|
|
moveit,
|
|
skills,
|
|
utils,
|
|
]
|
|
return nodes_to_start
|
|
|
|
|
|
def generate_launch_description():
|
|
declared_arguments = []
|
|
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_type",
|
|
description="Type of robot to launch, specified by name.",
|
|
choices=[
|
|
"rbs_arm",
|
|
"ar4",
|
|
"ur3",
|
|
"ur3e",
|
|
"ur5",
|
|
"ur5e",
|
|
"ur10",
|
|
"ur10e",
|
|
"ur16e",
|
|
"aubo-i5",
|
|
],
|
|
default_value="rbs_arm",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"controllers_file",
|
|
default_value="controllers.yaml",
|
|
description="YAML file containing configuration settings for the controllers.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_package",
|
|
default_value="rbs_arm",
|
|
description="Package containing the robot's URDF/XACRO description files. Can be set to use a custom description.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"description_file",
|
|
default_value="rbs_arm_modular.xacro",
|
|
description="URDF/XACRO file describing the robot's model.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_name",
|
|
default_value="arm0",
|
|
description="Prefix for joint names to support multi-robot setups. If changed, ensure joint names in controllers configuration are updated accordingly.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_package",
|
|
default_value="rbs_arm",
|
|
description="Package containing the MoveIt configuration files (SRDF/XACRO) for the robot. Can be customized.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"moveit_config_file",
|
|
default_value="rbs_arm.srdf.xacro",
|
|
description="MoveIt SRDF/XACRO file for robot configuration.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_sim_time",
|
|
default_value="true",
|
|
description="Enables simulation time in MoveIt, needed for trajectory planning in simulation.",
|
|
)
|
|
)
|
|
# declared_arguments.append(
|
|
# DeclareLaunchArgument(
|
|
# "gripper_name",
|
|
# default_value="",
|
|
# choices=["rbs_gripper", ""],
|
|
# description="Specify gripper name (leave empty if none).",
|
|
# )
|
|
# )
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"with_gripper",
|
|
default_value="true",
|
|
description="Specify if the robot includes a gripper.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_moveit",
|
|
default_value="true",
|
|
description="Specify if MoveIt should be launched.",
|
|
)
|
|
)
|
|
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_skills",
|
|
default_value="true",
|
|
description="Specify if skills should be launched.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_controllers",
|
|
default_value="true",
|
|
description="Specify if controllers should be launched.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"namespace",
|
|
default_value="",
|
|
description="ROS 2 namespace for the robot.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"x",
|
|
default_value="0.0",
|
|
description="X-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"y",
|
|
default_value="0.0",
|
|
description="Y-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"z",
|
|
default_value="0.0",
|
|
description="Z-coordinate for the robot's position in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"roll",
|
|
default_value="0.0",
|
|
description="Roll orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"pitch",
|
|
default_value="0.0",
|
|
description="Pitch orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"yaw",
|
|
default_value="0.0",
|
|
description="Yaw orientation of the robot in the world.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"multi_robot",
|
|
default_value="false",
|
|
description="Specify if the setup is for multiple robots.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description",
|
|
default_value="",
|
|
description="Custom robot description that overrides the internal default.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"control_space",
|
|
default_value="task",
|
|
choices=["task", "joint"],
|
|
description="Specify the control space for the robot (e.g., task space).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"control_strategy",
|
|
default_value="position",
|
|
choices=["position", "velocity", "effort"],
|
|
description="Specify the control strategy (e.g., position control).",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"robot_description_semantic",
|
|
default_value="",
|
|
description="Custom semantic robot description (SRDF) to override the default.",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"ee_link_name",
|
|
default_value="",
|
|
description="End effector name of robot arm",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"base_link_name",
|
|
default_value="",
|
|
description="Base link name if robot arm",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"assembly_config_name",
|
|
default_value="",
|
|
description="Assembly config name",
|
|
)
|
|
)
|
|
declared_arguments.append(
|
|
DeclareLaunchArgument(
|
|
"use_rbs_utils",
|
|
default_value="true",
|
|
description="Whwther use utils",
|
|
)
|
|
)
|
|
|
|
return LaunchDescription(
|
|
declared_arguments + [OpaqueFunction(function=launch_setup)]
|
|
)
|