Pose Estimation Skill Megapose6D fixes
This commit is contained in:
parent
ce03b17c48
commit
324ae82c20
9 changed files with 94102 additions and 220619 deletions
|
|
@ -1,155 +1,17 @@
|
|||
<?xml version="1.0"?>
|
||||
<root main_tree_to_execute="MainTree">
|
||||
<BehaviorTree ID="MainTree">
|
||||
<root main_tree_to_execute="PoseEstimation">
|
||||
<BehaviorTree ID="PoseEstimation">
|
||||
<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"
|
||||
<Action ID="PoseEstimation"
|
||||
ObjectName="!/home/shalenikol/robossembler_ws/src/robossembler-ros2/rbs_perception/config/str_param.json"
|
||||
ReqKind = "calibrate"
|
||||
server_name="/pose_estimation/change_state"
|
||||
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"
|
||||
<Action ID="PoseEstimation"
|
||||
ObjectName=""
|
||||
ReqKind = "activate"
|
||||
server_name="/pose_estimation/change_state"
|
||||
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>
|
||||
155
rbs_bt_executor/bt_trees/test_tree_main.xml
Normal file
155
rbs_bt_executor/bt_trees/test_tree_main.xml
Normal 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>
|
||||
|
|
@ -5,12 +5,12 @@
|
|||
<Action ID="PoseEstimation"
|
||||
ObjectName="fork"
|
||||
ReqKind = "configure"
|
||||
server_name="/image_sub2/change_state"
|
||||
server_name="/pose_estimation/change_state"
|
||||
server_timeout="1000"/>
|
||||
<Action ID="PoseEstimation"
|
||||
ObjectName="None"
|
||||
ObjectName=""
|
||||
ReqKind = "activate"
|
||||
server_name="/image_sub2/change_state"
|
||||
server_name="/pose_estimation/change_state"
|
||||
server_timeout="1000"/>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
|
|
@ -22,12 +22,10 @@ public:
|
|||
|
||||
while (!_set_params_client->wait_for_service(1s)) {
|
||||
if (!rclcpp::ok()) {
|
||||
RCLCPP_ERROR(_node->get_logger(),
|
||||
"Interrupted while waiting for the service. Exiting.");
|
||||
RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
|
||||
break;
|
||||
}
|
||||
RCLCPP_WARN(_node->get_logger(),
|
||||
"service not available, waiting again...");
|
||||
RCLCPP_WARN(_node->get_logger(), "service not available, waiting again...");
|
||||
}
|
||||
|
||||
_model_name = getInput<std::string>("ObjectName").value();
|
||||
|
|
@ -71,6 +69,9 @@ private:
|
|||
if (req_type == "configure") {
|
||||
set_mesh_param();
|
||||
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") {
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
|
||||
} else if (req_type == "deactivate") {
|
||||
|
|
@ -78,6 +79,7 @@ private:
|
|||
} else if (req_type == "cleanup") {
|
||||
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
|
||||
}
|
||||
//calibrate
|
||||
translation.set__id(transition_id_);
|
||||
return translation;
|
||||
}
|
||||
|
|
@ -91,6 +93,13 @@ private:
|
|||
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() {
|
||||
auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception");
|
||||
_model_path = build_model_path(_model_name, _package_name);
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -1,10 +1,12 @@
|
|||
{
|
||||
"nodeName":"pose_estimation",
|
||||
"cameraLink":"outer_rgbd_camera",
|
||||
"topicImage":"/outer_rgbd_camera/image",
|
||||
"topicCameraInfo":"/outer_rgbd_camera/camera_info",
|
||||
"topicDepth":"/outer_rgbd_camera/depth_image",
|
||||
"publishDelay": 3.3,
|
||||
"tf2_send_pose": 1,
|
||||
"mesh_scale": 0.001,
|
||||
"camera_info": {
|
||||
"fx": 924.855,
|
||||
"fy": 923.076,
|
||||
|
|
|
|||
5
rbs_perception/config/str_param.json
Normal file
5
rbs_perception/config/str_param.json
Normal 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"
|
||||
}
|
||||
|
|
@ -8,10 +8,12 @@
|
|||
|
||||
from typing import Optional
|
||||
import os
|
||||
import subprocess
|
||||
import shutil
|
||||
import json
|
||||
import tempfile
|
||||
from pathlib import Path
|
||||
import transforms3d as t3d
|
||||
|
||||
import rclpy
|
||||
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 sensor_msgs.msg import Image, CameraInfo
|
||||
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
|
||||
import cv2 # OpenCV library
|
||||
from megapose.scripts.run_inference_on_example import ModelPreload, run_inference_rbs #, run_inference
|
||||
|
||||
class PoseEstimator(Node):
|
||||
"""Our lifecycle node."""
|
||||
|
|
@ -41,6 +45,8 @@ class PoseEstimator(Node):
|
|||
for name, val in y.items():
|
||||
if name == "nodeName":
|
||||
self.nodeName = val
|
||||
elif name == "cameraLink":
|
||||
self.camera_link = val
|
||||
elif name == "topicImage":
|
||||
self.topicImage = val
|
||||
elif name == "topicCameraInfo":
|
||||
|
|
@ -53,6 +59,8 @@ class PoseEstimator(Node):
|
|||
self.topicSrv = val
|
||||
elif name == "tf2_send_pose":
|
||||
self.tf2_send_pose = val
|
||||
elif name == "mesh_scale":
|
||||
self.mesh_scale = val
|
||||
|
||||
def __init__(self, node_name, **kwargs):
|
||||
"""Construct the node."""
|
||||
|
|
@ -66,17 +74,22 @@ class PoseEstimator(Node):
|
|||
self._is_camerainfo = False
|
||||
self._K = [[0., 0., 0.], [0., 0., 0.], [0., 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.mesh_scale = 1.0
|
||||
self.quat_rotate_xyz = [0.5, 0.5, -0.5, -0.5]
|
||||
self.megapose_model = None
|
||||
self.darknet_path = ""
|
||||
|
||||
self.nodeName = node_name
|
||||
self.camera_link = "outer_rgbd_camera"
|
||||
self.topicImage = "/outer_rgb_camera/image"
|
||||
self.topicCameraInfo = "/outer_rgb_camera/camera_info"
|
||||
self.topicDepth = "/outer_rgbd_camera/depth_image"
|
||||
self.publishDelay = 2.0
|
||||
self.topicSrv = self.nodeName + "/detect6Dpose"
|
||||
self._InitService()
|
||||
self.camera_pose = Pose() #self.get_camera_pose()
|
||||
|
||||
self.tmpdir = tempfile.gettempdir()
|
||||
self.mytemppath = Path(self.tmpdir) / "rbs_per"
|
||||
|
|
@ -88,6 +101,8 @@ class PoseEstimator(Node):
|
|||
super().__init__(self.nodeName, **kwargs)
|
||||
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
|
||||
self.tf_broadcaster = TransformBroadcaster(self)
|
||||
# Used to convert between ROS and OpenCV images
|
||||
|
|
@ -102,23 +117,37 @@ class PoseEstimator(Node):
|
|||
self._count += 1
|
||||
|
||||
if self._pub is not None and self._pub.is_activated:
|
||||
# опубликуем результат оценки позы
|
||||
# опубликуем результат оценки позы # publish pose estimation result
|
||||
q = self._pose[0]
|
||||
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.position.x = t[0]
|
||||
p.position.y = t[1]
|
||||
p.position.z = t[2]
|
||||
p.orientation.w = q[0]
|
||||
p.orientation.x = q[1]
|
||||
p.orientation.y = q[2]
|
||||
p.orientation.z = q[3]
|
||||
p.position.x = t_mir[0] + cam_p.x
|
||||
p.position.y = t_mir[1] + cam_p.y
|
||||
p.position.z = t_mir[2] + cam_p.z
|
||||
p.orientation.w = q_mir[0]
|
||||
p.orientation.x = q_mir[1]
|
||||
p.orientation.y = q_mir[2]
|
||||
p.orientation.z = q_mir[3]
|
||||
self._pub.publish(p)
|
||||
|
||||
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
|
||||
"""
|
||||
|
|
@ -128,16 +157,16 @@ class PoseEstimator(Node):
|
|||
t.header.frame_id = 'world'
|
||||
t.child_frame_id = self.objName
|
||||
# coordinates
|
||||
tr = pose[1]
|
||||
t.transform.translation.x = tr[0]
|
||||
t.transform.translation.y = tr[1]
|
||||
t.transform.translation.z = tr[2]
|
||||
#tr = pose[1]
|
||||
t.transform.translation.x = tr.x #[0]
|
||||
t.transform.translation.y = tr.y #[1]
|
||||
t.transform.translation.z = tr.z #[2]
|
||||
# rotation
|
||||
q = pose[0]
|
||||
t.transform.rotation.x = q[1] # 0
|
||||
t.transform.rotation.y = q[2] # 1
|
||||
t.transform.rotation.z = q[3] # 2
|
||||
t.transform.rotation.w = q[0] # 3
|
||||
#q = pose[0]
|
||||
t.transform.rotation.w = q[0]
|
||||
t.transform.rotation.x = q[1]
|
||||
t.transform.rotation.y = q[2]
|
||||
t.transform.rotation.z = q[3]
|
||||
# Send the transformation
|
||||
self.tf_broadcaster.sendTransform(t)
|
||||
|
||||
|
|
@ -151,7 +180,30 @@ class PoseEstimator(Node):
|
|||
TransitionCallbackReturn.FAILURE transitions to "unconfigured".
|
||||
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):
|
||||
self.get_logger().info("Parameter 'mesh_path' not set")
|
||||
return TransitionCallbackReturn.FAILURE
|
||||
|
|
@ -179,8 +231,12 @@ class PoseEstimator(Node):
|
|||
self._pub = self.create_lifecycle_publisher(Pose, self.topicSrv, 10)
|
||||
self._timer = self.create_timer(self.publishDelay, self.publish)
|
||||
|
||||
# Preload Megapose model
|
||||
self.megapose_model = ModelPreload(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
if gtpose == None:
|
||||
# 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.')
|
||||
return TransitionCallbackReturn.SUCCESS
|
||||
|
|
@ -258,17 +314,23 @@ class PoseEstimator(Node):
|
|||
[k_[3], k_[4], k_[5]],
|
||||
[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"
|
||||
#{"label": "fork", "bbox_modal": [329, 189, 430, 270]}
|
||||
output_fn = tPath / "object_data.json"
|
||||
output_json_dict = {
|
||||
"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.append(output_json_dict)
|
||||
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]}
|
||||
output_fn = self.objPath / "camera_data.json"
|
||||
|
|
@ -287,8 +349,6 @@ class PoseEstimator(Node):
|
|||
"""
|
||||
Depth image callback function.
|
||||
"""
|
||||
#self.get_logger().info("Receiving depth image")
|
||||
|
||||
# Convert ROS Image message to OpenCV image
|
||||
current_frame = self.br.imgmsg_to_cv2(data)
|
||||
|
||||
|
|
@ -303,6 +363,43 @@ class PoseEstimator(Node):
|
|||
data = "No result file: '" + str(f) + "'"
|
||||
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):
|
||||
"""
|
||||
Image Callback function.
|
||||
|
|
@ -310,25 +407,58 @@ class PoseEstimator(Node):
|
|||
if not self._is_camerainfo:
|
||||
self.get_logger().warning("No data from CameraInfo")
|
||||
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
|
||||
current_frame = self.br.imgmsg_to_cv2(data)
|
||||
|
||||
# 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
|
||||
|
||||
# 6D pose estimation
|
||||
self.get_logger().info(f"megapose: begin {self._image_cnt} {self.objPath}")
|
||||
#run_inference(self.objPath,"megapose-1.0-RGB-multi-hypothesis")
|
||||
run_inference_rbs(self.megapose_model)
|
||||
detected = False
|
||||
darknet_bin = os.path.join(self.darknet_path, "darknet")
|
||||
if os.path.isfile(darknet_bin):
|
||||
# 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)
|
||||
if data[0] == "[":
|
||||
y = json.loads(data)[0]
|
||||
self._pose = y["TWO"]
|
||||
subprocess.run([darknet_bin, "detector", "test",
|
||||
"yolov4_objs2.data",
|
||||
"yolov4_objs2.cfg",
|
||||
"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():
|
||||
rclpy.init()
|
||||
|
|
|
|||
|
|
@ -127,7 +127,12 @@
|
|||
<include>
|
||||
<uri>model://fork</uri>
|
||||
<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>
|
||||
<!-->
|
||||
<uri>model://king</uri>
|
||||
<name>king1</name>
|
||||
<pose>1.5 0.2 0.25 0 0 0</pose>
|
||||
</-->
|
||||
</world>
|
||||
</sdf>
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue