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

Add pen to xarm lite6

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-11-08 16:55:10 +02:00
parent 8357da8151
commit 880c947cb9
5 changed files with 399 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
src/draw_svg/CMakeLists.txt
View File

@@ -52,6 +52,6 @@ if(BUILD_TESTING)
endif()
# Install directories
install(DIRECTORY launch rviz worlds DESTINATION share/${PROJECT_NAME})
install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME})
ament_package()

141
src/draw_svg/launch/lite6_pen.launch.py Normal file
View File

@@ -0,0 +1,141 @@
#!/usr/bin/env -S ros2 launch
"""Launch Python example for following a target"""
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):
use_sim_time = LaunchConfiguration("use_sim_time", default=True)
log_level = LaunchConfiguration("log_level", default='warn')
rviz_config = LaunchConfiguration('rviz_config', default=os.path.join(get_package_share_directory("draw_svg"), "rviz", "ign_moveit2_examples.rviz"))
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=6)
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)
#geometry_type = LaunchConfiguration('geometry_type', default='box')
geometry_type = LaunchConfiguration('geometry_type', default='cylinder')
geometry_mass = LaunchConfiguration('geometry_mass', default=0.05)
geometry_height = LaunchConfiguration('geometry_height', default=0.1)
geometry_radius = LaunchConfiguration('geometry_radius', default=0.005)
geometry_length = LaunchConfiguration('geometry_length', default=0.07)
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=True)
ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context)
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': 'false',
'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'])),
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(),
)
nodes = [
Node(
package="draw_svg",
executable="follow.py",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
Node(
package="draw_svg",
executable="draw_svg.py",
output="log",
arguments=["--ros-args", "--log-level", log_level],
parameters=[{"use_sim_time": use_sim_time}],
),
]
return [
nodes[0],
nodes[1],
robot_moveit_common_launch,
robot_gazebo_launch,
]
def generate_launch_description():
return LaunchDescription([
OpaqueFunction(function=launch_setup)
])

173
src/draw_svg/launch/robots/lite6_table.launch.py Normal file
View File

@@ -0,0 +1,173 @@
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'))
#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')
robot_description = {
'robot_description': get_xacro_file_content(
#xacro_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.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', 'draw_svg_world.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',
#'-x', '0.0',
'-y', '-0.5',
#'-y', '0.0',
'-z', '1.021',
#'-z', '0.0',
'-Y', '1.571',
#'-Y', '0.0',
],
)
# 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',
executable='spawner',
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)
])

81
src/draw_svg/urdf/xarm_pen.urdf.xacro Normal file
View File

@@ -0,0 +1,81 @@
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm$(arg dof)">
<!-- <robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6"> -->
<xacro:arg name="prefix" default=""/>
<xacro:arg name="hw_ns" default="xarm"/>
<xacro:arg name="limited" default="false"/>
<xacro:arg name="effort_control" default="false"/>
<xacro:arg name="velocity_control" default="false"/>
<xacro:arg name="add_gripper" default="false"/>
<xacro:arg name="add_vacuum_gripper" default="false"/>
<xacro:arg name="dof" default="6"/>
<xacro:arg name="robot_type" default="lite"/>
<xacro:arg name="ros2_control_plugin" default="uf_robot_hardware/UFRobotSystemHardware"/>
<xacro:arg name="ros2_control_params" default="$(find xarm_controller)/config/xarm$(arg dof)_controllers.yaml"/>
<xacro:arg name="add_other_geometry" default="false"/>
<xacro:arg name="geometry_type" default="box"/>
<xacro:arg name="geometry_mass" default="0.1"/>
<xacro:arg name="geometry_height" default="0.1"/>
<xacro:arg name="geometry_radius" default="0.1"/>
<xacro:arg name="geometry_length" default="0.1"/>
<xacro:arg name="geometry_width" default="0.1"/>
<xacro:arg name="geometry_mesh_filename" default=""/>
<xacro:arg name="geometry_mesh_origin_xyz" default="0 0 0"/>
<xacro:arg name="geometry_mesh_origin_rpy" default="0 0 0"/>
<xacro:arg name="geometry_mesh_tcp_xyz" default="0 0 0"/>
<xacro:arg name="geometry_mesh_tcp_rpy" default="0 0 0"/>
<xacro:arg name="robot_ip" default=""/>
<xacro:arg name="report_type" default="normal"/>
<xacro:arg name="baud_checkset" default="true"/>
<xacro:arg name="default_gripper_baud" default="2000000"/>
<!-- gazebo_plugin -->
<xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
<xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
<!-- load xarm device -->
<!-- Load Lite6 Robot Model URDF -->
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
<xacro:lite6_robot prefix="$(arg prefix)" namespace="xarm" limited="$(arg limited)"
effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
ros2_control_plugin="$(arg ros2_control_plugin)"
attach_to="world" xyz="0 0 0" rpy="0 0 0"
ros2_control_params="$(arg ros2_control_params)"
load_gazebo_plugin="false"
add_gripper="$(arg add_gripper)"
robot_ip="$(arg robot_ip)"
report_type="$(arg report_type)"
baud_checkset="$(arg baud_checkset)"
default_gripper_baud="$(arg default_gripper_baud)" />
<joint name="eef_joint" type="fixed">
<parent link="link_eef"/>
<child link="test_link"/>
</joint>
<link name="test_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>
</robot>
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
<xacro:lite6_urdf prefix=''/> -->
<!--
</robot> -->

4
src/draw_svg/worlds/follow_target.sdf → src/draw_svg/worlds/draw_svg_world.sdf
View File

@@ -1,5 +1,6 @@
<?xml version="1.0"?>
<sdf version="1.9">
<!-- <sdf version="1.9"> -->
<sdf version="1.7">
<world name="draw_svg_world">
<!-- Physics -->
@@ -104,6 +105,7 @@
</model>
<!-- <include>
<uri>model://ground_plane</uri>
<pose>0.0 -0.84 0 0 0 0</pose>
</include> -->
<!-- <include>
<uri>model://sun</uri>