test_urdf

robot_description/frida_description/launch/test_xarm_launch.launch.py

@@ -0,0 +1,283 @@
+#!/usr/bin/env python3
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2021, UFACTORY, Inc.
+# All rights reserved.
+#
+# Author: Vinman <[email protected]> <[email protected]>
+
+import os
+import yaml
+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.conditions import IfCondition
+from launch_ros.substitutions import FindPackageShare
+from launch.event_handlers import OnProcessExit, OnProcessStart
+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)
+    add_bio_gripper = LaunchConfiguration('add_bio_gripper', default=False)
+    dof = LaunchConfiguration('dof', default=6)
+    robot_type = LaunchConfiguration('robot_type', default='xarm')
+    ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
+
+    add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
+    add_d435i_links = LaunchConfiguration('add_d435i_links', default=True)
+    model1300 = LaunchConfiguration('model1300', default=False)
+    robot_sn = LaunchConfiguration('robot_sn', default='')
+    attach_to = LaunchConfiguration('attach_to', default='base_link')
+    attach_xyz = LaunchConfiguration('attach_xyz', default='"0.0049756 0 0.34"')
+    attach_rpy = LaunchConfiguration('attach_rpy', default='"0 0 1.57"')
+
+    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"')
+    kinematics_suffix = LaunchConfiguration('kinematics_suffix', default='')
+
+    load_controller = LaunchConfiguration('load_controller', default=False)
+    show_rviz = LaunchConfiguration('show_rviz', default=False)
+    no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
+
+    ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
+    moveit_config_dump = LaunchConfiguration('moveit_config_dump', default='')
+
+    moveit_config_dump = moveit_config_dump.perform(context)
+    moveit_config_dict = yaml.load(moveit_config_dump, Loader=yaml.FullLoader) if moveit_config_dump else {}
+    moveit_config_package_name = 'xarm_moveit_config'
+    xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context) if robot_type.perform(context) in ('xarm', 'lite') else '')
+
+    if not moveit_config_dict:
+        # 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(xarm_type)),
+            prefix=prefix.perform(context), 
+            add_gripper=add_gripper.perform(context) in ('True', 'true'),
+            add_bio_gripper=add_bio_gripper.perform(context) in ('True', 'true'),
+            ros_namespace=ros_namespace,
+            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('frida_description'), 'urdf', 'FRIDA.urdf.xacro']), 
+                arguments={
+                    'prefix': prefix,
+                    'dof': dof,
+                    'robot_type': robot_type,
+                    'add_gripper': add_gripper,
+                    'add_vacuum_gripper': add_vacuum_gripper,
+                    'add_bio_gripper': add_bio_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_realsense_d435i': add_realsense_d435i,
+                    'add_d435i_links': add_d435i_links,
+                    'model1300': model1300,
+                    'robot_sn': robot_sn,
+                    'attach_to': attach_to,
+                    'attach_xyz': attach_xyz,
+                    'attach_rpy': attach_rpy,
+                    '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,
+                    'kinematics_suffix': kinematics_suffix,
+                }
+            ),
+        }
+        moveit_config_dict = robot_description
+    else:
+        robot_description = {'robot_description': moveit_config_dict['robot_description']}
+
+    # robot state publisher node
+    robot_state_publisher_node = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        output='screen',
+        parameters=[{'use_sim_time': True}, robot_description],
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # gazebo launch
+    # gazebo_ros/launch/gazebo.launch.py
+    xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('frida_description'), 'worlds', 'empty_classic.world'])
+    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(xarm_type),
+            '-entity', 'UF_ROBOT',
+            '-x', '-0.2',
+            '-y', '-0.54' if robot_type.perform(context) == 'uf850' else '-0.5',
+            '-z', '0.2',
+            '-Y', '1.571',
+        ],
+        parameters=[{'use_sim_time': True}],
+    )
+
+    # rviz with moveit configuration
+    rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
+    rviz2_node = Node(
+        package='rviz2',
+        executable='rviz2',
+        name='rviz2',
+        output='screen',
+        arguments=['-d', rviz_config_file],
+        parameters=[
+            {
+                'robot_description': moveit_config_dict.get('robot_description', ''),
+                'robot_description_semantic': moveit_config_dict.get('robot_description_semantic', ''),
+                'robot_description_kinematics': moveit_config_dict.get('robot_description_kinematics', {}),
+                'robot_description_planning': moveit_config_dict.get('robot_description_planning', {}),
+                'planning_pipelines': moveit_config_dict.get('planning_pipelines', {}),
+                'use_sim_time': True
+            }
+        ],
+        # condition=IfCondition(show_rviz),
+        remappings=[
+            ('/tf', 'tf'),
+            ('/tf_static', 'tf_static'),
+        ]
+    )
+
+    # Load controllers
+    controllers = [
+        'joint_state_broadcaster',
+        '{}{}_traj_controller'.format(prefix.perform(context), xarm_type),
+    ]
+    if robot_type.perform(context) != 'lite' and add_gripper.perform(context) in ('True', 'true'):
+        controllers.append('{}{}_gripper_traj_controller'.format(prefix.perform(context), robot_type.perform(context)))
+    elif robot_type.perform(context) != 'lite' and add_bio_gripper.perform(context) in ('True', 'true'):
+        controllers.append('{}bio_gripper_traj_controller'.format(prefix.perform(context)))
+
+    controller_nodes = []
+    if load_controller.perform(context) in ('True', 'true'):
+        for controller in controllers:
+            controller_nodes.append(Node(
+                package='controller_manager',
+                executable='spawner',
+                output='screen',
+                arguments=[
+                    controller,
+                    '--controller-manager', '{}/controller_manager'.format(ros_namespace)
+                ],
+                parameters=[{'use_sim_time': True}],
+            ))
+
+    if len(controller_nodes) > 0:
+        return [
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=robot_state_publisher_node,
+                    on_start=gazebo_launch,
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=robot_state_publisher_node,
+                    on_start=gazebo_spawn_entity_node,
+                )
+            ),
+            RegisterEventHandler(
+                condition=IfCondition(show_rviz),
+                event_handler=OnProcessExit(
+                    target_action=gazebo_spawn_entity_node,
+                    on_exit=rviz2_node,
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessExit(
+                    target_action=gazebo_spawn_entity_node,
+                    on_exit=controller_nodes,
+                )
+            ),
+            robot_state_publisher_node,
+            # gazebo_launch,
+            # gazebo_spawn_entity_node,
+        ]
+    else:
+        return [
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=robot_state_publisher_node,
+                    on_start=gazebo_launch,
+                )
+            ),
+            RegisterEventHandler(
+                event_handler=OnProcessStart(
+                    target_action=robot_state_publisher_node,
+                    on_start=gazebo_spawn_entity_node,
+                )
+            ),
+            RegisterEventHandler(
+                condition=IfCondition(show_rviz),
+                event_handler=OnProcessExit(
+                    target_action=gazebo_spawn_entity_node,
+                    on_exit=rviz2_node,
+                )
+            ),
+            robot_state_publisher_node,
+            # gazebo_launch,
+            # gazebo_spawn_entity_node,
+        ]
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        OpaqueFunction(function=launch_setup)
+    ])

