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

calibrate mode for pose estimation

Browse source
This commit is contained in:
shalenikol 2023-12-01 10:26:21 +03:00
parent 4decc40c88
commit cb3082ca44
8 changed files with 94008 additions and 220628 deletions
Download patch file Download diff file Expand all files Collapse all files

17
rbs_bt_executor/bt_trees/6Dpe_test.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="fork"
ReqKind = "configure"
server_name="/image_sub2/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName="None"
ReqKind = "activate"
server_name="/image_sub2/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

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="fork"
GraspDirection = "y" ReqKind = "configure"
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>

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

1
rbs_perception/config/pose_estimation_config.json
View file

@ -5,6 +5,7 @@
"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,

101
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -4,6 +4,8 @@
ROS 2 program for 6D Pose Estimation ROS 2 program for 6D Pose Estimation
@shalenikol release 0.3 @shalenikol release 0.3
!!! version for testing 17.11.2023
""" """
from typing import Optional from typing import Optional
@ -12,6 +14,9 @@ import shutil
import json import json
import tempfile import tempfile
from pathlib import Path from pathlib import Path
#import pinocchio as pin
#from pinocchio.rpy import matrixToRpy, rpyToMatrix, rotate
import transforms3d as t3d
import rclpy import rclpy
from rclpy.lifecycle import Node from rclpy.lifecycle import Node
@ -20,6 +25,7 @@ from rclpy.lifecycle import State
from rclpy.lifecycle import TransitionCallbackReturn from rclpy.lifecycle import TransitionCallbackReturn
from rclpy.timer import Timer from rclpy.timer import Timer
#from tf.transformations import quaternion_from_euler
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
@ -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,8 +74,9 @@ 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.megapose_model = None self.megapose_model = None
self.nodeName = node_name self.nodeName = node_name
@ -77,6 +86,7 @@ class PoseEstimator(Node):
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 = 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"
@ -97,28 +107,56 @@ class PoseEstimator(Node):
self.objMeshFile = "" self.objMeshFile = ""
self.objPath = "" self.objPath = ""
def get_camera_pose(self) -> Pose:
# <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
p = Pose()
p.position.x = -2.
p.position.y = -0.55
p.position.z = 1.44
#R = rpyToMatrix([0, 0.5, -2.4])
#q = pin.Quaternion()
#q = t3d.euler.euler2quat(0., 0.5, -2.4)
p.orientation.w = 0.9524 #q[0]
p.orientation.x = -0.0476 #q[1]
p.orientation.y = 0.213 #q[2]
p.orientation.z = 0.213 #q[3]
return p
def publish(self): def publish(self):
"""Publish a new message when enabled.""" """Publish a new message when enabled."""
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]
"""
msgQ = 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] * self.mesh_scale
p.position.y = t[1] * self.mesh_scale
p.position.z = t[2] * self.mesh_scale
"""
p.position.x = t[0] p.position.x = t[0]
p.position.y = t[1] p.position.y = t[1]
p.position.z = t[2] p.position.z = t[2]
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 +166,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 +189,19 @@ 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] == "{": # json string
y = json.loads(str_param)
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"]
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 +229,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)
if gtpose == None:
# Preload Megapose model # Preload Megapose model
from megapose.scripts.run_inference_on_example import ModelPreload
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis") 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
@ -254,9 +308,9 @@ class PoseEstimator(Node):
self._res = [data.height, data.width] self._res = [data.height, data.width]
k_ = data.k k_ = data.k
self._K = [ self._K = [
[k_[0], k_[1], k_[2]], [k_[0]*2.0, k_[1], data.width / 2.0], # k_[2]], #
[k_[3], k_[4], k_[5]], [k_[3], k_[4]*2.0, data.height / 2.0], # k_[5]], #
[k_[6], k_[7], k_[8]] [k_[6], k_[7], k_[8]] #self.mesh_scale]
] ]
tPath = self.objPath / "inputs" tPath = self.objPath / "inputs"
@ -264,7 +318,7 @@ class PoseEstimator(Node):
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": [288,170,392,253] #[2,2,self._res[1]-4,self._res[0]-4]
} }
data = [] data = []
data.append(output_json_dict) data.append(output_json_dict)
@ -318,16 +372,17 @@ class PoseEstimator(Node):
cv2.imwrite(str(self.objPath / "image_rgb.png"), current_frame) cv2.imwrite(str(self.objPath / "image_rgb.png"), current_frame)
self._image_cnt += 1 self._image_cnt += 1
if self.megapose_model:
# 6D pose estimation # 6D pose estimation
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}") self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis") #run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
from megapose.scripts.run_inference_on_example import run_inference_rbs
run_inference_rbs(self.megapose_model) run_inference_rbs(self.megapose_model)
data = self.load_result(self.objPath) data = self.load_result(self.objPath)
if data[0] == "[": if data[0] == "[":
y = json.loads(data)[0] y = json.loads(data)[0]
self._pose = y["TWO"] self._pose = y["TWO"]
self.get_logger().info(f"megapose: end {self._image_cnt}") self.get_logger().info(f"megapose: end {self._image_cnt}")
def main(): def main():

2
rbs_simulation/worlds/mir.sdf
View file

@ -127,7 +127,7 @@
<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 0.25 0 0 1.57</pose>
</include> </include>
</world> </world>
</sdf> </sdf>