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

Import example code and basic prototype

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-10-18 17:20:50 +03:00
parent ed00da86c3
commit ae44ce87e9
33 changed files with 3882 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

50
src/draw_svg/CMakeLists.txt Normal file
View File

@@ -0,0 +1,50 @@
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)
#add_executable(draw_svg src/draw_svg.cpp)
#target_include_directories(draw_svg PUBLIC
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
#target_compile_features(draw_svg PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
#ament_target_dependencies(
# draw_svg
# "ign_moveit2_examples"
#)
#install(TARGETS draw_svg
# 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
DESTINATION lib/${PROJECT_NAME}
)
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 worlds DESTINATION share/${PROJECT_NAME})
ament_package()

150
src/draw_svg/launch/default.launch.py Executable file
View File

@@ -0,0 +1,150 @@
#!/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([robot_type, "_moveit_config"]),
"launch",
"move_group.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
View 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="panda",
description="Name of the robot to use.",
),
# Miscellaneous
DeclareLaunchArgument(
"rviz_config",
default_value=path.join(
get_package_share_directory("ign_moveit2_examples"),
"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.",
),
]

61
src/draw_svg/launch/robots/robot_panda.launch.py Executable file
View 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="panda",
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
View 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
View 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.",
),
]

24
src/draw_svg/package.xml Normal file
View File

@@ -0,0 +1,24 @@
<?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>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>

3
src/draw_svg/rebuild.sh Normal file
View File

@@ -0,0 +1,3 @@
rm -r build/ install/ log/
colcon build
#ros2 launch draw_svg draw_svg.launch.py

337
src/draw_svg/rviz/ign_moveit2_examples.rviz Normal file
View 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
View 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;
}

84
src/draw_svg/src/py/draw_svg.py Executable file
View File

@@ -0,0 +1,84 @@
#!/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
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
class PublishTarget(Node):
def __init__(self):
super().__init__('publisher')
self.publisher_ = self.create_publisher(PoseStamped, '/target_pose', 10)
timer_period = 3.0 # seconds
self.timer = self.create_timer(timer_period, self.timer_callback)
self.i = 0
#print(p)
#print(p.position)
#print(p.orientation)
def timer_callback(self):
p = Pose()
p.position.x = rand(0.1,0.4)
p.position.y = rand(0.1,0.4)
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 += 1
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)
minimal_publisher.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()

87
src/draw_svg/src/py/follow.py Normal file
View 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 pymoveit2.robots import panda
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=panda.joint_names(),
base_link_name=panda.base_link_name(),
end_effector_name=panda.end_effector_name(),
group_name=panda.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()

1
src/draw_svg/test.sh Normal file
View 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: {}}}'

140
src/draw_svg/tf.txt Normal file
View File

