update MainTree
This commit is contained in:
parent
9b9c706c97
commit
7c5ba539c5
7 changed files with 152 additions and 34 deletions
|
@ -1,9 +0,0 @@
|
||||||
<?xml version='1.0' encoding='utf-8'?>
|
|
||||||
<root BTCPP_format="4">
|
|
||||||
<BehaviorTree ID="FromGraver">
|
|
||||||
<Sequence>
|
|
||||||
<SubTree ID="Grasp" action_name="from_graver.pick" robot_name="{robot_name}"/>
|
|
||||||
<SubTree ID="Place" action_name="from_graver.place" robot_name="{robot_name}"/>
|
|
||||||
</Sequence>
|
|
||||||
</BehaviorTree>
|
|
||||||
</root>
|
|
|
@ -2,16 +2,36 @@
|
||||||
<root BTCPP_format="4">
|
<root BTCPP_format="4">
|
||||||
<BehaviorTree ID="Grasp">
|
<BehaviorTree ID="Grasp">
|
||||||
<Sequence>
|
<Sequence>
|
||||||
<Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{pregrasp}" pose="{grasp}" post_pose="{postgrasp}" service_name="/get_pick_place_poses" />
|
|
||||||
|
|
||||||
<Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
|
<Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
|
||||||
<Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
|
<Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
|
||||||
<Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
|
||||||
|
<Action ID="TwistCmdWithCond" action_name="/twist_cmd" robot_name="arm" twist="0;0;-4.0;0;0;0"/>
|
||||||
|
|
||||||
|
<!-- <Action ID="MoveToPose" pose="{grasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" /> -->
|
||||||
<Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
<Action ID="MoveToPose" pose="{postgrasp}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
||||||
|
|
||||||
</Sequence>
|
</Sequence>
|
||||||
</BehaviorTree>
|
</BehaviorTree>
|
||||||
|
|
||||||
<TreeNodesModel>
|
<TreeNodesModel>
|
||||||
|
<Action ID="MoveToPose" editable="true">
|
||||||
|
<input_port name="pose"/>
|
||||||
|
<input_port name="robot_name"/>
|
||||||
|
<input_port name="duration"/>
|
||||||
|
<input_port name="action_name"/>
|
||||||
|
</Action>
|
||||||
|
<Action ID="ToggleVacuumGrippper" editable="true">
|
||||||
|
<input_port name="enable"/>
|
||||||
|
<input_port name="service_name"/>
|
||||||
|
<input_port name="robot_name"/>
|
||||||
|
</Action>
|
||||||
|
|
||||||
|
<Action ID="TwistCmdWithCond" editable="true">
|
||||||
|
<input_port name="robot_name"/>
|
||||||
|
<input_port name="twist"/>
|
||||||
|
<!-- <input_port name="duration"/> -->
|
||||||
|
<input_port name="action_name"/>
|
||||||
|
</Action>
|
||||||
</TreeNodesModel>
|
</TreeNodesModel>
|
||||||
</root>
|
</root>
|
||||||
|
|
|
@ -1,25 +1,112 @@
|
||||||
<?xml version='1.0' encoding='utf-8'?>
|
<?xml version='1.0' encoding='utf-8'?>
|
||||||
<root BTCPP_format="4">
|
<root BTCPP_format="4">
|
||||||
<BehaviorTree ID="Main">
|
<BehaviorTree ID="Main">
|
||||||
<Repeat num_cycles="5">
|
|
||||||
<Sequence>
|
<Sequence>
|
||||||
<!-- <Condition ID="IsInPose" topic_name="/ee_pose"/> -->
|
<!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
|
||||||
<SubTree ID="ToGraver" robot_name="arm" />
|
|
||||||
|
|
||||||
<Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
|
<Fallback>
|
||||||
service_name="/get_named_pose" />
|
<Repeat num_cycles="1">
|
||||||
<Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
|
<Sequence>
|
||||||
action_name="/mtp_jtc" />
|
<!-- Preparing -->
|
||||||
|
|
||||||
<Delay delay_msec="2000">
|
<Action ID="GetGraspPlacePose" action_name="to_graver.pick"
|
||||||
<SubTree ID="FromGraver" robot_name="arm" />
|
pre_pose="{pregrasp}"
|
||||||
</Delay>
|
pose="{grasp}"
|
||||||
<!-- <Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;0.0" duration="3" -->
|
post_pose="{postgrasp}"
|
||||||
<!-- robot_name="arm" action_name="/mtjs_jtc" /> -->
|
service_name="/get_pick_place_poses" />
|
||||||
|
|
||||||
|
<Action ID="GetGraspPlacePose" action_name="to_graver.place"
|
||||||
|
pre_pose="{preplace}"
|
||||||
|
pose="{place}"
|
||||||
|
post_pose="{postplace}"
|
||||||
|
service_name="/get_pick_place_poses" />
|
||||||
|
|
||||||
|
<!-- Grasp shildik and place to the Graver -->
|
||||||
|
<SubTree ID="PickAndPlace"
|
||||||
|
robot_name="arm"
|
||||||
|
preplace="{preplace}"
|
||||||
|
place="{place}"
|
||||||
|
postplace="{postplace}"
|
||||||
|
pregrasp="{pregrasp}"
|
||||||
|
grasp="{grasp}"
|
||||||
|
postgrasp="{postgrasp}"
|
||||||
|
/>
|
||||||
|
|
||||||
|
|
||||||
|
<!-- Waiting position -->
|
||||||
|
<Action ID="GetNamedPose" pose_name="waiting" output_pose="{named_pose}"
|
||||||
|
service_name="/get_named_pose" />
|
||||||
|
<Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
|
||||||
|
action_name="/mtp_jtc" />
|
||||||
|
|
||||||
|
<!-- Send task and wait Graver -->
|
||||||
|
|
||||||
|
<!-- <Action ID="SendTask" /> -->
|
||||||
|
|
||||||
|
<!-- Pick shildik from graver-->
|
||||||
|
|
||||||
|
<Action ID="GetGraspPlacePose" action_name="from_graver.pick"
|
||||||
|
pre_pose="{pregrasp}"
|
||||||
|
pose="{grasp}"
|
||||||
|
post_pose="{postgrasp}"
|
||||||
|
service_name="/get_pick_place_poses" />
|
||||||
|
|
||||||
|
<Action ID="GetGraspPlacePose" action_name="from_graver.place"
|
||||||
|
pre_pose="{preplace}"
|
||||||
|
pose="{place}"
|
||||||
|
post_pose="{postplace}"
|
||||||
|
service_name="/get_pick_place_poses" />
|
||||||
|
|
||||||
|
<!-- Grasp shildik from Graver and move to the box-->
|
||||||
|
<SubTree ID="PickAndPlace"
|
||||||
|
robot_name="arm"
|
||||||
|
preplace="{preplace}"
|
||||||
|
place="{place}"
|
||||||
|
postplace="{postplace}"
|
||||||
|
pregrasp="{pregrasp}"
|
||||||
|
grasp="{grasp}"
|
||||||
|
postgrasp="{postgrasp}"
|
||||||
|
/>
|
||||||
|
|
||||||
|
<!-- Ending -->
|
||||||
|
</Sequence>
|
||||||
|
</Repeat>
|
||||||
|
|
||||||
|
<Sequence>
|
||||||
|
<!-- <Action ID="NotifyOperator" /> -->
|
||||||
|
<AlwaysFailure />
|
||||||
|
</Sequence>
|
||||||
|
</Fallback>
|
||||||
</Sequence>
|
</Sequence>
|
||||||
</Repeat>
|
|
||||||
</BehaviorTree>
|
</BehaviorTree>
|
||||||
|
|
||||||
|
<TreeNodesModel>
|
||||||
|
<Action ID="GetGraspPlacePose" editable="true">
|
||||||
|
<input_port name="action_name"/>
|
||||||
|
<input_port name="pre_pose"/>
|
||||||
|
<input_port name="pose"/>
|
||||||
|
<input_port name="post_pose"/>
|
||||||
|
<input_port name="service_name"/>
|
||||||
|
</Action>
|
||||||
|
|
||||||
|
<Action ID="GetNamedPose" editable="true">
|
||||||
|
<input_port name="pose_name"/>
|
||||||
|
<input_port name="output_pose"/>
|
||||||
|
<input_port name="service_name"/>
|
||||||
|
</Action>
|
||||||
|
|
||||||
|
<Action ID="NotifyOperator" editable="true" />
|
||||||
|
|
||||||
|
<Action ID="GetTask" editable="true">
|
||||||
|
<input_port name="name"/>
|
||||||
|
<input_port name="data"/>
|
||||||
|
<input_port name="num_cycles"/>
|
||||||
|
</Action>
|
||||||
|
|
||||||
|
<Action ID="SendTask" editable="true" />
|
||||||
|
</TreeNodesModel>
|
||||||
</root>
|
</root>
|
||||||
|
|
||||||
|
|
||||||
|
|
9
rbs_mill_assist/bt/xmls/PickAndPlace.xml
Normal file
9
rbs_mill_assist/bt/xmls/PickAndPlace.xml
Normal file
|
@ -0,0 +1,9 @@
|
||||||
|
<?xml version='1.0' encoding='utf-8'?>
|
||||||
|
<root BTCPP_format="4">
|
||||||
|
<BehaviorTree ID="PickAndPlace">
|
||||||
|
<Sequence>
|
||||||
|
<SubTree ID="Grasp" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{grasp}" robot_name="{robot_name}"/>
|
||||||
|
<SubTree ID="Place" preplace="{preplace}" place="{place}" postplace="{postplace}" robot_name="{robot_name}"/>
|
||||||
|
</Sequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
</root>
|
|
@ -2,7 +2,6 @@
|
||||||
<root BTCPP_format="4">
|
<root BTCPP_format="4">
|
||||||
<BehaviorTree ID="Place">
|
<BehaviorTree ID="Place">
|
||||||
<Sequence>
|
<Sequence>
|
||||||
<Action ID="GetGraspPlacePose" action_name="{action_name}" pre_pose="{preplace}" pose="{place}" post_pose="{postplace}" service_name="/get_pick_place_poses" />
|
|
||||||
|
|
||||||
<Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
|
<Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_moveit" />
|
||||||
<Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
<Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
||||||
|
@ -13,5 +12,16 @@
|
||||||
</BehaviorTree>
|
</BehaviorTree>
|
||||||
|
|
||||||
<TreeNodesModel>
|
<TreeNodesModel>
|
||||||
|
<Action ID="MoveToPose" editable="true">
|
||||||
|
<input_port name="pose"/>
|
||||||
|
<input_port name="robot_name"/>
|
||||||
|
<input_port name="duration"/>
|
||||||
|
<input_port name="action_name"/>
|
||||||
|
</Action>
|
||||||
|
<Action ID="ToggleVacuumGrippper" editable="true">
|
||||||
|
<input_port name="enable"/>
|
||||||
|
<input_port name="service_name"/>
|
||||||
|
<input_port name="robot_name"/>
|
||||||
|
</Action>
|
||||||
</TreeNodesModel>
|
</TreeNodesModel>
|
||||||
</root>
|
</root>
|
||||||
|
|
|
@ -1,9 +0,0 @@
|
||||||
<?xml version='1.0' encoding='utf-8'?>
|
|
||||||
<root BTCPP_format="4">
|
|
||||||
<BehaviorTree ID="ToGraver">
|
|
||||||
<Sequence>
|
|
||||||
<SubTree ID="Grasp" action_name="to_graver.pick" robot_name="{robot_name}"/>
|
|
||||||
<SubTree ID="Place" action_name="to_graver.place" robot_name="{robot_name}"/>
|
|
||||||
</Sequence>
|
|
||||||
</BehaviorTree>
|
|
||||||
</root>
|
|
10
rbs_mill_assist/bt/xmls/btmill.btproj
Normal file
10
rbs_mill_assist/bt/xmls/btmill.btproj
Normal file
|
@ -0,0 +1,10 @@
|
||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<root BTCPP_format="4" project_name="Project">
|
||||||
|
<include path="Grasp.xml"/>
|
||||||
|
<include path="Place.xml"/>
|
||||||
|
<include path="MainTree.xml"/>
|
||||||
|
<include path="PickAndPlace.xml"/>
|
||||||
|
<!-- Description of Node Models (used by Groot) -->
|
||||||
|
<TreeNodesModel>
|
||||||
|
</TreeNodesModel>
|
||||||
|
</root>
|
Loading…
Add table
Add a link
Reference in a new issue