forked from YikaiFu-cart/acRealman_xr
122 lines
4.3 KiB
Python
122 lines
4.3 KiB
Python
from launch import LaunchDescription
|
|
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
|
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
|
|
from launch_ros.actions import Node
|
|
from launch_ros.substitutions import FindPackageShare
|
|
|
|
|
|
def _as_bool(value: str) -> bool:
|
|
return value.strip().lower() in ("1", "true", "yes", "on")
|
|
|
|
|
|
def _config_file(name: str) -> PathJoinSubstitution:
|
|
return PathJoinSubstitution([
|
|
FindPackageShare("xr_rm_bringup"),
|
|
"config",
|
|
name,
|
|
])
|
|
|
|
|
|
def _udp_receiver_node() -> Node:
|
|
return Node(
|
|
package="xr_rm_input",
|
|
executable="udp_controller_receiver",
|
|
name="udp_controller_receiver",
|
|
output="screen",
|
|
parameters=[{
|
|
"udp_host": LaunchConfiguration("udp_host"),
|
|
"udp_port": LaunchConfiguration("udp_port"),
|
|
"left_topic": "/xr/left_controller",
|
|
"right_topic": "/xr/right_controller",
|
|
}],
|
|
)
|
|
|
|
|
|
def _single_arm_node(arm: str, use_mock: bool, move_to_initial_pose: bool) -> Node:
|
|
config_name = "left_arm_rm75.yaml" if arm == "left" else "right_arm_rm75.yaml"
|
|
robot_ip = LaunchConfiguration("left_robot_ip" if arm == "left" else "right_robot_ip")
|
|
return Node(
|
|
package="xr_rm_teleop",
|
|
executable="single_arm_velocity_teleop",
|
|
name="single_arm_velocity_teleop",
|
|
output="screen",
|
|
parameters=[
|
|
_config_file(config_name),
|
|
{
|
|
"use_mock": use_mock,
|
|
"robot_ip": robot_ip,
|
|
"robot_port": LaunchConfiguration("robot_port"),
|
|
"move_to_initial_pose_on_connect": move_to_initial_pose,
|
|
},
|
|
],
|
|
)
|
|
|
|
|
|
def _dual_arm_nodes(use_mock: bool, move_to_initial_pose: bool) -> list[Node]:
|
|
config_file = _config_file("dual_arm_rm75.yaml")
|
|
return [
|
|
Node(
|
|
package="xr_rm_teleop",
|
|
executable="single_arm_velocity_teleop",
|
|
name="left_arm_teleop",
|
|
output="screen",
|
|
parameters=[
|
|
config_file,
|
|
{
|
|
"use_mock": use_mock,
|
|
"robot_ip": LaunchConfiguration("left_robot_ip"),
|
|
"robot_port": LaunchConfiguration("robot_port"),
|
|
"move_to_initial_pose_on_connect": move_to_initial_pose,
|
|
},
|
|
],
|
|
),
|
|
Node(
|
|
package="xr_rm_teleop",
|
|
executable="single_arm_velocity_teleop",
|
|
name="right_arm_teleop",
|
|
output="screen",
|
|
parameters=[
|
|
config_file,
|
|
{
|
|
"use_mock": use_mock,
|
|
"robot_ip": LaunchConfiguration("right_robot_ip"),
|
|
"robot_port": LaunchConfiguration("robot_port"),
|
|
"move_to_initial_pose_on_connect": move_to_initial_pose,
|
|
},
|
|
],
|
|
),
|
|
]
|
|
|
|
|
|
def _launch_setup(context, *args, **kwargs):
|
|
del args, kwargs
|
|
arm = LaunchConfiguration("arm").perform(context).strip().lower()
|
|
use_mock = _as_bool(LaunchConfiguration("use_mock").perform(context))
|
|
move_to_initial_pose = _as_bool(
|
|
LaunchConfiguration("move_to_initial_pose_on_connect").perform(context)
|
|
)
|
|
|
|
if arm not in ("left", "right", "both"):
|
|
raise ValueError("arm must be one of: left, right, both")
|
|
|
|
nodes = [_udp_receiver_node()]
|
|
if arm == "both":
|
|
nodes.extend(_dual_arm_nodes(use_mock, move_to_initial_pose))
|
|
else:
|
|
nodes.append(_single_arm_node(arm, use_mock, move_to_initial_pose))
|
|
return nodes
|
|
|
|
|
|
def generate_launch_description() -> LaunchDescription:
|
|
return LaunchDescription([
|
|
DeclareLaunchArgument("arm", default_value="right"),
|
|
DeclareLaunchArgument("use_mock", default_value="true"),
|
|
DeclareLaunchArgument("udp_host", default_value="0.0.0.0"),
|
|
DeclareLaunchArgument("udp_port", default_value="15000"),
|
|
DeclareLaunchArgument("left_robot_ip", default_value="192.168.192.18"),
|
|
DeclareLaunchArgument("right_robot_ip", default_value="192.168.192.19"),
|
|
DeclareLaunchArgument("robot_port", default_value="8080"),
|
|
DeclareLaunchArgument("move_to_initial_pose_on_connect", default_value="false"),
|
|
OpaqueFunction(function=_launch_setup),
|
|
])
|