robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

Multi-Robot Setup

Browse source
This commit is contained in:
Ilya Uraev 2024-04-18 13:29:36 +00:00 • committed by Igor Brylyov
parent bc48e0c35a
commit d5e7768d90
45 changed files with 4519 additions and 861 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rbs_bringup/config/rbs.rviz
View file

@ -315,7 +315,7 @@ Visualization Manager:
Inertia: false
Mass: false
Name: RobotModel
TF Prefix: ""
TF tf_prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true

57
rbs_bringup/config/robot_scene.yaml Normal file
View file

@ -0,0 +1,57 @@
scene_config:
- name: arm1
type: rbs_arm
tool_type: rbs_gripper
ndof: 6
parent: world
initial_position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0
- name: arm2
type: rbs_arm
tool_type: rbs_gripper
ndof: 6
parent: world
initial_position:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
pose:
position:
x: 0.0
y: 0.5
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0
- name: helper
type: rbs_gripper
parent: rbs_platform
initial_position:
- 0.0
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
r: 0.0
p: 0.0
y: 0.0

285
rbs_bringup/launch/multi_robot.launch.py Normal file
View file

@ -0,0 +1,285 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
OpaqueFunction,
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
from nav2_common.launch import RewrittenYaml
import os
from ament_index_python import get_package_share_directory
from rbs_launch_utils.merged_yaml import MergedYaml
from rbs_launch_utils.launch_common import load_yaml
import yaml
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
robots_config_file = LaunchConfiguration("robots_config_file")
gazebo_world_filename = LaunchConfiguration("gazebo_world_filename")
ld = []
ld.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation_gazebo.launch.py'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': "false",
"gazebo_world_filename": gazebo_world_filename
}.items(),
condition=IfCondition(launch_simulation)
))
scene_file = robots_config_file.perform(context)
robots = load_yaml("rbs_bringup", "config/" + scene_file)
description_package = description_package.perform(context)
controllers_file = controllers_file.perform(context)
config = MergedYaml(context,
os.path.join(get_package_share_directory(description_package), "config", controllers_file),
root_keys=[i["name"] for i in robots["scene_config"]],
param_rewrites={},
convert_types=False,
).merge_yamls()
for robot in robots["scene_config"]:
namespace = "/" + robot["name"]
ld.append(IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot["type"],
"controllers_file": config,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot["name"],
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": launch_controllers,
"gazebo_gui": gazebo_gui,
"x": str(robot["pose"]["position"]["x"]),
"y": str(robot["pose"]["position"]["y"]),
"z": str(robot["pose"]["position"]["z"])
}.items()
))
gz_spawner = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', robot_name,
'-x', "0",#str(robot["spawn_point"][0]),
'-y', "0",#str(robot["spawn_point"][1]),
'-z', "0",#str(robot["spawn_point"][2]),
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
ld.append(gz_spawner)
return ld
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="rbs_arm",
description="Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="rbs_arm_modular.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_name",
default_value="arm0",
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="rbs_arm",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="rbs_arm.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Make MoveIt to use simulation time.\
This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit",
default_value="false",
description="Launch moveit?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_world_filename",
default_value="asm2.sdf",
description="Filename of Gazebo world file to launch")
)
declared_arguments.append(
DeclareLaunchArgument("robots_config_file",
default_value="robot_scene",
description="Filename for config file with robots in scene")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

402
rbs_bringup/launch/bringup.launch.py → rbs_bringup/launch/rbs_robot.launch.py
View file

@ -1,19 +1,197 @@
from launch import LaunchDescription, condition
from launch import LaunchContext, LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
RegisterEventHandler,
OpaqueFunction
)
from launch.event_handlers import OnProcessExit
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
import xacro
import os
from ament_index_python.packages import get_package_share_directory
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
hardware = LaunchConfiguration("hardware")
launch_controllers = LaunchConfiguration("launch_controllers")
gripper_name = LaunchConfiguration("gripper_name")
x_pos = LaunchConfiguration("x")
y_pos = LaunchConfiguration("y")
z_pos = LaunchConfiguration("z")
robot_name = robot_name.perform(context)
namespace = "/" + robot_name
robot_type = robot_type.perform(context)
description_package = description_package.perform(context)
description_file = description_file.perform(context)
controllers_file = controllers_file.perform(context)
remappings = [("/tf", "tf"), ("/tf_static", "tf_static")]
xacro_file = os.path.join(get_package_share_directory(description_package), "urdf", description_file)
robot_description_doc = xacro.process_file(
xacro_file,
mappings={
"gripper_name": gripper_name.perform(context),
"hardware": hardware.perform(context),
"simulation_controllers": controllers_file,
"namespace": namespace,
"x": x_pos.perform(context),
"y": y_pos.perform(context),
"z": z_pos.perform(context)
#TODO: add rotation and add probably via dict
}
)
robot_description_content = robot_description_doc.toprettyxml(indent=" ")
robot_description = {"robot_description": robot_description_content}
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
),
" ",
"name:=",robot_name," ",
"with_gripper:=",with_gripper_condition, " ",
"gripper_name:=", gripper_name, " ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
namespace=namespace,
output="both",
remappings=remappings,
parameters=[{"use_sim_time": True}, robot_description],
)
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare(description_package),
'launch',
'control.launch.py'
])
]),
launch_arguments={
'with_gripper': with_gripper_condition,
'robot_description': robot_description_content,
'start_joint_controller': start_joint_controller,
'initial_joint_controller': initial_joint_controller,
'controllers_file': controllers_file,
'cartesian_controllers': cartesian_controllers,
'namespace': namespace,
}.items(),
condition=IfCondition(launch_controllers))
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare(moveit_config_package),
'launch',
'moveit.launch.py'
])
]),
launch_arguments={
'robot_description': robot_description_content,
'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_semantic': robot_description_semantic_content,
'namespace': namespace,
}.items(),
condition=IfCondition(launch_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,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
'namespace': namespace
}.items()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_task_planner'),
'launch',
'task_planner.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner))
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_perception'),
'launch',
'perception.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception))
nodes_to_start = [
robot_state_publisher,
control,
moveit,
skills,
task_planner,
perception
]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"rbs_robot_type",
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
@ -44,9 +222,9 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument(
"prefix",
default_value='""',
description="Prefix of the joint names, useful for \
"robot_name",
default_value="arm0",
description="tf_prefix of the joint names, useful for \
multi-robot setup. If changed than also joint names in the controllers' configuration \
have to be updated.",
)
@ -88,6 +266,14 @@ def generate_launch_description():
This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
@ -151,197 +337,21 @@ def generate_launch_description():
default_value="false",
description="Launch gazebo with gui?")
)
# Initialize Arguments
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
prefix = LaunchConfiguration("prefix")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_rviz = LaunchConfiguration("launch_rviz")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
initial_joint_controllers_file_path = PathJoinSubstitution(
[FindPackageShare(description_package), "config", controllers_file]
declared_arguments.append(
DeclareLaunchArgument("x",
default_value="0.0",
description="Position of robot in world by X")
)
declared_arguments.append(
DeclareLaunchArgument("y",
default_value="0.0",
description="Position of robot in world by Y")
)
declared_arguments.append(
DeclareLaunchArgument("z",
default_value="0.0",
description="Position of robot in world by Z")
)
rviz_config_file = PathJoinSubstitution(
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
)
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
" ",
"gripper_name:=", "", " ",
"prefix:=", prefix, " ",
"hardware:=", hardware, " ",
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
]
)
robot_description = {"robot_description": robot_description_content}
robot_description_semantic_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config/moveit", "rbs_arm.srdf.xacro"]
),
" ",
"name:=","rbs_arm"," ",
"prefix:=",prefix," ",
]
)
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_content}
robot_description_kinematics = PathJoinSubstitution(
[FindPackageShare(moveit_config_package), "config", "kinematics.yaml"]
)
robot_state_publisher = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[{"use_sim_time": True}, robot_description],
)
rviz = Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output="log",
arguments=["-d", rviz_config_file],
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics
],
condition=IfCondition(launch_rviz))
control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_arm'),
'launch',
'control.launch.py'
])
]),
launch_arguments={
'with_gripper': with_gripper_condition,
'robot_description': robot_description_content,
'start_joint_controller': start_joint_controller,
'initial_joint_controller': initial_joint_controller,
'controllers_file': controllers_file,
"cartesian_controllers": cartesian_controllers
}.items(),
condition=IfCondition(launch_controllers))
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation.launch.py'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'gazebo_gui': gazebo_gui,
'rbs_robot_type': rbs_robot_type,
'env_manager': env_manager,
'debugger': "false"
}.items(),
condition=IfCondition(launch_simulation))
moveit = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_arm'),
'launch',
'moveit.launch.py'
])
]),
launch_arguments={
'robot_description': robot_description_content,
'moveit_config_package': moveit_config_package,
'moveit_config_file': moveit_config_file,
'use_sim_time': use_sim_time,
'prefix': prefix,
'with_gripper': with_gripper_condition,
'robot_description_semantic': robot_description_semantic_content
}.items(),
condition=IfCondition(launch_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,
'use_sim_time': use_sim_time,
'with_gripper_condition': with_gripper_condition,
}.items()
)
task_planner = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_task_planner'),
'launch',
'task_planner.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_task_planner))
perception = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_perception'),
'launch',
'perception.launch.py'
])
]),
launch_arguments={
# TBD
}.items(),
condition=IfCondition(launch_perception))
nodes_to_start = [
robot_state_publisher,
rviz,
control,
simulation,
moveit,
skills,
task_planner,
perception
]
return LaunchDescription(declared_arguments + nodes_to_start)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

