robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

update MainTree

Browse source
This commit is contained in:
Ilya Uraev 2025-03-17 17:31:36 +03:00
parent 9b9c706c97
commit 7c5ba539c5
7 changed files with 152 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rbs_mill_assist/bt/xmls/FromGraver.xml
View file

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

24
rbs_mill_assist/bt/xmls/Grasp.xml
View file

@ -2,16 +2,36 @@
<root BTCPP_format="4">
<BehaviorTree ID="Grasp">
<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="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" />
</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>

113
rbs_mill_assist/bt/xmls/MainTree.xml
View file

@ -1,25 +1,112 @@
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="Main">
<Repeat num_cycles="5">
<Sequence>
<!-- <Condition ID="IsInPose" topic_name="/ee_pose"/> -->
<SubTree ID="ToGraver" robot_name="arm" />
<!-- <Action ID="GetTask" name="" data="{task}" num_cycles="{num_cycles}"/> -->
<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" />
<Fallback>
<Repeat num_cycles="1">
<Sequence>
<!-- Preparing -->
<Delay delay_msec="2000">
<SubTree ID="FromGraver" robot_name="arm" />
</Delay>
<!-- <Action ID="MoveToJointState" joint_state="0.0;0.85;1.0;0.0;1.0;0.0" duration="3" -->
<!-- robot_name="arm" action_name="/mtjs_jtc" /> -->
<Action ID="GetGraspPlacePose" action_name="to_graver.pick"
pre_pose="{pregrasp}"
pose="{grasp}"
post_pose="{postgrasp}"
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>
</Repeat>
</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>

9
rbs_mill_assist/bt/xmls/PickAndPlace.xml Normal file
View 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>

12
rbs_mill_assist/bt/xmls/Place.xml
View file

@ -2,7 +2,6 @@
<root BTCPP_format="4">
<BehaviorTree ID="Place">
<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="{place}" robot_name="{robot_name}" duration="2" action_name="/mtp_jtc_cart" />
@ -13,5 +12,16 @@
</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>

9
rbs_mill_assist/bt/xmls/ToGraver.xml
View file

@ -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
View 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>