robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

add skill launch, refactor pick-place pose loader

Browse source
This commit is contained in:
Ilya Uraev 2023-11-30 13:47:33 +03:00
parent 034e172f62
commit d72c06efea
14 changed files with 1032 additions and 191 deletions
Download patch file Download diff file Expand all files Collapse all files

18
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -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" />

73
rbs_bt_executor/config/gripperPositions.yaml
View file

@ -1,73 +0,0 @@
box1:
PreGraspPose: [0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box2:
PreGraspPose: [0.0, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
box3:
PreGraspPose: [-0.25, 0.55, 0.6515, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.6515, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.67, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.67, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.055, 0.46, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.055, 0.6, 1.0, 0.0, 0.0, 0.0]
box4:
PreGraspPose: [0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box5:
PreGraspPose: [0.0, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [0.0, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [0.0, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [0.0, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, -0.0275, 0.49, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, -0.0275, 0.6, 1.0, 0.0, 0.0, 0.0]
box6:
PreGraspPose: [-0.25, 0.55, 0.3015, -0.707, 0.0, 0.0, 0.707]
GraspPose: [-0.25, 0.655, 0.3015, -0.707, 0.0, 0.0, 0.707]
PostGraspPose: [-0.25, 0.655, 0.32, -0.707, 0.0, 0.0, 0.707]
PostGraspPose2: [-0.25, 0.5, 0.32, -0.707, 0.0, 0.0, 0.707]
PrePlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
PlacePose: [-0.4500, 0.0, 0.56, 1.0, 0.0, 0.0, 0.0]
PostPlacePose: [-0.4500, 0.0, 0.6, 1.0, 0.0, 0.0, 0.0]
pre_place_joint_states:
- 0.11030024152330242
- 0.8306162497371689
- -1.7680363334650195
- -0.6262361658856159
- 1.4713289344704141
- -0.11066274809566562
scene_objects:
- box1
- box2
- box3
- box4
- box5
- box6
- last

51
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -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
]
)

33
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -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"),