nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix virtual pen, make moveit aware of pen

Browse Source
This commit is contained in:
Nicolas Hiillos
2023-03-23 09:36:49 +02:00
parent 7b99e220f5
commit 53c46c11c0
9 changed files with 472 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
src/custom_xarm_description/urdf/lite6/lite6.urdf.xacro
View File

@@ -363,5 +363,50 @@
<child <child
link="${prefix}link_eef" /> link="${prefix}link_eef" />
</joint> --> </joint> -->
<link name="${prefix}pen_link">
<visual>
<origin xyz="0 0 0" rpy="0 0 0" />
<geometry>
<cylinder radius="0.005" length="0.2"/>
</geometry>
<material name="Cyan">
<color rgba="0.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<geometry>
<box size="0 0 0"/>
</geometry>
</collision>
</link>
<joint name="${prefix}pen_joint" type="fixed">
<parent link="${prefix}link6"/>
<child link="${prefix}pen_link"/>
</joint>
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
<gazebo>
<plugin name="pen_position" filename="libgazebo_ros_p3d.so">
<!-- <alwaysOn>true</alwaysOn> -->
<ros>
<remapping>odom:=pen_position</remapping>
</ros>
<frame_name>world</frame_name>
<!-- <body_name>pen_link</body_name> -->
<body_name>${prefix}link6</body_name>
<!-- <body_name>${prefix}pen_link</body_name> -->
<!-- topic name is always /odom -->
<!-- <topic_name>pen_position</topic_name> -->
<!-- Update rate in Hz -->
<update_rate>10</update_rate>
<!-- <xyzOffsets>0 0 0</xyzOffsets> -->
<!-- <rpyOffsets>0 0 0</rpyOffsets> -->
</plugin>
</gazebo>
</xacro:macro> </xacro:macro>
</robot> </robot>

10
src/custom_xarm_description/urdf/lite6/lite6_robot_macro.xacro
View File

@@ -6,14 +6,14 @@
robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000 "> robot_ip:='' report_type:='normal' baud_checkset:='true' default_gripper_baud:=2000000 ">
<!-- include lite6 relative macros: --> <!-- include lite6 relative macros: -->
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.ros2_control.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.ros2_control.xacro" />
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.urdf.xacro" />
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.transmission.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.transmission.xacro" />
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.gazebo.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6.gazebo.xacro" />
<!-- gazebo_plugin --> <!-- gazebo_plugin -->
<xacro:if value="${load_gazebo_plugin}"> <xacro:if value="${load_gazebo_plugin}">
<xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/common/common.gazebo.xacro" />
<xacro:gazebo_ros_control_plugin prefix="${prefix}" ros2_control_params="${ros2_control_params}"/> <xacro:gazebo_ros_control_plugin prefix="${prefix}" ros2_control_params="${ros2_control_params}"/>
</xacro:if> </xacro:if>

4
src/custom_xarm_description/urdf/xarm_device.urdf.xacro
View File

@@ -33,11 +33,11 @@
<xacro:arg name="default_gripper_baud" default="2000000"/> <xacro:arg name="default_gripper_baud" default="2000000"/>
<!-- gazebo_plugin --> <!-- gazebo_plugin -->
<xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/common/common.gazebo.xacro" />
<xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/> <xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
<!-- load xarm device --> <!-- load xarm device -->
<xacro:include filename="$(find xarm_description)/urdf/xarm_device_macro.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/xarm_device_macro.xacro" />
<xacro:xarm_device prefix="$(arg prefix)" namespace="$(arg hw_ns)" limited="$(arg limited)" <xacro:xarm_device prefix="$(arg prefix)" namespace="$(arg hw_ns)" limited="$(arg limited)"
effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)" effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" dof="$(arg dof)" add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" dof="$(arg dof)"

4
src/custom_xarm_description/urdf/xarm_device_macro.xacro
View File

@@ -35,7 +35,7 @@
<xacro:if value="${dof == 6}"> <xacro:if value="${dof == 6}">
<xacro:if value="${robot_type == 'lite'}"> <xacro:if value="${robot_type == 'lite'}">
<!-- Load Lite6 Robot Model URDF --> <!-- Load Lite6 Robot Model URDF -->
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" /> <xacro:include filename="$(find custom_xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
<xacro:lite6_robot prefix="${prefix}" namespace="${namespace}" limited="${limited}" <xacro:lite6_robot prefix="${prefix}" namespace="${namespace}" limited="${limited}"
effort_control="${effort_control}" velocity_control="${velocity_control}" effort_control="${effort_control}" velocity_control="${velocity_control}"
ros2_control_plugin="${ros2_control_plugin}" ros2_control_plugin="${ros2_control_plugin}"
@@ -137,4 +137,4 @@
</xacro:if> </xacro:if>
</xacro:macro> </xacro:macro>
</robot> </robot>

11
src/custom_xarm_moveit_config/launch/_robot_moveit_gazebo_fake.launch.py
View File

@@ -13,6 +13,7 @@ from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node from launch_ros.actions import Node
from launch.actions import TimerAction
def launch_setup(context, *args, **kwargs): def launch_setup(context, *args, **kwargs):
prefix = LaunchConfiguration('prefix', default='') prefix = LaunchConfiguration('prefix', default='')
@@ -184,8 +185,8 @@ def launch_setup(context, *args, **kwargs):
# robot gazebo launch # robot gazebo launch
# xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py # xarm_gazebo/launch/_robot_beside_table_gazebo.launch.py
robot_gazebo_launch = IncludeLaunchDescription( robot_gazebo_launch = IncludeLaunchDescription(
#PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])), PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'launch', '_robot_beside_table_gazebo.launch.py'])),
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])), #PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('draw_svg'), 'launch', 'robots', 'lite6_table.launch.py'])),
launch_arguments={ launch_arguments={
'prefix': prefix, 'prefix': prefix,
'hw_ns': hw_ns, 'hw_ns': hw_ns,
@@ -198,6 +199,7 @@ def launch_setup(context, *args, **kwargs):
'robot_type': robot_type, 'robot_type': robot_type,
'ros2_control_plugin': ros2_control_plugin, 'ros2_control_plugin': ros2_control_plugin,
'load_controller': 'true', 'load_controller': 'true',
#'load_controller': 'false',
}.items(), }.items(),
) )
@@ -206,7 +208,10 @@ def launch_setup(context, *args, **kwargs):
robot_moveit_common_launch, robot_moveit_common_launch,
joint_state_publisher_node, joint_state_publisher_node,
ros2_control_launch, ros2_control_launch,
robot_gazebo_launch, # Wait before launching gazebo
TimerAction(period=5.0, actions=[
robot_gazebo_launch,
]),
] + load_controllers ] + load_controllers

4
src/custom_xarm_moveit_config/srdf/xarm.srdf.xacro
View File

@@ -8,8 +8,8 @@
<xacro:arg name="add_vacuum_gripper" default="false" /> <xacro:arg name="add_vacuum_gripper" default="false" />
<xacro:arg name="add_other_geometry" default="false" /> <xacro:arg name="add_other_geometry" default="false" />
<xacro:include filename="$(find xarm_moveit_config)/srdf/xarm_macro.srdf.xacro" /> <xacro:include filename="$(find custom_xarm_moveit_config)/srdf/xarm_macro.srdf.xacro" />
<xacro:xarm_macro_srdf prefix="$(arg prefix)" dof="$(arg dof)" robot_type="$(arg robot_type)" <xacro:xarm_macro_srdf prefix="$(arg prefix)" dof="$(arg dof)" robot_type="$(arg robot_type)"
add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" add_other_geometry="$(arg add_other_geometry)" /> add_gripper="$(arg add_gripper)" add_vacuum_gripper="$(arg add_vacuum_gripper)" add_other_geometry="$(arg add_other_geometry)" />
</robot> </robot>

4
src/custom_xarm_moveit_config/srdf/xarm_macro.srdf.xacro
View File