276
rbs_bringup/launch/single_robot.launch.py Normal file
View file

@ -0,0 +1,276 @@
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
OpaqueFunction
)
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node
import os
from ament_index_python.packages import get_package_share_directory
from nav2_common.launch import RewrittenYaml
def launch_setup(context, *args, **kwargs):
# Initialize Arguments
robot_type = LaunchConfiguration("robot_type")
# General arguments
with_gripper_condition = LaunchConfiguration("with_gripper")
controllers_file = LaunchConfiguration("controllers_file")
cartesian_controllers = LaunchConfiguration("cartesian_controllers")
description_package = LaunchConfiguration("description_package")
description_file = LaunchConfiguration("description_file")
robot_name = LaunchConfiguration("robot_name")
start_joint_controller = LaunchConfiguration("start_joint_controller")
initial_joint_controller = LaunchConfiguration("initial_joint_controller")
launch_simulation = LaunchConfiguration("launch_sim")
launch_moveit = LaunchConfiguration("launch_moveit")
launch_task_planner = LaunchConfiguration("launch_task_planner")
launch_perception = LaunchConfiguration("launch_perception")
moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time")
sim_gazebo = LaunchConfiguration("sim_gazebo")
hardware = LaunchConfiguration("hardware")
env_manager = LaunchConfiguration("env_manager")
launch_controllers = LaunchConfiguration("launch_controllers")
gazebo_gui = LaunchConfiguration("gazebo_gui")
gripper_name = LaunchConfiguration("gripper_name")
sim_gazebo = LaunchConfiguration("sim_gazebo")
launch_simulation = LaunchConfiguration("launch_sim")
simulation = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_simulation'),
'launch',
'simulation_gazebo.launch.py'
])
]),
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': "false"
}.items(),
condition=IfCondition(launch_simulation)
)
configured_params = RewrittenYaml(
source_file=os.path.join(
get_package_share_directory(
description_package.perform(context)),
"config",
controllers_file.perform(context)),
root_key=robot_name.perform(context),
param_rewrites={},
convert_types=True,
)
controller_paramfile = configured_params.perform(context)
namespace = "/" + robot_name.perform(context)
gz_spawner = Node(
package='ros_gz_sim',
executable='create',
arguments=[
'-name', robot_name.perform(context),
'-x', "0.5",
'-y', "0.5",
'-z', "0.0",
'-topic', namespace + '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
single_robot_setup = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
PathJoinSubstitution([
FindPackageShare('rbs_bringup'),
"launch",
"rbs_robot.launch.py"
])
]),
launch_arguments={
"env_manager": env_manager,
"with_gripper": with_gripper_condition,
"gripper_name": gripper_name,
"controllers_file": controllers_file,
"robot_type": robot_type,
"controllers_file": controller_paramfile,
"cartesian_controllers": cartesian_controllers,
"description_package": description_package,
"description_file": description_file,
"robot_name": robot_name,
"start_joint_controller": start_joint_controller,
"initial_joint_controller": initial_joint_controller,
"launch_simulation": launch_simulation,
"launch_moveit": launch_moveit,
"launch_task_planner": launch_task_planner,
"launch_perception": launch_perception,
"moveit_config_package": moveit_config_package,
"moveit_config_file": moveit_config_file,
"use_sim_time": use_sim_time,
"sim_gazebo": sim_gazebo,
"hardware": hardware,
"launch_controllers": "true",
"gazebo_gui": gazebo_gui,
"x": "0.5",
"y": "0.5",
"z": "0.5"
}.items()
)
nodes_to_start = [
simulation,
gz_spawner,
single_robot_setup
]
return nodes_to_start
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"robot_type",
description="Type of robot by name",
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
default_value="rbs_arm",
)
)
# General arguments
declared_arguments.append(
DeclareLaunchArgument(
"controllers_file",
default_value="rbs_arm_controllers_gazebosim.yaml",
description="YAML file with the controllers configuration.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_package",
default_value="rbs_arm",
description="Description package with robot URDF/XACRO files. Usually the argument \
is not set, it enables use of a custom description.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"description_file",
default_value="rbs_arm_modular.xacro",
description="URDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"robot_name",
default_value="arm0",
description="Name for robot, used to apply namespace for specific robot in multirobot setup",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"start_joint_controller",
default_value="false",
description="Enable headless mode for robot control",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"initial_joint_controller",
default_value="joint_trajectory_controller",
description="Robot controller to start.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_package",
default_value="rbs_arm",
description="MoveIt config package with robot SRDF/XACRO files. Usually the argument \
is not set, it enables use of a custom moveit config.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"moveit_config_file",
default_value="rbs_arm.srdf.xacro",
description="MoveIt SRDF/XACRO description file with the robot.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"use_sim_time",
default_value="true",
description="Make MoveIt to use simulation time.\
This is needed for the trajectory planing in simulation.",
)
)
declared_arguments.append(
DeclareLaunchArgument(
"gripper_name",
default_value="rbs_gripper",
choices=["rbs_gripper", ""],
description="choose gripper by name (leave empty if hasn't)",
)
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo",
default_value="true",
description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
description="Launch env_manager?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_sim",
default_value="true",
description="Launch simulator (Gazebo)?\
Most general arg")
)
declared_arguments.append(
DeclareLaunchArgument("launch_moveit",
default_value="false",
description="Launch moveit?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_perception",
default_value="false",
description="Launch perception?")
)
declared_arguments.append(
DeclareLaunchArgument("launch_task_planner",
default_value="false",
description="Launch task_planner?")
)
declared_arguments.append(
DeclareLaunchArgument("cartesian_controllers",
default_value="true",
description="Load cartesian\
controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("hardware",
choices=["gazebo", "mock"],
default_value="gazebo",
description="Choose your harware_interface")
)
declared_arguments.append(
DeclareLaunchArgument("launch_controllers",
default_value="true",
description="Launch controllers?")
)
declared_arguments.append(
DeclareLaunchArgument("gazebo_gui",
default_value="true",
description="Launch gazebo with gui?")
)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

