diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt index 5cb3129..d9b423c 100644 --- a/src/draw_svg/CMakeLists.txt +++ b/src/draw_svg/CMakeLists.txt @@ -52,6 +52,6 @@ if(BUILD_TESTING) endif() # Install directories -install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/src/draw_svg/launch/lite6_pen.launch.py b/src/draw_svg/launch/lite6_pen.launch.py new file mode 100644 index 0000000..06bbcfb --- /dev/null +++ b/src/draw_svg/launch/lite6_pen.launch.py @@ -0,0 +1,141 @@ +#!/usr/bin/env -S ros2 launch +"""Launch Python example for following a target""" + +import os +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + use_sim_time = LaunchConfiguration("use_sim_time", default=True) + log_level = LaunchConfiguration("log_level", default='warn') + rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz")) + + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=False) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=6) + robot_type = LaunchConfiguration('robot_type', default='lite') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + #geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_type = LaunchConfiguration('geometry_type', default='cylinder') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.05) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.005) + geometry_length = LaunchConfiguration('geometry_length', default=0.07) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + load_controller = LaunchConfiguration('load_controller', default=True) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + + ros2_control_plugin = 'gazebo_ros2_control/GazeboSystem' + controllers_name = 'fake_controllers' + moveit_controller_manager_key = 'moveit_simple_controller_manager' + moveit_controller_manager_value = 'moveit_simple_controller_manager/MoveItSimpleControllerManager' + + # robot moveit common launch + # xarm_moveit_config/launch/_robot_moveit_common.launch.py + robot_moveit_common_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + # 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false', + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'no_gui_ctrl': 'false', + 'ros2_control_plugin': ros2_control_plugin, + 'controllers_name': controllers_name, + 'moveit_controller_manager_key': moveit_controller_manager_key, + 'moveit_controller_manager_value': moveit_controller_manager_value, + 'add_other_geometry': add_other_geometry, + 'geometry_type': geometry_type, + 'geometry_mass': geometry_mass, + 'geometry_height': geometry_height, + 'geometry_radius': geometry_radius, + 'geometry_length': geometry_length, + 'geometry_width': geometry_width, + 'geometry_mesh_filename': geometry_mesh_filename, + 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, + 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, + 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, + 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, + 'use_sim_time': 'true' + }.items(), + ) + + # robot gazebo launch + # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py + robot_gazebo_launch = IncludeLaunchDescription( + #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])), + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])), + launch_arguments={ + 'prefix': prefix, + 'hw_ns': hw_ns, + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'dof': dof, + 'robot_type': robot_type, + 'ros2_control_plugin': ros2_control_plugin, + 'load_controller': 'true', + }.items(), + ) + + + nodes = [ + Node( + package="draw_svg", + executable="follow.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + Node( + package="draw_svg", + executable="draw_svg.py", + output="log", + arguments=["--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return [ + nodes[0], + nodes[1], + robot_moveit_common_launch, + robot_gazebo_launch, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/draw_svg/launch/robots/lite6_table.launch.py b/src/draw_svg/launch/robots/lite6_table.launch.py new file mode 100644 index 0000000..16be54b --- /dev/null +++ b/src/draw_svg/launch/robots/lite6_table.launch.py @@ -0,0 +1,173 @@ +import os +from ament_index_python import get_package_share_directory +from launch.launch_description_sources import load_python_launch_file_as_module +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from launch.event_handlers import OnProcessExit +from launch.actions import OpaqueFunction + + +def launch_setup(context, *args, **kwargs): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=False) + effort_control = LaunchConfiguration('effort_control', default=False) + velocity_control = LaunchConfiguration('velocity_control', default=False) + add_gripper = LaunchConfiguration('add_gripper', default=False) + add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) + dof = LaunchConfiguration('dof', default=7) + robot_type = LaunchConfiguration('robot_type', default='xarm') + ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem') + + add_other_geometry = LaunchConfiguration('add_other_geometry', default=False) + geometry_type = LaunchConfiguration('geometry_type', default='box') + geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) + geometry_height = LaunchConfiguration('geometry_height', default=0.1) + geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) + geometry_length = LaunchConfiguration('geometry_length', default=0.1) + geometry_width = LaunchConfiguration('geometry_width', default=0.1) + geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') + geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') + geometry_mesh_origin_rpy = LaunchConfiguration('geometry_mesh_origin_rpy', default='"0 0 0"') + geometry_mesh_tcp_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"') + geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') + load_controller = LaunchConfiguration('load_controller', default=False) + + ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) + + # ros2 control params + # xarm_controller/launch/lib/robot_controller_lib.py + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_controller'), 'launch', 'lib', 'robot_controller_lib.py')) + generate_ros2_control_params_temp_file = getattr(mod, 'generate_ros2_control_params_temp_file') + ros2_control_params = generate_ros2_control_params_temp_file( + os.path.join(get_package_share_directory('xarm_controller'), 'config', '{}{}_controllers.yaml'.format(robot_type.perform(context), dof.perform(context))), + prefix=prefix.perform(context), + add_gripper=add_gripper.perform(context) in ('True', 'true'), + ros_namespace=LaunchConfiguration('ros_namespace', default='').perform(context), + update_rate=1000, + #robot_type=robot_type.perform(context) + ) + + # robot_description + # xarm_description/launch/lib/robot_description_lib.py + mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('xarm_description'), 'launch', 'lib', 'robot_description_lib.py')) + #mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('draw_svg'), 'launch', 'robots', 'xarm_with_pen.py')) + get_xacro_file_content = getattr(mod, 'get_xacro_file_content') + robot_description = { + 'robot_description': get_xacro_file_content( + #xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), + xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']), + arguments={ + 'prefix': prefix, + 'dof': dof, + 'robot_type': robot_type, + 'add_gripper': add_gripper, + 'add_vacuum_gripper': add_vacuum_gripper, + 'hw_ns': hw_ns.perform(context).strip('/'), + 'limited': limited, + 'effort_control': effort_control, + 'velocity_control': velocity_control, + 'ros2_control_plugin': ros2_control_plugin, + 'ros2_control_params': ros2_control_params, + 'add_other_geometry': add_other_geometry, + 'geometry_type': geometry_type, + 'geometry_mass': geometry_mass, + 'geometry_height': geometry_height, + 'geometry_radius': geometry_radius, + 'geometry_length': geometry_length, + 'geometry_width': geometry_width, + 'geometry_mesh_filename': geometry_mesh_filename, + 'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz, + 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, + 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, + 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, + } + ), + } + + # robot state publisher node + robot_state_publisher_node = Node( + package='robot_state_publisher', + executable='robot_state_publisher', + output='screen', + parameters=[robot_description], + remappings=[ + ('/tf', 'tf'), + ('/tf_static', 'tf_static'), + ] + ) + + # gazebo launch + # gazebo_ros/launch/gazebo.launch.py + #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world']) + xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf']) + gazebo_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])), + launch_arguments={ + 'world': xarm_gazebo_world, + 'server_required': 'true', + 'gui_required': 'true', + }.items(), + ) + + # gazebo spawn entity node + gazebo_spawn_entity_node = Node( + package="gazebo_ros", + executable="spawn_entity.py", + output='screen', + arguments=[ + '-topic', 'robot_description', + '-entity', '{}{}'.format(robot_type.perform(context), dof.perform(context)), + '-x', '-0.2', + #'-x', '0.0', + '-y', '-0.5', + #'-y', '0.0', + '-z', '1.021', + #'-z', '0.0', + '-Y', '1.571', + #'-Y', '0.0', + ], + ) + + # Load controllers + controllers = [ + 'joint_state_broadcaster', + '{}{}{}_traj_controller'.format(prefix.perform(context), robot_type.perform(context), dof.perform(context)), + ] + if robot_type.perform(context) == 'xarm' and add_gripper.perform(context) in ('True', 'true'): + controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context))) + load_controllers = [] + if load_controller.perform(context) in ('True', 'true'): + for controller in controllers: + load_controllers.append(Node( + package='controller_manager', + #executable='spawner.py', + executable='spawner', + output='screen', + arguments=[ + controller, + '--controller-manager', '{}/controller_manager'.format(ros_namespace) + ], + )) + + return [ + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=gazebo_spawn_entity_node, + on_exit=load_controllers, + ) + ), + gazebo_launch, + robot_state_publisher_node, + gazebo_spawn_entity_node, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/draw_svg/urdf/xarm_pen.urdf.xacro b/src/draw_svg/urdf/xarm_pen.urdf.xacro new file mode 100644 index 0000000..06c2dd8 --- /dev/null +++ b/src/draw_svg/urdf/xarm_pen.urdf.xacro @@ -0,0 +1,81 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/draw_svg/worlds/follow_target.sdf b/src/draw_svg/worlds/draw_svg_world.sdf similarity index 98% rename from src/draw_svg/worlds/follow_target.sdf rename to src/draw_svg/worlds/draw_svg_world.sdf index 7157b38..1579a30 100644 --- a/src/draw_svg/worlds/follow_target.sdf +++ b/src/draw_svg/worlds/draw_svg_world.sdf @@ -1,5 +1,6 @@ - + + @@ -104,6 +105,7 @@