From da0ef3cb7147425ef35b9b96a337c3330fb31a67 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 17 Jul 2023 16:44:02 +0300 Subject: [PATCH] update rbs_simulation --- rbs_simulation/CMakeLists.txt | 2 +- .../launch/rbs_simulation.launch.py | 123 ++++++++++++++---- 2 files changed, 99 insertions(+), 26 deletions(-) diff --git a/rbs_simulation/CMakeLists.txt b/rbs_simulation/CMakeLists.txt index 164c689..6a9b3c9 100644 --- a/rbs_simulation/CMakeLists.txt +++ b/rbs_simulation/CMakeLists.txt @@ -12,7 +12,7 @@ find_package(ament_cmake REQUIRED) # find_package( REQUIRED) install( - DIRECTORY launch worlds + DIRECTORY launch worlds mujoco_model DESTINATION share/${PROJECT_NAME} ) diff --git a/rbs_simulation/launch/rbs_simulation.launch.py b/rbs_simulation/launch/rbs_simulation.launch.py index bdfed36..9c57807 100644 --- a/rbs_simulation/launch/rbs_simulation.launch.py +++ b/rbs_simulation/launch/rbs_simulation.launch.py @@ -1,9 +1,10 @@ import os -from launch import LaunchDescription +from launch import LaunchDescription, LaunchContext from launch.actions import ( DeclareLaunchArgument, IncludeLaunchDescription, - ExecuteProcess + ExecuteProcess, + OpaqueFunction ) from ament_index_python.packages import get_package_share_directory from launch.conditions import IfCondition, UnlessCondition @@ -12,6 +13,8 @@ from launch.substitutions import Command, FindExecutable, LaunchConfiguration, P from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from ur_moveit_config.launch_common import load_yaml +import xacro + def generate_launch_description(): declared_arguments = [] # UR specific arguments @@ -26,7 +29,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "with_gripper", - default_value="true", + default_value="false", description="With gripper or not?", ) ) @@ -158,7 +161,7 @@ def generate_launch_description(): # General arguments runtime_config_package = LaunchConfiguration("runtime_config_package") with_gripper_condition = LaunchConfiguration("with_gripper") - controllers_file = LaunchConfiguration("controllers_with_gripper_file") + controllers_file = LaunchConfiguration("controllers_file") description_package = LaunchConfiguration("description_package") description_file = LaunchConfiguration("description_file") @@ -183,6 +186,10 @@ def generate_launch_description(): world_config_file = PathJoinSubstitution( [FindPackageShare("rbs_simulation"), "worlds", "mir.sdf"] ) + + mujoco_model = PathJoinSubstitution( + [FindPackageShare("rbs_simulation"), "mujoco_model", "current_mj.xml"] + ) robot_description_content = Command( [ @@ -210,16 +217,36 @@ def generate_launch_description(): "prefix:=", prefix, " ", - "sim_ignition:=true", + "sim_mujoco:=true", " ", "simulation_controllers:=", initial_joint_controllers_file_path, " ", "with_gripper:=", - with_gripper_condition + with_gripper_condition, + " ", + "mujoco_model:=", + mujoco_model, ] ) robot_description = {"robot_description": robot_description_content} + + control_node = Node( + 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'), + # ('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'), + ] + ) robot_state_publisher_node = Node( package="robot_state_publisher", @@ -231,20 +258,20 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], ) # There may be other controllers of the joints, but this is the initially-started one initial_joint_controller_spawner_started = Node( package="controller_manager", executable="spawner", - arguments=[initial_joint_controller, "--controller-manager", "/controller_manager"], + arguments=[initial_joint_controller, "-c", "/controller_manager"], condition=IfCondition(start_joint_controller), ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", executable="spawner", - arguments=[initial_joint_controller, "--controller-manager", "/controller_manager", "--stopped"], + arguments=[initial_joint_controller, "-c", "/controller_manager", "--incactive"], condition=UnlessCondition(start_joint_controller), ) @@ -254,22 +281,32 @@ def generate_launch_description(): output='screen', condition=IfCondition(with_gripper_condition) ) + cartesian_motion_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["cartesian_motion_controller", "--inactive", "-c", "/controller_manager"], + ) + motion_control_handle_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["motion_control_handle", "--inactive", "-c", "/controller_manager"], + ) # 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"])]) + # # 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') # MoveIt Configuration robot_description_semantic_content = Command( @@ -457,14 +494,46 @@ def generate_launch_description(): # ] # ) + # remappings = [('/camera', '/camera/image'), + # ('/camera_info', '/camera/camera_info')] + # # Bridge + # bridge = Node( + # package='ros_gz_bridge', + # executable='parameter_bridge', + # arguments=[ + # '/camera@sensor_msgs/msg/Image@gz.msgs.Image', + # '/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', + # '/rgbd_camera/image@sensor_msgs/msg/Image@gz.msgs.Image', + # '/rgbd_camera/camera_info@sensor_msgs/msg/CameraInfo@gz.msgs.CameraInfo', + # '/rgbd_camera/depth_image@sensor_msgs/msg/Image@gz.msgs.Image', + # '/rgbd_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked' + # ], + # output='screen', + # remappings=remappings, + # ) + + # pc_filter = Node( + # package="rbs_perception", + # executable="pc_filter", + # output="screen", + # #prefix=['xterm -e gdb -ex run --args'], + # ) + + grasp_marker = Node( + package="rbs_perception", + executable="grasp_marker_publish.py", + name="hyak", + ) + nodes_to_start = [ + control_node, robot_state_publisher_node, joint_state_broadcaster_spawner, 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, @@ -474,6 +543,10 @@ def generate_launch_description(): grasp_pose_loader, assemble_state, moveit_planning_scene_init, + #bridge, + cartesian_motion_controller_spawner, + motion_control_handle_spawner, + #pc_filter #add_planning_scene_object ]