2
rbs_bringup/package.xml
View file

@ -9,6 +9,8 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>nav2_common</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

62
rbs_bringup/rbs_launch_utils/launch_common.py
View file

@ -1,58 +1,30 @@
#!/usr/bin/env python3
# Copyright (c) 2021 PickNik, Inc.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# * Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
#
# * Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in the
# documentation and/or other materials provided with the distribution.
#
# * Neither the name of the {copyright_holder} nor the names of its
# contributors may be used to endorse or promote products derived from
# this software without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
# Author: Lovro Ivanov
import math
import os
import sys
import yaml
from typing import Dict
from ament_index_python.packages import get_package_share_directory
def print_error(message: str) -> None:
"""Print an error message in red color."""
print("\033[91m{}\033[0m".format(message), file=sys.stderr)
def construct_angle_radians(loader, node):
def construct_angle_radians(loader, node) -> float:
"""Utility function to construct radian values from yaml."""
value = loader.construct_scalar(node)
try:
return float(value)
except SyntaxError:
raise Exception("invalid expression: %s" % value)
raise Exception("Invalid expression: %s" % value)
def construct_angle_degrees(loader, node):
def construct_angle_degrees(loader, node) -> float:
"""Utility function for converting degrees into radians from yaml."""
return math.radians(construct_angle_radians(loader, node))
def load_yaml(package_name, file_path):
def load_yaml(package_name: str, file_path: str) -> Dict:
package_path = get_package_share_directory(package_name)
absolute_file_path = os.path.join(package_path, file_path)
@ -60,25 +32,27 @@ def load_yaml(package_name, file_path):
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
print_error("Error: YAML support not available; install python-yaml")
sys.exit(1)
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
def load_yaml_abs(absolute_file_path):
print_error("Error: Unable to open the file {}".format(absolute_file_path))
sys.exit(1)
def load_yaml_abs(absolute_file_path: str) -> Dict:
try:
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
except Exception:
raise Exception("yaml support not available; install python-yaml")
print_error("Error: YAML support not available; install python-yaml")
sys.exit(1)
try:
with open(absolute_file_path) as file:
return yaml.safe_load(file)
except OSError: # parent of IOError, OSError *and* WindowsError where available
return None
print_error("Error: Unable to open the file {}".format(absolute_file_path))
sys.exit(1)

