diff --git a/src/draw_svg/CMakeLists.txt b/src/draw_svg/CMakeLists.txt deleted file mode 100644 index a73e8ce..0000000 --- a/src/draw_svg/CMakeLists.txt +++ /dev/null @@ -1,59 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(draw_svg) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -#find_package(ign_moveit2_examples REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) -#find_package(moveit_visual_tools REQUIRED) - -# Install C++ -set(SRC_CPP_DIR src/cpp) -# Example 0 - Follow target -set(EXECUTABLE_0 follow) -add_executable(${EXECUTABLE_0} ${SRC_CPP_DIR}/${EXECUTABLE_0}.cpp) -ament_target_dependencies(${EXECUTABLE_0} - rclcpp - geometry_msgs - moveit_ros_planning_interface -) -install(TARGETS - ${EXECUTABLE_0} - DESTINATION lib/${PROJECT_NAME} -) - -# Install Python -set(SRC_PY_DIR src/py) -install(PROGRAMS - ${SRC_PY_DIR}/draw_svg.py - ${SRC_PY_DIR}/follow.py - ${SRC_PY_DIR}/drawing_surface.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) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -# Install directories -install(DIRECTORY launch rviz urdf worlds DESTINATION share/${PROJECT_NAME}) - -ament_package() diff --git a/src/draw_svg/README.md b/src/draw_svg/README.md deleted file mode 100644 index 0a2d8db..0000000 --- a/src/draw_svg/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# draw_svg -WILL BE REMOVED - -Contains initial testing of libraries and launch files. diff --git a/src/draw_svg/launch/default.launch.py b/src/draw_svg/launch/default.launch.py deleted file mode 100755 index 6f79edf..0000000 --- a/src/draw_svg/launch/default.launch.py +++ /dev/null @@ -1,146 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch default world with the default robot (configurable)""" - -from os import path -from typing import List - -from ament_index_python.packages import get_package_share_directory -from launch_ros.substitutions import FindPackageShare - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - - -def generate_launch_description() -> LaunchDescription: - - # Declare all launch arguments - declared_arguments = generate_declared_arguments() - - # Get substitution for all arguments - world_type = LaunchConfiguration("world_type") - robot_type = LaunchConfiguration("robot_type") - rviz_config = LaunchConfiguration("rviz_config") - use_sim_time = LaunchConfiguration("use_sim_time") - ign_verbosity = LaunchConfiguration("ign_verbosity") - log_level = LaunchConfiguration("log_level") - - # Determine what world/robot combination to launch - declared_arguments.append( - DeclareLaunchArgument( - "__world_launch_basename", - default_value=["world_", world_type, ".launch.py"], - ), - ) - declared_arguments.append( - DeclareLaunchArgument( - "__robot_launch_basename", - default_value=["robot_", robot_type, ".launch.py"], - ), - ) - - # List of included launch descriptions - launch_descriptions = [ - # Launch Ignition Gazebo with the required ROS<->IGN bridges - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("draw_svg"), - "launch", - "worlds", - LaunchConfiguration("__world_launch_basename"), - ] - ) - ), - launch_arguments=[ - ("use_sim_time", use_sim_time), - ("ign_verbosity", ign_verbosity), - ("log_level", log_level), - ], - ), - # Spawn robot - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("draw_svg"), - "launch", - "robots", - LaunchConfiguration("__robot_launch_basename"), - ] - ) - ), - launch_arguments=[ - ("use_sim_time", use_sim_time), - ("ign_verbosity", ign_verbosity), - ("log_level", log_level), - ], - ), - # Launch move_group of MoveIt 2 - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [FindPackageShare("xarm_moveit_config"), "launch", "lite6_moveit_fake.launch.py"] - ) - ), - launch_arguments=[ - ("ros2_control_plugin", "ign"), - ("ros2_control_command_interface", "effort"), - # TODO: Re-enable colligion geometry for manipulator arm once spawning with specific joint configuration is enabled - ("collision_arm", "false"), - ("rviz_config", rviz_config), - ("use_sim_time", use_sim_time), - ("log_level", log_level), - ], - ), - ] - - return LaunchDescription(declared_arguments + launch_descriptions) - - -def generate_declared_arguments() -> List[DeclareLaunchArgument]: - """ - Generate list of all launch arguments that are declared for this launch script. - """ - - return [ - # World selection - DeclareLaunchArgument( - "world_type", - default_value="default", - description="Name of the world configuration to load.", - ), - # Robot selection - DeclareLaunchArgument( - "robot_type", - default_value="panda", - description="Name of the robot type to use.", - ), - # Miscellaneous - DeclareLaunchArgument( - "rviz_config", - default_value=path.join( - get_package_share_directory("draw_svg"), - "rviz", - "ign_moveit2_examples.rviz", - ), - description="Path to configuration for RViz2.", - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="true", - description="If true, use simulated clock.", - ), - DeclareLaunchArgument( - "ign_verbosity", - default_value="2", - description="Verbosity level for Ignition Gazebo (0~4).", - ), - DeclareLaunchArgument( - "log_level", - default_value="warn", - description="The level of logging that is applied to all ROS 2 nodes launched by this script.", - ), - ] diff --git a/src/draw_svg/launch/draw_svg.launch.py b/src/draw_svg/launch/draw_svg.launch.py deleted file mode 100755 index c5efda7..0000000 --- a/src/draw_svg/launch/draw_svg.launch.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch Python example for following a target""" - -from os import path -from typing import List - -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - - -def generate_launch_description() -> LaunchDescription: - - # Declare all launch arguments - declared_arguments = generate_declared_arguments() - - # Get substitution for all arguments - robot_type = LaunchConfiguration("robot_type") - rviz_config = LaunchConfiguration("rviz_config") - use_sim_time = LaunchConfiguration("use_sim_time") - ign_verbosity = LaunchConfiguration("ign_verbosity") - log_level = LaunchConfiguration("log_level") - - # List of included launch descriptions - launch_descriptions = [ - # Launch world with robot (configured for this example) - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("draw_svg"), - "launch", - "default.launch.py", - ] - ) - ), - launch_arguments=[ - ("world_type", "draw_svg"), - ("robot_type", robot_type), - ("rviz_config", rviz_config), - ("use_sim_time", use_sim_time), - ("ign_verbosity", ign_verbosity), - ("log_level", log_level), - ], - ), - ] - - # List of nodes to be launched - nodes = [ - # Run the example node (Python) - 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 LaunchDescription(declared_arguments + launch_descriptions + nodes) - - -def generate_declared_arguments() -> List[DeclareLaunchArgument]: - """ - Generate list of all launch arguments that are declared for this launch script. - """ - - return [ - # Robot selection - DeclareLaunchArgument( - "robot_type", - default_value="lite6", - description="Name of the robot to use.", - ), - # Miscellaneous - DeclareLaunchArgument( - "rviz_config", - default_value=path.join( - get_package_share_directory("xarm_description"), - "rviz", - "display.rviz", - ), - description="Path to configuration for RViz2.", - ), - DeclareLaunchArgument( - "use_sim_time", - default_value="true", - description="If true, use simulated clock.", - ), - DeclareLaunchArgument( - "ign_verbosity", - default_value="2", - description="Verbosity level for Ignition Gazebo (0~4).", - ), - DeclareLaunchArgument( - "log_level", - default_value="warn", - description="The level of logging that is applied to all ROS 2 nodes launched by this script.", - ), - ] diff --git a/src/draw_svg/launch/lite6_pen.launch.py b/src/draw_svg/launch/lite6_pen.launch.py deleted file mode 100644 index 5b583d1..0000000 --- a/src/draw_svg/launch/lite6_pen.launch.py +++ /dev/null @@ -1,255 +0,0 @@ -#!/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, Command, FindExecutable -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(), - ) - - # 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('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="draw_svg", - executable="follow", - 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="draw_svg.py", - output="log", - arguments=["--ros-args", "--log-level", log_level], - parameters=[{"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}], - ), - ] - - return [ - robot_moveit_common_launch, - robot_gazebo_launch, - ] + nodes - - -def generate_launch_description(): - return LaunchDescription([ - OpaqueFunction(function=launch_setup) - ]) diff --git a/src/draw_svg/launch/lite6_real.launch.py b/src/draw_svg/launch/lite6_real.launch.py deleted file mode 100644 index b80b288..0000000 --- a/src/draw_svg/launch/lite6_real.launch.py +++ /dev/null @@ -1,361 +0,0 @@ -import os -from launch import LaunchDescription -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 - -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 RegisterEventHandler, EmitEvent -from launch.event_handlers import OnProcessExit -from launch.events import Shutdown - - -def launch_setup(context, *args, **kwargs): - robot_ip = LaunchConfiguration('robot_ip', default='192.168.1.150') - report_type = LaunchConfiguration('report_type', default='normal') - prefix = LaunchConfiguration('prefix', default='') - hw_ns = LaunchConfiguration('hw_ns', default='ufactory') - 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"') - - baud_checkset = LaunchConfiguration('baud_checkset', default=True) - default_gripper_baud = LaunchConfiguration('default_gripper_baud', default=2000000) - - ros2_control_plugin = 'uf_robot_hardware/UFRobotSystemHardware' - controllers_name = LaunchConfiguration('controllers_name', default='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') - - xarm_type = '{}{}'.format(robot_type.perform(context), dof.perform(context)) - ros_namespace = LaunchConfiguration('ros_namespace', default='').perform(context) - - use_sim_time = LaunchConfiguration('use_sim_time', default=False) - log_level = LaunchConfiguration("log_level", default='warn') - - # # robot driver launch - # # xarm_api/launch/_robot_driver.launch.py - # robot_driver_launch = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_api'), 'launch', '_robot_driver.launch.py'])), - # launch_arguments={ - # 'robot_ip': robot_ip, - # 'report_type': report_type, - # 'dof': dof, - # 'hw_ns': hw_ns, - # 'add_gripper': add_gripper, - # 'prefix': prefix, - # 'baud_checkset': baud_checkset, - # 'default_gripper_baud': default_gripper_baud, - # 'robot_type': robot_type, - # }.items(), - # ) - - # robot description launch - # xarm_description/launch/_robot_description.launch.py - robot_description_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_description'), 'launch', '_robot_description.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, - 'joint_states_remapping': PathJoinSubstitution(['/', ros_namespace, hw_ns, 'joint_states']), - '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, - }.items(), - ) - - # robot_description_parameters - # xarm_moveit_config/launch/lib/robot_moveit_config_lib.py - moveit_config_package_name = 'xarm_moveit_config' - mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory(moveit_config_package_name), 'launch', 'lib', 'robot_moveit_config_lib.py')) - get_xarm_robot_description_parameters = getattr(mod, 'get_xarm_robot_description_parameters') - robot_description_parameters = get_xarm_robot_description_parameters( - xacro_urdf_file=PathJoinSubstitution([FindPackageShare('xarm_description'), 'urdf', 'xarm_device.urdf.xacro']), - xacro_srdf_file=PathJoinSubstitution([FindPackageShare('xarm_moveit_config'), 'srdf', 'xarm.srdf.xacro']), - urdf_arguments={ - 'prefix': prefix, - 'hw_ns': hw_ns.perform(context).strip('/'), - '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, - '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, - }, - srdf_arguments={ - 'prefix': prefix, - 'dof': dof, - 'robot_type': robot_type, - 'add_gripper': add_gripper, - 'add_vacuum_gripper': add_vacuum_gripper, - 'add_other_geometry': add_other_geometry, - }, - arguments={ - 'context': context, - 'xarm_type': xarm_type, - } - ) - - load_yaml = getattr(mod, 'load_yaml') - controllers_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, '{}.yaml'.format(controllers_name.perform(context))) - ompl_planning_yaml = load_yaml(moveit_config_package_name, 'config', xarm_type, 'ompl_planning.yaml') - kinematics_yaml = robot_description_parameters['robot_description_kinematics'] - joint_limits_yaml = robot_description_parameters.get('robot_description_planning', None) - - 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)) - - # 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) - - # 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', - parameters=[ - robot_description_parameters, - ompl_planning_pipeline_config, - trajectory_execution, - plan_execution, - moveit_controllers, - planning_scene_monitor_parameters, - {'use_sim_time': use_sim_time}, - ], - ) - - # rviz with moveit configuration - # rviz_config_file = PathJoinSubstitution([FindPackageShare(moveit_config_package_name), 'config', xarm_type, 'planner.rviz' if no_gui_ctrl.perform(context) == 'true' else 'moveit.rviz']) - 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_parameters, - ompl_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'], - ) - - - # joint state publisher node - joint_state_publisher_node = Node( - package='joint_state_publisher', - executable='joint_state_publisher', - name='joint_state_publisher', - output='screen', - parameters=[{'source_list': ['{}/joint_states'.format(hw_ns.perform(context))]}], - remappings=[ - ('follow_joint_trajectory', '{}{}_traj_controller/follow_joint_trajectory'.format(prefix.perform(context), xarm_type)), - ], - ) - - # ros2 control launch - # xarm_controller/launch/_ros2_control.launch.py - ros2_control_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource(PathJoinSubstitution([FindPackageShare('xarm_controller'), 'launch', '_ros2_control.launch.py'])), - launch_arguments={ - 'robot_ip': robot_ip, - 'report_type': report_type, - 'baud_checkset': baud_checkset, - 'default_gripper_baud': default_gripper_baud, - '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, - }.items(), - ) - - control_node = Node( - package='controller_manager', - executable='spawner', - output='screen', - arguments=[ - '{}{}_traj_controller'.format(prefix.perform(context), xarm_type), - '--controller-manager', '{}/controller_manager'.format(ros_namespace) - ], - ) - - nodes = [ - Node( - package="draw_svg", - executable="follow", - output="log", - arguments=["--ros-args", "--log-level", log_level], - parameters=[ - #robot_description_parameters['robot_description'], - #robot_description_parameters['robot_description_semantic'], - #robot_description_parameters['robot_description_planning'], - #robot_description_parameters['robot_description_kinematics'], - robot_description_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 [ - - RegisterEventHandler(event_handler=OnProcessExit( - target_action=rviz2_node, - on_exit=[EmitEvent(event=Shutdown())] - )), - rviz2_node, - static_tf, - move_group_node, - - - robot_description_launch, - joint_state_publisher_node, - ros2_control_launch, - control_node, - # robot_driver_launch, - ] + nodes - - -def generate_launch_description(): - return LaunchDescription([ - OpaqueFunction(function=launch_setup) - ]) diff --git a/src/draw_svg/launch/log_position.launch.py b/src/draw_svg/launch/log_position.launch.py deleted file mode 100755 index 4e2c233..0000000 --- a/src/draw_svg/launch/log_position.launch.py +++ /dev/null @@ -1,41 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch Python example for following a target""" - -from os import path -from typing import List - -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - -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 generate_launch_description() -> LaunchDescription: - log_position_node = Node( - package="draw_svg", - executable="log_position", - output='log', - arguments=[ - ], - ) - return LaunchDescription([log_position_node,]) - -def generate_declared_arguments(): - return [ - OpaqueFunction(function=launch_setup) - ] diff --git a/src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py b/src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py deleted file mode 100755 index a757fd7..0000000 --- a/src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py +++ /dev/null @@ -1,168 +0,0 @@ -#!/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) - ]) diff --git a/src/draw_svg/launch/robots/lite6_table.launch.py b/src/draw_svg/launch/robots/lite6_table.launch.py deleted file mode 100644 index c6cced0..0000000 --- a/src/draw_svg/launch/robots/lite6_table.launch.py +++ /dev/null @@ -1,176 +0,0 @@ -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('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')) - get_xacro_file_content = getattr(mod, 'get_xacro_file_content') - robot_description = { - 'robot_description': get_xacro_file_content( - xacro_file=PathJoinSubstitution([FindPackageShare('custom_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']) - #xarm_gazebo_world = PathJoinSubstitution([FindPackageShare('custom_xarm_gazebo'), 'worlds', 'table.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(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: - #print("Attempting to load: ", controller) - #input() - 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) - ]) diff --git a/src/draw_svg/launch/robots/robot_lite6.launch.py b/src/draw_svg/launch/robots/robot_lite6.launch.py deleted file mode 100755 index 4e5ff71..0000000 --- a/src/draw_svg/launch/robots/robot_lite6.launch.py +++ /dev/null @@ -1,61 +0,0 @@ -#!/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.", - ), - ] diff --git a/src/draw_svg/launch/robots/robot_panda.launch.py b/src/draw_svg/launch/robots/robot_panda.launch.py deleted file mode 100755 index 01b1f73..0000000 --- a/src/draw_svg/launch/robots/robot_panda.launch.py +++ /dev/null @@ -1,61 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch script for spawning Franka Emika Panda 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.", - ), - ] diff --git a/src/draw_svg/launch/worlds/world_default.launch.py b/src/draw_svg/launch/worlds/world_default.launch.py deleted file mode 100755 index 750e2c3..0000000 --- a/src/draw_svg/launch/worlds/world_default.launch.py +++ /dev/null @@ -1,91 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch default.sdf and the required ROS<->IGN bridges""" - -from typing import List - -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - - -def generate_launch_description() -> LaunchDescription: - - # Declare all launch arguments - declared_arguments = generate_declared_arguments() - - # Get substitution for all arguments - world = LaunchConfiguration("world") - use_sim_time = LaunchConfiguration("use_sim_time") - ign_verbosity = LaunchConfiguration("ign_verbosity") - log_level = LaunchConfiguration("log_level") - - # List of included launch descriptions - launch_descriptions = [ - # Launch Ignition Gazebo - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ros_ign_gazebo"), - "launch", - "ign_gazebo.launch.py", - ] - ) - ), - launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], - ), - ] - - # List of nodes to be launched - nodes = [ - # ros_ign_bridge (clock -> ROS 2) - Node( - package="ros_ign_bridge", - executable="parameter_bridge", - output="log", - arguments=[ - "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", - "--ros-args", - "--log-level", - log_level, - ], - parameters=[{"use_sim_time": use_sim_time}], - ), - ] - - return LaunchDescription(declared_arguments + launch_descriptions + nodes) - - -def generate_declared_arguments() -> List[DeclareLaunchArgument]: - """ - Generate list of all launch arguments that are declared for this launch script. - """ - - return [ - # World for Ignition Gazebo - DeclareLaunchArgument( - "world", - default_value="default.sdf", - description="Name or filepath of world to load.", - ), - # Miscellaneous - DeclareLaunchArgument( - "use_sim_time", - default_value="true", - description="If true, use simulated clock.", - ), - DeclareLaunchArgument( - "ign_verbosity", - default_value="2", - description="Verbosity level for Ignition Gazebo (0~4).", - ), - DeclareLaunchArgument( - "log_level", - default_value="warn", - description="The level of logging that is applied to all ROS 2 nodes launched by this script.", - ), - ] diff --git a/src/draw_svg/launch/worlds/world_draw_svg.launch.py b/src/draw_svg/launch/worlds/world_draw_svg.launch.py deleted file mode 100755 index 2380cc5..0000000 --- a/src/draw_svg/launch/worlds/world_draw_svg.launch.py +++ /dev/null @@ -1,113 +0,0 @@ -#!/usr/bin/env -S ros2 launch -"""Launch worlds/follow_target.sdf and the required ROS<->IGN bridges""" - -from os import path -from typing import List - -from ament_index_python.packages import get_package_share_directory -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution - - -def generate_launch_description() -> LaunchDescription: - - # Declare all launch arguments - declared_arguments = generate_declared_arguments() - - # Get substitution for all arguments - world = LaunchConfiguration("world") - use_sim_time = LaunchConfiguration("use_sim_time") - ign_verbosity = LaunchConfiguration("ign_verbosity") - log_level = LaunchConfiguration("log_level") - - # List of included launch descriptions - launch_descriptions = [ - # Launch Ignition Gazebo - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution( - [ - FindPackageShare("ros_ign_gazebo"), - "launch", - "ign_gazebo.launch.py", - ] - ) - ), - launch_arguments=[("ign_args", [world, " -r -v ", ign_verbosity])], - ), - ] - - # List of nodes to be launched - nodes = [ - # ros_ign_bridge (clock -> ROS 2) - Node( - package="ros_ign_bridge", - executable="parameter_bridge", - output="log", - arguments=[ - "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock", - "--ros-args", - "--log-level", - log_level, - ], - parameters=[{"use_sim_time": use_sim_time}], - ), - # ros_ign_bridge (target pose -> ROS 2) - #Node( - # package="ros_ign_bridge", - # executable="parameter_bridge", - # output="log", - # arguments=[ - # "/model/target/pose" - # + "@" - # + "geometry_msgs/msg/PoseStamped[ignition.msgs.Pose", - # "--ros-args", - # "--log-level", - # log_level, - # ], - # parameters=[{"use_sim_time": use_sim_time}], - # remappings=[("/model/target/pose", "/target_pose")], - #), - ] - - return LaunchDescription(declared_arguments + launch_descriptions + nodes) - - -def generate_declared_arguments() -> List[DeclareLaunchArgument]: - """ - Generate list of all launch arguments that are declared for this launch script. - """ - - return [ - # World for Ignition Gazebo - DeclareLaunchArgument( - "world", - default_value=path.join( - get_package_share_directory("draw_svg"), - "worlds", - "follow_target.sdf", - ), - description="Name or filepath of world to load.", - ), - # Miscellaneous - DeclareLaunchArgument( - "use_sim_time", - default_value="true", - description="If true, use simulated clock.", - ), - DeclareLaunchArgument( - "ign_verbosity", - default_value="2", - description="Verbosity level for Ignition Gazebo (0~4).", - ), - DeclareLaunchArgument( - "log_level", - default_value="warn", - description="The level of logging that is applied to all ROS 2 nodes launched by this script.", - ), - ] diff --git a/src/draw_svg/launch/xarm_draw_svg.launch.py b/src/draw_svg/launch/xarm_draw_svg.launch.py deleted file mode 100755 index f391098..0000000 --- a/src/draw_svg/launch/xarm_draw_svg.launch.py +++ /dev/null @@ -1,135 +0,0 @@ -#!/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('draw_svg'), 'launch', 'robots', '_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}], - ) - - # 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, - ] - - -def generate_launch_description(): - return LaunchDescription([ - OpaqueFunction(function=launch_setup) - ]) diff --git a/src/draw_svg/package.xml b/src/draw_svg/package.xml deleted file mode 100644 index 9669ba3..0000000 --- a/src/draw_svg/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - draw_svg - 0.0.0 - TODO: Package description - root - TODO: License declaration - - ament_cmake - - - rclcpp - geometry_msgs - tf2_ros - python3-lxml - python3-tk - python-imaging - python-numpy - python3-pil.imagetk - xarm_description - xarm_moveit_config - - - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/draw_svg/rebuild.sh b/src/draw_svg/rebuild.sh deleted file mode 100644 index 02bb610..0000000 --- a/src/draw_svg/rebuild.sh +++ /dev/null @@ -1,4 +0,0 @@ -rm -r build/ install/ log/ -rosdep install --from-paths . --ignore-src -r -y -colcon build -#ros2 launch draw_svg draw_svg.launch.py diff --git a/src/draw_svg/rviz/ign_moveit2_examples.rviz b/src/draw_svg/rviz/ign_moveit2_examples.rviz deleted file mode 100644 index 07baebb..0000000 --- a/src/draw_svg/rviz/ign_moveit2_examples.rviz +++ /dev/null @@ -1,337 +0,0 @@ -Panels: - - Class: rviz_common/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /MotionPlanning1 - Splitter Ratio: 0.6278260946273804 - Tree Height: 412 - - Class: rviz_common/Selection - Name: Selection - - Class: rviz_common/Tool Properties - Expanded: - - /2D Goal Pose1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz_common/Views - Expanded: - - /Current View1 - - /Current View1/Focal Point1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_common/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Visualization Manager: - Class: "" - Displays: - - Acceleration_Scaling_Factor: 1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 - Name: MotionPlanning - Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Loop Animation: true - Robot Alpha: 0.25 - Robot Color: 150; 50; 150 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 0.1s - Trail Step Size: 1 - Trajectory Topic: /display_planned_path - Planning Metrics: - Payload: 1 - Show Joint Torques: false - Show Manipulability: false - Show Manipulability Index: false - Show Weight Limit: false - TextHeight: 0.07999999821186066 - Planning Request: - Colliding Link Color: 255; 0; 0 - Goal State Alpha: 0.5 - Goal State Color: 250; 128; 0 - Interactive Marker Size: 0.10000000149011612 - Joint Violation Color: 255; 0; 255 - Planning Group: arm - Query Goal State: true - Query Start State: false - Show Workspace: false - Start State Alpha: 0.5 - Start State Color: 0; 255; 0 - Planning Scene Topic: monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.8999999761581421 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 1 - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Fixed Frame: panda_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz_default_plugins/Interact - Hide Inactive Objects: true - - Class: rviz_default_plugins/MoveCamera - - Class: rviz_default_plugins/Select - - Class: rviz_default_plugins/FocusCamera - - Class: rviz_default_plugins/Measure - Line color: 128; 128; 0 - - Class: rviz_default_plugins/SetInitialPose - Covariance x: 0.25 - Covariance y: 0.25 - Covariance yaw: 0.06853891909122467 - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /initialpose - - Class: rviz_default_plugins/SetGoal - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /goal_pose - - Class: rviz_default_plugins/PublishPoint - Single click: true - Topic: - Depth: 5 - Durability Policy: Volatile - History Policy: Keep Last - Reliability Policy: Reliable - Value: /clicked_point - Transformation: - Current: - Class: rviz_default_plugins/TF - Value: true - Views: - Current: - Class: rviz_default_plugins/Orbit - Distance: 1.5 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: 0.3333333432674408 - Y: 0 - Z: 0.5 - Focal Shape Fixed Size: false - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.39269909262657166 - Target Frame: - Value: Orbit (rviz) - Yaw: 0.7853981852531433 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 720 - Hide Left Dock: false - Hide Right Dock: true - MotionPlanning: - collapsed: false - MotionPlanning - Trajectory Slider: - collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000247000003b0fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000007901000003fb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000046000001eb000000ff01000003fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000005201000003fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000232000001c4000001b9010000030000000100000110000003b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000046000003b0000000d701000003fb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000048fc0100000002fb0000000800540069006d0065000000000000000780000002d701000003fb0000000800540069006d0065010000000000000450000000000000000000000538000003b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: true - Width: 1280 - X: 0 - Y: 0 diff --git a/src/draw_svg/src/cpp/draw_svg.cpp b/src/draw_svg/src/cpp/draw_svg.cpp deleted file mode 100644 index d37dc92..0000000 --- a/src/draw_svg/src/cpp/draw_svg.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -int main(int argc, char ** argv) -{ - (void) argc; - (void) argv; - - printf("hello world draw_svg package\n"); - return 0; -} diff --git a/src/draw_svg/src/cpp/follow.cpp b/src/draw_svg/src/cpp/follow.cpp deleted file mode 100644 index de63e40..0000000 --- a/src/draw_svg/src/cpp/follow.cpp +++ /dev/null @@ -1,141 +0,0 @@ -//#include "follow.h" - -#include -#include -#include -#include -#include - -#include -#include -//#include -#include - -const std::string MOVE_GROUP = "lite6"; - -class MoveItFollowTarget : public rclcpp::Node -{ -public: - /// Constructor - MoveItFollowTarget(); - - /// Move group interface for the robot - moveit::planning_interface::MoveGroupInterface move_group_; - /// Subscriber for target pose - rclcpp::Subscription::SharedPtr target_pose_sub_; - /// Target pose that is used to detect changes - geometry_msgs::msg::Pose previous_target_pose_; - - bool moved = false; - std::vector waypoints; - - -private: - /// Callback that plans and executes trajectory each time the target pose is changed - void target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); -}; - -MoveItFollowTarget::MoveItFollowTarget() : Node("follow_target"), - move_group_(std::shared_ptr(std::move(this)), MOVE_GROUP) -{ - // Use upper joint velocity and acceleration limits - this->move_group_.setMaxAccelerationScalingFactor(1.0); - this->move_group_.setMaxVelocityScalingFactor(1.0); - - // Subscribe to target pose - target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); - - RCLCPP_INFO(this->get_logger(), "Initialization successful."); -} - -void MoveItFollowTarget::target_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - // Return if target pose is unchanged - if (msg->pose == this->previous_target_pose_) - { - return; - } - - RCLCPP_INFO(this->get_logger(), "Target pose has changed. Planning and executing..."); - - //if (this->moved) - if (false) - { - //https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/how_to_guides/using_ompl_constrained_planning/src/ompl_constrained_planning_tutorial.cpp - // - moveit_msgs::msg::PositionConstraint plane_constraint; - plane_constraint.header.frame_id = this->move_group_.getPoseReferenceFrame(); - plane_constraint.link_name = this->move_group_.getEndEffectorLink(); - shape_msgs::msg::SolidPrimitive plane; - plane.type = shape_msgs::msg::SolidPrimitive::BOX; - //plane.dimensions = { 5.0, 0.0005, 5.0 }; - plane.dimensions = { 5.0, 1.0, 5.0 }; - plane_constraint.constraint_region.primitives.emplace_back(plane); - - geometry_msgs::msg::Pose plane_pose; - plane_pose.position.x = 0.14; - plane_pose.position.y = -0.3; - plane_pose.position.z = 1.0; - plane_pose.orientation.x = 0.0; - plane_pose.orientation.y = 0.0; - plane_pose.orientation.z = 0.0; - plane_pose.orientation.w = 0.0; - plane_constraint.constraint_region.primitive_poses.emplace_back(plane_pose); - plane_constraint.weight = 1.0; - - moveit_msgs::msg::Constraints plane_constraints; - plane_constraints.position_constraints.emplace_back(plane_constraint); - plane_constraints.name = "use_equality_constraints"; - this->move_group_.setPathConstraints(plane_constraints); - - // And again, configure and solve the planning problem - this->move_group_.setPoseTarget(msg->pose); - this->move_group_.setPlanningTime(10.0); - //success = (this->move_group_.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS); - //RCLCPP_INFO(this->get_logger(), "Plan with plane constraint %s", success ? "" : "FAILED"); - this->move_group_.move(); - } - //else - //{ - // // Plan and execute motion - // this->move_group_.setPoseTarget(msg->pose); - // this->move_group_.move(); - //} - - waypoints.push_back(msg->pose); - - if (waypoints.size() >= 2) - { - moveit_msgs::msg::RobotTrajectory trajectory; - - // dangerous with real robot - // https://moveit.picknik.ai/galactic/doc/examples/move_group_interface/move_group_interface_tutorial.html - const double jump_threshold = 0.0; - - const double eef_step = 0.01; - double fraction = this->move_group_.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); - RCLCPP_INFO(this->get_logger(), "Visualizing plan 4 (Cartesian path) (%.2f%% achieved)", fraction * 100.0); - - this->move_group_.execute(trajectory); - - waypoints.clear(); - } - - // Update for next callback - previous_target_pose_ = msg->pose; - this->moved = true; -} - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - - auto target_follower = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(target_follower); - executor.spin(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -} diff --git a/src/draw_svg/src/cpp/log_position.cpp b/src/draw_svg/src/cpp/log_position.cpp deleted file mode 100644 index 8d90182..0000000 --- a/src/draw_svg/src/cpp/log_position.cpp +++ /dev/null @@ -1,76 +0,0 @@ -#include -#include -#include -#include - -#include -#include -//#include -//https://github.com/AndrejOrsula/ign_moveit2_examples/blob/master/examples/cpp/ex_follow_target.cpp -const std::string MOVE_GROUP = "lite6"; - -class PositionLogger : public rclcpp::Node -{ -public: - /// Constructor - PositionLogger(); - - /// Move group interface for the robot - moveit::planning_interface::MoveGroupInterface move_group_; - /// Subscriber for target pose - //rclcpp::Subscription::SharedPtr target_pose_sub_; - /// Target pose that is used to detect changes - //geometry_msgs::msg::Pose previous_target_pose_; - -private: - /// Callback that plans and executes trajectory each time the target pose is changed - void log_position(); -}; - -PositionLogger::PositionLogger() : Node("position_logger"), - move_group_(std::shared_ptr(std::move(this)), MOVE_GROUP) -{ - // Use upper joint velocity and acceleration limits - this->move_group_.setMaxAccelerationScalingFactor(1.0); - this->move_group_.setMaxVelocityScalingFactor(1.0); - - // Subscribe to target pose - //target_pose_sub_ = this->create_subscription("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1)); - auto ros_clock = rclcpp::Clock::make_shared(); - auto timer_ = rclcpp::create_timer(this, ros_clock, rclcpp::Duration::from_nanoseconds(5000000), - [=]() - { - this->log_position(); - }); - - RCLCPP_INFO(this->get_logger(), "Initialization successful."); -} - -void PositionLogger::log_position() -{ - auto const logger = rclcpp::get_logger("position_logger"); - - auto msg = this->move_group_.getCurrentPose(); - auto p = msg.pose.position; - char a[100]; - std::sprintf(a, "Position x: %f, y: %f, z: %f", p.x,p.y,p.z); - std::string s(a); - - //rclcpp::RCLCPP_INFO(logger, a); - RCLCPP_INFO(logger, "AA"); -} - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - // TODO Try moveit_commander::roscpp_initialize - - auto pl = std::make_shared(); - - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(pl); - executor.spin(); - - rclcpp::shutdown(); - return EXIT_SUCCESS; -} diff --git a/src/draw_svg/src/py/draw_svg.py b/src/draw_svg/src/py/draw_svg.py deleted file mode 100755 index 2366ceb..0000000 --- a/src/draw_svg/src/py/draw_svg.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python3 -"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo""" - -import rclpy -from geometry_msgs.msg import Pose, PoseStamped -from pymoveit2 import MoveIt2 -from pymoveit2.robots import panda -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from rclpy.qos import QoSProfile -from random import uniform as rand -import math -#from tf2_ros.transformations import quaternion_from_euler -import lxml.etree as ET - - -def quaternion_from_euler(ai, aj, ak): - ai /= 2.0 - aj /= 2.0 - ak /= 2.0 - ci = math.cos(ai) - si = math.sin(ai) - cj = math.cos(aj) - sj = math.sin(aj) - ck = math.cos(ak) - sk = math.sin(ak) - cc = ci*ck - cs = ci*sk - sc = si*ck - ss = si*sk - - q = [0,0,0,0] - q[0] = cj*sc - sj*cs - q[1] = cj*ss + sj*cc - q[2] = cj*cs - sj*sc - q[3] = cj*cc + sj*ss - - return q - -def translate(val, lmin, lmax, rmin, rmax): - lspan = lmax - lmin - rspan = rmax - rmin - val = float(val - lmin) / float(lspan) - return rmin + (val * rspan) - -def map_point_function(x_pixels, y_pixels, xlim_lower, xlim_upper, ylim_lower, ylim_upper): - def map_point(xpix,ypix): - x = translate(xpix, 0, x_pixels, xlim_lower, xlim_upper) - y = translate(ypix, 0, y_pixels, ylim_lower, ylim_upper) - return (x,y) - return map_point - - -class PublishTarget(Node): - def __init__(self): - super().__init__('publisher') - self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10) - timer_period = 4.0 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.i = 0 - - # TODO get dimensions from svg - - #print(p) - #print(p.position) - #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.2, 0.4, -0.1, 0.1) - self.points = [] - for child in svg: - if (child.tag == 'line'): - self.points.append((float(child.get('x1')), float(child.get('y1')))) - self.points.append((float(child.get('x2')), float(child.get('y2')))) - - - def timer_callback(self): - next_point = self.points[self.i] - point = self.map_point(float(next_point[0]),float(next_point[1])) - p = Pose() - p.position.x = point[0] - p.position.y = point[1] - p.position.z = 0.1 - q = quaternion_from_euler(0.0, math.pi, 0.0) - #p.orientation = q - p.orientation.x = q[0] - p.orientation.y = q[1] - p.orientation.z = q[2] - p.orientation.w = q[3] - ps = PoseStamped() - ps.pose = p - #print(ps) - self.publisher_.publish(ps) - self.get_logger().info('Publishing to /target_pose: "%s"' % p) - self.i = (self.i + 1) % len(self.points) - -def main(args=None): - rclpy.init(args=args) - - publisher = PublishTarget() - - rclpy.spin(publisher) - - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) - publisher.destroy_node() - rclpy.shutdown() - - -if __name__ == "__main__": - main() diff --git a/src/draw_svg/src/py/drawing_surface.py b/src/draw_svg/src/py/drawing_surface.py deleted file mode 100755 index 83871a9..0000000 --- a/src/draw_svg/src/py/drawing_surface.py +++ /dev/null @@ -1,207 +0,0 @@ -#!/usr/bin/env python3 -"""GUI for virtual drawing surface""" - -import rclpy -from geometry_msgs.msg import Pose, PoseWithCovariance, Point -from nav_msgs.msg import Odometry -from sensor_msgs.msg import Image as SensorImage -from robots import lite6 -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy - -from queue import Queue - -import tkinter as tk -import math - -import threading - -from PIL import ImageTk -from PIL import Image as PImage -import numpy as np - - -def translate(val, lmin, lmax, rmin, rmax): - lspan = lmax - lmin - rspan = rmax - rmin - val = float(val - lmin) / float(lspan) - return rmin + (val * rspan) - -class DrawingApp(tk.Tk): - def __init__(self, point_queue, image_queue): - tk.Tk.__init__(self) - - self.point_queue = point_queue - self.image_queue = image_queue - self.width = 400 - self.height = 400 - self.img = PImage.new("RGB", (self.width, self.height), (255, 255, 255)) - self.arr = np.array(self.img) - self.frame_delay = 1 #ms - self.window_update_delay = 300 #ms - self.counter = 0 - self.read_queue() - - self.canvas = tk.Canvas(self,width=self.width,height=self.height) - - - self.tk_image = ImageTk.PhotoImage(self.img) - self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image) - self.canvas.pack(side='top', expand=True, fill='both') - - def reset(): - self.img = PImage.new("RGB", (self.width, self.height), (255, 255, 255)) - self.arr = np.array(self.img) - tk.Button(self.canvas, text= "reset", command=reset).pack() - - self.draw_window() - - def draw(self,x,y,z): - # putpixel is too slow - #self.img.putpixel((int(x), int(y)), (255, 0, 0)) - self.arr[x,y,1] = 0 - self.arr[x,y,2] = 0 - self.arr[x+1,y,1] = 0 - self.arr[x+1,y,2] = 0 - self.arr[x+1,y+1,1] = 0 - self.arr[x+1,y+1,2] = 0 - self.arr[x,y+1,1] = 0 - self.arr[x,y+1,2] = 0 - #print("Set x:{} y:{} to:{}".format(x,y,255)) - - def draw_window(self): - self.img = PImage.fromarray(self.arr) - self.tk_image = ImageTk.PhotoImage(self.img) - self.canvas.create_image(self.width/2,self.height/2,image=self.tk_image) - self.image_queue.put(self.get_image_message()) - #self.after(self.window_update_delay, lambda: self.draw_window()) - - def read_queue(self): - '''Read queue and draw circle every 10 ms''' - self.counter = self.counter + self.frame_delay - if self.point_queue.empty(): - self.after(self.frame_delay, lambda: self.read_queue()) - return - - while not self.point_queue.empty(): - p = self.point_queue.get() - #x = translate(p.x, -1.0, 0.5, 0, self.width) - #y = translate(p.y, 0.5, -1.0, 0, self.height) - x = translate(p.x, -1.0, 0.5, 0, self.width) - y = translate(p.y, -1.0, 0.5, 0, self.height) - self.draw(int(x),int(y),0) - - if self.counter >= self.window_update_delay: - #print("Redraw") - self.counter = 0 - self.draw_window() - - self.after(self.frame_delay, lambda: self.read_queue()) - - # https://stackoverflow.com/questions/64373334/how-can-i-publish-pil-image-binary-through-ros-without-opencv - def get_image_message(self): - msg = SensorImage() - #msg.header.stamp = rospy.Time.now() - msg.height = self.img.height - msg.width = self.img.width - msg.encoding = "rgb8" - msg.is_bigendian = False - msg.step = 3 * self.img.width - msg.data = np.array(self.img).tobytes() - return msg - -class LogPen(Node): - def __init__(self, point_queue=Queue()): - - self.queue = point_queue - super().__init__("log_pen") - - # Create callback group that allows execution of callbacks in parallel without restrictions - self._callback_group = ReentrantCallbackGroup() - - self.create_subscription( - msg_type=Odometry, - topic="/pen_position", - callback=self.pen_position_callback, - qos_profile=QoSProfile(reliability=ReliabilityPolicy.BEST_EFFORT, - history=HistoryPolicy.KEEP_LAST, - depth=1), - callback_group=self._callback_group, - ) - self.get_logger().info("Initialization successful.") - def pen_position_callback(self, msg: Odometry): - """ - Log position of pen - """ - p = msg.pose.pose.position - #self.get_logger().info("x:{} y:{} z:{}".format(p.x, p.y, p.z)) - self.queue.put(p) - -class PublishImage(Node): - def __init__(self, image_queue=Queue()): - - self.image_queue = image_queue - super().__init__("publish_image") - - self.publisher_ = self.create_publisher(SensorImage, '/drawing_surface/image_raw', 10) - timer_period = 0.0002 # seconds - self.timer = self.create_timer(timer_period, self.timer_callback) - self.get_logger().info("Initialization successful.") - - def timer_callback(self): - """ - Output image from queue - """ - if self.image_queue.empty(): - return - - img = self.image_queue.get() - #self.get_logger().info("Publishing image {}x{}".format(img.width,img.height)) - - self.publisher_.publish(img) - - - -def main(args=None): - - rclpy.init(args=args) - - log_pen = LogPen() - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(log_pen) - executor.spin() - - rclpy.shutdown() - exit(0) - -def start_node_thread(constructor, queue=Queue()): - - node = constructor(queue) - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(node) - executor.spin() - - -if __name__ == "__main__": - point_queue = Queue() - image_queue = Queue() - - rclpy.init() - - global app - app = DrawingApp(point_queue, image_queue) - - global log_thread - log_thread = threading.Thread(target=start_node_thread, args=[LogPen, point_queue]) - log_thread.start() - - global image_thread - image_thread = threading.Thread(target=start_node_thread, args=[PublishImage, image_queue]) - image_thread.start() - - app.mainloop() - rclpy.shutdown() - exit(0) diff --git a/src/draw_svg/src/py/follow.py b/src/draw_svg/src/py/follow.py deleted file mode 100755 index bd358b4..0000000 --- a/src/draw_svg/src/py/follow.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python3 -"""Example that uses MoveIt 2 to follow a target inside Ignition Gazebo""" - -import rclpy -from geometry_msgs.msg import Pose, PoseStamped -from pymoveit2 import MoveIt2 -from robots import lite6 -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from rclpy.qos import QoSProfile - - -class FollowTarget(Node): - def __init__(self): - - super().__init__("follow_py") - - # Create callback group that allows execution of callbacks in parallel without restrictions - self._callback_group = ReentrantCallbackGroup() - - # Create MoveIt 2 interface - self._moveit2 = MoveIt2( - node=self, - 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, - ) - # Use upper joint velocity and acceleration limits - self._moveit2.max_velocity = 1.0 - self._moveit2.max_acceleration = 1.0 - - # Create a subscriber for target pose - p = Pose() - p.position.x=0.5 - p.position.y=0.5 - self.__previous_target_pose = p - self.create_subscription( - msg_type=PoseStamped, - topic="/target_pose", - callback=self.target_pose_callback, - qos_profile=QoSProfile(depth=1), - callback_group=self._callback_group, - ) - - self.get_logger().info("Initialization successful.") - - def target_pose_callback(self, msg: PoseStamped): - """ - Plan and execute trajectory each time the target pose is changed - """ - - # Return if target pose is unchanged - if msg.pose == self.__previous_target_pose: - return - - self.get_logger().info("Target pose has changed. Planning and executing...") - - # Options https://github.com/AndrejOrsula/pymoveit2/blob/master/pymoveit2/moveit2.py - # Plan and execute motion - self._moveit2.move_to_pose( - position=msg.pose.position, - quat_xyzw=msg.pose.orientation, - cartesian=True, - ) - - # Update for next callback - self.__previous_target_pose = msg.pose - -def main(args=None): - - rclpy.init(args=args) - - target_follower = FollowTarget() - - executor = rclpy.executors.MultiThreadedExecutor(2) - executor.add_node(target_follower) - executor.spin() - - rclpy.shutdown() - exit(0) - - -if __name__ == "__main__": - main() diff --git a/src/draw_svg/src/py/readsvg.py b/src/draw_svg/src/py/readsvg.py deleted file mode 100755 index 39a221d..0000000 --- a/src/draw_svg/src/py/readsvg.py +++ /dev/null @@ -1,22 +0,0 @@ -#!/usr/bin/env python3 - -import lxml.etree as ET - -xml = ET.parse('svg/test.svg') -svg = xml.getroot() -print(svg) - -# AttributeError: 'lxml.etree._ElementTree' object has no attribute 'tag' -print(svg.tag) - -# TypeError: 'lxml.etree._ElementTree' object is not subscriptable -print(svg[0]) - -# TypeError: 'lxml.etree._ElementTree' object is not iterable -for child in svg: - print('width:', svg.get('width')) - if (child.tag == 'line'): - print(child.get('x1')) - print(child.get('x2')) - print(child.get('y1')) - print(child.get('y2')) diff --git a/src/draw_svg/src/py/robots/lite6.py b/src/draw_svg/src/py/robots/lite6.py deleted file mode 100644 index bd66959..0000000 --- a/src/draw_svg/src/py/robots/lite6.py +++ /dev/null @@ -1,35 +0,0 @@ -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", - ] diff --git a/src/draw_svg/svg/test.svg b/src/draw_svg/svg/test.svg deleted file mode 100644 index 1c69c4a..0000000 --- a/src/draw_svg/svg/test.svg +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/src/draw_svg/test.sh b/src/draw_svg/test.sh deleted file mode 100644 index fcd3028..0000000 --- a/src/draw_svg/test.sh +++ /dev/null @@ -1 +0,0 @@ -ros2 topic pub --once /target_pose geometry_msgs/msg/PoseStamped '{header:{}, pose:{position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {}}}' diff --git a/src/draw_svg/urdf/lite6.tmp.urdf b/src/draw_svg/urdf/lite6.tmp.urdf deleted file mode 100644 index 5035766..0000000 --- a/src/draw_svg/urdf/lite6.tmp.urdf +++ /dev/null @@ -1,436 +0,0 @@ - - - - - - - - - gazebo_ros2_control/GazeboSystem - /root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml - - - - - - - - - - - gazebo_ros2_control/GazeboSystem - xarm - False - P - R - normal - 6 - True - 2000000 - lite - False - - - - -6.283185307179586 - 6.283185307179586 - - - -3.14 - 3.14 - - - - - - - - -2.61799 - 2.61799 - - - -3.14 - 3.14 - - - - - - - - -0.061087 - 5.235988 - - - -3.14 - 3.14 - - - - - - - - -6.283185307179586 - 6.283185307179586 - - - -3.14 - 3.14 - - - - - - - - -2.1642 - 2.1642 - - - -3.14 - 3.14 - - - - - - - - -6.283185307179586 - 6.283185307179586 - - - -3.14 - 3.14 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - transmission_interface/SimpleTransmission - - hardware_interface/PositionJointInterface - - - hardware_interface/PositionJointInterface - 100 - - - - true - - - true - - - true - - - true - - - true - - - true - - - true - - - - - - - - - - - - - - - - - - - - - - - - - - - odom:=pen_position - - world - - link6 - - - - 10 - - - - - - - diff --git a/src/draw_svg/urdf/xarm_pen.urdf.xacro b/src/draw_svg/urdf/xarm_pen.urdf.xacro deleted file mode 100644 index 3c4c9f8..0000000 --- a/src/draw_svg/urdf/xarm_pen.urdf.xacro +++ /dev/null @@ -1,100 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - odom:=pen_position - - world - - link6 - - - - 10 - - - - - - - - - diff --git a/src/draw_svg/worlds/draw_svg_world.sdf b/src/draw_svg/worlds/draw_svg_world.sdf deleted file mode 100644 index 3de14ed..0000000 --- a/src/draw_svg/worlds/draw_svg_world.sdf +++ /dev/null @@ -1,169 +0,0 @@ - - - - - - - - - ignition-physics-dartsim-plugin - - - bullet - - - - 0.005 - 1.0 - - - - - - - 0.8 0.8 0.8 - false - - - - - - - - - - - - true - 0 0 10 0 0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.3 0.3 -0.9 - - - - - - - - - 0 0 0 0 0 0 - true - - - - - 0 0 1 - - - - - - - 0 0 1 - 4 4 - - - - 0.2 0.2 0.2 1 - 0.2 0.2 0.2 1 - - - - - - -0.14 -0.3 0.5 0 0 0 - true - - - - - 0 0 1 - 0.28 0.21 - - - - - - - - 0 0 1 - 0.28 0.21 - - - - - - - - 400 - 400 - - - - - - - - - - - - - - - -