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}
|
||||
)
|
||||
|
||||
# Install Python modules
|
||||
ament_python_install_package(rbs_launch_utils)
|
||||
ament_python_install_module(rbs_launch_utils/launch_common.py)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# 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: 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
|
||||
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_ros.actions import Node
|
||||
from launch_ros.substitutions import FindPackageShare
|
||||
# from rbs_launch_utils.launch_common import load_yaml_abs
|
||||
|
||||
def generate_launch_description():
|
||||
declared_arguments = []
|
||||
|
@ -192,7 +193,7 @@ def generate_launch_description():
|
|||
)
|
||||
|
||||
rviz_config_file = PathJoinSubstitution(
|
||||
[FindPackageShare("rbs_bringup"), "config", "fork_view.rviz"]
|
||||
[FindPackageShare("rbs_bringup"), "config", "rbs.rviz"]
|
||||
)
|
||||
|
||||
mujoco_model = PathJoinSubstitution(
|
||||
|
@ -311,6 +312,24 @@ def generate_launch_description():
|
|||
}.items(),
|
||||
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(
|
||||
PythonLaunchDescriptionSource([
|
||||
PathJoinSubstitution([
|
||||
|
@ -343,6 +362,7 @@ def generate_launch_description():
|
|||
control,
|
||||
simulation,
|
||||
moveit,
|
||||
skills,
|
||||
task_planner,
|
||||
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>
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box1"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
@ -27,7 +26,7 @@
|
|||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box2"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
@ -50,7 +48,7 @@
|
|||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box3"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
@ -73,7 +70,7 @@
|
|||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box4"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
@ -96,7 +92,7 @@
|
|||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box5"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
@ -119,7 +114,7 @@
|
|||
|
||||
<Action ID="GetPickPlacePoses"
|
||||
ObjectName="box6"
|
||||
GraspDirection = "y"
|
||||
GraspDirection = "z"
|
||||
PlaceDirection = "z"
|
||||
GraspPose="{GraspPose}"
|
||||
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="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="{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="{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" />
|
||||
|
|
|
@ -6,48 +6,20 @@ from ament_index_python.packages import get_package_share_directory
|
|||
import os
|
||||
import yaml
|
||||
import math
|
||||
|
||||
|
||||
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
|
||||
from rbs_launch_utils.launch_common import load_yaml
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
|
||||
bt_config = os.path.join(
|
||||
get_package_share_directory('rbs_bt_executor'),
|
||||
'config',
|
||||
'params.yaml'
|
||||
)
|
||||
points = load_yaml(
|
||||
package_name="rbs_bt_executor",
|
||||
file_path="config/points.yaml"
|
||||
)
|
||||
# bt_config = os.path.join(
|
||||
# get_package_share_directory('rbs_bt_executor'),
|
||||
# 'config',
|
||||
# 'params.yaml'
|
||||
# )
|
||||
# points = load_yaml(
|
||||
# package_name="rbs_bt_executor",
|
||||
# file_path="config/points.yaml"
|
||||
# )
|
||||
|
||||
gripperPoints = load_yaml(
|
||||
package_name="rbs_bt_executor",
|
||||
|
@ -76,7 +48,6 @@ def generate_launch_description():
|
|||
return LaunchDescription([
|
||||
Node(
|
||||
package='behavior_tree',
|
||||
namespace='',
|
||||
executable='bt_engine',
|
||||
#prefix=['xterm -e gdb -ex run --args'],
|
||||
parameters=[
|
||||
|
@ -88,7 +59,7 @@ def generate_launch_description():
|
|||
"rbs_add_planning_scene_object",
|
||||
"rbs_pose_estimation"
|
||||
]},
|
||||
gripperPoints,
|
||||
# gripperPoints,
|
||||
robot_description_semantic
|
||||
]
|
||||
)
|
||||
|
|
|
@ -9,9 +9,7 @@ using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
|||
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
|
||||
public:
|
||||
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
|
||||
: BtService<GetPickPlacePoseClient>(name, config) {
|
||||
RCLCPP_INFO(_node->get_logger(), "Start the node");
|
||||
}
|
||||
: BtService<GetPickPlacePoseClient>(name, config) {}
|
||||
|
||||
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
|
||||
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
|
||||
|
@ -31,24 +29,23 @@ public:
|
|||
|
||||
BT::NodeStatus handle_response(
|
||||
GetPickPlacePoseClient::Response::SharedPtr response) override {
|
||||
RCLCPP_INFO(_node->get_logger(),
|
||||
"\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 "
|
||||
"orientation.z: %f \n\t orientation.w: %f",
|
||||
response->grasp[2].position.x, response->grasp[2].position.y,
|
||||
response->grasp[2].position.z, response->grasp[2].orientation.x,
|
||||
response->grasp[2].orientation.y,
|
||||
response->grasp[2].orientation.z,
|
||||
response->grasp[2].orientation.w);
|
||||
// RCLCPP_INFO(_node->get_logger(),
|
||||
// "\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 "
|
||||
// "orientation.z: %f \n\t orientation.w: %f",
|
||||
// response->grasp[1].position.x, response->grasp[1].position.y,
|
||||
// response->grasp[1].position.z, response->grasp[1].orientation.x,
|
||||
// response->grasp[1].orientation.y,
|
||||
// response->grasp[1].orientation.z,
|
||||
// response->grasp[1].orientation.w);
|
||||
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
|
||||
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
|
||||
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
|
||||
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
|
||||
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose",
|
||||
response->grasp[2]);
|
||||
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
|
||||
|
||||
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
|
||||
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;
|
||||
}
|
||||
|
||||
|
@ -60,8 +57,8 @@ public:
|
|||
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>(
|
||||
"PostPostGraspPose"), // TODO: change to std::vector<>
|
||||
// TODO: change to std::vector<>
|
||||
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
|
||||
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
|
||||
|
|
|
@ -124,10 +124,10 @@
|
|||
<name>box6</name>
|
||||
<pose>-0.25 0.75 0.025 0 0 0</pose>
|
||||
</include>
|
||||
<include>
|
||||
<!-- <include>
|
||||
<uri>model://fork</uri>
|
||||
<name>fork_gt</name>
|
||||
<pose>-0.5 0 0.25 0 0 0</pose>
|
||||
</include>
|
||||
</include> -->
|
||||
</world>
|
||||
</sdf>
|
||||
|
|
|
@ -32,6 +32,8 @@ find_package(tf2_eigen REQUIRED)
|
|||
find_package(tf2_msgs REQUIRED)
|
||||
find_package(tinyxml2_vendor REQUIRED)
|
||||
find_package(TinyXML2 REQUIRED)
|
||||
find_package (Eigen3 3.3 REQUIRED)
|
||||
|
||||
|
||||
# Default to Fortress
|
||||
set(SDF_VER 12)
|
||||
|
@ -114,7 +116,7 @@ target_compile_definitions(pick_place_pose_loader
|
|||
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
|
||||
|
||||
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(pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer" EXECUTABLE pick_place_pose_loader_service_server)
|
||||
|
@ -155,6 +157,11 @@ install(
|
|||
DESTINATION include
|
||||
)
|
||||
|
||||
install(
|
||||
DIRECTORY launch config
|
||||
DESTINATION share/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
install(
|
||||
TARGETS
|
||||
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 "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_ros/transform_listener.h>
|
||||
namespace rbs_skill_actions
|
||||
{
|
||||
class GetGraspPickPoseServer : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
|
||||
namespace rbs_skill_actions {
|
||||
class GetGraspPickPoseServer : public rclcpp::Node {
|
||||
public:
|
||||
explicit GetGraspPickPoseServer(rclcpp::NodeOptions options);
|
||||
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);
|
||||
|
||||
private:
|
||||
static std::vector<geometry_msgs::msg::Pose>
|
||||
collect_pose(const Eigen::Isometry3d &graspPose,
|
||||
const geometry_msgs::msg::Vector3 &move_direction,
|
||||
const Eigen::Vector3d &scale_vec);
|
||||
rclcpp::Service<rbs_skill_interfaces::srv::GetPickPlacePoses>::SharedPtr srv_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_{nullptr};
|
||||
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
geometry_msgs::msg::TransformStamped tf_data;
|
||||
void handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response);
|
||||
geometry_msgs::msg::TransformStamped place_pose_tf;
|
||||
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);
|
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 <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 GetGraspPlacePoseService = rbs_skill_interfaces::srv::GetPickPlacePoses;
|
||||
using namespace std::chrono_literals;
|
||||
// rclcpp::Node::SharedPtr g_node = nullptr;
|
||||
|
||||
|
||||
|
||||
GetGraspPlacePoseServer::GetGraspPickPoseServer(rclcpp::NodeOptions options)
|
||||
: Node("grasp_place_pose_loader", options.allow_undeclared_parameters(true).automatically_declare_parameters_from_overrides(true))
|
||||
{
|
||||
tf_buffer_ =
|
||||
std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_listener_ =
|
||||
std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
: Node("grasp_place_pose_loader",
|
||||
options.allow_undeclared_parameters(true)
|
||||
.automatically_declare_parameters_from_overrides(true)) {
|
||||
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(this->get_clock());
|
||||
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||
|
||||
srv_ = create_service<GetGraspPlacePoseService>("/get_pick_place_pose_service",
|
||||
std::bind(&GetGraspPickPoseServer::handle_server, this, std::placeholders::_1, std::placeholders::_2));
|
||||
srv_ = create_service<GetGraspPlacePoseService>(
|
||||
"/get_pick_place_pose_service",
|
||||
std::bind(&GetGraspPickPoseServer::handle_server, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
void
|
||||
GetGraspPlacePoseServer::handle_server(const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr response)
|
||||
{
|
||||
std::string rrr = "ASSEMBLE_" + request->object_name; // TODO: replace with better name
|
||||
void GetGraspPlacePoseServer::handle_server(
|
||||
const rbs_skill_interfaces::srv::GetPickPlacePoses::Request::SharedPtr
|
||||
request,
|
||||
rbs_skill_interfaces::srv::GetPickPlacePoses::Response::SharedPtr
|
||||
response) {
|
||||
std::string object_name =
|
||||
request->object_name + "_place"; // TODO: replace with better name
|
||||
// Load place pose from TF2
|
||||
try {
|
||||
tf_data = tf_buffer_->lookupTransform(
|
||||
"world", rrr.c_str(),
|
||||
place_pose_tf = tf_buffer_->lookupTransform("world", object_name.c_str(),
|
||||
tf2::TimePointZero);
|
||||
} catch (const tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Could not transform %s to %s: %s",
|
||||
rrr.c_str(), "world", ex.what());
|
||||
} catch (const tf2::TransformException &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transform %s to %s: %s",
|
||||
object_name.c_str(), "world", ex.what());
|
||||
return;
|
||||
}
|
||||
// Load grasp pose from TF2
|
||||
try {
|
||||
grasp_pose_tf = tf_buffer_->lookupTransform(
|
||||
"world", request->object_name.c_str(), tf2::TimePointZero);
|
||||
} catch (const tf2::TransformException &ex) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Could not transforms %s to %s: %s",
|
||||
request->object_name.c_str(), "world", ex.what());
|
||||
}
|
||||
|
||||
RCLCPP_DEBUG(this->get_logger(),
|
||||
"Grasp pose from tf \n\tx=%f\n\ty=%f\n\tz=%f",
|
||||
grasp_pose_tf.transform.translation.x,
|
||||
grasp_pose_tf.transform.translation.y,
|
||||
grasp_pose_tf.transform.translation.z);
|
||||
// TODO: Here need to check the parameter
|
||||
// We can use another parameter library from PickNik
|
||||
grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose").as_double_array();
|
||||
RCLCPP_INFO(this->get_logger(), "\nGrasp direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->grasp_direction.x,
|
||||
request->grasp_direction.y,
|
||||
// grasp_param_pose = this->get_parameter(request->object_name + ".GraspPose")
|
||||
// .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_INFO(this->get_logger(), "\nPlace direction \n x: %.2f \n y: %.2f \n z: %.2f",
|
||||
request->place_direction.x,
|
||||
request->place_direction.y,
|
||||
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);
|
||||
Eigen::Affine3d grasp_pose = get_Affine_from_arr(grasp_param_pose);
|
||||
Eigen::Affine3d place_pose = tf2::transformToEigen(tf_data);
|
||||
//std::cout << "grasp_point = " << std::endl << grasp_pose.translation() << std::endl << grasp_pose.linear() << std::endl;
|
||||
//std::cout << "place_pose = " << std::endl << place_pose.translation() << std::endl << place_pose.linear() << std::endl;
|
||||
Eigen::Vector3d vec_grasp(0.15,0.15,0.02);
|
||||
Eigen::Vector3d vec_place(0,0,0.15);
|
||||
response->grasp = collect_pose(grasp_pose, request->grasp_direction, vec_grasp);
|
||||
response->place = collect_pose(place_pose, request->place_direction, vec_place);
|
||||
|
||||
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>
|
||||
GetGraspPlacePoseServer::collect_pose(
|
||||
const Eigen::Affine3d pose,
|
||||
const geometry_msgs::msg::Vector3 direction,
|
||||
const Eigen::Vector3d scale_vec)
|
||||
{
|
||||
std::vector<geometry_msgs::msg::Pose> pose_v_;
|
||||
pose_v_.push_back(tf2::toMsg(pose));
|
||||
Eigen::Vector3d posedir;
|
||||
Eigen::Affine3d pose_ = pose;
|
||||
tf2::fromMsg(direction, posedir);
|
||||
Eigen::Matrix3d Ixy = Eigen::Matrix3d::Zero();//posedir * posedir.transpose();
|
||||
Ixy.diagonal() = posedir;
|
||||
Eigen::Matrix3d Iz = Eigen::Matrix3d::Zero();
|
||||
Iz.diagonal() = Eigen::Vector3d::UnitZ();
|
||||
std::vector<geometry_msgs::msg::Pose> GetGraspPlacePoseServer::collect_pose(
|
||||
const Eigen::Isometry3d &graspPose,
|
||||
const geometry_msgs::msg::Vector3 &move_direction,
|
||||
const Eigen::Vector3d &scale_vec) {
|
||||
std::vector<geometry_msgs::msg::Pose> poses{};
|
||||
// Add GraspPose as base point
|
||||
Eigen::Isometry3d grasp_pose{graspPose};
|
||||
Eigen::Quaterniond theGraspOrientation(grasp_pose.linear());
|
||||
theGraspOrientation =
|
||||
theGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||
grasp_pose.rotate(theGraspOrientation);
|
||||
poses.push_back(tf2::toMsg(grasp_pose));
|
||||
// PreGrasp calculation
|
||||
Eigen::Vector3d preGraspTranslation =
|
||||
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]
|
||||
{
|
||||
std::cout << "\n TBD" << std::endl;
|
||||
}
|
||||
else if (posedir.cwiseEqual(Eigen::Vector3d::UnitY()).all()) // IF Direction == [0,1,0]
|
||||
{
|
||||
// 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_;
|
||||
}
|
||||
// PostGrasp calculation
|
||||
Eigen::Vector3d postGraspTranslation =
|
||||
static_cast<Eigen::Vector3d>(graspPose.translation()) + scale_vec;
|
||||
Eigen::Quaterniond postGraspOrientation(graspPose.linear());
|
||||
postGraspOrientation =
|
||||
postGraspOrientation * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitX());
|
||||
|
||||
Eigen::Affine3d
|
||||
GetGraspPlacePoseServer::get_Affine_from_arr(const std::vector<double> pose)
|
||||
{
|
||||
Eigen::Vector3d point(std::vector<double>(pose.begin(), pose.begin()+3).data());
|
||||
Eigen::Quaterniond quat(std::vector<double>(pose.begin()+3, pose.end()).data());
|
||||
Eigen::Affine3d aff;
|
||||
aff.translation() = point;
|
||||
aff.linear() = quat.toRotationMatrix();
|
||||
return aff;
|
||||
Eigen::Isometry3d postGraspPose = Eigen::Isometry3d::Identity();
|
||||
postGraspPose.translate(postGraspTranslation);
|
||||
postGraspPose.rotate(postGraspOrientation);
|
||||
poses.push_back(tf2::toMsg(postGraspPose));
|
||||
|
||||
return poses;
|
||||
}
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue