Place xarm in custom world with drawing surface

src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py

@@ -0,0 +1,168 @@
+#!/usr/bin/env python3
+
+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'))
+    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']),
+            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', 'follow_target.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',
+            '-y', '-0.5',
+            '-z', '1.021',
+            '-Y', '1.571',
+        ],
+    )
+
+    # 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',
+                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)
+    ])

src/draw_svg/launch/xarm_draw_svg.launch.py

@@ -77,7 +77,7 @@ def launch_setup(context, *args, **kwargs):
     # 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', '_robot_beside_table_gazebo.launch.py'])),
         launch_arguments={
             'prefix': prefix,
             'hw_ns': hw_ns,
@@ -110,11 +110,22 @@ def launch_setup(context, *args, **kwargs):
             parameters=[{"use_sim_time": True}],
         )
 
+    # ros_ign_gazebo_create
+    #model = LaunchConfiguration("model")
+    #ros_ign_bridge = 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 [
         robot_gazebo_launch,
         robot_moveit_common_launch,
         followNode,
         drawNode,
+        #ros_ign_bridge,
     ]
 
 

src/draw_svg/src/py/draw_svg.py

@@ -66,7 +66,7 @@ class PublishTarget(Node):
         #print(p.orientation)
         xml = ET.parse('svg/test.svg')
         svg = xml.getroot()
-        self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.0, 0.5, 0.3, 0.8)
+        self.map_point = map_point_function(float(svg.get('width')), float(svg.get('height')), 0.1, 0.5, -0.2, 0.2)
         self.points = []
         for child in svg:
             if (child.tag == 'line'):

src/draw_svg/worlds/follow_target.sdf

@@ -51,7 +51,7 @@
     <!-- Models -->
     <!--        -->
     <!-- Ground -->
-    <model name="ground_plane">
+    <!-- <model name="ground_plane">
          <pose>0 0 0 0 0 0</pose>
          <static>true</static>
          <link name="ground_plane_link">
@@ -75,7 +75,44 @@
          </material>
          </visual>
          </link>
+         </model> -->
+    <model name="drawing_surface">
+      <pose>-0.14 -0.3 1.0 0 0 0</pose>
+      <static>true</static>
+      <link name="drawing_surface_link">
+        <collision name="drawing_surface_collision">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>0.28 0.21</size>
+            </plane>
+          </geometry>
+        </collision>
+        <visual name="drawing_surface_visual">
+          <geometry>
+            <plane>
+              <normal>0 0 1</normal>
+              <size>0.28 0.21</size>
+            </plane>
+          </geometry>
+          <material>
+            <diffuse>0.8 0.8 0.8 1</diffuse>
+            <specular>0.8 0.8 0.8 1</specular>
+          </material>
+        </visual>
+      </link>
     </model>
+    <!-- <include>
+         <uri>model://ground_plane</uri>
+         </include> -->
+    <!-- <include>
+         <uri>model://sun</uri>
+         </include> -->
+    <include>
+      <uri>model://table</uri>
+      <name>table</name>
+      <pose>0.0 -0.84 0 0 0 0</pose>
+    </include>
 
     <!-- Static target -->
     <!-- <model name="target">