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

refactor rbs_simulation launch file

Browse source
This commit is contained in:
Ilya Uraev 2023-07-24 12:47:11 +03:00
parent 5498003cb1
commit d0625eb65e
1 changed files with 51 additions and 65 deletions
Download patch file Download diff file Expand all files Collapse all files

116
rbs_simulation/launch/rbs_simulation.launch.py
View file

@ -151,7 +151,15 @@ def generate_launch_description():
DeclareLaunchArgument("launch_rviz", default_value="true", description="Launch RViz?")
)
declared_arguments.append(
DeclareLaunchArgument("sim_gazebo", default_value="false", description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("sim_mujoco", default_value="true", description="Gazebo Simulation")
)
declared_arguments.append(
DeclareLaunchArgument("sim_fake", default_value="false", description="Gazebo Simulation")
)
# Initialize Arguments
rbs_robot_type = LaunchConfiguration("rbs_robot_type")
@ -173,6 +181,9 @@ def generate_launch_description():
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")
sim_mujoco = LaunchConfiguration("sim_mujoco")
sim_fake = LaunchConfiguration("sim_fake")
initial_joint_controllers_file_path = PathJoinSubstitution(
@ -190,6 +201,12 @@ def generate_launch_description():
mujoco_model = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"]
)
assemble_dir = os.path.join(
get_package_share_directory("rbs_task_planner"), "example", "sdf_models"
)
points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")
robot_description_content = Command(
[
@ -199,34 +216,18 @@ def generate_launch_description():
[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,
" ",
"prefix:=",
prefix,
" ",
"sim_mujoco:=true",
" ",
"simulation_controllers:=",
initial_joint_controllers_file_path,
" ",
"with_gripper:=",
with_gripper_condition,
" ",
"mujoco_model:=",
mujoco_model,
"safety_limits:=", safety_limits, " ",
"safety_pos_margin:=", safety_pos_margin, " ",
"safety_k_position:=", safety_k_position, " ",
"name:=", "ur", " ",
"ur_type:=", rbs_robot_type, " ",
"prefix:=", prefix, " ",
"sim_mujoco:=", sim_mujoco, " ",
"sim_gazebo:=", sim_gazebo, " ",
"sim_fake:=", sim_fake, " ",
"simulation_controllers:=", initial_joint_controllers_file_path, " ",
"with_gripper:=", with_gripper_condition, " ",
"mujoco_model:=", mujoco_model,
]
)
robot_description = {"robot_description": robot_description_content}
@ -235,15 +236,11 @@ def generate_launch_description():
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, initial_joint_controllers_file_path],
#prefix=['xterm -e gdb -ex run --args'],
output="both",
remappings=[
('motion_control_handle/target_frame', 'target_frame'),
('cartesian_motion_controller/target_frame', 'target_frame'),
('motion_control_handle/target_frame', 'target_frame'),
('cartesian_compliance_controller/target_frame', 'target_frame'),
# ('cartesian_force_controller/target_wrench', 'target_wrench'),
('cartesian_compliance_controller/target_wrench', 'target_wrench'),
# ('cartesian_force_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
('cartesian_compliance_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
]
)
@ -299,20 +296,22 @@ def generate_launch_description():
)
# Gazebo nodes
# gazebo = IncludeLaunchDescription(
# PythonLaunchDescriptionSource(
# [os.path.join(get_package_share_directory('ros_ign_gazebo'),
# 'launch', 'ign_gazebo.launch.py')]),
# launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])])
# # Spawn robot
# gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
# arguments=[
# '-name', rbs_robot_type,
# '-x', '0.0',
# '-z', '0.0',
# '-y', '0.0',
# '-topic', '/robot_description'],
# output='screen')
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_ign_gazebo'),
'launch', 'ign_gazebo.launch.py')]),
launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])],
condition=IfCondition(sim_gazebo))
# Spawn robot
gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
arguments=[
'-name', rbs_robot_type,
'-x', '0.0',
'-z', '0.0',
'-y', '0.0',
'-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
# MoveIt Configuration
robot_description_semantic_content = Command(
@ -424,17 +423,6 @@ def generate_launch_description():
condition=IfCondition(with_gripper_condition)
)
#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},
# ]
#)
move_cartesian_path_action_server = Node(
package="rbs_skill_servers",
executable="move_cartesian_path_action_server",
@ -456,9 +444,7 @@ def generate_launch_description():
{"use_sim_time": use_sim_time},
]
)
points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")
grasp_pose_loader = Node(
package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server",
@ -475,7 +461,7 @@ def generate_launch_description():
output="screen",
parameters=[
{'assemble_prefix': 'ASSEMBLE_'},
{'assemble_dir': '/home/splinter1984/tmp_ws/src/robossembler-ros2/rbs_task_planner/example/sdf_models/'}
{'assemble_dir': assemble_dir}
]
)
@ -539,8 +525,8 @@ def generate_launch_description():
rviz_node,
initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started,
# gazebo,
# gazebo_spawn_robot,
gazebo,
gazebo_spawn_robot,
move_group_node,
gripper_controller,
gripper_control_node,