chage runtime to rbs_arm and formatting the code
This commit is contained in:
parent
2755209c66
commit
6608560a27
2 changed files with 59 additions and 95 deletions
|
@ -1,4 +1,3 @@
|
|||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import (
|
||||
DeclareLaunchArgument,
|
||||
|
@ -9,7 +8,6 @@ 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
|
||||
# from rbs_launch_utils.launch_common import load_yaml_abs
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
|
@ -17,50 +15,22 @@ def generate_launch_description():
|
|||
DeclareLaunchArgument(
|
||||
"rbs_robot_type",
|
||||
description="Type of robot by name",
|
||||
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
||||
default_value="ur5e",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"safety_limits",
|
||||
default_value="true",
|
||||
description="Enables the safety limits controller if true.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"safety_pos_margin",
|
||||
default_value="0.15",
|
||||
description="The margin to lower and upper limits in the safety controller.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"safety_k_position",
|
||||
default_value="20",
|
||||
description="k-position factor in the safety controller.",
|
||||
choices=["rbs_arm","ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
||||
default_value="rbs_arm",
|
||||
)
|
||||
)
|
||||
# General arguments
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"controllers_file",
|
||||
default_value="ur_plus_gripper_controllers.yaml",
|
||||
description="YAML file with the controllers configuration.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"controllers_with_gripper_file",
|
||||
default_value="ur_plus_gripper_controllers.yaml",
|
||||
default_value="rbs_arm_controllers_gazebosim.yaml",
|
||||
description="YAML file with the controllers configuration.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_package",
|
||||
default_value="ur_description",
|
||||
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.",
|
||||
)
|
||||
|
@ -68,7 +38,7 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"description_file",
|
||||
default_value="ur.urdf.xacro",
|
||||
default_value="rbs_arm_modular.xacro",
|
||||
description="URDF/XACRO description file with the robot.",
|
||||
)
|
||||
)
|
||||
|
@ -84,7 +54,7 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"start_joint_controller",
|
||||
default_value="false",
|
||||
default_value="true",
|
||||
description="Enable headless mode for robot control",
|
||||
)
|
||||
)
|
||||
|
@ -95,19 +65,10 @@ def generate_launch_description():
|
|||
description="Robot controller to start.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"initial_gripper_controller",
|
||||
default_value="gripper_controller",
|
||||
description="Robot controller to start.",
|
||||
)
|
||||
)
|
||||
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"moveit_config_package",
|
||||
default_value="ur_moveit_config",
|
||||
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.",
|
||||
)
|
||||
|
@ -115,7 +76,7 @@ def generate_launch_description():
|
|||
declared_arguments.append(
|
||||
DeclareLaunchArgument(
|
||||
"moveit_config_file",
|
||||
default_value="ur.srdf.xacro",
|
||||
default_value="rbs_arm.srdf.xacro",
|
||||
description="MoveIt SRDF/XACRO description file with the robot.",
|
||||
)
|
||||
)
|
||||
|
@ -123,48 +84,65 @@ def generate_launch_description():
|
|||
DeclareLaunchArgument(
|
||||
"use_sim_time",
|
||||
default_value="true",
|
||||
description="Make MoveIt to use simulation time. This is needed for the trajectory planing in simulation.",
|
||||
description="Make MoveIt to use simulation time.\
|
||||
This is needed for the trajectory planing in simulation.",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("with_gripper", default_value="true", description="With gripper or not?")
|
||||
DeclareLaunchArgument("with_gripper",
|
||||
default_value="true",
|
||||
description="With gripper or not?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
|
||||
DeclareLaunchArgument("launch_rviz",
|
||||
default_value="true",
|
||||
description="Launch RViz?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("sim_gazebo", default_value="true", description="Gazebo Simulation")
|
||||
DeclareLaunchArgument("sim_gazebo",
|
||||
default_value="true",
|
||||
description="Gazebo Simulation")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("sim_mujoco", default_value="false", description="Mujoco Simulation")
|
||||
DeclareLaunchArgument("env_manager",
|
||||
default_value="true",
|
||||
description="Launch env_manager?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("sim_fake", default_value="false", description="Mock contollers")
|
||||
DeclareLaunchArgument("launch_sim",
|
||||
default_value="true",
|
||||
description="Launch simulator (Gazebo)?\
|
||||
Most general arg")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("env_manager", default_value="true", description="Launch env_manager?")
|
||||
DeclareLaunchArgument("launch_moveit",
|
||||
default_value="true",
|
||||
description="Launch moveit?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_sim", default_value="true", description="Launch simulator (Gazebo)? Most general arg")
|
||||
DeclareLaunchArgument("launch_perception",
|
||||
default_value="false",
|
||||
description="Launch perception?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_moveit", default_value="true", description="Launch moveit?")
|
||||
DeclareLaunchArgument("launch_task_planner",
|
||||
default_value="false",
|
||||
description="Launch task_planner?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_perception", default_value="false", description="Launch perception?")
|
||||
DeclareLaunchArgument("cartesian_controllers",
|
||||
default_value="false",
|
||||
description="Load cartesian\
|
||||
controllers?")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("launch_task_planner", default_value="false", description="Launch task_planner?")
|
||||
DeclareLaunchArgument("hardware",
|
||||
choices=["gazebo", "mock"],
|
||||
default_value="gazebo",
|
||||
description="Choose your harware_interface")
|
||||
)
|
||||
declared_arguments.append(
|
||||
DeclareLaunchArgument("cartesian_controllers", default_value="false", description="Load cartesian controllers?")
|
||||
)
|
||||
|
||||
# Initialize Arguments
|
||||
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
|
||||
safety_limits = LaunchConfiguration("safety_limits")
|
||||
safety_pos_margin = LaunchConfiguration("safety_pos_margin")
|
||||
safety_k_position = LaunchConfiguration("safety_k_position")
|
||||
# General arguments
|
||||
with_gripper_condition = LaunchConfiguration("with_gripper")
|
||||
controllers_file = LaunchConfiguration("controllers_file")
|
||||
|
@ -183,8 +161,7 @@ def generate_launch_description():
|
|||
moveit_config_file = LaunchConfiguration("moveit_config_file")
|
||||
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||
sim_gazebo = LaunchConfiguration("sim_gazebo")
|
||||
sim_mujoco = LaunchConfiguration("sim_mujoco")
|
||||
sim_fake = LaunchConfiguration("sim_fake")
|
||||
hardware = LaunchConfiguration("hardware")
|
||||
env_manager = LaunchConfiguration("env_manager")
|
||||
|
||||
|
||||
|
@ -195,44 +172,36 @@ def generate_launch_description():
|
|||
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]
|
||||
),
|
||||
PathJoinSubstitution([FindPackageShare(description_package), "urdf", description_file]),
|
||||
" ",
|
||||
"safety_limits:=", safety_limits, " ",
|
||||
"safety_pos_margin:=", safety_pos_margin, " ",
|
||||
"safety_k_position:=", safety_k_position, " ",
|
||||
"name:=", "ur", " ",
|
||||
"ur_type:=", rbs_robot_type, " ",
|
||||
"gripper_name:=", "", " ",
|
||||
"prefix:=", prefix, " ",
|
||||
"sim_mujoco:=", sim_mujoco, " ",
|
||||
"sim_gazebo:=", sim_gazebo, " ",
|
||||
"sim_fake:=", sim_fake, " ",
|
||||
"hardware:=", hardware, " ",
|
||||
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
|
||||
"with_gripper:=", with_gripper_condition, " ",
|
||||
|
||||
]
|
||||
)
|
||||
|
||||
robot_description = {"robot_description": robot_description_content}
|
||||
|
||||
# MoveIt Configuration
|
||||
robot_description_semantic_content = Command(
|
||||
[
|
||||
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||
" ",
|
||||
PathJoinSubstitution(
|
||||
[FindPackageShare(moveit_config_package), "srdf", moveit_config_file]
|
||||
), " ",
|
||||
"name:=", "ur", " ",
|
||||
"prefix:=", prefix, " ",
|
||||
"with_gripper:=", with_gripper_condition
|
||||
[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"]
|
||||
|
@ -260,7 +229,7 @@ def generate_launch_description():
|
|||
control = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('ur_description'),
|
||||
FindPackageShare('rbs_arm'),
|
||||
'launch',
|
||||
'control.launch.py'
|
||||
])
|
||||
|
@ -292,7 +261,7 @@ def generate_launch_description():
|
|||
moveit = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
FindPackageShare('ur_moveit_config'),
|
||||
FindPackageShare('rbs_arm'),
|
||||
'launch',
|
||||
'moveit.launch.py'
|
||||
])
|
||||
|
@ -322,7 +291,6 @@ def generate_launch_description():
|
|||
'robot_description_kinematics': robot_description_kinematics,
|
||||
'use_sim_time': use_sim_time,
|
||||
'with_gripper_condition': with_gripper_condition,
|
||||
'points_params_filepath': "gripperPositions.yaml"
|
||||
}.items()
|
||||
)
|
||||
|
||||
|
@ -363,7 +331,3 @@ def generate_launch_description():
|
|||
perception
|
||||
]
|
||||
return LaunchDescription(declared_arguments + nodes_to_start)
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
|
@ -18,8 +18,8 @@ def generate_launch_description():
|
|||
DeclareLaunchArgument(
|
||||
"rbs_robot_type",
|
||||
description="Type of robot by name",
|
||||
choices=["ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
||||
default_value="ur5e",
|
||||
choices=["rbs_arm" ,"ur3", "ur3e", "ur5", "ur5e", "ur10", "ur10e", "ur16e"],
|
||||
default_value="rbs_arm",
|
||||
)
|
||||
)
|
||||
declared_arguments.append(
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue