69 lines
No EOL
5.2 KiB
XML
69 lines
No EOL
5.2 KiB
XML
<?xml version="1.0"?>
|
|
<root main_tree_to_execute="MainTree">
|
|
<BehaviorTree ID="MainTree">
|
|
<Sequence>
|
|
<Action ID="GetPickPlacePoses"
|
|
ObjectName="box1"
|
|
GraspPose="{GraspPose}"
|
|
PostGraspPose="{PostGraspPose}"
|
|
PostGraspPose2="{PostGraspPose2}"
|
|
PreGraspPose="{PreGraspPose}"
|
|
PlacePose="{PlacePose}"
|
|
PrePlacePose="{PrePlacePose}"
|
|
PostPlacePose="{PostPlacePose}"
|
|
PrePlaceJointState="{PrePlaceJointState}"
|
|
NextObject = "{NextObject}"
|
|
server_name="/get_pick_place_pose_service"
|
|
server_timeout="1000"/>
|
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" 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="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="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" 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="AddPlanningSceneObject" ObjectName="{NextObject}" pose="{PlacePose}" server_name="/add_planing_scene_object" server_timeout="1000"/> -->
|
|
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
|
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
<Repeat num_cycles="5">
|
|
<Sequence>
|
|
<Action ID="GetPickPlacePoses"
|
|
ObjectName="{NextObject}"
|
|
GraspPose="{GraspPose}"
|
|
PostGraspPose="{PostGraspPose}"
|
|
PostGraspPose2="{PostGraspPose2}"
|
|
PreGraspPose="{PreGraspPose}"
|
|
PlacePose="{PlacePose}"
|
|
PrePlacePose="{PrePlacePose}"
|
|
PostPlacePose="{PostPlacePose}"
|
|
PrePlaceJointState="{PrePlaceJointState}"
|
|
NextObject = "{NextObject}"
|
|
server_name="/get_pick_place_pose_service"
|
|
server_timeout="1000"/>
|
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" 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="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="{PostGraspPose2}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
<!-- <Action ID="MoveToJointState" robot_name="ur_manipulator_gripper" PrePlaceJointState="{PrePlaceJointState}" server_name="/move_to_joint_states" 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" />
|
|
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
|
|
</Sequence>
|
|
</Repeat>
|
|
|
|
|
|
|
|
|
|
</Sequence>
|
|
</BehaviorTree>
|
|
|
|
<TreeNodesModel>
|
|
<Action ID="MoveToPose">
|
|
<input_port name="robot_name"/>
|
|
<input_port name="pose_name"/>
|
|
</Action>
|
|
</TreeNodesModel>
|
|
</root> |