hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

using target ID inpolicy test passed

Browse Source
This commit is contained in:
0nhc 2024-10-13 01:22:11 -05:00
parent 52cb65dee8
commit 620010d05a
2 changed files with 5 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/active_grasp/active_perception_policy.py
View File

@ -59,6 +59,9 @@ class ActivePerceptionPolicy(MultiViewPolicy):
for i in range(img_shape[0]): for i in range(img_shape[0]):
for j in range(img_shape[1]): for j in range(img_shape[1]):
seg_id = seg_img[i, j] seg_id = seg_img[i, j]
# print(seg_id)
if(int(seg_id) == int(target_id)):
print("woahhhhhhhhhhhhhhhhh!")
def best_grasp_prediction_is_stable(self): def best_grasp_prediction_is_stable(self):

4
src/active_grasp/controller.py
View File

@ -109,7 +109,7 @@ class GraspController:
r = rospy.Rate(self.policy_rate) r = rospy.Rate(self.policy_rate)
while not self.policy.done: while not self.policy.done:
depth_img, seg_image, pose, q = self.get_state() depth_img, seg_image, pose, q = self.get_state()
target_seg_id = self.get_target_id(TargetIDRequest()) target_seg_id = self.get_target_id(TargetIDRequest()).id
self.policy.update(depth_img, seg_image, target_seg_id, pose, q) self.policy.update(depth_img, seg_image, target_seg_id, pose, q)
r.sleep() r.sleep()
rospy.sleep(0.2) # Wait for a zero command to be sent to the robot. rospy.sleep(0.2) # Wait for a zero command to be sent to the robot.
@ -122,7 +122,7 @@ class GraspController:
msg = copy.deepcopy(self.latest_depth_msg) msg = copy.deepcopy(self.latest_depth_msg)
depth_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001 depth_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) * 0.001
msg = copy.deepcopy(self.latest_seg_msg) msg = copy.deepcopy(self.latest_seg_msg)
seg_img = self.cv_bridge.imgmsg_to_cv2(msg).astype(np.float32) seg_img = self.cv_bridge.imgmsg_to_cv2(msg)
pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp) pose = tf.lookup(self.base_frame, self.cam_frame, msg.header.stamp)
return depth_img, seg_img, pose, q return depth_img, seg_img, pose, q