refactor: add tree without moveit
This commit is contained in:
parent
fc95407d87
commit
f8a4b329e1
11 changed files with 244 additions and 1 deletions
|
|
@ -10,4 +10,5 @@ bt_action_server:
|
|||
- rbs_bt_executor/bt_plugins
|
||||
|
||||
behavior_trees:
|
||||
- rbs_mill_assist/xmls
|
||||
- rbs_mill_assist/xmls/moveit
|
||||
- rbs_mill_assist/xmls/no_moveit
|
||||
|
|
|
|||
34
rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml
Normal file
34
rbs_mill_assist/bt/xmls/no_moveit/FromGraver.xml
Normal file
|
|
@ -0,0 +1,34 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="FromGraver">
|
||||
<Sequence>
|
||||
<Action ID="GetNamedPose" pose_name="to_graver_prom" relative_to="base_link"
|
||||
output_pose="{a}"
|
||||
service_name="/get_tf_frame_pose" />
|
||||
|
||||
<!-- <Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3" -->
|
||||
<!-- try_plan_untill_success="false" action_name="/mtp_jtc_cart" /> -->
|
||||
|
||||
<SubTree ID="GraspNoMoveit" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}"
|
||||
robot_name="{robot_name}" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
|
||||
try_plan_untill_success="false" action_name="/mtp_jtc" />
|
||||
|
||||
<Action ID="GetNamedPose" pose_name="preparing_grasp_slot" relative_to="base_link" output_pose="{b}"
|
||||
service_name="/get_tf_frame_pose" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{b}" robot_name="{robot_name}" duration="4"
|
||||
action_name="/mtp_jtc" />
|
||||
|
||||
<Action ID="GetNamedPose" pose_name="preparing_place_slot" relative_to="base_link" output_pose="{c}"
|
||||
service_name="/get_tf_frame_pose" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{c}" robot_name="{robot_name}" duration="4"
|
||||
action_name="/mtp_jtc" />
|
||||
|
||||
<SubTree ID="PlaceNoMoveit" preplace="{preplace}" place="{place}" postplace="{postplace}"
|
||||
robot_name="{robot_name}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
36
rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml
Normal file
36
rbs_mill_assist/bt/xmls/no_moveit/GraspNoMoveit.xml
Normal file
|
|
@ -0,0 +1,36 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="GraspNoMoveit">
|
||||
<Sequence>
|
||||
<Action ID="MoveToPose" pose="{pregrasp}" robot_name="{robot_name}" duration="3" try_plan_untill_success="true" action_name="/mtp_jtc_cart" />
|
||||
<Action ID="ToggleVacuumGrippper" enable="true" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
|
||||
|
||||
<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" />
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<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>
|
||||
</root>
|
||||
116
rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml
Normal file
116
rbs_mill_assist/bt/xmls/no_moveit/MainTree.xml
Normal file
|
|
@ -0,0 +1,116 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="MainNoMoveit">
|
||||
|
||||
<Sequence>
|
||||
<!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
|
||||
|
||||
<Fallback>
|
||||
<Repeat num_cycles="1">
|
||||
<Sequence>
|
||||
<!-- Preparing -->
|
||||
|
||||
<Action ID="GetSlotGraspPoses" from_pose="slot_1"
|
||||
relative_to="base_link"
|
||||
pregrasp="{pregrasp}"
|
||||
grasp="{grasp}"
|
||||
postgrasp="{postgrasp}"
|
||||
service_name="/get_grasping_pose" />
|
||||
|
||||
<Action ID="GetPlacePoses" to_pose="conductor"
|
||||
relative_to="base_link"
|
||||
preplace="{preplace}"
|
||||
place="{place}"
|
||||
postplace="{postplace}"
|
||||
service_name="/get_grasping_pose" />
|
||||
|
||||
<!-- Grasp shildik and place to the Graver -->
|
||||
<SubTree ID="ToGraver"
|
||||
robot_name="arm"
|
||||
preplace="{preplace}"
|
||||
place="{place}"
|
||||
postplace="{postplace}"
|
||||
pregrasp="{pregrasp}"
|
||||
grasp="{grasp}"
|
||||
postgrasp="{postgrasp}"
|
||||
/>
|
||||
|
||||
|
||||
<!-- Waiting position -->
|
||||
<Action ID="GetNamedPose" pose_name="waiting" relative_to="base_link" output_pose="{named_pose}"
|
||||
service_name="/get_tf_frame_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="GetSlotGraspPoses" from_pose="conductor"
|
||||
relative_to="base_link"
|
||||
pregrasp="{pregrasp}"
|
||||
grasp="{grasp}"
|
||||
postgrasp="{postgrasp}"
|
||||
service_name="/get_grasping_pose" />
|
||||
|
||||
<Action ID="GetPlacePoses" to_pose="bunker"
|
||||
relative_to="base_link"
|
||||
preplace="{preplace}"
|
||||
place="{place}"
|
||||
postplace="{postplace}"
|
||||
service_name="/get_grasping_pose" />
|
||||
|
||||
<!-- Grasp shildik from Graver and move to the box-->
|
||||
<SubTree ID="FromGraver"
|
||||
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>
|
||||
|
||||
</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>
|
||||
|
||||
|
||||
28
rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml
Normal file
28
rbs_mill_assist/bt/xmls/no_moveit/PlaceNoMoveit.xml
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="PlaceNoMoveit">
|
||||
<Sequence>
|
||||
|
||||
|
||||
<Action ID="MoveToPose" pose="{preplace}" robot_name="{robot_name}" duration="4" action_name="/mtp_jtc" />
|
||||
<Action ID="MoveToPose" pose="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
||||
<Action ID="ToggleVacuumGrippper" enable="false" service_name="/gz_ros2_vacuum_gripper_plugin/toggle" />
|
||||
<Action ID="MoveToPose" pose="{postplace}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
|
||||
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
||||
<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>
|
||||
</root>
|
||||
28
rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml
Normal file
28
rbs_mill_assist/bt/xmls/no_moveit/ToGraver.xml
Normal file
|
|
@ -0,0 +1,28 @@
|
|||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="ToGraver">
|
||||
<Sequence>
|
||||
<Action ID="GetNamedPose" pose_name="preparing_grasp_slot" relative_to="base_link"
|
||||
output_pose="{a}"
|
||||
service_name="/get_tf_frame_pose" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
|
||||
try_plan_untill_success="false" action_name="/mtp_jtc_cart" />
|
||||
|
||||
<SubTree ID="GraspNoMoveit" pregrasp="{pregrasp}" grasp="{grasp}" postgrasp="{postgrasp}"
|
||||
robot_name="{robot_name}" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{a}" robot_name="{robot_name}" duration="3"
|
||||
try_plan_untill_success="false" action_name="/mtp_jtc_cart" />
|
||||
|
||||
<Action ID="GetNamedPose" pose_name="to_graver_prom" relative_to="base_link" output_pose="{b}"
|
||||
service_name="/get_tf_frame_pose" />
|
||||
|
||||
<Action ID="MoveToPose" pose="{b}" robot_name="{robot_name}" duration="4"
|
||||
action_name="/mtp_jtc" />
|
||||
|
||||
<SubTree ID="PlaceNoMoveit" preplace="{preplace}" place="{place}" postplace="{postplace}"
|
||||
robot_name="{robot_name}" />
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
Loading…
Add table
Add a link
Reference in a new issue