robossembler/runtime
7
0
Fork 0
Code Issues 4 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

168
rbs_skill_servers/launch/skills.launch.py
View file

@ -1,19 +1,97 @@
import os
from launch import LaunchDescription, LaunchContext
from collections import namedtuple
from os import name
from launch import LaunchDescription
from launch.actions import (
DeclareLaunchArgument,
IncludeLaunchDescription,
ExecuteProcess,
OpaqueFunction
)
from ament_index_python.packages import get_package_share_directory
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch.conditions import IfCondition
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from rbs_launch_utils.launch_common import load_yaml
def launch_setup(context, *args, **kwargs):
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
namespace = LaunchConfiguration("namespace")
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
namespace=namespace,
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
# FIXME: The name of this node, "move_topose,"
# is intended to be different from the actual MoveToPose node.
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
namespace=namespace,
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
namespace=namespace,
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
# grasp_pose_loader
]
return nodes_to_start
def generate_launch_description():
@ -41,77 +119,9 @@ def generate_launch_description():
declared_arguments.append(
DeclareLaunchArgument("points_params_filepath", default_value="")
)
robot_description_decl = LaunchConfiguration("robot_description")
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
use_sim_time = LaunchConfiguration("use_sim_time")
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
robot_description = {"robot_description": robot_description_decl}
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
points_params = load_yaml(
"rbs_skill_servers", "config/gripperPositions.yaml"
declared_arguments.append(
DeclareLaunchArgument("namespace", default_value="")
)
move_topose_action_server = Node(
package="rbs_skill_servers",
executable="move_topose_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
gripper_control_node = Node(
package="rbs_skill_servers",
executable="gripper_control_action_server",
parameters= [
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
],
condition=IfCondition(with_gripper_condition)
)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
move_joint_state_action_server = Node(
package="rbs_skill_servers",
executable="move_to_joint_states_action_server",
parameters=[
robot_description,
robot_description_semantic,
robot_description_kinematics,
{"use_sim_time": use_sim_time},
]
)
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
output="screen"
)
nodes_to_start =[
move_topose_action_server,
gripper_control_node,
move_cartesian_path_action_server,
move_joint_state_action_server,
grasp_pose_loader
]
return LaunchDescription(declared_arguments + nodes_to_start)
return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])