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

Pose Estimation Skill Megapose6D fixes

Browse source
This commit is contained in:
shalenikol 2023-12-22 10:50:41 +00:00 • committed by Igor Brylyov
parent ce03b17c48
commit 324ae82c20
9 changed files with 94102 additions and 220619 deletions
Download patch file Download diff file Expand all files Collapse all files

158
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -1,155 +1,17 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<root main_tree_to_execute="MainTree"> <root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="MainTree"> <BehaviorTree ID="PoseEstimation">
<Sequence> <Sequence>
<Action ID="GetPickPlacePoses" <Action ID="PoseEstimation"
ObjectName="box1" ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
GraspDirection = "y" ReqKind = "calibrate"
PlaceDirection = "z" server_name="/pose_estimation/change_state"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/> 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="PoseEstimation"
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" /> ObjectName=""
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" /> ReqKind = "activate"
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" /> server_name="/pose_estimation/change_state"
<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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/> 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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose_name"/>
</Action>
</TreeNodesModel>
</root> </root>

155
rbs_bt_executor/bt_trees/test_tree_main.xml Normal file
View file

@ -0,0 +1,155 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
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="{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" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose_name"/>
</Action>
</TreeNodesModel>
</root>

6
rbs_bt_executor/bt_trees/6Dpe_test.xml → rbs_bt_executor/bt_trees/test_tree_pe_configure.xml
View file

@ -5,12 +5,12 @@
<Action ID="PoseEstimation" <Action ID="PoseEstimation"
ObjectName="fork" ObjectName="fork"
ReqKind = "configure" ReqKind = "configure"
server_name="/image_sub2/change_state" server_name="/pose_estimation/change_state"
server_timeout="1000"/> server_timeout="1000"/>
<Action ID="PoseEstimation" <Action ID="PoseEstimation"
ObjectName="None" ObjectName=""
ReqKind = "activate" ReqKind = "activate"
server_name="/image_sub2/change_state" server_name="/pose_estimation/change_state"
server_timeout="1000"/> server_timeout="1000"/>
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>

