From d0625eb65e140fca1e802462d41f234386fd8440 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 24 Jul 2023 12:47:11 +0300 Subject: [PATCH] refactor rbs_simulation launch file --- .../launch/rbs_simulation.launch.py | 116 ++++++++---------- 1 file changed, 51 insertions(+), 65 deletions(-) diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index 1f9328e..8858d9f 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -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,