robot_description/frida_description/urdf/FRIDA.urdf.xacro

@@ -44,7 +44,7 @@
   <xacro:arg name="attach_rpy" default="0 0 1.57"/>
   <xacro:arg name="mesh_suffix" default="stl"/>
   <xacro:arg name="kinematics_suffix" default=""/>
-  <xacro:arg name="sim_mode" default="false"/>
+  <xacro:arg name="sim_mode" default="true"/>
 
 
   <xacro:include filename="robot.xacro"/>
@@ -70,10 +70,10 @@
     model1300="$(arg model1300)" attach_to="$(arg attach_to)" attach_xyz="$(arg attach_xyz)" attach_rpy="$(arg attach_rpy)"
     robot_sn="$(arg robot_sn)" use_gazebo_camera="$(arg use_gazebo_camera)" mesh_suffix="$(arg mesh_suffix)" kinematics_suffix="$(arg kinematics_suffix)" />
  <!-- Gripper -->
-  <xacro:include filename="gripper.xacro" />
+  <!-- <xacro:include filename="gripper.xacro" />
   <xacro:load_gripper attach_to="link_eef" xyz="0 0 0" rpy="0 -1.5708 0" />
 
   <xacro:include filename="zed.xacro" />
-  <xacro:load_zed attach_to="gripper" xyz="0.030461 0 -0.050649" rpy="0 0.81804 0" />
+  <xacro:load_zed attach_to="gripper" xyz="0.030461 0 -0.050649" rpy="0 0.81804 0" /> -->
 
 </robot>   

robot_description/frida_description/urdf/gripper.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-<xacro:property name="mesh_path" value="package://frida_description/urdf/meshes/" />
+<xacro:property name="mesh_path" value="file://$(find frida_description)/urdf/meshes/" />
 
 <xacro:macro name="load_gripper" params="attach_to:='' xyz:='0 0 0' rpy:='0 0 0'">
     <xacro:unless value="${attach_to == ''}">

robot_description/frida_description/urdf/robot.xacro

@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
 
-<xacro:property name="mesh_path" value="package://frida_description/urdf/meshes/" />
+<xacro:property name="mesh_path" value="file://$(find frida_description)/urdf/meshes/" />
 <xacro:property name="material_rgba" value="0.745 0.203 0.482 1" />
 <xacro:property name="lidar_rgba" value="0.102 0.102 0.102 1" />
 <xacro:property name="intel_rgba" value="0.776 0.757 0.737 1" />

robot_description/frida_description/worlds/empty_classic.world

@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<sdf version="1.4">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+
+    <physics type="ode">
+      <max_step_size>0.001</max_step_size>
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
+      <!-- (2022-12-20) humble: change gravity from [0 0 -9.81] to [0 0 0] -->
+      <gravity>0 0 0</gravity>
+    </physics>
+
+  </world>
+</sdf>