17
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -22,12 +22,10 @@ public:
while (!_set_params_client->wait_for_service(1s)) { while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) { if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(), RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
"Interrupted while waiting for the service. Exiting.");
break; break;
} }
RCLCPP_WARN(_node->get_logger(), RCLCPP_WARN(_node->get_logger(), "service not available, waiting again...");
"service not available, waiting again...");
} }
_model_name = getInput<std::string>("ObjectName").value(); _model_name = getInput<std::string>("ObjectName").value();
@ -71,6 +69,9 @@ private:
if (req_type == "configure") { if (req_type == "configure") {
set_mesh_param(); set_mesh_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE; transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "calibrate") {
set_str_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "activate") { } else if (req_type == "activate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE; transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
} else if (req_type == "deactivate") { } else if (req_type == "deactivate") {
@ -78,6 +79,7 @@ private:
} else if (req_type == "cleanup") { } else if (req_type == "cleanup") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP; transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
} }
//calibrate
translation.set__id(transition_id_); translation.set__id(transition_id_);
return translation; return translation;
} }
@ -91,6 +93,13 @@ private:
return model_path; return model_path;
} }
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(), "Set string parameter: <" + _model_name + ">");
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
}
void set_mesh_param() { void set_mesh_param() {
auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception"); auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name); _model_path = build_model_path(_model_name, _package_name);

314163
rbs_perception/config/fork.ply
View file

File diff suppressed because it is too large Load diff

2
rbs_perception/config/pose_estimation_config.json
View file

@ -1,10 +1,12 @@
{ {
"nodeName":"pose_estimation", "nodeName":"pose_estimation",
"cameraLink":"outer_rgbd_camera",
"topicImage":"/outer_rgbd_camera/image", "topicImage":"/outer_rgbd_camera/image",
"topicCameraInfo":"/outer_rgbd_camera/camera_info", "topicCameraInfo":"/outer_rgbd_camera/camera_info",
"topicDepth":"/outer_rgbd_camera/depth_image", "topicDepth":"/outer_rgbd_camera/depth_image",
"publishDelay": 3.3, "publishDelay": 3.3,
"tf2_send_pose": 1, "tf2_send_pose": 1,
"mesh_scale": 0.001,
"camera_info": { "camera_info": {
"fx": 924.855, "fx": 924.855,
"fy": 923.076, "fy": 923.076,

5
rbs_perception/config/str_param.json Normal file
View file

@ -0,0 +1,5 @@
{
"mesh_path":"/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/fork.ply",
"gtpose_0":[1.3,0.0,0.0,0.0,0.0,0.0],
"darknet_path_0":"/home/shalenikol/test_detection"
}

208
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -8,10 +8,12 @@
from typing import Optional from typing import Optional
import os import os
import subprocess
import shutil import shutil
import json import json
import tempfile import tempfile
from pathlib import Path from pathlib import Path
import transforms3d as t3d
import rclpy import rclpy
from rclpy.lifecycle import Node from rclpy.lifecycle import Node
@ -23,10 +25,12 @@ from rclpy.timer import Timer
from ament_index_python.packages import get_package_share_directory from ament_index_python.packages import get_package_share_directory
from sensor_msgs.msg import Image, CameraInfo from sensor_msgs.msg import Image, CameraInfo
from geometry_msgs.msg import Pose, TransformStamped from geometry_msgs.msg import Pose, TransformStamped
from tf2_ros import TransformBroadcaster from tf2_ros import TransformBroadcaster, TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images
import cv2 # OpenCV library import cv2 # OpenCV library
from megapose.scripts.run_inference_on_example import ModelPreload, run_inference_rbs #, run_inference
class PoseEstimator(Node): class PoseEstimator(Node):
"""Our lifecycle node.""" """Our lifecycle node."""
@ -41,6 +45,8 @@ class PoseEstimator(Node):
for name, val in y.items(): for name, val in y.items():
if name == "nodeName": if name == "nodeName":
self.nodeName = val self.nodeName = val
elif name == "cameraLink":
self.camera_link = val
elif name == "topicImage": elif name == "topicImage":
self.topicImage = val self.topicImage = val
elif name == "topicCameraInfo": elif name == "topicCameraInfo":
@ -53,6 +59,8 @@ class PoseEstimator(Node):
self.topicSrv = val self.topicSrv = val
elif name == "tf2_send_pose": elif name == "tf2_send_pose":
self.tf2_send_pose = val self.tf2_send_pose = val
elif name == "mesh_scale":
self.mesh_scale = val
def __init__(self, node_name, **kwargs): def __init__(self, node_name, **kwargs):
"""Construct the node.""" """Construct the node."""
@ -66,17 +74,22 @@ class PoseEstimator(Node):
self._is_camerainfo = False self._is_camerainfo = False
self._K = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]] self._K = [[0., 0., 0.], [0., 0., 0.], [0., 0., 0.]]
self._res = [0, 0] self._res = [0, 0]
self._pose = [[1., 0., 0., 0.], [0., 0., 0.]] self._pose = [[1., 0., 0., 0.], [0., 0., 0.]] # pose in format "TWO" (megapose)
self.tf2_send_pose = 0 self.tf2_send_pose = 0
self.mesh_scale = 1.0
self.quat_rotate_xyz = [0.5, 0.5, -0.5, -0.5]
self.megapose_model = None self.megapose_model = None
self.darknet_path = ""
self.nodeName = node_name self.nodeName = node_name
self.camera_link = "outer_rgbd_camera"
self.topicImage = "/outer_rgb_camera/image" self.topicImage = "/outer_rgb_camera/image"
self.topicCameraInfo = "/outer_rgb_camera/camera_info" self.topicCameraInfo = "/outer_rgb_camera/camera_info"
self.topicDepth = "/outer_rgbd_camera/depth_image" self.topicDepth = "/outer_rgbd_camera/depth_image"
self.publishDelay = 2.0 self.publishDelay = 2.0
self.topicSrv = self.nodeName + "/detect6Dpose" self.topicSrv = self.nodeName + "/detect6Dpose"
self._InitService() self._InitService()
self.camera_pose = Pose() #self.get_camera_pose()
self.tmpdir = tempfile.gettempdir() self.tmpdir = tempfile.gettempdir()
self.mytemppath = Path(self.tmpdir) / "rbs_per" self.mytemppath = Path(self.tmpdir) / "rbs_per"
@ -88,6 +101,8 @@ class PoseEstimator(Node):
super().__init__(self.nodeName, **kwargs) super().__init__(self.nodeName, **kwargs)
self.declare_parameter("mesh_path", rclpy.Parameter.Type.STRING) self.declare_parameter("mesh_path", rclpy.Parameter.Type.STRING)
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# Initialize the transform broadcaster # Initialize the transform broadcaster
self.tf_broadcaster = TransformBroadcaster(self) self.tf_broadcaster = TransformBroadcaster(self)
# Used to convert between ROS and OpenCV images # Used to convert between ROS and OpenCV images
@ -102,23 +117,37 @@ class PoseEstimator(Node):
self._count += 1 self._count += 1
if self._pub is not None and self._pub.is_activated: if self._pub is not None and self._pub.is_activated:
# опубликуем результат оценки позы # опубликуем результат оценки позы # publish pose estimation result
q = self._pose[0] q = self._pose[0]
t = self._pose[1] t = self._pose[1]
# transform from View space in World space
# rotate
q_mir = t3d.quaternions.qmult(self.quat_rotate_xyz, q)
# translate (xyz = +z-x-y)
t_mir = [t[2], -t[0], -t[1]]
cam_p = self.camera_pose.position
#cam_Q = self.camera_pose.orientation
# q_rot = [msgQ.w, msgQ.x, msgQ.y, msgQ.z]
# q = t3d.quaternions.qinverse(q) #q[0] = -q[0]
# q_mir = t3d.quaternions.qmult(q_rot,q)
#q_mir = q
p = Pose() p = Pose()
p.position.x = t[0] p.position.x = t_mir[0] + cam_p.x
p.position.y = t[1] p.position.y = t_mir[1] + cam_p.y
p.position.z = t[2] p.position.z = t_mir[2] + cam_p.z
p.orientation.w = q[0] p.orientation.w = q_mir[0]
p.orientation.x = q[1] p.orientation.x = q_mir[1]
p.orientation.y = q[2] p.orientation.y = q_mir[2]
p.orientation.z = q[3] p.orientation.z = q_mir[3]
self._pub.publish(p) self._pub.publish(p)
if self.tf2_send_pose: if self.tf2_send_pose:
self.tf_obj_pose(self._pose) self.tf_obj_pose(p.position,q_mir) #(self._pose)
def tf_obj_pose(self,pose): def tf_obj_pose(self, tr, q):
""" """
Передача позиции объекта в tf2 Передача позиции объекта в tf2
""" """
@ -128,16 +157,16 @@ class PoseEstimator(Node):
t.header.frame_id = 'world' t.header.frame_id = 'world'
t.child_frame_id = self.objName t.child_frame_id = self.objName
# coordinates # coordinates
tr = pose[1] #tr = pose[1]
t.transform.translation.x = tr[0] t.transform.translation.x = tr.x #[0]
t.transform.translation.y = tr[1] t.transform.translation.y = tr.y #[1]
t.transform.translation.z = tr[2] t.transform.translation.z = tr.z #[2]
# rotation # rotation
q = pose[0] #q = pose[0]
t.transform.rotation.x = q[1] # 0 t.transform.rotation.w = q[0]
t.transform.rotation.y = q[2] # 1 t.transform.rotation.x = q[1]
t.transform.rotation.z = q[3] # 2 t.transform.rotation.y = q[2]
t.transform.rotation.w = q[0] # 3 t.transform.rotation.z = q[3]
# Send the transformation # Send the transformation
self.tf_broadcaster.sendTransform(t) self.tf_broadcaster.sendTransform(t)
@ -151,7 +180,30 @@ class PoseEstimator(Node):
TransitionCallbackReturn.FAILURE transitions to "unconfigured". TransitionCallbackReturn.FAILURE transitions to "unconfigured".
TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing" TransitionCallbackReturn.ERROR or any uncaught exceptions to "errorprocessing"
""" """
mesh_path = self.get_parameter("mesh_path").get_parameter_value().string_value gtpose = None
str_param = self.get_parameter("mesh_path").get_parameter_value().string_value
if str_param[0] == "!": # filename json
json_file = str_param[1:]
if not os.path.isfile(json_file):
self.get_logger().info("not JSon '"+ json_file +"'")
return TransitionCallbackReturn.FAILURE
str_param = Path(json_file).read_text()
try:
y = json.loads(str_param)
except json.JSONDecodeError as e:
self.get_logger().info(f"JSon error: {e}")
return TransitionCallbackReturn.FAILURE
if "mesh_path" not in y:
self.get_logger().info("JSon 'mesh_path' not set")
return TransitionCallbackReturn.FAILURE
mesh_path = y["mesh_path"]
if "gtpose" in y:
gtpose = y["gtpose"]
if "darknet_path" in y:
self.darknet_path = y["darknet_path"]
else:
mesh_path = str_param
if not os.path.isfile(mesh_path): if not os.path.isfile(mesh_path):
self.get_logger().info("Parameter 'mesh_path' not set") self.get_logger().info("Parameter 'mesh_path' not set")
return TransitionCallbackReturn.FAILURE return TransitionCallbackReturn.FAILURE
@ -179,8 +231,12 @@ class PoseEstimator(Node):
self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 10) self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 10)
self._timer = self.create_timer(self.publishDelay, self.publish) self._timer = self.create_timer(self.publishDelay, self.publish)
# Preload Megapose model if gtpose == None:
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis") # Preload Megapose model
from megapose.scripts.run_inference_on_example import ModelPreload
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
else:
self._pose = [t3d.euler.euler2quat(gtpose[3], gtpose[4], gtpose[5]), gtpose[:3]]
self.get_logger().info('on_configure() is called.') self.get_logger().info('on_configure() is called.')
return TransitionCallbackReturn.SUCCESS return TransitionCallbackReturn.SUCCESS
@ -258,17 +314,23 @@ class PoseEstimator(Node):
[k_[3], k_[4], k_[5]], [k_[3], k_[4], k_[5]],
[k_[6], k_[7], k_[8]] [k_[6], k_[7], k_[8]]
] ]
# self._K = [
# [k_[0]*2.0, k_[1], data.width / 2.0], # k_[2]], #
# [k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
# [k_[6], k_[7], k_[8]] #self.mesh_scale]
# ]
# *** for test
tPath = self.objPath / "inputs" tPath = self.objPath / "inputs"
#{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
output_fn = tPath / "object_data.json" output_fn = tPath / "object_data.json"
output_json_dict = { output_json_dict = {
"label": self.objName, "label": self.objName,
"bbox_modal": [2,2,self._res[1]-4,self._res[0]-4] "bbox_modal": [168,173,260,252] # 0.2 0 0 -> [288,170,392,253]
} }
data = [] data = []
data.append(output_json_dict) data.append(output_json_dict)
output_fn.write_text(json.dumps(data)) output_fn.write_text(json.dumps(data))
# ***
#{"K": [[25.0, 0.0, 8.65], [0.0, 25.0, 6.5], [0.0, 0.0, 1.0]], "resolution": [480, 640]} #{"K": [[25.0, 0.0, 8.65], [0.0, 25.0, 6.5], [0.0, 0.0, 1.0]], "resolution": [480, 640]}
output_fn = self.objPath / "camera_data.json" output_fn = self.objPath / "camera_data.json"
@ -287,8 +349,6 @@ class PoseEstimator(Node):
""" """
Depth image callback function. Depth image callback function.
""" """
#self.get_logger().info("Receiving depth image")
# Convert ROS Image message to OpenCV image # Convert ROS Image message to OpenCV image
current_frame = self.br.imgmsg_to_cv2(data) current_frame = self.br.imgmsg_to_cv2(data)
@ -302,6 +362,43 @@ class PoseEstimator(Node):
else: else:
data = "No result file: '" + str(f) + "'" data = "No result file: '" + str(f) + "'"
return data return data
def rel2bbox(self, rel_coord):
bb_w = rel_coord["width"]
bb_h = rel_coord["height"]
x = int((rel_coord["center_x"] - bb_w/2.) * self._res[1])
y = int((rel_coord["center_y"] - bb_h/2.) * self._res[0])
w = int(bb_w * self._res[1])
h = int(bb_h * self._res[0])
return [x,y,w,h]
def yolo2megapose(self, res_json: str, path_to: Path) -> bool:
str_param = Path(res_json).read_text()
y = json.loads(str_param)[0]
conf = 0.5 # threshold of detection
found_coord = None
for detections in y["objects"]:
if detections["name"] == self.objName:
c_conf = detections["confidence"]
if c_conf > conf:
conf = c_conf
found_coord = detections["relative_coordinates"]
if found_coord:
bbox = self.rel2bbox(found_coord)
else:
bbox = [2, 2, self._res[1]-4, self._res[0]-4]
#tPath = path_to / "inputs"
output_fn = path_to / "inputs/object_data.json"
output_json_dict = {
"label": self.objName,
"bbox_modal": bbox #[288,170,392,253]
}
data = []
data.append(output_json_dict)
output_fn.write_text(json.dumps(data))
return bool(found_coord)
def listener_callback(self, data): def listener_callback(self, data):
""" """
@ -310,25 +407,58 @@ class PoseEstimator(Node):
if not self._is_camerainfo: if not self._is_camerainfo:
self.get_logger().warning("No data from CameraInfo") self.get_logger().warning("No data from CameraInfo")
return return
# get camera pose
camera_name = self.camera_link #self.topicImage.split('/')[1]
try:
t = self.tf_buffer.lookup_transform("world", camera_name, rclpy.time.Time())
except TransformException as ex:
self.get_logger().info(f'Could not transform {camera_name} to world: {ex}')
return
self.camera_pose.position.x = t.transform.translation.x
self.camera_pose.position.y = t.transform.translation.y
self.camera_pose.position.z = t.transform.translation.z
self.camera_pose.orientation.w = t.transform.rotation.w
self.camera_pose.orientation.x = t.transform.rotation.x
self.camera_pose.orientation.y = t.transform.rotation.y
self.camera_pose.orientation.z = t.transform.rotation.z
# Convert ROS Image message to OpenCV image # Convert ROS Image message to OpenCV image
current_frame = self.br.imgmsg_to_cv2(data) current_frame = self.br.imgmsg_to_cv2(data)
# Save image for Megapose # Save image for Megapose
cv2.imwrite(str(self.objPath / "image_rgb.png"), current_frame) frame_im = str(self.objPath / "image_rgb.png")
cv2.imwrite(frame_im, current_frame)
self._image_cnt += 1 self._image_cnt += 1
# 6D pose estimation detected = False
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}") darknet_bin = os.path.join(self.darknet_path, "darknet")
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis") if os.path.isfile(darknet_bin):
run_inference_rbs(self.megapose_model) # object detection (YoloV4 - darknet)
self.get_logger().info(f"darknet: begin {self._image_cnt}")
result_json = str(self.objPath / "res.json")
data = self.load_result(self.objPath) subprocess.run([darknet_bin, "detector", "test",
if data[0] == "[": "yolov4_objs2.data",
y = json.loads(data)[0] "yolov4_objs2.cfg",
self._pose = y["TWO"] "yolov4.weights",
"-dont_show", "-ext_output", # -thresh 0.5
"-out", result_json, frame_im],
cwd = self.darknet_path #, stdout = subprocess.PIPE
)
detected = self.yolo2megapose(result_json, self.objPath)
self.get_logger().info(f"darknet: end {self._image_cnt}")
self.get_logger().info(f"megapose: end {self._image_cnt}") if detected and self.megapose_model:
# 6D pose estimation
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
from megapose.scripts.run_inference_on_example import run_inference_rbs
run_inference_rbs(self.megapose_model)
data = self.load_result(self.objPath)
if data[0] == "[":
y = json.loads(data)[0]
self._pose = y["TWO"]
self.get_logger().info(f"megapose: end {self._image_cnt}")
def main(): def main():
rclpy.init() rclpy.init()

7
rbs_simulation/worlds/mir.sdf
View file

@ -127,7 +127,12 @@
<include> <include>
<uri>model://fork</uri> <uri>model://fork</uri>
<name>fork_gt</name> <name>fork_gt</name>
<pose>-0.5 0 0.25 0 0 0</pose> <pose>1.5 0.3 0.25 0 0 0</pose>
</include> </include>
<!-->
<uri>model://king</uri>
<name>king1</name>
<pose>1.5 0.2 0.25 0 0 0</pose>
</-->
</world> </world>
</sdf> </sdf>