add skill launch, refactor pick-place pose loader
This commit is contained in:
parent
034e172f62
commit
d72c06efea
14 changed files with 1032 additions and 191 deletions
|
|
@ -16,6 +16,10 @@ install(
|
||||||
DESTINATION share/${PROJECT_NAME}
|
DESTINATION share/${PROJECT_NAME}
|
||||||
)
|
)
|
||||||
|
|
||||||
|
# Install Python modules
|
||||||
|
ament_python_install_package(rbs_launch_utils)
|
||||||
|
ament_python_install_module(rbs_launch_utils/launch_common.py)
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
find_package(ament_lint_auto REQUIRED)
|
find_package(ament_lint_auto REQUIRED)
|
||||||
# the following line skips the linter which checks for copyrights
|
# the following line skips the linter which checks for copyrights
|
||||||
|
|
|
||||||
622
rbs_bringup/config/rbs.rviz
Normal file
622
rbs_bringup/config/rbs.rviz
Normal file
|
|
@ -0,0 +1,622 @@
|
||||||
|
Panels:
|
||||||
|
- Class: rviz_common/Displays
|
||||||
|
Help Height: 138
|
||||||
|
Name: Displays
|
||||||
|
Property Tree Widget:
|
||||||
|
Expanded:
|
||||||
|
- /Global Options1
|
||||||
|
- /TF1
|
||||||
|
- /TF1/Frames1
|
||||||
|
Splitter Ratio: 0.49496981501579285
|
||||||
|
Tree Height: 1468
|
||||||
|
- 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
|
||||||
|
Name: Views
|
||||||
|
Splitter Ratio: 0.5
|
||||||
|
Visualization Manager:
|
||||||
|
Class: ""
|
||||||
|
Displays:
|
||||||
|
- Alpha: 0.5
|
||||||
|
Cell Size: 1
|
||||||
|
Class: rviz_default_plugins/Grid
|
||||||
|
Color: 160; 160; 164
|
||||||
|
Enabled: true
|
||||||
|
Line Style:
|
||||||
|
Line Width: 0.029999999329447746
|
||||||
|
Value: Lines
|
||||||
|
Name: Grid
|
||||||
|
Normal Cell Count: 0
|
||||||
|
Offset:
|
||||||
|
X: 0
|
||||||
|
Y: 0
|
||||||
|
Z: 0
|
||||||
|
Plane: XY
|
||||||
|
Plane Cell Count: 10
|
||||||
|
Reference Frame: <Fixed Frame>
|
||||||
|
Value: true
|
||||||
|
- Class: rviz_default_plugins/MarkerArray
|
||||||
|
Enabled: true
|
||||||
|
Name: MarkerArray
|
||||||
|
Namespaces:
|
||||||
|
{}
|
||||||
|
Topic:
|
||||||
|
Depth: 5
|
||||||
|
Durability Policy: Volatile
|
||||||
|
History Policy: Keep Last
|
||||||
|
Reliability Policy: Reliable
|
||||||
|
Value: /display_contacts
|
||||||
|
Value: true
|
||||||
|
- Class: moveit_rviz_plugin/PlanningScene
|
||||||
|
Enabled: true
|
||||||
|
Move Group Namespace: ""
|
||||||
|
Name: PlanningScene
|
||||||
|
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
|
||||||
|
base:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_link_inertia:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
box1_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box2_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box3_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box4_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box5_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box6_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
flange:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
forearm_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
ft_frame:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
inner_rgbd_camera:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
outer_rgbd_camera:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_finger_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_finger_tip_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_inner_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_finger_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_finger_tip_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_inner_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_grasp_point:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
shoulder_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
tool0:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
upper_arm_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
wrist_1_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
wrist_2_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
wrist_3_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
Robot Alpha: 1
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Value: true
|
||||||
|
- Class: rviz_default_plugins/TF
|
||||||
|
Enabled: true
|
||||||
|
Frame Timeout: 15
|
||||||
|
Frames:
|
||||||
|
All Enabled: false
|
||||||
|
base:
|
||||||
|
Value: false
|
||||||
|
base_link:
|
||||||
|
Value: false
|
||||||
|
base_link_inertia:
|
||||||
|
Value: false
|
||||||
|
box1:
|
||||||
|
Value: true
|
||||||
|
box1_place:
|
||||||
|
Value: true
|
||||||
|
box2:
|
||||||
|
Value: true
|
||||||
|
box2_place:
|
||||||
|
Value: true
|
||||||
|
box3:
|
||||||
|
Value: true
|
||||||
|
box3_place:
|
||||||
|
Value: true
|
||||||
|
box4:
|
||||||
|
Value: true
|
||||||
|
box4_place:
|
||||||
|
Value: true
|
||||||
|
box5:
|
||||||
|
Value: true
|
||||||
|
box5_place:
|
||||||
|
Value: true
|
||||||
|
box6:
|
||||||
|
Value: true
|
||||||
|
box6_place:
|
||||||
|
Value: true
|
||||||
|
flange:
|
||||||
|
Value: false
|
||||||
|
forearm_link:
|
||||||
|
Value: false
|
||||||
|
fork_gt:
|
||||||
|
Value: true
|
||||||
|
ft_frame:
|
||||||
|
Value: false
|
||||||
|
inner_rgbd_camera:
|
||||||
|
Value: false
|
||||||
|
outer_rgbd_camera:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_base_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_left_finger_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_left_finger_tip_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_left_inner_knuckle_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_left_knuckle_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_right_finger_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_right_finger_tip_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_right_inner_knuckle_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_85_right_knuckle_link:
|
||||||
|
Value: false
|
||||||
|
robotiq_grasp_point:
|
||||||
|
Value: true
|
||||||
|
shoulder_link:
|
||||||
|
Value: false
|
||||||
|
tool0:
|
||||||
|
Value: false
|
||||||
|
upper_arm_link:
|
||||||
|
Value: false
|
||||||
|
world:
|
||||||
|
Value: false
|
||||||
|
wrist_1_link:
|
||||||
|
Value: false
|
||||||
|
wrist_2_link:
|
||||||
|
Value: false
|
||||||
|
wrist_3_link:
|
||||||
|
Value: false
|
||||||
|
Marker Scale: 0.4000000059604645
|
||||||
|
Name: TF
|
||||||
|
Show Arrows: true
|
||||||
|
Show Axes: true
|
||||||
|
Show Names: false
|
||||||
|
Tree:
|
||||||
|
world:
|
||||||
|
base_link:
|
||||||
|
base:
|
||||||
|
{}
|
||||||
|
base_link_inertia:
|
||||||
|
shoulder_link:
|
||||||
|
upper_arm_link:
|
||||||
|
forearm_link:
|
||||||
|
wrist_1_link:
|
||||||
|
wrist_2_link:
|
||||||
|
wrist_3_link:
|
||||||
|
flange:
|
||||||
|
tool0:
|
||||||
|
inner_rgbd_camera:
|
||||||
|
{}
|
||||||
|
robotiq_85_base_link:
|
||||||
|
robotiq_85_left_inner_knuckle_link:
|
||||||
|
{}
|
||||||
|
robotiq_85_left_knuckle_link:
|
||||||
|
robotiq_85_left_finger_link:
|
||||||
|
robotiq_85_left_finger_tip_link:
|
||||||
|
{}
|
||||||
|
robotiq_85_right_inner_knuckle_link:
|
||||||
|
{}
|
||||||
|
robotiq_85_right_knuckle_link:
|
||||||
|
robotiq_85_right_finger_link:
|
||||||
|
robotiq_85_right_finger_tip_link:
|
||||||
|
{}
|
||||||
|
robotiq_grasp_point:
|
||||||
|
{}
|
||||||
|
ft_frame:
|
||||||
|
{}
|
||||||
|
box1:
|
||||||
|
{}
|
||||||
|
box1_place:
|
||||||
|
{}
|
||||||
|
box2:
|
||||||
|
{}
|
||||||
|
box2_place:
|
||||||
|
{}
|
||||||
|
box3:
|
||||||
|
{}
|
||||||
|
box3_place:
|
||||||
|
{}
|
||||||
|
box4:
|
||||||
|
{}
|
||||||
|
box4_place:
|
||||||
|
{}
|
||||||
|
box5:
|
||||||
|
{}
|
||||||
|
box5_place:
|
||||||
|
{}
|
||||||
|
box6:
|
||||||
|
{}
|
||||||
|
box6_place:
|
||||||
|
{}
|
||||||
|
fork_gt:
|
||||||
|
{}
|
||||||
|
outer_rgbd_camera:
|
||||||
|
{}
|
||||||
|
Update Interval: 0
|
||||||
|
Value: true
|
||||||
|
- Class: moveit_rviz_plugin/Trajectory
|
||||||
|
Color Enabled: false
|
||||||
|
Enabled: true
|
||||||
|
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
|
||||||
|
base:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
base_link_inertia:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
box1_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box2_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box3_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box4_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box5_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
box6_place:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
flange:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
forearm_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
ft_frame:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
inner_rgbd_camera:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
outer_rgbd_camera:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_base_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_finger_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_finger_tip_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_inner_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_left_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_finger_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_finger_tip_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_inner_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_85_right_knuckle_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
robotiq_grasp_point:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
shoulder_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
tool0:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
upper_arm_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
world:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
wrist_1_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
wrist_2_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
wrist_3_link:
|
||||||
|
Alpha: 1
|
||||||
|
Show Axes: false
|
||||||
|
Show Trail: false
|
||||||
|
Value: true
|
||||||
|
Loop Animation: false
|
||||||
|
Name: Trajectory
|
||||||
|
Robot Alpha: 0.5
|
||||||
|
Robot Color: 150; 50; 150
|
||||||
|
Robot Description: robot_description
|
||||||
|
Show Robot Collision: false
|
||||||
|
Show Robot Visual: true
|
||||||
|
Show Trail: false
|
||||||
|
State Display Time: 3x
|
||||||
|
Trail Step Size: 1
|
||||||
|
Trajectory Topic: /display_planned_path
|
||||||
|
Use Sim Time: false
|
||||||
|
Value: true
|
||||||
|
Enabled: true
|
||||||
|
Global Options:
|
||||||
|
Background Color: 48; 48; 48
|
||||||
|
Fixed Frame: world
|
||||||
|
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: 6.619869709014893
|
||||||
|
Enable Stereo Rendering:
|
||||||
|
Stereo Eye Separation: 0.05999999865889549
|
||||||
|
Stereo Focal Distance: 1
|
||||||
|
Swap Stereo Eyes: false
|
||||||
|
Value: false
|
||||||
|
Focal Point:
|
||||||
|
X: 0.2489434778690338
|
||||||
|
Y: -0.013962505385279655
|
||||||
|
Z: 0.13800443708896637
|
||||||
|
Focal Shape Fixed Size: true
|
||||||
|
Focal Shape Size: 0.05000000074505806
|
||||||
|
Invert Z Axis: false
|
||||||
|
Name: Current View
|
||||||
|
Near Clip Distance: 0.009999999776482582
|
||||||
|
Pitch: 0.6253980398178101
|
||||||
|
Target Frame: <Fixed Frame>
|
||||||
|
Value: Orbit (rviz)
|
||||||
|
Yaw: 1.2853964567184448
|
||||||
|
Saved: ~
|
||||||
|
Window Geometry:
|
||||||
|
Displays:
|
||||||
|
collapsed: false
|
||||||
|
Height: 1872
|
||||||
|
Hide Left Dock: false
|
||||||
|
Hide Right Dock: false
|
||||||
|
QMainWindow State: 000000ff00000000fd00000004000000000000025f000006b4fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000006b40000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032c000002170000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000007a00ffffff000000010000015f000006b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000006b40000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000082e000006b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
|
||||||
|
Selection:
|
||||||
|
collapsed: false
|
||||||
|
Tool Properties:
|
||||||
|
collapsed: false
|
||||||
|
Trajectory - Trajectory Slider:
|
||||||
|
collapsed: false
|
||||||
|
Views:
|
||||||
|
collapsed: false
|
||||||
|
Width: 3076
|
||||||
|
X: 124
|
||||||
|
Y: 54
|
||||||
|
|
@ -9,6 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
||||||
from launch_ros.actions import Node
|
from launch_ros.actions import Node
|
||||||
from launch_ros.substitutions import FindPackageShare
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
# from rbs_launch_utils.launch_common import load_yaml_abs
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
declared_arguments = []
|
declared_arguments = []
|
||||||
|
|
@ -192,7 +193,7 @@ def generate_launch_description():
|
||||||
)
|
)
|
||||||
|
|
||||||
rviz_config_file = PathJoinSubstitution(
|
rviz_config_file = PathJoinSubstitution(
|
||||||
[FindPackageShare("rbs_bringup"), "config", "fork_view.rviz"]
|
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
|
||||||
)
|
)
|
||||||
|
|
||||||
mujoco_model = PathJoinSubstitution(
|
mujoco_model = PathJoinSubstitution(
|
||||||
|
|
@ -310,6 +311,24 @@ def generate_launch_description():
|
||||||
'robot_description_semantic': robot_description_semantic_content
|
'robot_description_semantic': robot_description_semantic_content
|
||||||
}.items(),
|
}.items(),
|
||||||
condition=IfCondition(launch_moveit))
|
condition=IfCondition(launch_moveit))
|
||||||
|
|
||||||
|
skills = IncludeLaunchDescription(
|
||||||
|
PythonLaunchDescriptionSource([
|
||||||
|
PathJoinSubstitution([
|
||||||
|
FindPackageShare('rbs_skill_servers'),
|
||||||
|
'launch',
|
||||||
|
'skills.launch.py'
|
||||||
|
])
|
||||||
|
]),
|
||||||
|
launch_arguments={
|
||||||
|
'robot_description': robot_description_content,
|
||||||
|
'robot_description_semantic': robot_description_semantic_content,
|
||||||
|
'robot_description_kinematics': robot_description_kinematics,
|
||||||
|
'use_sim_time': use_sim_time,
|
||||||
|
'with_gripper_condition': with_gripper_condition,
|
||||||
|
'points_params_filepath': "gripperPositions.yaml"
|
||||||
|
}.items()
|
||||||
|
)
|
||||||
|
|
||||||
task_planner = IncludeLaunchDescription(
|
task_planner = IncludeLaunchDescription(
|
||||||
PythonLaunchDescriptionSource([
|
PythonLaunchDescriptionSource([
|
||||||
|
|
@ -343,6 +362,7 @@ def generate_launch_description():
|
||||||
control,
|
control,
|
||||||
simulation,
|
simulation,
|
||||||
moveit,
|
moveit,
|
||||||
|
skills,
|
||||||
task_planner,
|
task_planner,
|
||||||
perception
|
perception
|
||||||
]
|
]
|
||||||
|
|
|
||||||
0
rbs_bringup/rbs_launch_utils/__init__.py
Normal file
0
rbs_bringup/rbs_launch_utils/__init__.py
Normal file
84
rbs_bringup/rbs_launch_utils/launch_common.py
Normal file
84
rbs_bringup/rbs_launch_utils/launch_common.py
Normal file
|
|
@ -0,0 +1,84 @@
|
||||||
|
#!/usr/bin/env python3
|
||||||
|
|
||||||
|
# Copyright (c) 2021 PickNik, Inc.
|
||||||
|
#
|
||||||
|
# Redistribution and use in source and binary forms, with or without
|
||||||
|
# modification, are permitted provided that the following conditions are met:
|
||||||
|
#
|
||||||
|
# * Redistributions of source code must retain the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer.
|
||||||
|
#
|
||||||
|
# * Redistributions in binary form must reproduce the above copyright
|
||||||
|
# notice, this list of conditions and the following disclaimer in the
|
||||||
|
# documentation and/or other materials provided with the distribution.
|
||||||
|
#
|
||||||
|
# * Neither the name of the {copyright_holder} nor the names of its
|
||||||
|
# contributors may be used to endorse or promote products derived from
|
||||||
|
# this software without specific prior written permission.
|
||||||
|
#
|
||||||
|
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||||
|
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||||
|
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||||
|
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
|
||||||
|
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||||
|
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||||
|
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||||
|
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||||
|
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||||
|
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||||
|
# POSSIBILITY OF SUCH DAMAGE.
|
||||||
|
|
||||||
|
#
|
||||||
|
# Author: Lovro Ivanov
|
||||||
|
|
||||||
|
import math
|
||||||
|
import os
|
||||||
|
import yaml
|
||||||
|
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
|
||||||
|
|
||||||
|
def construct_angle_radians(loader, node):
|
||||||
|
"""Utility function to construct radian values from yaml."""
|
||||||
|
value = loader.construct_scalar(node)
|
||||||
|
try:
|
||||||
|
return float(value)
|
||||||
|
except SyntaxError:
|
||||||
|
raise Exception("invalid expression: %s" % value)
|
||||||
|
|
||||||
|
|
||||||
|
def construct_angle_degrees(loader, node):
|
||||||
|
"""Utility function for converting degrees into radians from yaml."""
|
||||||
|
return math.radians(construct_angle_radians(loader, node))
|
||||||
|
|
||||||
|
|
||||||
|
def load_yaml(package_name, file_path):
|
||||||
|
package_path = get_package_share_directory(package_name)
|
||||||
|
absolute_file_path = os.path.join(package_path, file_path)
|
||||||
|
|
||||||
|
try:
|
||||||
|
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
||||||
|
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
||||||
|
except Exception:
|
||||||
|
raise Exception("yaml support not available; install python-yaml")
|
||||||
|
|
||||||
|
try:
|
||||||
|
with open(absolute_file_path) as file:
|
||||||
|
return yaml.safe_load(file)
|
||||||
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def load_yaml_abs(absolute_file_path):
|
||||||
|
|
||||||
|
try:
|
||||||
|
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
||||||
|
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
||||||
|
except Exception:
|
||||||
|
raise Exception("yaml support not available; install python-yaml")
|
||||||
|
|
||||||
|
try:
|
||||||
|
with open(absolute_file_path) as file:
|
||||||
|
return yaml.safe_load(file)
|
||||||
|
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
||||||
|
return None
|
||||||
|
|
@ -4,7 +4,7 @@
|
||||||
<Sequence>
|
<Sequence>
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box1"
|
ObjectName="box1"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -19,7 +19,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
@ -27,7 +26,7 @@
|
||||||
|
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box2"
|
ObjectName="box2"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -42,7 +41,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
@ -50,7 +48,7 @@
|
||||||
|
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box3"
|
ObjectName="box3"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -65,7 +63,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
@ -73,7 +70,7 @@
|
||||||
|
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box4"
|
ObjectName="box4"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -88,7 +85,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
@ -96,7 +92,7 @@
|
||||||
|
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box5"
|
ObjectName="box5"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -111,7 +107,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
@ -119,7 +114,7 @@
|
||||||
|
|
||||||
<Action ID="GetPickPlacePoses"
|
<Action ID="GetPickPlacePoses"
|
||||||
ObjectName="box6"
|
ObjectName="box6"
|
||||||
GraspDirection = "y"
|
GraspDirection = "z"
|
||||||
PlaceDirection = "z"
|
PlaceDirection = "z"
|
||||||
GraspPose="{GraspPose}"
|
GraspPose="{GraspPose}"
|
||||||
PostGraspPose="{PostGraspPose}"
|
PostGraspPose="{PostGraspPose}"
|
||||||
|
|
@ -134,7 +129,6 @@
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
||||||
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
||||||
|
|
|
||||||
|
|
@ -6,48 +6,20 @@ from ament_index_python.packages import get_package_share_directory
|
||||||
import os
|
import os
|
||||||
import yaml
|
import yaml
|
||||||
import math
|
import math
|
||||||
|
from rbs_launch_utils.launch_common import load_yaml
|
||||||
|
|
||||||
def construct_angle_radians(loader, node):
|
|
||||||
"""Utility function to construct radian values from yaml."""
|
|
||||||
value = loader.construct_scalar(node)
|
|
||||||
try:
|
|
||||||
return float(value)
|
|
||||||
except SyntaxError:
|
|
||||||
raise Exception("invalid expression: %s" % value)
|
|
||||||
|
|
||||||
def construct_angle_degrees(loader, node):
|
|
||||||
"""Utility function for converting degrees into radians from yaml."""
|
|
||||||
return math.radians(construct_angle_radians(loader, node))
|
|
||||||
|
|
||||||
def load_yaml(package_name, file_path):
|
|
||||||
package_path = get_package_share_directory(package_name)
|
|
||||||
absolute_file_path = os.path.join(package_path, file_path)
|
|
||||||
|
|
||||||
try:
|
|
||||||
yaml.SafeLoader.add_constructor("!radians", construct_angle_radians)
|
|
||||||
yaml.SafeLoader.add_constructor("!degrees", construct_angle_degrees)
|
|
||||||
except Exception:
|
|
||||||
raise Exception("yaml support not available; install python-yaml")
|
|
||||||
|
|
||||||
try:
|
|
||||||
with open(absolute_file_path) as file:
|
|
||||||
return yaml.safe_load(file)
|
|
||||||
except OSError: # parent of IOError, OSError *and* WindowsError where available
|
|
||||||
return None
|
|
||||||
|
|
||||||
|
|
||||||
def generate_launch_description():
|
def generate_launch_description():
|
||||||
|
|
||||||
bt_config = os.path.join(
|
# bt_config = os.path.join(
|
||||||
get_package_share_directory('rbs_bt_executor'),
|
# get_package_share_directory('rbs_bt_executor'),
|
||||||
'config',
|
# 'config',
|
||||||
'params.yaml'
|
# 'params.yaml'
|
||||||
)
|
# )
|
||||||
points = load_yaml(
|
# points = load_yaml(
|
||||||
package_name="rbs_bt_executor",
|
# package_name="rbs_bt_executor",
|
||||||
file_path="config/points.yaml"
|
# file_path="config/points.yaml"
|
||||||
)
|
# )
|
||||||
|
|
||||||
gripperPoints = load_yaml(
|
gripperPoints = load_yaml(
|
||||||
package_name="rbs_bt_executor",
|
package_name="rbs_bt_executor",
|
||||||
|
|
@ -76,7 +48,6 @@ def generate_launch_description():
|
||||||
return LaunchDescription([
|
return LaunchDescription([
|
||||||
Node(
|
Node(
|
||||||
package='behavior_tree',
|
package='behavior_tree',
|
||||||
namespace='',
|
|
||||||
executable='bt_engine',
|
executable='bt_engine',
|
||||||
#prefix=['xterm -e gdb -ex run --args'],
|
#prefix=['xterm -e gdb -ex run --args'],
|
||||||
parameters=[
|
parameters=[
|
||||||
|
|
@ -88,7 +59,7 @@ def generate_launch_description():
|
||||||
"rbs_add_planning_scene_object",
|
"rbs_add_planning_scene_object",
|
||||||
"rbs_pose_estimation"
|
"rbs_pose_estimation"
|
||||||
]},
|
]},
|
||||||
gripperPoints,
|
# gripperPoints,
|
||||||
robot_description_semantic
|
robot_description_semantic
|
||||||
]
|
]
|
||||||
)
|
)
|
||||||
|
|
|
||||||
|
|
@ -9,9 +9,7 @@ using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||||
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
|
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
|
||||||
public:
|
public:
|
||||||
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
|
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
|
||||||
: BtService<GetPickPlacePoseClient>(name, config) {
|
: BtService<GetPickPlacePoseClient>(name, config) {}
|
||||||
RCLCPP_INFO(_node->get_logger(), "Start the node");
|
|
||||||
}
|
|
||||||
|
|
||||||
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
|
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
|
||||||
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
|
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
|
||||||
|
|
@ -31,24 +29,23 @@ public:
|
||||||
|
|
||||||
BT::NodeStatus handle_response(
|
BT::NodeStatus handle_response(
|
||||||
GetPickPlacePoseClient::Response::SharedPtr response) override {
|
GetPickPlacePoseClient::Response::SharedPtr response) override {
|
||||||
RCLCPP_INFO(_node->get_logger(),
|
// RCLCPP_INFO(_node->get_logger(),
|
||||||
"\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
|
// "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
|
||||||
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
|
// "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
|
||||||
"orientation.z: %f \n\t orientation.w: %f",
|
// "orientation.z: %f \n\t orientation.w: %f",
|
||||||
response->grasp[2].position.x, response->grasp[2].position.y,
|
// response->grasp[1].position.x, response->grasp[1].position.y,
|
||||||
response->grasp[2].position.z, response->grasp[2].orientation.x,
|
// response->grasp[1].position.z, response->grasp[1].orientation.x,
|
||||||
response->grasp[2].orientation.y,
|
// response->grasp[1].orientation.y,
|
||||||
response->grasp[2].orientation.z,
|
// response->grasp[1].orientation.z,
|
||||||
response->grasp[2].orientation.w);
|
// response->grasp[1].orientation.w);
|
||||||
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
|
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
|
||||||
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
|
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
|
||||||
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
|
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
|
||||||
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
|
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
|
||||||
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose",
|
|
||||||
response->grasp[2]);
|
|
||||||
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
|
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
|
||||||
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
|
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
|
||||||
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
|
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
|
||||||
return BT::NodeStatus::SUCCESS;
|
return BT::NodeStatus::SUCCESS;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
@ -60,8 +57,8 @@ public:
|
||||||
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
|
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
|
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
|
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>(
|
// TODO: change to std::vector<>
|
||||||
"PostPostGraspPose"), // TODO: change to std::vector<>
|
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
|
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
|
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
|
||||||
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
|
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
|
||||||
|
|
|
||||||
|
|
@ -124,10 +124,10 @@
|
||||||
<name>box6</name>
|
<name>box6</name>
|
||||||
<pose>-0.25 0.75 0.025 0 0 0</pose>
|
<pose>-0.25 0.75 0.025 0 0 0</pose>
|
||||||
</include>
|
</include>
|
||||||
<include>
|
<!-- <include>
|
||||||
<uri>model://fork</uri>
|
<uri>model://fork</uri>
|
||||||
<name>fork_gt</name>
|
<name>fork_gt</name>
|
||||||
<pose>-0.5 0 0.25 0 0 0</pose>
|
<pose>-0.5 0 0.25 0 0 0</pose>
|
||||||
</include>
|
</include> -->
|
||||||
</world>
|
</world>
|
||||||
</sdf>
|
</sdf>
|
||||||
|
|
|
||||||
|
|
@ -32,6 +32,8 @@ find_package(tf2_eigen REQUIRED)
|
||||||
find_package(tf2_msgs REQUIRED)
|
find_package(tf2_msgs REQUIRED)
|
||||||
find_package(tinyxml2_vendor REQUIRED)
|
find_package(tinyxml2_vendor REQUIRED)
|
||||||
find_package(TinyXML2 REQUIRED)
|
find_package(TinyXML2 REQUIRED)
|
||||||
|
find_package (Eigen3 3.3 REQUIRED)
|
||||||
|
|
||||||
|
|
||||||
# Default to Fortress
|
# Default to Fortress
|
||||||
set(SDF_VER 12)
|
set(SDF_VER 12)
|
||||||
|
|
@ -114,7 +116,7 @@ target_compile_definitions(pick_place_pose_loader
|
||||||
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
||||||
|
|
||||||
ament_target_dependencies(gripper_action_server ${deps})
|
ament_target_dependencies(gripper_action_server ${deps})
|
||||||
ament_target_dependencies(pick_place_pose_loader ${deps})
|
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3)
|
||||||
|
|
||||||
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
|
rclcpp_components_register_node(gripper_action_server PLUGIN "rbs_skill_actions::GripperControlActionServer" EXECUTABLE gripper_control_action_server)
|
||||||
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
|
rclcpp_components_register_node(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
|
||||||
|
|
@ -155,6 +157,11 @@ install(
|
||||||
DESTINATION include
|
DESTINATION include
|
||||||
)
|
)
|
||||||
|
|
||||||
|
install(
|
||||||
|
DIRECTORY launch config
|
||||||
|
DESTINATION share/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
install(
|
install(
|
||||||
TARGETS
|
TARGETS
|
||||||
move_topose_action_server
|
move_topose_action_server
|
||||||
|
|
|
||||||
|
|
@ -1,29 +1,39 @@
|
||||||
#include <rclcpp/rclcpp.hpp>
|
|
||||||
#include "rclcpp_components/register_node_macro.hpp"
|
|
||||||
#include <memory>
|
|
||||||
#include <algorithm>
|
|
||||||
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "moveit_msgs/msg/grasp.hpp"
|
#include "rclcpp_components/register_node_macro.hpp"
|
||||||
|
#include <Eigen/Core>
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <geometry_msgs/msg/detail/transform_stamped__struct.hpp>
|
||||||
|
#include <memory>
|
||||||
|
#include <rclcpp/rclcpp.hpp>
|
||||||
#include <tf2_eigen/tf2_eigen.hpp>
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
#include <tf2_ros/transform_listener.h>
|
#include <tf2_ros/transform_listener.h>
|
||||||
namespace rbs_skill_actions
|
|
||||||
{
|
namespace rbs_skill_actions {
|
||||||
class GetGraspPickPoseServer : public rclcpp::Node
|
class GetGraspPickPoseServer : public rclcpp::Node {
|
||||||
{
|
public:
|
||||||
public:
|
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
|
||||||
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
|
|
||||||
private:
|
private:
|
||||||
static std::vector<geometry_msgs::msg::Pose> collect_pose(const Eigen::Affine3d pose, const geometry_msgs::msg::Vector3 direction, const Eigen::Vector3d scale_vec);
|
static std::vector<geometry_msgs::msg::Pose>
|
||||||
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
collect_pose(const Eigen::Isometry3d &graspPose,
|
||||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
|
const geometry_msgs::msg::Vector3 &move_direction,
|
||||||
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
|
const Eigen::Vector3d &scale_vec);
|
||||||
geometry_msgs::msg::TransformStamped tf_data;
|
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
||||||
void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
|
||||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response);
|
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||||
std::vector<double> grasp_param_pose;
|
geometry_msgs::msg::TransformStamped place_pose_tf;
|
||||||
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
|
geometry_msgs::msg::TransformStamped grasp_pose_tf;
|
||||||
};
|
|
||||||
}
|
void handle_server(
|
||||||
|
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||||
|
request,
|
||||||
|
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||||
|
response);
|
||||||
|
std::vector<double> grasp_param_pose;
|
||||||
|
Eigen::Affine3d get_Affine_from_arr(const std::vector<double> pose);
|
||||||
|
};
|
||||||
|
} // namespace rbs_skill_actions
|
||||||
|
|
||||||
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);
|
RCLCPP_COMPONENTS_REGISTER_NODE(rbs_skill_actions::GetGraspPickPoseServer);
|
||||||
117
rbs_skill_servers/launch/skills.launch.py
Normal file
117
rbs_skill_servers/launch/skills.launch.py
Normal file
|
|
@ -0,0 +1,117 @@
|
||||||
|
import os
|
||||||
|
from launch import LaunchDescription, LaunchContext
|
||||||
|
from launch.actions import (
|
||||||
|
DeclareLaunchArgument,
|
||||||
|
IncludeLaunchDescription,
|
||||||
|
ExecuteProcess,
|
||||||
|
OpaqueFunction
|
||||||
|
)
|
||||||
|
from ament_index_python.packages import get_package_share_directory
|
||||||
|
from launch.conditions import IfCondition, UnlessCondition
|
||||||
|
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||||
|
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
from rbs_launch_utils.launch_common import load_yaml
|
||||||
|
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
|
||||||
|
declared_arguments = []
|
||||||
|
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument(
|
||||||
|
"robot_description",
|
||||||
|
default_value="",
|
||||||
|
description="robot description string",
|
||||||
|
)
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("robot_description_semantic", default_value="")
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("robot_description_kinematics", default_value="")
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("use_sim_time", default_value="")
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("with_gripper_condition", default_value="")
|
||||||
|
)
|
||||||
|
declared_arguments.append(
|
||||||
|
DeclareLaunchArgument("points_params_filepath", default_value="")
|
||||||
|
)
|
||||||
|
|
||||||
|
robot_description_decl = LaunchConfiguration("robot_description")
|
||||||
|
robot_description_semantic_decl = LaunchConfiguration("robot_description_semantic")
|
||||||
|
robot_description_kinematics = LaunchConfiguration("robot_description_kinematics")
|
||||||
|
use_sim_time = LaunchConfiguration("use_sim_time")
|
||||||
|
with_gripper_condition = LaunchConfiguration("with_gripper_condition")
|
||||||
|
points_params_filepath_decl = LaunchConfiguration("points_params_filepath")
|
||||||
|
robot_description = {"robot_description": robot_description_decl}
|
||||||
|
robot_description_semantic = {"robot_description_semantic": robot_description_semantic_decl}
|
||||||
|
|
||||||
|
points_params = load_yaml(
|
||||||
|
"rbs_skill_servers", "config/gripperPositions.yaml"
|
||||||
|
)
|
||||||
|
|
||||||
|
move_topose_action_server = Node(
|
||||||
|
package="rbs_skill_servers",
|
||||||
|
executable="move_topose_action_server",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_kinematics,
|
||||||
|
{"use_sim_time": use_sim_time},
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
gripper_control_node = Node(
|
||||||
|
package="rbs_skill_servers",
|
||||||
|
executable="gripper_control_action_server",
|
||||||
|
parameters= [
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_kinematics,
|
||||||
|
{"use_sim_time": use_sim_time},
|
||||||
|
],
|
||||||
|
condition=IfCondition(with_gripper_condition)
|
||||||
|
)
|
||||||
|
|
||||||
|
move_cartesian_path_action_server = Node(
|
||||||
|
package="rbs_skill_servers",
|
||||||
|
executable="move_cartesian_path_action_server",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_kinematics,
|
||||||
|
{"use_sim_time": use_sim_time},
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
move_joint_state_action_server = Node(
|
||||||
|
package="rbs_skill_servers",
|
||||||
|
executable="move_to_joint_states_action_server",
|
||||||
|
parameters=[
|
||||||
|
robot_description,
|
||||||
|
robot_description_semantic,
|
||||||
|
robot_description_kinematics,
|
||||||
|
{"use_sim_time": use_sim_time},
|
||||||
|
]
|
||||||
|
)
|
||||||
|
|
||||||
|
grasp_pose_loader = Node(
|
||||||
|
package="rbs_skill_servers",
|
||||||
|
executable="pick_place_pose_loader_service_server",
|
||||||
|
output="screen"
|
||||||
|
)
|
||||||
|
|
||||||
|
nodes_to_start =[
|
||||||
|
move_topose_action_server,
|
||||||
|
gripper_control_node,
|
||||||
|
move_cartesian_path_action_server,
|
||||||
|
move_joint_state_action_server,
|
||||||
|
grasp_pose_loader
|
||||||
|
]
|
||||||
|
|
||||||
|
return LaunchDescription(declared_arguments + nodes_to_start)
|
||||||
|
|
@ -1,110 +1,125 @@
|
||||||
//#include "rbs_skill_servers"
|
|
||||||
#include "rbs_skill_servers/pick_place_pose_loader.hpp"
|
#include "rbs_skill_servers/pick_place_pose_loader.hpp"
|
||||||
|
#include <Eigen/src/Core/Matrix.h>
|
||||||
|
#include <Eigen/src/Geometry/AngleAxis.h>
|
||||||
|
#include <Eigen/src/Geometry/Quaternion.h>
|
||||||
|
#include <Eigen/src/Geometry/Transform.h>
|
||||||
|
#include <cmath>
|
||||||
|
#include <rclcpp/logging.hpp>
|
||||||
|
#include <tf2_eigen/tf2_eigen.hpp>
|
||||||
|
// #include <Eigen/src/Core/Matrix.h>
|
||||||
|
// #include <Eigen/src/Geometry/Transform.h>
|
||||||
|
// #include <rclcpp/logging.hpp>
|
||||||
|
// #include <tf2/LinearMath/Quaternion.h>
|
||||||
|
// #include <tf2/exceptions.h>
|
||||||
|
// #include <tf2/time.h>
|
||||||
|
// #include <tf2_eigen/tf2_eigen.hpp>
|
||||||
|
|
||||||
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
|
using GetGraspPlacePoseServer = rbs_skill_actions::GetGraspPickPoseServer;
|
||||||
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
using GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||||
using namespace std::chrono_literals;
|
using namespace std::chrono_literals;
|
||||||
// rclcpp::Node::SharedPtr g_node = nullptr;
|
// rclcpp::Node::SharedPtr g_node = nullptr;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
|
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
|
||||||
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
|
: Node("grasp_place_pose_loader",
|
||||||
{
|
options.allow_undeclared_parameters(true)
|
||||||
tf_buffer_ =
|
.automatically_declare_parameters_from_overrides(true)) {
|
||||||
std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||||
tf_listener_ =
|
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||||
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
|
||||||
|
|
||||||
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service",
|
srv_ = create_service<GetGraspPlacePoseService>(
|
||||||
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2));
|
"/get_pick_place_pose_service",
|
||||||
|
std::bind(&GetGraspPickPoseServer::handle_server, this,
|
||||||
|
std::placeholders::_1, std::placeholders::_2));
|
||||||
}
|
}
|
||||||
|
|
||||||
void
|
void GetGraspPlacePoseServer::handle_server(
|
||||||
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
|
request,
|
||||||
{
|
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||||
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
|
response) {
|
||||||
try {
|
std::string object_name =
|
||||||
tf_data = tf_buffer_->lookupTransform(
|
request->object_name + "_place"; // TODO: replace with better name
|
||||||
"world", rrr.c_str(),
|
// Load place pose from TF2
|
||||||
tf2::TimePointZero);
|
try {
|
||||||
} catch (const tf2::TransformException & ex) {
|
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
|
||||||
RCLCPP_ERROR(
|
tf2::TimePointZero);
|
||||||
this->get_logger(), "Could not transform %s to %s: %s",
|
} catch (const tf2::TransformException &ex) {
|
||||||
rrr.c_str(), "world", ex.what());
|
RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
|
||||||
return;
|
object_name.c_str(), "world", ex.what());
|
||||||
}
|
return;
|
||||||
// TODO: Here need to check the parameter
|
}
|
||||||
// We can use another parameter library from PickNik
|
// Load grasp pose from TF2
|
||||||
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
|
try {
|
||||||
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
grasp_pose_tf = tf_buffer_->lookupTransform(
|
||||||
request->grasp_direction.x,
|
"world", request->object_name.c_str(), tf2::TimePointZero);
|
||||||
request->grasp_direction.y,
|
} catch (const tf2::TransformException &ex) {
|
||||||
request->grasp_direction.z);
|
RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
|
||||||
RCLCPP_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
request->object_name.c_str(), "world", ex.what());
|
||||||
request->place_direction.x,
|
}
|
||||||
request->place_direction.y,
|
|
||||||
request->place_direction.z);
|
RCLCPP_DEBUG(this->get_logger(),
|
||||||
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
|
"Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
|
||||||
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
|
grasp_pose_tf.transform.translation.x,
|
||||||
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
|
grasp_pose_tf.transform.translation.y,
|
||||||
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
|
grasp_pose_tf.transform.translation.z);
|
||||||
Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
|
// TODO: Here need to check the parameter
|
||||||
Eigen::Vector3d vec_place(0,0,0.15);
|
// We can use another parameter library from PickNik
|
||||||
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
|
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
|
||||||
response->place = collect_pose(place_pose, request->place_direction, vec_place);
|
// .as_double_array();
|
||||||
|
RCLCPP_DEBUG(this->get_logger(),
|
||||||
|
"\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||||
|
request->grasp_direction.x, request->grasp_direction.y,
|
||||||
|
request->grasp_direction.z);
|
||||||
|
RCLCPP_DEBUG(this->get_logger(),
|
||||||
|
"\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||||
|
request->place_direction.x, request->place_direction.y,
|
||||||
|
request->place_direction.z);
|
||||||
|
|
||||||
|
auto grasp_pose = tf2::transformToEigen(grasp_pose_tf);
|
||||||
|
auto place_pose = tf2::transformToEigen(place_pose_tf);
|
||||||
|
|
||||||
|
Eigen::Vector3d scale_grasp(0, 0, 0.10);
|
||||||
|
Eigen::Vector3d scale_place(0, 0, 0.15);
|
||||||
|
response->grasp =
|
||||||
|
collect_pose(grasp_pose, request->grasp_direction, scale_grasp);
|
||||||
|
response->place =
|
||||||
|
collect_pose(place_pose, request->place_direction, scale_place);
|
||||||
}
|
}
|
||||||
|
|
||||||
std::vector<geometry_msgs::msg::Pose>
|
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
|
||||||
GetGraspPlacePoseServer::collect_pose(
|
const Eigen::Isometry3d &graspPose,
|
||||||
const Eigen::Affine3d pose,
|
const geometry_msgs::msg::Vector3 &move_direction,
|
||||||
const geometry_msgs::msg::Vector3 direction,
|
const Eigen::Vector3d &scale_vec) {
|
||||||
const Eigen::Vector3d scale_vec)
|
std::vector<geometry_msgs::msg::Pose> poses{};
|
||||||
{
|
// Add GraspPose as base point
|
||||||
std::vector<geometry_msgs::msg::Pose> pose_v_;
|
Eigen::Isometry3d grasp_pose{graspPose};
|
||||||
pose_v_.push_back(tf2::toMsg(pose));
|
Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
|
||||||
Eigen::Vector3d posedir;
|
theGraspOrientation =
|
||||||
Eigen::Affine3d pose_ = pose;
|
theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||||
tf2::fromMsg(direction, posedir);
|
grasp_pose.rotate(theGraspOrientation);
|
||||||
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
|
poses.push_back(tf2::toMsg(grasp_pose));
|
||||||
Ixy.diagonal() = posedir;
|
// PreGrasp calculation
|
||||||
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
|
Eigen::Vector3d preGraspTranslation =
|
||||||
Iz.diagonal() = Eigen::Vector3d::UnitZ();
|
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
|
||||||
|
Eigen::Quaterniond preGraspOrientation(graspPose.linear());
|
||||||
|
preGraspOrientation =
|
||||||
|
preGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||||
|
Eigen::Isometry3d preGraspPose = Eigen::Isometry3d::Identity();
|
||||||
|
preGraspPose.translate(preGraspTranslation);
|
||||||
|
preGraspPose.rotate(preGraspOrientation);
|
||||||
|
poses.push_back(tf2::toMsg(preGraspPose));
|
||||||
|
|
||||||
if (posedir.cwiseEqual(Eigen::Vector3d::UnitX()).all()) // IF Direction == [1,0,0]
|
// PostGrasp calculation
|
||||||
{
|
Eigen::Vector3d postGraspTranslation =
|
||||||
std::cout << "\n TBD" << std::endl;
|
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
|
||||||
}
|
Eigen::Quaterniond postGraspOrientation(graspPose.linear());
|
||||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
|
postGraspOrientation =
|
||||||
{
|
postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||||
// Gp -- grasp point frame
|
|
||||||
pose_.pretranslate(-(Ixy * scale_vec)); // Gp-y
|
|
||||||
pose_v_.push_back(tf2::toMsg(pose_));
|
|
||||||
pose_.pretranslate(Iz * scale_vec); // Gp-y + z
|
|
||||||
pose_v_.push_back(tf2::toMsg(pose_));
|
|
||||||
pose_.pretranslate(Ixy * scale_vec); // Gp+z
|
|
||||||
pose_v_.push_back(tf2::toMsg(pose_));
|
|
||||||
}
|
|
||||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitZ()).all()) // IF Direction == [0,0,1]
|
|
||||||
{
|
|
||||||
pose_.pretranslate(Ixy * scale_vec); // Choose Pre Grasp == Post Grasp
|
|
||||||
pose_v_.push_back(tf2::toMsg(pose_)); // So calculate only Pre Grasp
|
|
||||||
}
|
|
||||||
else
|
|
||||||
{
|
|
||||||
std::cout << "\n TBD" << std::endl;
|
|
||||||
}
|
|
||||||
return pose_v_;
|
|
||||||
}
|
|
||||||
|
|
||||||
Eigen::Affine3d
|
Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
|
||||||
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
|
postGraspPose.translate(postGraspTranslation);
|
||||||
{
|
postGraspPose.rotate(postGraspOrientation);
|
||||||
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
|
poses.push_back(tf2::toMsg(postGraspPose));
|
||||||
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
|
|
||||||
Eigen::Affine3d aff;
|
return poses;
|
||||||
aff.translation() = point;
|
|
||||||
aff.linear() = quat.toRotationMatrix();
|
|
||||||
return aff;
|
|
||||||
}
|
}
|
||||||
|
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue