Compare commits
1 Commits
721da6ed4d
...
master
| Author | SHA1 | Date | |
|---|---|---|---|
| 6b9a733339 |
@@ -49,6 +49,7 @@ RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
|
|||||||
rm -rf ${WS_LOG_DIR}
|
rm -rf ${WS_LOG_DIR}
|
||||||
|
|
||||||
# Build packages
|
# Build packages
|
||||||
|
COPY ./src/draw_svg ${WS_SRC_DIR}/draw_svg
|
||||||
COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
|
COPY ./src/drawing_controller ${WS_SRC_DIR}/drawing_controller
|
||||||
COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
|
COPY ./src/axidraw_controller ${WS_SRC_DIR}/axidraw_controller
|
||||||
COPY ./src/virtual_drawing_surface ${WS_SRC_DIR}/virtual_drawing_surface
|
COPY ./src/virtual_drawing_surface ${WS_SRC_DIR}/virtual_drawing_surface
|
||||||
@@ -58,7 +59,7 @@ RUN pip install -r ${WS_SRC_DIR}/virtual_drawing_surface/requirements.txt
|
|||||||
RUN apt-get update
|
RUN apt-get update
|
||||||
RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
|
RUN source "/opt/ros/${ROS_DISTRO}/setup.bash" && \
|
||||||
source "${WS_INSTALL_DIR}/local_setup.bash" && \
|
source "${WS_INSTALL_DIR}/local_setup.bash" && \
|
||||||
colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
|
colcon build --merge-install --symlink-install --cmake-args "-DCMAKE_BUILD_TYPE=Release" --paths ${WS_SRC_DIR}/draw_svg ${WS_SRC_DIR}/drawing_controller ${WS_SRC_DIR}/axidraw_controller ${WS_SRC_DIR}/virtual_drawing_surface && \
|
||||||
rm -rf ${WS_LOG_DIR}
|
rm -rf ${WS_LOG_DIR}
|
||||||
|
|
||||||
# Build lite6 and xarm packages
|
# Build lite6 and xarm packages
|
||||||
|
|||||||
18
README.md
18
README.md
@@ -29,26 +29,18 @@ docker builder prune --all --force
|
|||||||
bash .docker/run.bash
|
bash .docker/run.bash
|
||||||
```
|
```
|
||||||
|
|
||||||
|
### Develop
|
||||||
If active changes are being made, run:
|
If active changes are being made, run:
|
||||||
``` sh
|
``` sh
|
||||||
bash .docker/devel.bash
|
bash .docker/devel.bash
|
||||||
```
|
```
|
||||||
This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
|
This will mount the host `drawing-robot-ros2` directory in the container at `src/drawing-robot-ros2`.
|
||||||
|
|
||||||
## TODO Building locally
|
So to test changes in the container
|
||||||
|
|
||||||
Requirements:
|
|
||||||
- python3-pip
|
|
||||||
- python3-pil.imagetk
|
|
||||||
- ros-humble-moveit
|
|
||||||
- ros-humble-ros-gz
|
|
||||||
- ignition-fortress
|
|
||||||
|
|
||||||
``` sh
|
``` sh
|
||||||
./rebuild.sh
|
cd src/drawing-robot-ros2/src
|
||||||
```
|
colcon build
|
||||||
``` sh
|
source install/local_setup.bash
|
||||||
source src/install/local_setup.bash
|
|
||||||
```
|
```
|
||||||
|
|
||||||
## Running
|
## Running
|
||||||
|
|||||||
59
src/draw_svg/CMakeLists.txt
Normal file
59
src/draw_svg/CMakeLists.txt
Normal file
@@ -0,0 +1,59 @@
|
|||||||
|
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()
|
||||||
4
src/draw_svg/README.md
Normal file
4
src/draw_svg/README.md
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
# draw_svg
|
||||||
|
WILL BE REMOVED
|
||||||
|
|
||||||
|
Contains initial testing of libraries and launch files.
|
||||||
146
src/draw_svg/launch/default.launch.py
Executable file
146
src/draw_svg/launch/default.launch.py
Executable file
@@ -0,0 +1,146 @@
|
|||||||
|
#!/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.",
|
||||||
|
),
|
||||||
|
]
|
||||||
112
src/draw_svg/launch/draw_svg.launch.py
Executable file
112
src/draw_svg/launch/draw_svg.launch.py
Executable file
@@ -0,0 +1,112 @@
|
|||||||
|
#!/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.",
|
||||||
|
),
|
||||||
|
]
|
||||||
255
src/draw_svg/launch/lite6_pen.launch.py
Normal file
255
src/draw_svg/launch/lite6_pen.launch.py
Normal file
@@ -0,0 +1,255 @@
|
|||||||
|
#!/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)
|
||||||
|
])
|
||||||
361
src/draw_svg/launch/lite6_real.launch.py
Normal file
361
src/draw_svg/launch/lite6_real.launch.py
Normal file
@@ -0,0 +1,361 @@
|
|||||||
|
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)
|
||||||
|
])
|
||||||
41
src/draw_svg/launch/log_position.launch.py
Executable file
41
src/draw_svg/launch/log_position.launch.py
Executable file
@@ -0,0 +1,41 @@
|
|||||||
|
#!/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)
|
||||||
|
]
|
||||||
168
src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py
Executable file
168
src/draw_svg/launch/robots/_robot_beside_table_gazebo.launch.py
Executable file
@@ -0,0 +1,168 @@
|
|||||||
|
#!/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)
|
||||||
|
])
|
||||||
176
src/draw_svg/launch/robots/lite6_table.launch.py
Normal file
176
src/draw_svg/launch/robots/lite6_table.launch.py
Normal file
@@ -0,0 +1,176 @@
|
|||||||
|
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)
|
||||||
|
])
|
||||||
61
src/draw_svg/launch/robots/robot_lite6.launch.py
Executable file
61
src/draw_svg/launch/robots/robot_lite6.launch.py
Executable file
@@ -0,0 +1,61 @@
|
|||||||
|
#!/usr/bin/env -S ros2 launch
|
||||||
|
"""Launch script for spawning ufactory xarm lite6 into Ignition Gazebo world"""
|
||||||
|
|
||||||
|
from typing import List
|
||||||
|
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
|
||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.actions import DeclareLaunchArgument
|
||||||
|
from launch.substitutions import LaunchConfiguration
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description() -> LaunchDescription:
|
||||||
|
|
||||||
|
# Declare all launch arguments
|
||||||
|
declared_arguments = generate_declared_arguments()
|
||||||
|
|
||||||
|
# Get substitution for all arguments
|
||||||
|
model = LaunchConfiguration("model")
|
||||||
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||||
|
log_level = LaunchConfiguration("log_level")
|
||||||
|
|
||||||
|
# List of nodes to be launched
|
||||||
|
nodes = [
|
||||||
|
# ros_ign_gazebo_create
|
||||||
|
Node(
|
||||||
|
package="ros_ign_gazebo",
|
||||||
|
executable="create",
|
||||||
|
output="log",
|
||||||
|
arguments=["-file", model, "--ros-args", "--log-level", log_level],
|
||||||
|
parameters=[{"use_sim_time": use_sim_time}],
|
||||||
|
),
|
||||||
|
]
|
||||||
|
|
||||||
|
return LaunchDescription(declared_arguments + nodes)
|
||||||
|
|
||||||
|
|
||||||
|
def generate_declared_arguments() -> List[DeclareLaunchArgument]:
|
||||||
|
"""
|
||||||
|
Generate list of all launch arguments that are declared for this launch script.
|
||||||
|
"""
|
||||||
|
|
||||||
|
return [
|
||||||
|
# Model for Ignition Gazebo
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"model",
|
||||||
|
default_value="lite6",
|
||||||
|
description="Name or filepath of model to load.",
|
||||||
|
),
|
||||||
|
# Miscellaneous
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"use_sim_time",
|
||||||
|
default_value="true",
|
||||||
|
description="If true, use simulated clock.",
|
||||||
|
),
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"log_level",
|
||||||
|
default_value="warn",
|
||||||
|
description="The level of logging that is applied to all ROS 2 nodes launched by this script.",
|
||||||
|
),
|
||||||
|
]
|
||||||
61
src/draw_svg/launch/robots/robot_panda.launch.py
Executable file
61
src/draw_svg/launch/robots/robot_panda.launch.py
Executable file
@@ -0,0 +1,61 @@
|
|||||||
|
#!/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.",
|
||||||
|
),
|
||||||
|
]
|
||||||
91
src/draw_svg/launch/worlds/world_default.launch.py
Executable file
91
src/draw_svg/launch/worlds/world_default.launch.py
Executable file
@@ -0,0 +1,91 @@
|
|||||||
|
#!/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.",
|
||||||
|
),
|
||||||
|
]
|
||||||
113
src/draw_svg/launch/worlds/world_draw_svg.launch.py
Executable file
113
src/draw_svg/launch/worlds/world_draw_svg.launch.py
Executable file
@@ -0,0 +1,113 @@
|
|||||||
|
#!/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.",
|
||||||
|
),
|
||||||
|
]
|
||||||
135
src/draw_svg/launch/xarm_draw_svg.launch.py
Executable file
135
src/draw_svg/launch/xarm_draw_svg.launch.py
Executable file
@@ -0,0 +1,135 @@
|
|||||||
|
#!/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)
|
||||||
|
])
|
||||||
32
src/draw_svg/package.xml
Normal file
32
src/draw_svg/package.xml
Normal file
@@ -0,0 +1,32 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>draw_svg</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>TODO: Package description</description>
|
||||||
|
<maintainer email="root@todo.todo">root</maintainer>
|
||||||
|
<license>TODO: License declaration</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<!-- <depend>ign_moveit2_examples</depend> -->
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>geometry_msgs</depend>
|
||||||
|
<depend>tf2_ros</depend>
|
||||||
|
<depend>python3-lxml</depend>
|
||||||
|
<depend>python3-tk</depend>
|
||||||
|
<depend>python-imaging</depend>
|
||||||
|
<depend>python-numpy</depend>
|
||||||
|
<depend>python3-pil.imagetk</depend>
|
||||||
|
<depend>xarm_description</depend>
|
||||||
|
<depend>xarm_moveit_config</depend>
|
||||||
|
<!-- <depend>moveit_visual_tools</depend> -->
|
||||||
|
<!-- <depend>moveit_ros_planning_interface</depend> -->
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
4
src/draw_svg/rebuild.sh
Normal file
4
src/draw_svg/rebuild.sh
Normal file
@@ -0,0 +1,4 @@
|
|||||||
|
rm -r build/ install/ log/
|
||||||
|
rosdep install --from-paths . --ignore-src -r -y
|
||||||
|
colcon build
|
||||||
|
#ros2 launch draw_svg draw_svg.launch.py
|
||||||
337
src/draw_svg/rviz/ign_moveit2_examples.rviz
Normal file
337
src/draw_svg/rviz/ign_moveit2_examples.rviz
Normal file
@@ -0,0 +1,337 @@
|
|||||||
|
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: <Fixed 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
|
||||||
10
src/draw_svg/src/cpp/draw_svg.cpp
Normal file
10
src/draw_svg/src/cpp/draw_svg.cpp
Normal file
@@ -0,0 +1,10 @@
|
|||||||
|
#include <cstdio>
|
||||||
|
|
||||||
|
int main(int argc, char ** argv)
|
||||||
|
{
|
||||||
|
(void) argc;
|
||||||
|
(void) argv;
|
||||||
|
|
||||||
|
printf("hello world draw_svg package\n");
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
141
src/draw_svg/src/cpp/follow.cpp
Normal file
141
src/draw_svg/src/cpp/follow.cpp
Normal file
@@ -0,0 +1,141 @@
|
|||||||
|
//#include "follow.h"
|
||||||
|
|
||||||
|
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||||
|
#include <geometry_msgs/msg/pose.hpp>
|
||||||
|
#include <moveit/move_group_interface/move_group_interface.h>
|
||||||
|
#include <rclcpp/qos.hpp>
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
|
||||||
|
#include <moveit/planning_scene_interface/planning_scene_interface.h>
|
||||||
|
#include <moveit_msgs/msg/collision_object.hpp>
|
||||||
|
//#include <moveit_visual_tools/moveit_visual_tools.h>
|
||||||
|
#include <std_msgs/msg/color_rgba.hpp>
|
||||||
|
|
||||||
|
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<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_sub_;
|
||||||
|
/// Target pose that is used to detect changes
|
||||||
|
geometry_msgs::msg::Pose previous_target_pose_;
|
||||||
|
|
||||||
|
bool moved = false;
|
||||||
|
std::vector<geometry_msgs::msg::Pose> 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<rclcpp::Node>(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<geometry_msgs::msg::PoseStamped>("/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<MoveItFollowTarget>();
|
||||||
|
|
||||||
|
rclcpp::executors::SingleThreadedExecutor executor;
|
||||||
|
executor.add_node(target_follower);
|
||||||
|
executor.spin();
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return EXIT_SUCCESS;
|
||||||
|
}
|
||||||
76
src/draw_svg/src/cpp/log_position.cpp
Normal file
76
src/draw_svg/src/cpp/log_position.cpp
Normal file
@@ -0,0 +1,76 @@
|
|||||||
|
#include <memory>
|
||||||
|
#include <cstdio>
|
||||||
|
#include <iostream>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
|
#include <moveit/move_group_interface/move_group_interface.h>
|
||||||
|
//#include <moveit/moveit_commander>
|
||||||
|
//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<geometry_msgs::msg::PoseStamped>::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<rclcpp::Node>(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<geometry_msgs::msg::PoseStamped>("/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<PositionLogger>();
|
||||||
|
|
||||||
|
rclcpp::executors::SingleThreadedExecutor executor;
|
||||||
|
executor.add_node(pl);
|
||||||
|
executor.spin();
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return EXIT_SUCCESS;
|
||||||
|
}
|
||||||
112
src/draw_svg/src/py/draw_svg.py
Executable file
112
src/draw_svg/src/py/draw_svg.py
Executable file
@@ -0,0 +1,112 @@
|
|||||||
|
#!/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()
|
||||||
207
src/draw_svg/src/py/drawing_surface.py
Executable file
207
src/draw_svg/src/py/drawing_surface.py
Executable file
@@ -0,0 +1,207 @@
|
|||||||
|
#!/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)
|
||||||
87
src/draw_svg/src/py/follow.py
Executable file
87
src/draw_svg/src/py/follow.py
Executable file
@@ -0,0 +1,87 @@
|
|||||||
|
#!/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()
|
||||||
22
src/draw_svg/src/py/readsvg.py
Executable file
22
src/draw_svg/src/py/readsvg.py
Executable file
@@ -0,0 +1,22 @@
|
|||||||
|
#!/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'))
|
||||||
35
src/draw_svg/src/py/robots/lite6.py
Normal file
35
src/draw_svg/src/py/robots/lite6.py
Normal file
@@ -0,0 +1,35 @@
|
|||||||
|
from typing import List
|
||||||
|
|
||||||
|
MOVE_GROUP_ARM: str = "lite6"
|
||||||
|
MOVE_GROUP_GRIPPER: str = "gripper"
|
||||||
|
|
||||||
|
OPEN_GRIPPER_JOINT_POSITIONS: List[float] = [0.04, 0.04]
|
||||||
|
CLOSED_GRIPPER_JOINT_POSITIONS: List[float] = [0.0, 0.0]
|
||||||
|
|
||||||
|
# https://github.com/xArm-Developer/xarm_ros2/blob/master/xarm_moveit_config/srdf/_lite6_macro.srdf.xacro
|
||||||
|
def joint_names(prefix: str = "") -> List[str]:
|
||||||
|
return [
|
||||||
|
prefix + "world_joint",
|
||||||
|
prefix + "joint1",
|
||||||
|
prefix + "joint2",
|
||||||
|
prefix + "joint3",
|
||||||
|
prefix + "joint4",
|
||||||
|
prefix + "joint5",
|
||||||
|
prefix + "joint6",
|
||||||
|
prefix + "joint_eef",
|
||||||
|
]
|
||||||
|
|
||||||
|
|
||||||
|
def base_link_name(prefix: str = "") -> str:
|
||||||
|
return prefix + "link_base"
|
||||||
|
|
||||||
|
|
||||||
|
def end_effector_name(prefix: str = "") -> str:
|
||||||
|
return prefix + "link_eef"
|
||||||
|
|
||||||
|
|
||||||
|
def gripper_joint_names(prefix: str = "") -> List[str]:
|
||||||
|
return [
|
||||||
|
prefix + "link_eef",
|
||||||
|
prefix + "link_eef",
|
||||||
|
]
|
||||||
7
src/draw_svg/svg/test.svg
Normal file
7
src/draw_svg/svg/test.svg
Normal file
@@ -0,0 +1,7 @@
|
|||||||
|
<svg height="210" width="500">
|
||||||
|
<line x1="100" y1="200" x2="250" y2="10" style="stroke:rgb(255,0,0);stroke-width:2" />
|
||||||
|
<line x1="250" y1="10" x2="400" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
|
||||||
|
<line x1="400" y1="200" x2="20" y2="90" style="stroke:rgb(255,0,0);stroke-width:2" />
|
||||||
|
<line x1="20" y1="90" x2="450" y2="90" style="stroke:rgb(255,0,0);stroke-width:2" />
|
||||||
|
<line x1="450" y1="90" x2="100" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" />
|
||||||
|
</svg>
|
||||||
|
After Width: | Height: | Size: 480 B |
1
src/draw_svg/test.sh
Normal file
1
src/draw_svg/test.sh
Normal file
@@ -0,0 +1 @@
|
|||||||
|
ros2 topic pub --once /target_pose geometry_msgs/msg/PoseStamped '{header:{}, pose:{position: {x: 0.0, y: 0.0, z: 0.0}, orientation: {}}}'
|
||||||
436
src/draw_svg/urdf/lite6.tmp.urdf
Normal file
436
src/draw_svg/urdf/lite6.tmp.urdf
Normal file
@@ -0,0 +1,436 @@
|
|||||||
|
<?xml version="1.0" ?>
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<!-- | This document was autogenerated by xacro from urdf/xarm_pen.urdf.xacro | -->
|
||||||
|
<!-- | EDITING THIS FILE BY HAND IS NOT RECOMMENDED | -->
|
||||||
|
<!-- =================================================================================== -->
|
||||||
|
<robot name="xarm6">
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
|
||||||
|
<robot_sim_type>gazebo_ros2_control/GazeboSystem</robot_sim_type>
|
||||||
|
<parameters>/root/ws/install/share/xarm_controller/config/xarm6_controllers.yaml</parameters>
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
<link name="world"/>
|
||||||
|
<joint name="world_joint" type="fixed">
|
||||||
|
<parent link="world"/>
|
||||||
|
<child link="link_base"/>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
</joint>
|
||||||
|
<ros2_control name="UFRobotSystem" type="system">
|
||||||
|
<hardware>
|
||||||
|
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
|
||||||
|
<param name="hw_ns">xarm</param>
|
||||||
|
<param name="velocity_control">False</param>
|
||||||
|
<param name="prefix">P</param>
|
||||||
|
<param name="robot_ip">R</param>
|
||||||
|
<param name="report_type">normal</param>
|
||||||
|
<param name="dof">6</param>
|
||||||
|
<param name="baud_checkset">True</param>
|
||||||
|
<param name="default_gripper_baud">2000000</param>
|
||||||
|
<param name="robot_type">lite</param>
|
||||||
|
<param name="add_gripper">False</param>
|
||||||
|
</hardware>
|
||||||
|
<joint name="joint1">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-6.283185307179586</param>
|
||||||
|
<param name="max">6.283185307179586</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
<joint name="joint2">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-2.61799</param>
|
||||||
|
<param name="max">2.61799</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
<joint name="joint3">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-0.061087</param>
|
||||||
|
<param name="max">5.235988</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
<joint name="joint4">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-6.283185307179586</param>
|
||||||
|
<param name="max">6.283185307179586</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
<joint name="joint5">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-2.1642</param>
|
||||||
|
<param name="max">2.1642</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
<joint name="joint6">
|
||||||
|
<command_interface name="position">
|
||||||
|
<param name="min">-6.283185307179586</param>
|
||||||
|
<param name="max">6.283185307179586</param>
|
||||||
|
</command_interface>
|
||||||
|
<command_interface name="velocity">
|
||||||
|
<param name="min">-3.14</param>
|
||||||
|
<param name="max">3.14</param>
|
||||||
|
</command_interface>
|
||||||
|
<state_interface name="position"/>
|
||||||
|
<state_interface name="velocity"/>
|
||||||
|
<!-- <state_interface name="effort"/> -->
|
||||||
|
</joint>
|
||||||
|
</ros2_control>
|
||||||
|
<material name="White">
|
||||||
|
<color rgba="1.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
<material name="Silver">
|
||||||
|
<color rgba="0.753 0.753 0.753 1.0"/>
|
||||||
|
</material>
|
||||||
|
<link name="link_base">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00829544579053192 3.26357432323433E-05 0.0631194584987089"/>
|
||||||
|
<mass value="1.65393501783165"/>
|
||||||
|
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/base.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<link name="link1">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.00036 0.03788 -0.0027"/>
|
||||||
|
<mass value="1.169"/>
|
||||||
|
<inertia ixx="1.45164E-03" ixy="1.24E-05" ixz="-6.7E-06" iyy="8.873E-04" iyz="1.255E-04" izz="1.31993E-03"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link1.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint1" type="revolute">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0.2435"/>
|
||||||
|
<parent link="link_base"/>
|
||||||
|
<child link="link1"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="50.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link2">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.178 0.0 0.0576"/>
|
||||||
|
<mass value="1.192"/>
|
||||||
|
<inertia ixx="1.5854E-03" ixy="-6.766E-06" ixz="-1.15136E-03" iyy="5.6097E-03" iyz="1.14E-06" izz="4.85E-03"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link2.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint2" type="revolute">
|
||||||
|
<origin rpy="1.5708 -1.5708 3.1416" xyz="0 0 0"/>
|
||||||
|
<parent link="link1"/>
|
||||||
|
<child link="link2"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="50.0" lower="-2.61799" upper="2.61799" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link3">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.07285 -0.030 -0.0009"/>
|
||||||
|
<mass value="0.930"/>
|
||||||
|
<inertia ixx="8.861E-04" ixy="-3.9287E-04" ixz="7.066E-05" iyy="1.5785E-03" iyz="-2.445E-05" izz="1.84677E-03"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link3.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint3" type="revolute">
|
||||||
|
<origin rpy="-3.1416 0 1.5708" xyz="0.2002 0 0"/>
|
||||||
|
<parent link="link2"/>
|
||||||
|
<child link="link3"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="32.0" lower="-0.061087" upper="5.235988" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link4">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="-0.0004 -0.0275 -0.0817"/>
|
||||||
|
<mass value="1.31"/>
|
||||||
|
<inertia ixx="3.705E-03" ixy="-2.0E-06" ixz="7.17E-06" iyy="3.0455E-03" iyz="-9.3188E-04" izz="1.5413E-03"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link4.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint4" type="revolute">
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0.087 -0.22761 0"/>
|
||||||
|
<parent link="link3"/>
|
||||||
|
<child link="link4"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="32.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link5">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0 0.010 0.0019"/>
|
||||||
|
<mass value="0.784"/>
|
||||||
|
<inertia ixx="5.668E-04" ixy="6E-07" ixz="-5.3E-06" iyy="5.077E-04" iyz="-4.8E-07" izz="5.3E-04"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="White"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link5.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint5" type="revolute">
|
||||||
|
<origin rpy="1.5708 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="link4"/>
|
||||||
|
<child link="link5"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="32.0" lower="-2.1642" upper="2.1642" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link6">
|
||||||
|
<inertial>
|
||||||
|
<origin rpy="0 0 0" xyz="0.0 -0.00194 -0.0102"/>
|
||||||
|
<mass value="0.180"/>
|
||||||
|
<inertia ixx="7.726E-05" ixy="1E-06" ixz="4E-07" iyy="8.5665E-05" iyz="-6E-07" izz="1.4814E-04"/>
|
||||||
|
</inertial>
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="Silver"/>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<mesh filename="file:////root/ws/install/share/xarm_description/meshes/lite6/visual/link6.stl"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<joint name="joint6" type="revolute">
|
||||||
|
<origin rpy="-1.5708 0 0" xyz="0 0.0625 0"/>
|
||||||
|
<parent link="link5"/>
|
||||||
|
<child link="link6"/>
|
||||||
|
<axis xyz="0 0 1"/>
|
||||||
|
<limit effort="20.0" lower="-6.283185307179586" upper="6.283185307179586" velocity="3.14"/>
|
||||||
|
<dynamics damping="1.0" friction="1.0"/>
|
||||||
|
</joint>
|
||||||
|
<link name="link_eef"/>
|
||||||
|
<joint name="joint_eef" type="fixed">
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<parent link="link6"/>
|
||||||
|
<child link="link_eef"/>
|
||||||
|
</joint>
|
||||||
|
<transmission name="tran1">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint1">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor1">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="tran2">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint2">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor2">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="tran3">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint3">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor3">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="tran4">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint4">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor3">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="tran5">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint5">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor5">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<transmission name="tran6">
|
||||||
|
<type>transmission_interface/SimpleTransmission</type>
|
||||||
|
<joint name="joint6">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
</joint>
|
||||||
|
<actuator name="motor6">
|
||||||
|
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
|
||||||
|
<mechanicalReduction>100</mechanicalReduction>
|
||||||
|
</actuator>
|
||||||
|
</transmission>
|
||||||
|
<gazebo reference="link_base">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link1">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link2">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link3">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link4">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link5">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<gazebo reference="link6">
|
||||||
|
<selfCollide>true</selfCollide>
|
||||||
|
</gazebo>
|
||||||
|
<joint name="eef_joint" type="fixed">
|
||||||
|
<parent link="link_eef"/>
|
||||||
|
<child link="pen_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pen_link">
|
||||||
|
<visual>
|
||||||
|
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||||
|
<geometry>
|
||||||
|
<cylinder length="0.2" radius="0.005"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="Cyan">
|
||||||
|
<color rgba="0.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0 0 0"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
|
||||||
|
<gazebo>
|
||||||
|
<plugin filename="libgazebo_ros_p3d.so" name="pen_position">
|
||||||
|
<!-- <alwaysOn>true</alwaysOn> -->
|
||||||
|
<ros>
|
||||||
|
<remapping>odom:=pen_position</remapping>
|
||||||
|
</ros>
|
||||||
|
<frame_name>world</frame_name>
|
||||||
|
<!-- <body_name>pen_link</body_name> -->
|
||||||
|
<body_name>link6</body_name>
|
||||||
|
<!-- topic name is always /odom -->
|
||||||
|
<!-- <topic_name>pen_position</topic_name> -->
|
||||||
|
<!-- Update rate in Hz -->
|
||||||
|
<update_rate>10</update_rate>
|
||||||
|
<!-- <xyzOffsets>0 0 0</xyzOffsets> -->
|
||||||
|
<!-- <rpyOffsets>0 0 0</rpyOffsets> -->
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
</robot>
|
||||||
|
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
|
||||||
|
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
|
||||||
|
<xacro:lite6_urdf prefix=''/> -->
|
||||||
|
<!--
|
||||||
|
</robot> -->
|
||||||
100
src/draw_svg/urdf/xarm_pen.urdf.xacro
Normal file
100
src/draw_svg/urdf/xarm_pen.urdf.xacro
Normal file
@@ -0,0 +1,100 @@
|
|||||||
|
<?xml version='1.0' encoding='utf-8'?>
|
||||||
|
<robot xmlns:xacro="http://ros.org/wiki/xacro" name="xarm$(arg dof)">
|
||||||
|
<!-- <robot xmlns:xacro="http://ros.org/wiki/xacro" name="lite6"> -->
|
||||||
|
<xacro:arg name="prefix" default=""/>
|
||||||
|
<xacro:arg name="hw_ns" default="xarm"/>
|
||||||
|
<xacro:arg name="limited" default="false"/>
|
||||||
|
<xacro:arg name="effort_control" default="false"/>
|
||||||
|
<xacro:arg name="velocity_control" default="false"/>
|
||||||
|
<xacro:arg name="add_gripper" default="false"/>
|
||||||
|
<xacro:arg name="add_vacuum_gripper" default="false"/>
|
||||||
|
<xacro:arg name="dof" default="6"/>
|
||||||
|
<xacro:arg name="robot_type" default="lite"/>
|
||||||
|
<xacro:arg name="ros2_control_plugin" default="uf_robot_hardware/UFRobotSystemHardware"/>
|
||||||
|
<xacro:arg name="ros2_control_params" default="$(find xarm_controller)/config/xarm$(arg dof)_controllers.yaml"/>
|
||||||
|
|
||||||
|
<xacro:arg name="add_other_geometry" default="false"/>
|
||||||
|
<xacro:arg name="geometry_type" default="box"/>
|
||||||
|
<xacro:arg name="geometry_mass" default="0.1"/>
|
||||||
|
<xacro:arg name="geometry_height" default="0.1"/>
|
||||||
|
<xacro:arg name="geometry_radius" default="0.1"/>
|
||||||
|
<xacro:arg name="geometry_length" default="0.1"/>
|
||||||
|
<xacro:arg name="geometry_width" default="0.1"/>
|
||||||
|
<xacro:arg name="geometry_mesh_filename" default=""/>
|
||||||
|
<xacro:arg name="geometry_mesh_origin_xyz" default="0 0 0"/>
|
||||||
|
<xacro:arg name="geometry_mesh_origin_rpy" default="0 0 0"/>
|
||||||
|
<xacro:arg name="geometry_mesh_tcp_xyz" default="0 0 0"/>
|
||||||
|
<xacro:arg name="geometry_mesh_tcp_rpy" default="0 0 0"/>
|
||||||
|
|
||||||
|
<xacro:arg name="robot_ip" default=""/>
|
||||||
|
<xacro:arg name="report_type" default="normal"/>
|
||||||
|
<xacro:arg name="baud_checkset" default="true"/>
|
||||||
|
<xacro:arg name="default_gripper_baud" default="2000000"/>
|
||||||
|
|
||||||
|
<!-- gazebo_plugin -->
|
||||||
|
<xacro:include filename="$(find xarm_description)/urdf/common/common.gazebo.xacro" />
|
||||||
|
<xacro:gazebo_ros_control_plugin ros2_control_params="$(arg ros2_control_params)"/>
|
||||||
|
|
||||||
|
<!-- load xarm device -->
|
||||||
|
<!-- Load Lite6 Robot Model URDF -->
|
||||||
|
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6_robot_macro.xacro" />
|
||||||
|
<xacro:lite6_robot prefix="$(arg prefix)" namespace="xarm" limited="$(arg limited)"
|
||||||
|
effort_control="$(arg effort_control)" velocity_control="$(arg velocity_control)"
|
||||||
|
ros2_control_plugin="$(arg ros2_control_plugin)"
|
||||||
|
attach_to="world" xyz="0 0 0" rpy="0 0 0"
|
||||||
|
ros2_control_params="$(arg ros2_control_params)"
|
||||||
|
load_gazebo_plugin="false"
|
||||||
|
add_gripper="$(arg add_gripper)"
|
||||||
|
robot_ip="$(arg robot_ip)"
|
||||||
|
report_type="$(arg report_type)"
|
||||||
|
baud_checkset="$(arg baud_checkset)"
|
||||||
|
default_gripper_baud="$(arg default_gripper_baud)" />
|
||||||
|
|
||||||
|
<joint name="eef_joint" type="fixed">
|
||||||
|
<parent link="link_eef"/>
|
||||||
|
<child link="pen_link"/>
|
||||||
|
</joint>
|
||||||
|
<link name="pen_link">
|
||||||
|
<visual>
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<cylinder radius="0.005" length="0.2"/>
|
||||||
|
</geometry>
|
||||||
|
<material name="Cyan">
|
||||||
|
<color rgba="0.0 1.0 1.0 1.0"/>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<collision>
|
||||||
|
<geometry>
|
||||||
|
<box size="0 0 0"/>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
</link>
|
||||||
|
|
||||||
|
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_p3d.cpp -->
|
||||||
|
<gazebo>
|
||||||
|
<plugin name="pen_position" filename="libgazebo_ros_p3d.so">
|
||||||
|
<!-- <alwaysOn>true</alwaysOn> -->
|
||||||
|
<ros>
|
||||||
|
<remapping>odom:=pen_position</remapping>
|
||||||
|
</ros>
|
||||||
|
<frame_name>world</frame_name>
|
||||||
|
<!-- <body_name>pen_link</body_name> -->
|
||||||
|
<body_name>link6</body_name>
|
||||||
|
<!-- topic name is always /odom -->
|
||||||
|
<!-- <topic_name>pen_position</topic_name> -->
|
||||||
|
<!-- Update rate in Hz -->
|
||||||
|
<update_rate>10</update_rate>
|
||||||
|
<!-- <xyzOffsets>0 0 0</xyzOffsets> -->
|
||||||
|
<!-- <rpyOffsets>0 0 0</rpyOffsets> -->
|
||||||
|
</plugin>
|
||||||
|
</gazebo>
|
||||||
|
|
||||||
|
</robot>
|
||||||
|
<!-- <robot name="lite6_robot" xmlns:xacro="http://www.ros.org/wiki/xacro" xmlns:xi="http://www.w3.org/2001/XInclude">
|
||||||
|
<xacro:include filename="$(find xarm_description)/urdf/lite6/lite6.urdf.xacro"/>
|
||||||
|
<xacro:lite6_urdf prefix=''/> -->
|
||||||
|
|
||||||
|
<!--
|
||||||
|
</robot> -->
|
||||||
169
src/draw_svg/worlds/draw_svg_world.sdf
Normal file
169
src/draw_svg/worlds/draw_svg_world.sdf
Normal file
@@ -0,0 +1,169 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<!-- <sdf version="1.9"> -->
|
||||||
|
<sdf version="1.7">
|
||||||
|
<world name="draw_svg_world">
|
||||||
|
|
||||||
|
<!-- Physics -->
|
||||||
|
<plugin filename="ignition-gazebo-physics-system" name="ignition::gazebo::systems::Physics">
|
||||||
|
<engine>
|
||||||
|
<filename>ignition-physics-dartsim-plugin</filename>
|
||||||
|
</engine>
|
||||||
|
<dart>
|
||||||
|
<collision_detector>bullet</collision_detector>
|
||||||
|
</dart>
|
||||||
|
</plugin>
|
||||||
|
<physics name="physics_config" type="dart">
|
||||||
|
<max_step_size>0.005</max_step_size>
|
||||||
|
<real_time_factor>1.0</real_time_factor>
|
||||||
|
</physics>
|
||||||
|
|
||||||
|
<!-- Scene -->
|
||||||
|
<plugin filename="ignition-gazebo-scene-broadcaster-system" name="ignition::gazebo::systems::SceneBroadcaster">
|
||||||
|
</plugin>
|
||||||
|
<scene>
|
||||||
|
<ambient>0.8 0.8 0.8</ambient>
|
||||||
|
<grid>false</grid>
|
||||||
|
</scene>
|
||||||
|
|
||||||
|
<!-- User Commands (transform control) -->
|
||||||
|
<plugin filename="ignition-gazebo-user-commands-system" name="ignition::gazebo::systems::UserCommands">
|
||||||
|
</plugin>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- -->
|
||||||
|
<!-- Illumination -->
|
||||||
|
<!-- -->
|
||||||
|
<light type="directional" name="sun">
|
||||||
|
<cast_shadows>true</cast_shadows>
|
||||||
|
<pose>0 0 10 0 0 0</pose>
|
||||||
|
<diffuse>0.8 0.8 0.8 1</diffuse>
|
||||||
|
<specular>0.2 0.2 0.2 1</specular>
|
||||||
|
<attenuation>
|
||||||
|
<range>1000</range>
|
||||||
|
<constant>0.9</constant>
|
||||||
|
<linear>0.01</linear>
|
||||||
|
<quadratic>0.001</quadratic>
|
||||||
|
</attenuation>
|
||||||
|
<direction>-0.3 0.3 -0.9</direction>
|
||||||
|
</light>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- -->
|
||||||
|
<!-- Models -->
|
||||||
|
<!-- -->
|
||||||
|
<!-- Ground -->
|
||||||
|
<model name="ground_plane">
|
||||||
|
<pose>0 0 0 0 0 0</pose>
|
||||||
|
<static>true</static>
|
||||||
|
<link name="ground_plane_link">
|
||||||
|
<collision name="ground_plane_collision">
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual name="ground_plane_visual">
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
<size>4 4</size>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
<material>
|
||||||
|
<diffuse>0.2 0.2 0.2 1</diffuse>
|
||||||
|
<specular>0.2 0.2 0.2 1</specular>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
<model name="drawing_surface">
|
||||||
|
<pose>-0.14 -0.3 0.5 0 0 0</pose>
|
||||||
|
<static>true</static>
|
||||||
|
<link name="drawing_surface_link">
|
||||||
|
<collision name="drawing_surface_collision">
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
<size>0.28 0.21</size>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
</collision>
|
||||||
|
<visual name="drawing_surface_visual">
|
||||||
|
<origin xyz="0 0 0" rpy="0 0 0" />
|
||||||
|
<geometry>
|
||||||
|
<plane>
|
||||||
|
<normal>0 0 1</normal>
|
||||||
|
<size>0.28 0.21</size>
|
||||||
|
</plane>
|
||||||
|
</geometry>
|
||||||
|
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/src/gazebo_ros_video.cpp -->
|
||||||
|
<!-- https://github.com/ros-simulation/gazebo_ros_pkgs/blob/foxy/gazebo_plugins/worlds/gazebo_ros_video_demo.world -->
|
||||||
|
<!-- TODO: note docs outdated: https://classic.gazebosim.org/tutorials?tut=ros_gzplugins -->
|
||||||
|
<plugin name="drawing_surface" filename="libgazebo_ros_video.so">
|
||||||
|
<!-- <ros>
|
||||||
|
<remapping>~/image_raw:=/camera1/image_raw</remapping>
|
||||||
|
</ros> -->
|
||||||
|
<height>400</height>
|
||||||
|
<width>400</width>
|
||||||
|
</plugin>
|
||||||
|
</visual>
|
||||||
|
<!-- <inertial>
|
||||||
|
<origin xyz="0 0 1" rpy="0 0 0"/>
|
||||||
|
<mass value="1"/>
|
||||||
|
<inertia
|
||||||
|
ixx="1.0" ixy="0.0" ixz="0.0"
|
||||||
|
iyy="1.0" iyz="0.0"
|
||||||
|
izz="1.0"/>
|
||||||
|
</inertial> -->
|
||||||
|
</link>
|
||||||
|
</model>
|
||||||
|
<!-- <include>
|
||||||
|
<uri>model://ground_plane</uri>
|
||||||
|
<pose>0.0 -0.84 0 0 0 0</pose>
|
||||||
|
</include> -->
|
||||||
|
<!-- <include>
|
||||||
|
<uri>model://sun</uri>
|
||||||
|
</include> -->
|
||||||
|
<!-- <include>
|
||||||
|
<uri>model://table</uri>
|
||||||
|
<name>table</name>
|
||||||
|
<pose>0.0 -0.84 0 0 0 0</pose>
|
||||||
|
</include>
|
||||||
|
-->
|
||||||
|
<!-- Static target -->
|
||||||
|
<!-- <model name="target">
|
||||||
|
<static>true</static>
|
||||||
|
<pose>0.5 -0.25 0.5 3.1415927 0 0</pose>
|
||||||
|
<link name="target_link">
|
||||||
|
<visual name="target_visual">
|
||||||
|
<cast_shadows>false</cast_shadows>
|
||||||
|
<geometry>
|
||||||
|
<box>
|
||||||
|
<size>0.04 0.04 0.04</size>
|
||||||
|
</box>
|
||||||
|
</geometry>
|
||||||
|
<material>
|
||||||
|
<diffuse>0.1 0.1 0.1 1</diffuse>
|
||||||
|
<specular>0.4 0.4 0.4 1</specular>
|
||||||
|
</material>
|
||||||
|
</visual>
|
||||||
|
</link>
|
||||||
|
-->
|
||||||
|
<!-- Pose publisher plugin, used to determine the target pose to reach -->
|
||||||
|
<!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
|
||||||
|
<publish_nested_model_pose>true</publish_nested_model_pose>
|
||||||
|
<publish_link_pose>false</publish_link_pose>
|
||||||
|
<publish_collision_pose>false</publish_collision_pose>
|
||||||
|
<publish_visual_pose>false</publish_visual_pose>
|
||||||
|
</plugin>
|
||||||
|
</model> -->
|
||||||
|
<!-- <plugin filename="ignition-gazebo-pose-publisher-system" name="ignition::gazebo::systems::PosePublisher">
|
||||||
|
<publish_link_pose>true</publish_link_pose>
|
||||||
|
<publish_collision_pose>false</publish_collision_pose>
|
||||||
|
<publish_visual_pose>false</publish_visual_pose>
|
||||||
|
<publish_nested_model_pose>false</publish_nested_model_pose>
|
||||||
|
</plugin> -->
|
||||||
|
|
||||||
|
</world>
|
||||||
|
</sdf>
|
||||||
@@ -111,71 +111,108 @@ def launch_setup(context, *args, **kwargs):
|
|||||||
}.items(),
|
}.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, " ",
|
||||||
|
|
||||||
mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('custom_xarm_description'), 'launch', 'lib', 'robot_description_lib.py'))
|
"add_other_geometry:=", add_other_geometry, " ",
|
||||||
#mod = load_python_launch_file_as_module(os.path.join(get_package_share_directory('draw_svg'), 'launch', 'robots', 'xarm_with_pen.py'))
|
"geometry_type:=", geometry_type, " ",
|
||||||
get_xacro_file_content = getattr(mod, 'get_xacro_file_content')
|
"geometry_mass:=", geometry_mass, " ",
|
||||||
robot_description = {
|
"geometry_height:=", geometry_height, " ",
|
||||||
'robot_description': get_xacro_file_content(
|
"geometry_radius:=", geometry_radius, " ",
|
||||||
xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_description'), 'urdf', 'xarm_device.urdf.xacro']),
|
"geometry_length:=", geometry_length, " ",
|
||||||
#xacro_file=PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'xarm_pen.urdf.xacro']),
|
"geometry_width:=", geometry_width, " ",
|
||||||
arguments={
|
"geometry_mesh_filename:=", geometry_mesh_filename, " ",
|
||||||
'prefix': prefix,
|
"geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
|
||||||
'dof': dof,
|
"geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
|
||||||
'robot_type': robot_type,
|
"geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
|
||||||
'add_gripper': add_gripper,
|
"geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
|
||||||
'add_vacuum_gripper': add_vacuum_gripper,
|
|
||||||
'hw_ns': hw_ns.perform(context).strip('/'),
|
#"robot_ip:=", robot_ip, " ",
|
||||||
'limited': limited,
|
#"report_type:=", report_type, " ",
|
||||||
'effort_control': effort_control,
|
#"baud_checkset:=", baud_checkset, " ",
|
||||||
'velocity_control': velocity_control,
|
#"default_gripper_baud:=", default_gripper_baud, " ",
|
||||||
'ros2_control_plugin': ros2_control_plugin,
|
]
|
||||||
#'ros2_control_params': ros2_control_params,
|
)
|
||||||
'add_other_geometry': add_other_geometry,
|
# TODO fix URDF loading
|
||||||
'geometry_type': geometry_type,
|
# 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"
|
||||||
'geometry_mass': geometry_mass,
|
_robot_description_xml = Command(['cat ', PathJoinSubstitution([FindPackageShare('draw_svg'), 'urdf', 'lite6.tmp.urdf'])])
|
||||||
'geometry_height': geometry_height,
|
robot_description = {"robot_description": _robot_description_xml}
|
||||||
'geometry_radius': geometry_radius,
|
# SRDF
|
||||||
'geometry_length': geometry_length,
|
_robot_description_semantic_xml = Command(
|
||||||
'geometry_width': geometry_width,
|
[
|
||||||
'geometry_mesh_filename': geometry_mesh_filename,
|
PathJoinSubstitution([FindExecutable(name="xacro")]),
|
||||||
'geometry_mesh_origin_xyz': geometry_mesh_origin_xyz,
|
" ",
|
||||||
'geometry_mesh_origin_rpy': geometry_mesh_origin_rpy,
|
PathJoinSubstitution(
|
||||||
'geometry_mesh_tcp_xyz': geometry_mesh_tcp_xyz,
|
[
|
||||||
'geometry_mesh_tcp_rpy': geometry_mesh_tcp_rpy,
|
FindPackageShare('custom_xarm_moveit_config'),
|
||||||
}
|
"srdf",
|
||||||
|
#"_lite6_macro.srdf.xacro",
|
||||||
|
"xarm.srdf.xacro",
|
||||||
|
]
|
||||||
),
|
),
|
||||||
}
|
" ",
|
||||||
|
#"name:=", "lite6", " ",
|
||||||
|
"prefix:=", " ",
|
||||||
|
#"hw_ns:=", hw_ns, " ",
|
||||||
|
#"limited:=", limited, " ",
|
||||||
|
#"effort_control:=", effort_control, " ",
|
||||||
|
#"velocity_control:=", velocity_control, " ",
|
||||||
|
#"add_gripper:=", add_gripper, " ",
|
||||||
|
#"add_vacuum_gripper:=", add_vacuum_gripper, " ",
|
||||||
|
"dof:=", dof, " ",
|
||||||
|
"robot_type:=", robot_type, " ",
|
||||||
|
#"ros2_control_plugin:=", ros2_control_plugin, " ",
|
||||||
|
#"ros2_control_params:=", ros2_control_params, " ",
|
||||||
|
|
||||||
|
#"add_other_geometry:=", add_other_geometry, " ",
|
||||||
|
#"geometry_type:=", geometry_type, " ",
|
||||||
|
#"geometry_mass:=", geometry_mass, " ",
|
||||||
|
#"geometry_height:=", geometry_height, " ",
|
||||||
|
#"geometry_radius:=", geometry_radius, " ",
|
||||||
|
#"geometry_length:=", geometry_length, " ",
|
||||||
|
#"geometry_width:=", geometry_width, " ",
|
||||||
|
#"geometry_mesh_filename:=", geometry_mesh_filename, " ",
|
||||||
|
#"geometry_mesh_origin_xyz:=", geometry_mesh_origin_xyz, " ",
|
||||||
|
#"geometry_mesh_origin_rpy:=", geometry_mesh_origin_rpy, " ",
|
||||||
|
#"geometry_mesh_tcp_xyz:=", geometry_mesh_tcp_xyz, " ",
|
||||||
|
#"geometry_mesh_tcp_rpy:=", geometry_mesh_tcp_rpy, " ",
|
||||||
|
|
||||||
|
#"robot_ip:=", robot_ip, " ",
|
||||||
|
#"report_type:=", report_type, " ",
|
||||||
|
#"baud_checkset:=", baud_checkset, " ",
|
||||||
|
#"default_gripper_baud:=", default_gripper_baud, " ",
|
||||||
|
]
|
||||||
|
)
|
||||||
robot_description_semantic = {
|
robot_description_semantic = {
|
||||||
'robot_description_semantic': get_xacro_file_content(
|
"robot_description_semantic": _robot_description_semantic_xml
|
||||||
xacro_file=PathJoinSubstitution([FindPackageShare('custom_xarm_moveit_config'), 'srdf', 'xarm.srdf.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,
|
|
||||||
}
|
|
||||||
),
|
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -3,7 +3,7 @@
|
|||||||
#include <robot_controller/robot_controller.hpp>
|
#include <robot_controller/robot_controller.hpp>
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cstdlib>
|
#include <cstdlib>
|
||||||
#include <queue>
|
//#include <queue>
|
||||||
|
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include <iostream>
|
#include <iostream>
|
||||||
@@ -28,7 +28,6 @@
|
|||||||
|
|
||||||
|
|
||||||
#include <moveit_msgs/msg/constraints.hpp>
|
#include <moveit_msgs/msg/constraints.hpp>
|
||||||
#include <moveit_msgs/msg/robot_trajectory.hpp>
|
|
||||||
#include <moveit_msgs/msg/motion_plan_request.hpp>
|
#include <moveit_msgs/msg/motion_plan_request.hpp>
|
||||||
#include <moveit_msgs/msg/motion_sequence_request.hpp>
|
#include <moveit_msgs/msg/motion_sequence_request.hpp>
|
||||||
#include <moveit_msgs/msg/motion_sequence_item.hpp>
|
#include <moveit_msgs/msg/motion_sequence_item.hpp>
|
||||||
@@ -66,11 +65,6 @@ public:
|
|||||||
* Move group interface for the robot
|
* Move group interface for the robot
|
||||||
*/
|
*/
|
||||||
moveit::planning_interface::MoveGroupInterface move_group;
|
moveit::planning_interface::MoveGroupInterface move_group;
|
||||||
moveit::core::RobotStatePtr move_group_state;
|
|
||||||
|
|
||||||
std::queue<moveit_msgs::msg::RobotTrajectory> trajectory_queue;
|
|
||||||
rclcpp::TimerBase::SharedPtr trajectory_timer_;
|
|
||||||
bool busy = false;
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* TODO Use instead of MoveGroupInterface
|
* TODO Use instead of MoveGroupInterface
|
||||||
@@ -127,10 +121,6 @@ public:
|
|||||||
// Subscribe to target pose
|
// Subscribe to target pose
|
||||||
//target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
|
//target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
|
||||||
|
|
||||||
//
|
|
||||||
trajectory_timer_ = this->create_wall_timer(
|
|
||||||
10ms, std::bind(&Lite6Controller::executeTrajectoryFromQueue, this));
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
||||||
void setup()
|
void setup()
|
||||||
@@ -145,9 +135,6 @@ public:
|
|||||||
|
|
||||||
planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(this->shared_from_this(),
|
planning_scene_monitor_ = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>(this->shared_from_this(),
|
||||||
"robot_description");
|
"robot_description");
|
||||||
//RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Getting initial robot state");
|
|
||||||
//move_group.startStateMonitor(5);
|
|
||||||
|
|
||||||
this->sequence_client_ =
|
this->sequence_client_ =
|
||||||
this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
|
this->create_client<moveit_msgs::srv::GetMotionSequence>("plan_sequence_path");
|
||||||
|
|
||||||
@@ -165,6 +152,7 @@ public:
|
|||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
move_group.setPlanningTime(30.0);
|
move_group.setPlanningTime(30.0);
|
||||||
RCLCPP_INFO(this->get_logger(), "Initialization successful.");
|
RCLCPP_INFO(this->get_logger(), "Initialization successful.");
|
||||||
|
|
||||||
@@ -286,12 +274,8 @@ public:
|
|||||||
msr.items.push_back(msi);
|
msr.items.push_back(msi);
|
||||||
}
|
}
|
||||||
msr.items.back().blend_radius = 0.0; // Last element blend must be 0
|
msr.items.back().blend_radius = 0.0; // Last element blend must be 0
|
||||||
|
moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
|
||||||
moveit_msgs::msg::RobotState state_msg;
|
moveit_msgs::msg::RobotState state_msg;
|
||||||
if (move_group_state == NULL)
|
|
||||||
{
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Getting robot state");
|
|
||||||
move_group_state = move_group.getCurrentState(10);
|
|
||||||
}
|
|
||||||
moveit::core::robotStateToRobotStateMsg(*move_group_state, state_msg);
|
moveit::core::robotStateToRobotStateMsg(*move_group_state, state_msg);
|
||||||
msr.items.front().req.start_state = state_msg;
|
msr.items.front().req.start_state = state_msg;
|
||||||
return msr;
|
return msr;
|
||||||
@@ -398,17 +382,6 @@ public:
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
void executeTrajectoryFromQueue()
|
|
||||||
{
|
|
||||||
if (busy || trajectory_queue.empty())
|
|
||||||
return;
|
|
||||||
busy = true;
|
|
||||||
RCLCPP_INFO(this->get_logger(), "Executing next trajectory from queue");
|
|
||||||
move_group.execute(trajectory_queue.front());
|
|
||||||
trajectory_queue.pop();
|
|
||||||
busy = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Callback that executes path on robot
|
* Callback that executes path on robot
|
||||||
*/
|
*/
|
||||||
@@ -439,14 +412,8 @@ public:
|
|||||||
{
|
{
|
||||||
status = "success";
|
status = "success";
|
||||||
RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
|
RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
|
||||||
RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
|
RCLCPP_INFO(this->get_logger(), "Executing result");
|
||||||
|
move_group.execute(ts[0]);
|
||||||
trajectory_queue.push(ts[0]);
|
|
||||||
|
|
||||||
// Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
|
|
||||||
robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
|
|
||||||
rt.setRobotTrajectoryMsg(*move_group_state, ts[0]);
|
|
||||||
move_group_state = rt.getLastWayPointPtr();
|
|
||||||
|
|
||||||
status = status + "," + pointsToString(&goal->motion.path,0,0,0);
|
status = status + "," + pointsToString(&goal->motion.path,0,0,0);
|
||||||
appendLineToFile("OUTPUT.csv", status);
|
appendLineToFile("OUTPUT.csv", status);
|
||||||
|
|||||||
Reference in New Issue
Block a user