51
rbs_bringup/rbs_launch_utils/merged_yaml.py Normal file
View file

@ -0,0 +1,51 @@
from typing import Dict
from typing import List
from typing import Text
from typing import Optional
import yaml
import tempfile
import launch
from nav2_common.launch import RewrittenYaml
from rbs_launch_utils.launch_common import load_yaml_abs
class MergedYaml:
"""
Apply RewrittenYaml for list of config files and combine it in one file
"""
def __init__(self, context: launch.LaunchContext,
source_file: Text = "",
root_keys: List = [],
param_rewrites: Dict = {},
convert_types = False,
key_rewrites: Dict = {}):
self.__root_keys = root_keys
self.__context = context
self.__param_rewrites = param_rewrites
self.__convet_types = convert_types
#TODO: work with multiple source files
# for different robots
self.__source_file = source_file
self.__key_rewrites = key_rewrites
def get_config_file(self) -> List[Text]:
config_list = []
for key in self.__root_keys:
yaml = RewrittenYaml(
self.__source_file,
self.__param_rewrites,
key,
self.__key_rewrites,
self.__convet_types)
config_list.append(yaml.perform(self.__context))
return config_list
def merge_yamls(self) -> Text:
configs = self.get_config_file()
yamls = []
for file in configs:
yamls.append(load_yaml_abs(file))
merged_yaml = tempfile.NamedTemporaryFile(mode='w', delete=False, suffix=".yaml")
yaml.dump_all(yamls, merged_yaml)
return merged_yaml.name