diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt index 0ca4ed3..1e383ce 100644 --- a/src/draw_svg/CMakeLists.txt +++ b/src/draw_svg/CMakeLists.txt @@ -32,6 +32,10 @@ install(PROGRAMS ${SRC_PY_DIR}/follow.py DESTINATION lib/${PROJECT_NAME} ) +install(PROGRAMS + ${SRC_PY_DIR}/robots/lite6.py + DESTINATION lib/${PROJECT_NAME}/robots/ +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/src/draw_svg/launch/default.launch.py b/src/draw_svg/launch/default.launch.py index a66b44d..6f79edf 100755 --- a/src/draw_svg/launch/default.launch.py +++ b/src/draw_svg/launch/default.launch.py @@ -82,11 +82,7 @@ def generate_launch_description() -> LaunchDescription: IncludeLaunchDescription( PythonLaunchDescriptionSource( PathJoinSubstitution( - [ - FindPackageShare([robot_type, "_moveit_config"]), - "launch", - "move_group.launch.py", - ] + [FindPackageShare("xarm_moveit_config"), "launch", "lite6_moveit_fake.launch.py"] ) ), launch_arguments=[ diff --git a/src/draw_svg/launch/draw_svg.launch.py b/src/draw_svg/launch/draw_svg.launch.py index bb5f38a..c5efda7 100755 --- a/src/draw_svg/launch/draw_svg.launch.py +++ b/src/draw_svg/launch/draw_svg.launch.py @@ -81,16 +81,16 @@ def generate_declared_arguments() -> List[DeclareLaunchArgument]: # Robot selection DeclareLaunchArgument( "robot_type", - default_value="panda", + default_value="lite6", description="Name of the robot to use.", ), # Miscellaneous DeclareLaunchArgument( "rviz_config", default_value=path.join( - get_package_share_directory("ign_moveit2_examples"), + get_package_share_directory("xarm_description"), "rviz", - "ign_moveit2_examples.rviz", + "display.rviz", ), description="Path to configuration for RViz2.", ), diff --git a/src/draw_svg/launch/robots/robot_lite6.launch.py b/src/draw_svg/launch/robots/robot_lite6.launch.py new file mode 100644 index 0000000..4e5ff71 --- /dev/null +++ b/src/draw_svg/launch/robots/robot_lite6.launch.py @@ -0,0 +1,61 @@ +#!/usr/bin/env -S ros2 launch +"""Launch script for spawning ufactory xarm lite6 into Ignition Gazebo world""" + +from typing import List + +from launch_ros.actions import Node + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + + +def generate_launch_description() -> LaunchDescription: + + # Declare all launch arguments + declared_arguments = generate_declared_arguments() + + # Get substitution for all arguments + model = LaunchConfiguration("model") + use_sim_time = LaunchConfiguration("use_sim_time") + log_level = LaunchConfiguration("log_level") + + # List of nodes to be launched + nodes = [ + # ros_ign_gazebo_create + Node( + package="ros_ign_gazebo", + executable="create", + output="log", + arguments=["-file", model, "--ros-args", "--log-level", log_level], + parameters=[{"use_sim_time": use_sim_time}], + ), + ] + + return LaunchDescription(declared_arguments + nodes) + + +def generate_declared_arguments() -> List[DeclareLaunchArgument]: + """ + Generate list of all launch arguments that are declared for this launch script. + """ + + return [ + # Model for Ignition Gazebo + DeclareLaunchArgument( + "model", + default_value="lite6", + description="Name or filepath of model to load.", + ), + # Miscellaneous + DeclareLaunchArgument( + "use_sim_time", + default_value="true", + description="If true, use simulated clock.", + ), + DeclareLaunchArgument( + "log_level", + default_value="warn", + description="The level of logging that is applied to all ROS 2 nodes launched by this script.", + ), + ] diff --git a/src/draw_svg/launch/robots/robot_panda.launch.py b/src/draw_svg/launch/robots/robot_panda.launch.py index 4747b34..01b1f73 100755 --- a/src/draw_svg/launch/robots/robot_panda.launch.py +++ b/src/draw_svg/launch/robots/robot_panda.launch.py @@ -44,7 +44,7 @@ def generate_declared_arguments() -> List[DeclareLaunchArgument]: # Model for Ignition Gazebo DeclareLaunchArgument( "model", - default_value="panda", + default_value="lite6", description="Name or filepath of model to load.", ), # Miscellaneous diff --git a/src/draw_svg/launch/xarm_draw_svg.launch.py b/src/draw_svg/launch/xarm_draw_svg.launch.py new file mode 100644 index 0000000..8038f89 --- /dev/null +++ b/src/draw_svg/launch/xarm_draw_svg.launch.py @@ -0,0 +1,124 @@ +#!/usr/bin/env python3 + +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.substitutions import FindPackageShare + + +def launch_setup(context, *args, **kwargs): + prefix = LaunchConfiguration('prefix', default='') + hw_ns = LaunchConfiguration('hw_ns', default='xarm') + limited = LaunchConfiguration('limited', default=True) + 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') + no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False) + + 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"') + + 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': no_gui_ctrl, + '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'])), + 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(), + ) + + # List of nodes to be launched + # Run the example node (Python) + followNode = Node( + package="draw_svg", + executable="follow.py", + output="log", + arguments=["--ros-args", "--log-level", "warn"], + parameters=[{"use_sim_time": True}], + ) + drawNode = Node( + package="draw_svg", + executable="draw_svg.py", + output="log", + arguments=["--ros-args", "--log-level", "warn"], + parameters=[{"use_sim_time": True}], + ) + + return [ + robot_gazebo_launch, + robot_moveit_common_launch, + followNode, + drawNode, + ] + + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/draw_svg/package.xml b/src/draw_svg/package.xml index 8822291..6f0282b 100644 --- a/src/draw_svg/package.xml +++ b/src/draw_svg/package.xml @@ -14,6 +14,8 @@ geometry_msgs tf2_ros python-lxml + xarm_description + xarm_moveit_config ament_lint_auto diff --git a/src/draw_svg/src/py/follow.py b/src/draw_svg/src/py/follow.py index 0e0828d..bd358b4 100755 --- a/src/draw_svg/src/py/follow.py +++ b/src/draw_svg/src/py/follow.py @@ -4,7 +4,7 @@ import rclpy from geometry_msgs.msg import Pose, PoseStamped from pymoveit2 import MoveIt2 -from pymoveit2.robots import panda +from robots import lite6 from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.qos import QoSProfile @@ -21,10 +21,10 @@ class FollowTarget(Node): # Create MoveIt 2 interface self._moveit2 = MoveIt2( node=self, - joint_names=panda.joint_names(), - base_link_name=panda.base_link_name(), - end_effector_name=panda.end_effector_name(), - group_name=panda.MOVE_GROUP_ARM, + joint_names=lite6.joint_names(), + base_link_name=lite6.base_link_name(), + end_effector_name=lite6.end_effector_name(), + group_name=lite6.MOVE_GROUP_ARM, execute_via_moveit=True, callback_group=self._callback_group, ) diff --git a/src/draw_svg/src/py/robots/lite6.py b/src/draw_svg/src/py/robots/lite6.py new file mode 100644 index 0000000..bd66959 --- /dev/null +++ b/src/draw_svg/src/py/robots/lite6.py @@ -0,0 +1,35 @@ +from typing import List + +MOVE_GROUP_ARM: str = "lite6" +MOVE_GROUP_GRIPPER: str = "gripper" + +OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.04, 0.04] +CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0, 0.0] + +# https://github.com/xArm-Developer/xarm_ros2/blob/master/xarm_moveit_config/srdf/_lite6_macro.srdf.xacro +def joint_names(prefix: str = "") -> List[str]: + return [ + prefix + "world_joint", + prefix + "joint1", + prefix + "joint2", + prefix + "joint3", + prefix + "joint4", + prefix + "joint5", + prefix + "joint6", + prefix + "joint_eef", + ] + + +def base_link_name(prefix: str = "") -> str: + return prefix + "link_base" + + +def end_effector_name(prefix: str = "") -> str: + return prefix + "link_eef" + + +def gripper_joint_names(prefix: str = "") -> List[str]: + return [ + prefix + "link_eef", + prefix + "link_eef", + ]