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"?>
|
<?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
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"
|
<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>
|
|
@ -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);
|
||||||
|
|
File diff suppressed because it is too large
Load diff
|
@ -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
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
|
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()
|
||||||
|
|
|
@ -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>
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue