Bill Finger
ef4b015491
- **Added support for assembly configuration in `rbs_bringup` and `rbs_robot` launches**: - Introduced new launch arguments: `use_rbs_utils` and `assembly_config_name`. - Integrated `rbs_utils`'s `utils.launch.py` for handling assembly configurations. - **Simplified `skills.launch.py`**: - Removed redundant `assembly_config` node setup. - **Enhanced `rbs_utils`**: - Added installation of `launch` files in `CMakeLists.txt`. - Created a new `utils.launch.py` for dynamically loading assembly configurations. - Refactored `assembly_config_service.py` to utilize `get_asm_config` for streamlined configuration file resolution. - Improved `rbs_bringup` setup to include additional parameters and nodes for assembly configuration. These changes centralize assembly configuration handling and enhance modularity across launch setups.
409 lines
13 KiB
Python
409 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)),
|
|
"config",
|
|
"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",
|
|
],
|
|
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)]
|
|
)
|