Switch default robot to xarm lite6

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)

src/draw_svg/launch/default.launch.py

@@ -82,11 +82,7 @@ def generate_launch_description() -> LaunchDescription:
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(
                 PathJoinSubstitution(
-                    [
+                    [FindPackageShare("xarm_moveit_config"), "launch", "lite6_moveit_fake.launch.py"]
-                        FindPackageShare([robot_type, "_moveit_config"]),
-                        "launch",
-                        "move_group.launch.py",
-                    ]
                 )
             ),
             launch_arguments=[

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.",
         ),

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.",
+        ),
+    ]

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

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)
+    ])

src/draw_svg/package.xml

@@ -14,6 +14,8 @@
   <depend>geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>python-lxml</depend>
+  <depend>xarm_description</depend>
+  <depend>xarm_moveit_config</depend>
   <!-- <depend>moveit_ros_planning_interface</depend> -->
 
   <test_depend>ament_lint_auto</test_depend>

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(),
+            joint_names=lite6.joint_names(),
-            base_link_name=panda.base_link_name(),
+            base_link_name=lite6.base_link_name(),
-            end_effector_name=panda.end_effector_name(),
+            end_effector_name=lite6.end_effector_name(),
-            group_name=panda.MOVE_GROUP_ARM,
+            group_name=lite6.MOVE_GROUP_ARM,
             execute_via_moveit=True,
             callback_group=self._callback_group,
         )

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",
+    ]