@@ -9,7 +9,7 @@
</xacro:if> </xacro:if>
<xacro:if value="${dof == 6}"> <xacro:if value="${dof == 6}">
<xacro:if value="${robot_type == 'lite'}"> <xacro:if value="${robot_type == 'lite'}">
<xacro:include filename="$(find xarm_moveit_config)/srdf/_lite6_macro.srdf.xacro" /> <xacro:include filename="$(find custom_xarm_moveit_config)/srdf/_lite6_macro.srdf.xacro" />
<xacro:lite6_macro_srdf prefix="${prefix}" <xacro:lite6_macro_srdf prefix="${prefix}"
add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}" /> add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}" />
</xacro:if> </xacro:if>
@@ -25,4 +25,4 @@
add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}" /> add_gripper="${add_gripper}" add_vacuum_gripper="${add_vacuum_gripper}" add_other_geometry="${add_other_geometry}" />
</xacro:unless> </xacro:unless>
</xacro:macro> </xacro:macro>
</robot> </robot>

7
src/draw_svg/launch/robots/lite6_table.launch.py
View File

@@ -54,12 +54,12 @@ def launch_setup(context, *args, **kwargs):
# robot_description # robot_description
# xarm_description/launch/lib/robot_description_lib.py # 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('custom_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')) #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') get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
robot_description = { robot_description = {
'robot_description': get_xacro_file_content( 'robot_description': get_xacro_file_content(
xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
#xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']), #xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']),
arguments={ arguments={
'prefix': prefix, 'prefix': prefix,
@@ -105,6 +105,7 @@ def launch_setup(context, *args, **kwargs):
# gazebo_ros/launch/gazebo.launch.py # gazebo_ros/launch/gazebo.launch.py
#xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world']) #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('xarm_gazebo'), 'worlds', 'table.world'])
xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf']) xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('draw_svg'), 'worlds', 'draw_svg_world.sdf'])
#xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'table.world'])
gazebo_launch = IncludeLaunchDescription( gazebo_launch = IncludeLaunchDescription(
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])), PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py'])),
launch_arguments={ launch_arguments={
@@ -143,6 +144,8 @@ def launch_setup(context, *args, **kwargs):
load_controllers = [] load_controllers = []
if load_controller.perform(context) in ('True', 'true'): if load_controller.perform(context) in ('True', 'true'):
for controller in controllers: for controller in controllers:
#print("Attempting to load: ", controller)
#input()
load_controllers.append(Node( load_controllers.append(Node(
package='controller_manager', package='controller_manager',
#executable='spawner.py', #executable='spawner.py',

448
src/lite6_controller/launch/lite6_gazebo.launch.py
View File

@@ -1,47 +1,63 @@
import os #!/usr/bin/env -S ros2 launch
from launch import LaunchDescription """Launch Python example for following a target"""
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
from launch_ros.actions import Node
import os
from ament_index_python import get_package_share_directory from ament_index_python import get_package_share_directory
from launch.launch_description_sources import load_python_launch_file_as_module from launch.launch_description_sources import load_python_launch_file_as_module
from launch import LaunchDescription from launch import LaunchDescription
from launch.actions import RegisterEventHandler, EmitEvent from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument, RegisterEventHandler
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, Command, FindExecutable
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.event_handlers import OnProcessExit from launch.event_handlers import OnProcessExit
from launch.events import Shutdown from launch.actions import OpaqueFunction
def launch_setup(context, *args, **kwargs): def launch_setup(context, *args, **kwargs):
use_sim_time = LaunchConfiguration("use_sim_time", default=True)
log_level = LaunchConfiguration("log_level", default='info')
rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
prefix = LaunchConfiguration('prefix', default='') prefix = LaunchConfiguration('prefix', default='')
hw_ns = LaunchConfiguration('hw_ns', default='ufactory') hw_ns = LaunchConfiguration('hw_ns', default='xarm')
limited = LaunchConfiguration('limited', default=True) limited = LaunchConfiguration('limited', default=False)
effort_control = LaunchConfiguration('effort_control', default=False) effort_control = LaunchConfiguration('effort_control', default=False)
velocity_control = LaunchConfiguration('velocity_control', default=False) velocity_control = LaunchConfiguration('velocity_control', default=False)
add_gripper = LaunchConfiguration('add_gripper', default=False) add_gripper = LaunchConfiguration('add_gripper', default=False)
add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False) add_vacuum_gripper = LaunchConfiguration('add_vacuum_gripper', default=False)
dof = LaunchConfiguration('dof', default=6)
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False) 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) add_other_geometry = LaunchConfiguration('add_other_geometry', default=False)
geometry_type = LaunchConfiguration('geometry_type', default='box') #geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.1) geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.05)
geometry_height = LaunchConfiguration('geometry_height', default=0.1) geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.1) geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
geometry_length = LaunchConfiguration('geometry_length', default=0.1) geometry_length = LaunchConfiguration('geometry_length', default=0.07)
geometry_width = LaunchConfiguration('geometry_width', default=0.1) geometry_width = LaunchConfiguration('geometry_width', default=0.1)
geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='') geometry_mesh_filename = LaunchConfiguration('geometry_mesh_filename', default='')
geometry_mesh_origin_xyz = LaunchConfiguration('geometry_mesh_origin_xyz', default='"0 0 0"') 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_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_xyz = LaunchConfiguration('geometry_mesh_tcp_xyz', default='"0 0 0"')
geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"') geometry_mesh_tcp_rpy = LaunchConfiguration('geometry_mesh_tcp_rpy', default='"0 0 0"')
load_controller = LaunchConfiguration('load_controller', default=True)
# robot moveit fake launch ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
# xarm_moveit_config/launch/_robot_moveit_fake.launch.py
robot_moveit_gazebo_fake_launch = IncludeLaunchDescription( no_gui_ctrl = LaunchConfiguration('no_gui_ctrl', default=False)
PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'launch', '_robot_moveit_gazebo_fake.launch.py'])),
ros2_control_plugin = LaunchConfiguration('ros2_control_plugin', default='gazebo_ros2_control/GazeboSystem')
controllers_name = LaunchConfiguration('controllers_name', default='fake_controllers')
moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_simple_controller_manager')
moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='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('custom_xarm_moveit_config'), 'launch', '_robot_moveit_common.launch.py'])),
launch_arguments={ launch_arguments={
'prefix': prefix, 'prefix': prefix,
'hw_ns': hw_ns, 'hw_ns': hw_ns,
@@ -49,11 +65,15 @@ def launch_setup(context, *args, **kwargs):
'effort_control': effort_control, 'effort_control': effort_control,
'velocity_control': velocity_control, 'velocity_control': velocity_control,
'add_gripper': add_gripper, 'add_gripper': add_gripper,
# 'add_gripper': add_gripper if robot_type.perform(context) == 'xarm' else 'false',
'add_vacuum_gripper': add_vacuum_gripper, 'add_vacuum_gripper': add_vacuum_gripper,
'dof': '6', 'dof': dof,
'robot_type': 'lite', 'robot_type': robot_type,
'no_gui_ctrl': 'false', 'no_gui_ctrl': 'false',
'add_realsense_d435i': add_realsense_d435i, '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, 'add_other_geometry': add_other_geometry,
'geometry_type': geometry_type, 'geometry_type': geometry_type,
'geometry_mass': geometry_mass, 'geometry_mass': geometry_mass,
@@ -66,12 +86,167 @@ def launch_setup(context, *args, **kwargs):
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy, 'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz, 'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy, 'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
'use_sim_time': 'true'
}.items(), }.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('custom_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(),
)
# URDF
_robot_description_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']
),
#PathJoinSubstitution(
# [
# FindPackageShare('xarm_description'),
# "urdf",
# "lite6",
# #"lite6.urdf.xacro",
# "lite6_robot_macro.xacro",
# ]
#),
" ",
#"name:=", "lite6", " ",
"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, " ",
#"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_ip:=", robot_ip, " ",
#"report_type:=", report_type, " ",
#"baud_checkset:=", baud_checkset, " ",
#"default_gripper_baud:=", default_gripper_baud, " ",
]
)
# TODO fix URDF loading
# xacro urdf/xarm_pen.urdf.xacro prefix:= hw_ns:=xarm dof:=6 limited:=false effort_control:=false velocity_control:=false add_gripper:=false add_vacuum_gripper:=false robot_type:=lite ros2_control_plugin:=gazebo_ros2_control/GazeboSystem add_other_geometry:=false geometry_type:=cylinder geometry_mass:=0.05 geometry_height:=0.1 geometry_radius:=0.005 geometry_length:=0.07 geometry_width:=0.1 geometry_mesh_filename:= geometry_mesh_origin_xyz:="0 0 0" geometry_mesh_origin_rpy:="0 0 0" geometry_mesh_tcp_xyz:="0 0 0" geometry_mesh_tcp_rpy:="0 0 0"
_robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
robot_description = {"robot_description": _robot_description_xml}
# SRDF
_robot_description_semantic_xml = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare('custom_xarm_moveit_config'),
"srdf",
#"_lite6_macro.srdf.xacro",
"xarm.srdf.xacro",
]
),
" ",
#"name:=", "lite6", " ",
"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, " ",
#"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_ip:=", robot_ip, " ",
#"report_type:=", report_type, " ",
#"baud_checkset:=", baud_checkset, " ",
#"default_gripper_baud:=", default_gripper_baud, " ",
]
)
robot_description_semantic = {
"robot_description_semantic": _robot_description_semantic_xml
}
nodes = [
Node(
package="lite6_controller",
executable="lite6_controller",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[
robot_description,
robot_description_semantic,
{"use_sim_time": use_sim_time},
],
),
Node(
package="draw_svg",
executable="drawing_surface.py",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
]
# ######################3
#moveit_controller_manager_key = LaunchConfiguration('moveit_controller_manager_key', default='moveit_fake_controller_manager')
#moveit_controller_manager_value = LaunchConfiguration('moveit_controller_manager_value', default='moveit_fake_controller_manager/MoveItFakeControllerManager')
add_realsense_d435i = LaunchConfiguration('add_realsense_d435i', default=False)
moveit_config_package_name = 'custom_xarm_moveit_config' moveit_config_package_name = 'custom_xarm_moveit_config'
xarm_type = 'lite6' xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context))
ros2_control_plugin = 'uf_robot_hardware/UFRobotFakeSystemHardware'
# robot_description_parameters # robot_description_parameters
# xarm_moveit_config/launch/lib/robot_moveit_config_lib.py # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py
@@ -88,8 +263,8 @@ def launch_setup(context, *args, **kwargs):
'velocity_control': velocity_control, 'velocity_control': velocity_control,
'add_gripper': add_gripper, 'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper, 'add_vacuum_gripper': add_vacuum_gripper,
'dof': '6', 'dof': dof,
'robot_type': 'lite', 'robot_type': robot_type,
'ros2_control_plugin': ros2_control_plugin, 'ros2_control_plugin': ros2_control_plugin,
'add_realsense_d435i': add_realsense_d435i, 'add_realsense_d435i': add_realsense_d435i,
'add_other_geometry': add_other_geometry, 'add_other_geometry': add_other_geometry,
@@ -107,8 +282,8 @@ def launch_setup(context, *args, **kwargs):
}, },
srdf_arguments={ srdf_arguments={
'prefix': prefix, 'prefix': prefix,
'dof': '6', 'dof': dof,
'robot_type': 'lite', 'robot_type': robot_type,
'add_gripper': add_gripper, 'add_gripper': add_gripper,
'add_vacuum_gripper': add_vacuum_gripper, 'add_vacuum_gripper': add_vacuum_gripper,
'add_other_geometry': add_other_geometry, 'add_other_geometry': add_other_geometry,
@@ -119,32 +294,209 @@ def launch_setup(context, *args, **kwargs):
} }
) )
lite6_controller = Node( load_yaml = getattr(mod, 'load_yaml')
package="lite6_controller", controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context)))
executable="lite6_controller", ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml')
output="log", kinematics_yaml = robot_description_parameters['robot_description_kinematics']
#arguments=["--ros-args", "--log-level", log_level], joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None)
# FIX acceleration limits
for i in range(1,7):
joint_limits_yaml['joint_limits']['joint{}'.format(i)]['has_acceleration_limits'] = True
joint_limits_yaml['joint_limits']['joint{}'.format(i)]['max_acceleration'] = 1.0
kinematics_yaml['kinematics_solver'] = 'kdl_kinematics_plugin/KDLKinematicsPlugin'
#kinematics_yaml['kinematics_solver'] = 'lma_kinematics_plugin/LMAKinematicsPlugin'
#kinematics_yaml['kinematics_solver_search_resolution'] = 0.005
kinematics_yaml['kinematics_solver_timeout'] = 10.0
kinematics_yaml['kinematics_solver_attempts'] = 10
#print(joint_limits_yaml)
#quit()
#if add_gripper.perform(context) in ('True', 'true'):
# gripper_controllers_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), '{}.yaml'.format(controllers_name.perform(context)))
# gripper_ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'ompl_planning.yaml')
# gripper_joint_limits_yaml = load_yaml(moveit_config_package_name, 'config', '{}_gripper'.format(robot_type.perform(context)), 'joint_limits.yaml')
# if gripper_controllers_yaml and 'controller_names' in gripper_controllers_yaml:
# for name in gripper_controllers_yaml['controller_names']:
# if name in gripper_controllers_yaml:
# if name not in controllers_yaml['controller_names']:
# controllers_yaml['controller_names'].append(name)
# controllers_yaml[name] = gripper_controllers_yaml[name]
# if gripper_ompl_planning_yaml:
# ompl_planning_yaml.update(gripper_ompl_planning_yaml)
# if joint_limits_yaml and gripper_joint_limits_yaml:
# joint_limits_yaml['joint_limits'].update(gripper_joint_limits_yaml['joint_limits'])
add_prefix_to_moveit_params = getattr(mod, 'add_prefix_to_moveit_params')
add_prefix_to_moveit_params(
controllers_yaml=controllers_yaml, ompl_planning_yaml=ompl_planning_yaml,
kinematics_yaml=kinematics_yaml, joint_limits_yaml=joint_limits_yaml,
prefix=prefix.perform(context))
robot_description_parameters['cartesian_limits'] = {}
robot_description_parameters['cartesian_limits']['max_trans_vel'] = 1
robot_description_parameters['cartesian_limits']['max_trans_acc'] = 2.25
robot_description_parameters['cartesian_limits']['max_trans_dec'] = -5
robot_description_parameters['cartesian_limits']['max_rot_vel'] = 1.57
# Planning pipeline
# https://github.com/AndrejOrsula/panda_moveit2_config/blob/master/launch/move_group_fake_control.launch.py
#planning_pipeline = {
# "planning_pipelines": ["ompl", "pilz_industrial_motion_planner"],
# "default_planning_pipeline": "pilz_industrial_motion_planner",
# "ompl": {
# "planning_plugin": "ompl_interface/OMPLPlanner",
# # TODO: Re-enable `default_planner_request_adapters/AddRuckigTrajectorySmoothing` once its issues are resolved
# "request_adapters": "default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints",
# # TODO: Reduce start_state_max_bounds_error once spawning with specific joint configuration is enabled
# "start_state_max_bounds_error": 0.31416,
# },
# "pilz_industrial_motion_planner": {
# "planning_plugin": "pilz_industrial_motion_planner::CommandPlanner",
# "default_planner_config": "PTP",
# "capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService",
# },
#}
# Kinematics
#kinematics = load_yaml('panda_moveit2_config', 'config/kinematics.yaml')
# Planning Configuration
ompl_planning_pipeline_config = {
'move_group': {
'planning_plugin': 'ompl_interface/OMPLPlanner',
'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
'start_state_max_bounds_error': 0.1,
}
}
ompl_planning_pipeline_config['move_group'].update(ompl_planning_yaml)
pilz_planning_pipeline_config = {
'move_group': {
'planning_plugin': 'pilz_industrial_motion_planner/CommandPlanner',
# Disable AddTimeOptimalParameterization to fix motion blending https://github.com/ros-planning/moveit/issues/2905
'request_adapters': """default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
#'request_adapters': """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""",
"default_planner_config": "PTP",
}
}
move_group_capabilities = {
"capabilities": "pilz_industrial_motion_planner/MoveGroupSequenceAction pilz_industrial_motion_planner/MoveGroupSequenceService"
}
# Moveit controllers Configuration
moveit_controllers = {
moveit_controller_manager_key.perform(context): controllers_yaml,
'moveit_controller_manager': moveit_controller_manager_value.perform(context),
}
# Trajectory Execution Configuration
trajectory_execution = {
'moveit_manage_controllers': True,
'trajectory_execution.allowed_execution_duration_scaling': 1.2,
'trajectory_execution.allowed_goal_duration_margin': 0.5,
'trajectory_execution.allowed_start_tolerance': 0.01,
'trajectory_execution.execution_duration_monitoring': False
}
plan_execution = {
'plan_execution.record_trajectory_state_frequency': 10.0,
}
planning_scene_monitor_parameters = {
'publish_planning_scene': True,
'publish_geometry_updates': True,
'publish_state_updates': True,
'publish_transforms_updates': True,
# "planning_scene_monitor_options": {
# "name": "planning_scene_monitor",
# "robot_description": "robot_description",
# "joint_state_topic": "/joint_states",
# "attached_collision_object_topic": "/move_group/planning_scene_monitor",
# "publish_planning_scene_topic": "/move_group/publish_planning_scene",
# "monitored_planning_scene_topic": "/move_group/monitored_planning_scene",
# "wait_for_initial_state_timeout": 10.0,
# },
}
# Start the actual move_group node/action server
move_group_node = Node(
package='moveit_ros_move_group',
executable='move_group',
output='screen',
arguments=['--log-level', 'debug'],
parameters=[ parameters=[
robot_description_parameters, robot_description_parameters,
#robot_description, #ompl_planning_pipeline_config,
#robot_description_semantic, pilz_planning_pipeline_config,
#{"use_sim_time": use_sim_time}, move_group_capabilities,
trajectory_execution,
plan_execution,
moveit_controllers,
planning_scene_monitor_parameters,
{'use_sim_time': use_sim_time},
], ],
) )
virtual_drawing_surface = Node( # rviz with moveit configuration
package="virtual_drawing_surface", # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
executable="drawing_surface.py", rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'rviz', 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz'])
output="log", rviz2_node = Node(
#arguments=["--ros-args", "--log-level", log_level], package='rviz2',
#parameters=[{"use_sim_time": use_sim_time}], executable='rviz2',
name='rviz2',
output='screen',
arguments=['-d', rviz_config_file],
parameters=[
robot_description_parameters,
#ompl_planning_pipeline_config,
pilz_planning_pipeline_config,
{'use_sim_time': use_sim_time},
],
remappings=[
('/tf', 'tf'),
('/tf_static', 'tf_static'),
]
)
# Static TF
static_tf = Node(
package='tf2_ros',
executable='static_transform_publisher',
name='static_transform_publisher',
output='screen',
arguments=['0.0', '0.0', '0.0', '0.0', '0.0', '0.0', 'world', 'link_base'],
parameters=[{'use_sim_time': use_sim_time}],
) )
return [ return [
robot_moveit_gazebo_fake_launch, #RegisterEventHandler(event_handler=OnProcessExit(
virtual_drawing_surface, # target_action=rviz2_node,
lite6_controller, # on_exit=[EmitEvent(event=Shutdown())]
] #)),
#rviz2_node,
static_tf,
move_group_node,
robot_gazebo_launch,
] + nodes
# ######################3
#return [
# robot_moveit_common_launch,
# robot_gazebo_launch,
#] + nodes
def generate_launch_description(): def generate_launch_description():