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

Switch default robot to xarm lite6

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-10-25 13:01:34 +03:00
parent 1e382d01a0
commit 2a3b4fcbe6
9 changed files with 236 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/draw_svg/CMakeLists.txt
View File

@@ -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)

6
src/draw_svg/launch/default.launch.py
View File

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

6
src/draw_svg/launch/draw_svg.launch.py
View File

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

61
src/draw_svg/launch/robots/robot_lite6.launch.py Normal file
View File

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

2
src/draw_svg/launch/robots/robot_panda.launch.py
View File

@@ -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

124
src/draw_svg/launch/xarm_draw_svg.launch.py Normal file
View File

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

2
src/draw_svg/package.xml
View File

@@ -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>

10
src/draw_svg/src/py/follow.py
View File

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

35
src/draw_svg/src/py/robots/lite6.py Normal file
View File

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