You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
213 lines
8.5 KiB
213 lines
8.5 KiB
#!/usr/bin/env python3
|
|
|
|
import copy
|
|
import pickle
|
|
import pathlib
|
|
import subprocess
|
|
import numpy as np
|
|
|
|
import rospy
|
|
from std_msgs.msg import Empty, Float32
|
|
from geometry_msgs.msg import PointStamped
|
|
from sensor_msgs.msg import PointCloud2
|
|
from visualization_msgs.msg import Marker
|
|
import hello_helpers.hello_misc as hm
|
|
|
|
import stretch_pyfunmap.manipulation_planning as mp
|
|
import stretch_pyfunmap.segment_max_height_image as sm
|
|
|
|
|
|
class GraspPointNode(hm.HelloNode):
|
|
|
|
def __init__(self):
|
|
hm.HelloNode.__init__(self)
|
|
self.active = False
|
|
self.update_perception = False
|
|
|
|
self.surface_mask = None
|
|
self.grasp_targets = None
|
|
self.best_target = None
|
|
|
|
pc_pkl_fpath = str(pathlib.Path(subprocess.Popen("rospack find stretch_demos", shell=True, stdout=subprocess.PIPE).stdout.read().decode()[:-1]) / 'empty_pc.pkl')
|
|
with open(pc_pkl_fpath, 'rb') as inp:
|
|
self.empty_pc = pickle.load(inp)
|
|
|
|
def enable_callback(self, msg):
|
|
self.surface_mask = None
|
|
self.grasp_targets = None
|
|
self.best_target = None
|
|
self.update_perception = True
|
|
self.active = True
|
|
print(' - grasp_point enabled')
|
|
|
|
def mhi_save_callback(self, msg):
|
|
with open('/home/hello-robot/mhi_save.pickle', 'wb') as outp:
|
|
pickle.dump(self.manip.max_height_im, outp, protocol=pickle.HIGHEST_PROTOCOL)
|
|
print(' - grasp_point mhi_save')
|
|
|
|
def disable_callback(self, msg):
|
|
self.active = False
|
|
self.update_perception = False
|
|
rospy.sleep(1)
|
|
self.surface_mask = None
|
|
self.grasp_targets = None
|
|
self.best_target = None
|
|
print(' - grasp_point disabled')
|
|
|
|
def grasp_point_callback(self, point_msg):
|
|
self.grasp_point(point_msg)
|
|
|
|
def clicked_point_callback(self, clicked_point_msg):
|
|
self.grasp_point(clicked_point_msg)
|
|
|
|
def grasp_point(self, point_msg):
|
|
if not self.active:
|
|
print('grasp_point not active')
|
|
return
|
|
if self.grasp_targets is None:
|
|
print('cannot assign point to object because no grasp targets available')
|
|
return
|
|
self.update_perception = False
|
|
|
|
points_to_image_mat, _ = self.manip.max_height_im.get_points_to_image_mat(point_msg.header.frame_id, self.tf2_buffer)
|
|
p_x = point_msg.point.x
|
|
p_y = point_msg.point.y
|
|
p_z = point_msg.point.z
|
|
point_xyz = np.array([p_x, p_y, p_z, 1.0])
|
|
point_pix = np.matmul(points_to_image_mat, point_xyz)[:3]
|
|
i_x, i_y, _ = point_pix
|
|
|
|
h, w = self.manip.max_height_im.image.shape
|
|
if not (i_x >= 0 and i_x < w and i_y >= 0 and i_y < h):
|
|
print('point does not fall within the max height image')
|
|
self.update_perception = True
|
|
return
|
|
|
|
min_distance = float('inf')
|
|
best_target = None
|
|
for target in self.grasp_targets:
|
|
dist = np.sqrt((i_x - target['location_xy_pix'][0]) ** 2 + (i_y - target['location_xy_pix'][1]) ** 2)
|
|
if dist < min_distance:
|
|
min_distance = dist
|
|
best_target = target
|
|
if best_target is None:
|
|
print('couldnt find grasp target close to point')
|
|
self.update_perception = True
|
|
return
|
|
self.best_target = best_target
|
|
|
|
self.manip.perform_cartesian_grasp(best_target, self, tooltip_frame='link_grasp_center')
|
|
msg = Float32()
|
|
msg.data = best_target['location_above_surface_m']
|
|
self.result_pub.publish(msg)
|
|
self.update_perception = True
|
|
|
|
def perceive_objects(self):
|
|
if not self.update_perception:
|
|
return
|
|
|
|
self.manip.update(self.point_cloud, self.tf2_buffer)
|
|
surface_mask, plane_parameters = sm.find_closest_flat_surface(self.manip.max_height_im, None)
|
|
if surface_mask is None:
|
|
return
|
|
self.surface_mask = surface_mask
|
|
|
|
grasp_targets = sm.find_objects_to_grasp(surface_mask, plane_parameters, self.manip.max_height_im)
|
|
if len(grasp_targets) == 0:
|
|
return
|
|
self.grasp_targets = grasp_targets
|
|
|
|
self.ready_pub.publish(Empty())
|
|
|
|
def publish_markers(self):
|
|
if self.surface_mask is not None:
|
|
image_copy = self.manip.max_height_im.image.copy()
|
|
image_copy[self.surface_mask <= 0] = 0
|
|
surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
|
|
self.surface_marker_pub.publish(surface_pc)
|
|
else:
|
|
self.surface_marker_pub.publish(self.empty_pc)
|
|
|
|
if self.grasp_targets is not None:
|
|
grasp_targets = self.grasp_targets
|
|
objects_mask = grasp_targets[0]['object_selector'].copy()
|
|
for target in grasp_targets:
|
|
objects_mask = objects_mask | target['object_selector']
|
|
|
|
if self.best_target is not None:
|
|
not_best_target_mask = ~ self.best_target['object_selector'].copy()
|
|
objects_mask = objects_mask & not_best_target_mask
|
|
|
|
image_copy = self.manip.max_height_im.image.copy()
|
|
image_copy[objects_mask <= 0] = 0
|
|
objects_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
|
|
self.objects_marker_pub.publish(objects_pc)
|
|
else:
|
|
self.objects_marker_pub.publish(self.empty_pc)
|
|
|
|
if self.best_target is not None:
|
|
image_copy = self.manip.max_height_im.image.copy()
|
|
image_copy[self.best_target['object_selector'] <= 0] = 0
|
|
surface_pc = self.manip.max_height_im.to_point_cloud(substitute_image=image_copy)
|
|
self.gtpc_marker_pub.publish(surface_pc)
|
|
|
|
marker = Marker()
|
|
marker.header.stamp = rospy.Time.now()
|
|
marker.header.frame_id = 'map'
|
|
marker.type = marker.SPHERE
|
|
marker.action = marker.ADD
|
|
marker.lifetime = rospy.Duration(0.2)
|
|
image_to_points_mat, _ = self.manip.max_height_im.get_image_to_points_mat('map', self.tf2_buffer)
|
|
i_x = self.best_target['location_xy_pix'][0]
|
|
i_y = self.best_target['location_xy_pix'][1]
|
|
i_z = self.best_target['location_z_pix']
|
|
point_pix = np.array([i_x, i_y, i_z, 1.0])
|
|
point_xyz = np.matmul(image_to_points_mat, point_pix)
|
|
marker.pose.position.x = point_xyz[0]
|
|
marker.pose.position.y = point_xyz[1]
|
|
marker.pose.position.z = point_xyz[2]
|
|
marker.pose.orientation.w = 1.0
|
|
marker.scale.x = 0.05
|
|
marker.scale.y = 0.05
|
|
marker.scale.z = 0.05
|
|
marker.color.r = 0.0
|
|
marker.color.g = 0.1
|
|
marker.color.b = 0.9
|
|
marker.color.a = 1.0
|
|
self.gtaxes_marker_pub.publish(marker)
|
|
else:
|
|
self.gtpc_marker_pub.publish(self.empty_pc)
|
|
marker = Marker()
|
|
marker.action = marker.DELETEALL
|
|
self.gtaxes_marker_pub.publish(marker)
|
|
|
|
def main(self):
|
|
hm.HelloNode.main(self, 'grasp_point', 'grasp_point', wait_for_first_pointcloud=True)
|
|
self.surface_marker_pub = rospy.Publisher('/grasp_point/surface_marker', PointCloud2, queue_size=1)
|
|
self.objects_marker_pub = rospy.Publisher('/grasp_point/objects_marker', PointCloud2, queue_size=1)
|
|
self.gtpc_marker_pub = rospy.Publisher('/grasp_point/gtpc_marker', PointCloud2, queue_size=1)
|
|
self.gtaxes_marker_pub = rospy.Publisher('/grasp_point/gtaxes_marker', Marker, queue_size=1)
|
|
|
|
self.clicked_point_sub = rospy.Subscriber('/clicked_point', PointStamped, self.clicked_point_callback)
|
|
self.trigger_sub = rospy.Subscriber('/grasp_point/trigger_grasp_point', PointStamped, self.grasp_point_callback)
|
|
self.enable_sub = rospy.Subscriber('/grasp_point/enable', Empty, self.enable_callback)
|
|
self.ready_pub = rospy.Publisher('/grasp_point/ready', Empty, queue_size=1)
|
|
self.mhi_save_sub = rospy.Subscriber('/grasp_point/mhi_save', Empty, self.mhi_save_callback)
|
|
self.disable_sub = rospy.Subscriber('/grasp_point/disable', Empty, self.disable_callback)
|
|
self.result_pub = rospy.Publisher('/grasp_point/result', Float32, queue_size=1)
|
|
|
|
self.manip = mp.ManipulationView(self.tf2_buffer, None, self.get_tool())
|
|
|
|
rate = rospy.Rate(10.0)
|
|
while not rospy.is_shutdown():
|
|
self.perceive_objects()
|
|
self.publish_markers()
|
|
rate.sleep()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
try:
|
|
node = GraspPointNode()
|
|
node.main()
|
|
except KeyboardInterrupt:
|
|
pass
|