@@ -0,0 +1,140 @@
/apply_planning_scene
/check_state_validity
/clear_octomap
/compute_cartesian_path
/compute_fk
/compute_ik
/controller_manager/configure_and_start_controller
/controller_manager/configure_controller
/controller_manager/describe_parameters
/controller_manager/get_parameter_types
/controller_manager/get_parameters
/controller_manager/list_controller_types
/controller_manager/list_controllers
/controller_manager/list_hardware_interfaces
/controller_manager/list_parameters
/controller_manager/load_and_configure_controller
/controller_manager/load_and_start_controller
/controller_manager/load_controller
/controller_manager/reload_controller_libraries
/controller_manager/set_parameters
/controller_manager/set_parameters_atomically
/controller_manager/switch_controller
/controller_manager/unload_controller
/follow_py/describe_parameters
/follow_py/get_parameter_types
/follow_py/get_parameters
/follow_py/list_parameters
/follow_py/set_parameters
/follow_py/set_parameters_atomically
/get_planner_params
/get_planning_scene
/gripper_trajectory_controller/describe_parameters
/gripper_trajectory_controller/get_parameter_types
/gripper_trajectory_controller/get_parameters
/gripper_trajectory_controller/list_parameters
/gripper_trajectory_controller/set_parameters
/gripper_trajectory_controller/set_parameters_atomically
/ignition_ros_control/describe_parameters
/ignition_ros_control/get_parameter_types
/ignition_ros_control/get_parameters
/ignition_ros_control/list_parameters
/ignition_ros_control/set_parameters
/ignition_ros_control/set_parameters_atomically
/interactive_marker_display_94740324816224/describe_parameters
/interactive_marker_display_94740324816224/get_parameter_types
/interactive_marker_display_94740324816224/get_parameters
/interactive_marker_display_94740324816224/list_parameters
/interactive_marker_display_94740324816224/set_parameters
/interactive_marker_display_94740324816224/set_parameters_atomically
/joint_state_broadcaster/describe_parameters
/joint_state_broadcaster/get_parameter_types
/joint_state_broadcaster/get_parameters
/joint_state_broadcaster/list_parameters
/joint_state_broadcaster/set_parameters
/joint_state_broadcaster/set_parameters_atomically
/joint_trajectory_controller/describe_parameters
/joint_trajectory_controller/get_parameter_types
/joint_trajectory_controller/get_parameters
/joint_trajectory_controller/list_parameters
/joint_trajectory_controller/set_parameters
/joint_trajectory_controller/set_parameters_atomically
/load_map
/move_group/describe_parameters
/move_group/get_parameter_types
/move_group/get_parameters
/move_group/list_parameters
/move_group/set_parameters
/move_group/set_parameters_atomically
/move_group_interface_node/describe_parameters
/move_group_interface_node/get_parameter_types
/move_group_interface_node/get_parameters
/move_group_interface_node/list_parameters
/move_group_interface_node/set_parameters
/move_group_interface_node/set_parameters_atomically
/move_group_private_93885309918944/describe_parameters
/move_group_private_93885309918944/get_parameter_types
/move_group_private_93885309918944/get_parameters
/move_group_private_93885309918944/list_parameters
/move_group_private_93885309918944/set_parameters
/move_group_private_93885309918944/set_parameters_atomically
/moveit_simple_controller_manager/describe_parameters
/moveit_simple_controller_manager/get_parameter_types
/moveit_simple_controller_manager/get_parameters
/moveit_simple_controller_manager/list_parameters
/moveit_simple_controller_manager/set_parameters
/moveit_simple_controller_manager/set_parameters_atomically
/plan_kinematic_path
/publisher/describe_parameters
/publisher/get_parameter_types
/publisher/get_parameters
/publisher/list_parameters
/publisher/set_parameters
/publisher/set_parameters_atomically
/query_planner_interface
/robot_state_publisher/describe_parameters
/robot_state_publisher/get_parameter_types
/robot_state_publisher/get_parameters
/robot_state_publisher/list_parameters
/robot_state_publisher/set_parameters
/robot_state_publisher/set_parameters_atomically
/ros_ign_bridge/describe_parameters
/ros_ign_bridge/get_parameter_types
/ros_ign_bridge/get_parameters
/ros_ign_bridge/list_parameters
/ros_ign_bridge/set_parameters
/ros_ign_bridge/set_parameters_atomically
/rviz/describe_parameters
/rviz/get_parameter_types
/rviz/get_parameters
/rviz/list_parameters
/rviz/set_parameters
/rviz/set_parameters_atomically
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/get_interactive_markers
/rviz_private_139912250992336/describe_parameters
/rviz_private_139912250992336/get_parameter_types
/rviz_private_139912250992336/get_parameters
/rviz_private_139912250992336/list_parameters
/rviz_private_139912250992336/set_parameters
/rviz_private_139912250992336/set_parameters_atomically
/save_map
/servo_node/change_control_dimensions
/servo_node/change_drift_dimensions
/servo_node/describe_parameters
/servo_node/get_parameter_types
/servo_node/get_parameters
/servo_node/list_parameters
/servo_node/pause_servo
/servo_node/reset_servo_status
/servo_node/set_parameters
/servo_node/set_parameters_atomically
/servo_node/start_servo
/servo_node/stop_servo
/servo_node/unpause_servo
/servo_node_private_94162402444928/describe_parameters
/servo_node_private_94162402444928/get_parameter_types
/servo_node_private_94162402444928/get_parameters
/servo_node_private_94162402444928/list_parameters
/servo_node_private_94162402444928/set_parameters
/servo_node_private_94162402444928/set_parameters_atomically
/set_planner_params

109
src/draw_svg/worlds/follow_target.sdf Normal file
View File

@@ -0,0 +1,109 @@
<?xml version="1.0"?>
<sdf version="1.9">
<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.8 0.8 0.8 1</diffuse>
<specular>0.8 0.8 0.8 1</specular>
</material>
</visual>
</link>
</model>
<!-- 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> -->
</world>
</sdf>