robossembler/runtime
7
0
Fork 0
Code Issues 5 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?") 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 # Initialize Arguments
rbs_robot_type = LaunchConfiguration("rbs_robot_type") rbs_robot_type = LaunchConfiguration("rbs_robot_type")
@ -173,6 +181,9 @@ def generate_launch_description():
moveit_config_package = LaunchConfiguration("moveit_config_package") moveit_config_package = LaunchConfiguration("moveit_config_package")
moveit_config_file = LaunchConfiguration("moveit_config_file") moveit_config_file = LaunchConfiguration("moveit_config_file")
use_sim_time = LaunchConfiguration("use_sim_time") 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( initial_joint_controllers_file_path = PathJoinSubstitution(
@ -190,6 +201,12 @@ def generate_launch_description():
mujoco_model = PathJoinSubstitution( mujoco_model = PathJoinSubstitution(
[FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"] [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( robot_description_content = Command(
[ [
@ -199,34 +216,18 @@ def generate_launch_description():
[FindPackageShare(description_package), "urdf", description_file] [FindPackageShare(description_package), "urdf", description_file]
), ),
" ", " ",
"safety_limits:=", "safety_limits:=", safety_limits, " ",
safety_limits, "safety_pos_margin:=", safety_pos_margin, " ",
" ", "safety_k_position:=", safety_k_position, " ",
"safety_pos_margin:=", "name:=", "ur", " ",
safety_pos_margin, "ur_type:=", rbs_robot_type, " ",
" ", "prefix:=", prefix, " ",
"safety_k_position:=", "sim_mujoco:=", sim_mujoco, " ",
safety_k_position, "sim_gazebo:=", sim_gazebo, " ",
" ", "sim_fake:=", sim_fake, " ",
"name:=", "simulation_controllers:=", initial_joint_controllers_file_path, " ",
"ur", "with_gripper:=", with_gripper_condition, " ",
" ", "mujoco_model:=", mujoco_model,
"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,
] ]
) )
robot_description = {"robot_description": robot_description_content} robot_description = {"robot_description": robot_description_content}
@ -235,15 +236,11 @@ def generate_launch_description():
package="controller_manager", package="controller_manager",
executable="ros2_control_node", executable="ros2_control_node",
parameters=[robot_description, initial_joint_controllers_file_path], parameters=[robot_description, initial_joint_controllers_file_path],
#prefix=['xterm -e gdb -ex run --args'],
output="both", output="both",
remappings=[ remappings=[
('motion_control_handle/target_frame', 'target_frame'), ('motion_control_handle/target_frame', 'target_frame'),
('cartesian_motion_controller/target_frame', 'target_frame'),
('cartesian_compliance_controller/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_compliance_controller/target_wrench', 'target_wrench'),
# ('cartesian_force_controller/ft_sensor_wrench', 'ft_sensor_wrench'),
('cartesian_compliance_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 nodes
# gazebo = IncludeLaunchDescription( gazebo = IncludeLaunchDescription(
# PythonLaunchDescriptionSource( PythonLaunchDescriptionSource(
# [os.path.join(get_package_share_directory('ros_ign_gazebo'), [os.path.join(get_package_share_directory('ros_ign_gazebo'),
# 'launch', 'ign_gazebo.launch.py')]), 'launch', 'ign_gazebo.launch.py')]),
# launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])]) launch_arguments=[('ign_args', [' -r ',world_config_file, " --physics-engine ignition-physics-dartsim-plugin --render-engine ogre2"])],
# # Spawn robot condition=IfCondition(sim_gazebo))
# gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create', # Spawn robot
# arguments=[ gazebo_spawn_robot = Node(package='ros_ign_gazebo', executable='create',
# '-name', rbs_robot_type, arguments=[
# '-x', '0.0', '-name', rbs_robot_type,
# '-z', '0.0', '-x', '0.0',
# '-y', '0.0', '-z', '0.0',
# '-topic', '/robot_description'], '-y', '0.0',
# output='screen') '-topic', '/robot_description'],
output='screen',
condition=IfCondition(sim_gazebo))
# MoveIt Configuration # MoveIt Configuration
robot_description_semantic_content = Command( robot_description_semantic_content = Command(
@ -424,17 +423,6 @@ def generate_launch_description():
condition=IfCondition(with_gripper_condition) 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( move_cartesian_path_action_server = Node(
package="rbs_skill_servers", package="rbs_skill_servers",
executable="move_cartesian_path_action_server", executable="move_cartesian_path_action_server",
@ -456,9 +444,7 @@ def generate_launch_description():
{"use_sim_time": use_sim_time}, {"use_sim_time": use_sim_time},
] ]
) )
points_params = load_yaml("rbs_bt_executor", "config/gripperPositions.yaml")
grasp_pose_loader = Node( grasp_pose_loader = Node(
package="rbs_skill_servers", package="rbs_skill_servers",
executable="pick_place_pose_loader_service_server", executable="pick_place_pose_loader_service_server",
@ -475,7 +461,7 @@ def generate_launch_description():
output="screen", output="screen",
parameters=[ parameters=[
{'assemble_prefix': 'ASSEMBLE_'}, {'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, rviz_node,
initial_joint_controller_spawner_stopped, initial_joint_controller_spawner_stopped,
initial_joint_controller_spawner_started, initial_joint_controller_spawner_started,
# gazebo, gazebo,
# gazebo_spawn_robot, gazebo_spawn_robot,
move_group_node, move_group_node,
gripper_controller, gripper_controller,
gripper_control_node, gripper_control_node,