def center(self, *args):
while (self.image is None or self.model is None) and not rospy.is_shutdown():
rospy.sleep(0.1)
if rospy.is_shutdown():
sys.exit(0)
im = np.asarray(self.bridge.imgmsg_to_cv(self.image))
dists = np.sum((im - [0, 255, 0]) ** 2, axis=2)
px = np.unravel_index(np.argmin(dists.flatten()), dists.shape)
target_pt = self.model.projectPixelTo3dRay((px[1], px[0]))
print "pointing at %s" % str(target_pt)
pt = transform_listener.transform_point("base_footprint", self.model.tf_frame, Point(*target_pt))
self.head.look_at_relative_point(pt.x, pt.y, pt.z)
self.center()
def handle_look_at_blob(msg):
target_color = np.array([msg.blob_color.r, msg.blob_color.g, msg.blob_color.b])
try:
cloud = _point_cloud.read_points_np(cloud_msg)
colors = _point_cloud.float2rgb(cloud[:,:,3])
color_dists = [np.linalg.norm(c-target_color) for c in colors[0,:]]
target_pt = cloud[0,np.argmin(color_dists),:3]
pt = transform_listener.transform_point('base_footprint', cloud_msg.header.frame_id, Point(*target_pt))
head.look_at_relative_point(pt.x+0.2,pt.y-0.09,pt.z)
except Exception, e:
print e
return LookAtBlobResponse(Bool(False))
def center(self, *args):
while (self.image is None
or self.model is None) and not rospy.is_shutdown():
rospy.sleep(0.1)
if rospy.is_shutdown(): sys.exit(0)
im = np.asarray(self.bridge.imgmsg_to_cv(self.image))
dists = np.sum((im - [0, 255, 0])**2, axis=2)
px = np.unravel_index(np.argmin(dists.flatten()), dists.shape)
target_pt = self.model.projectPixelTo3dRay((px[1], px[0]))
print 'pointing at %s' % str(target_pt)
pt = transform_listener.transform_point('base_footprint',
self.model.tf_frame,
Point(*target_pt))
self.head.look_at_relative_point(pt.x, pt.y, pt.z)
self.center()
def handle_look_at_blob(msg):
target_color = np.array(
[msg.blob_color.r, msg.blob_color.g, msg.blob_color.b])
try:
cloud = _point_cloud.read_points_np(cloud_msg)
colors = _point_cloud.float2rgb(cloud[:, :, 3])
color_dists = [np.linalg.norm(c - target_color) for c in colors[0, :]]
target_pt = cloud[0, np.argmin(color_dists), :3]
pt = transform_listener.transform_point('base_footprint',
cloud_msg.header.frame_id,
Point(*target_pt))
head.look_at_relative_point(pt.x + 0.2, pt.y - 0.09, pt.z)
except Exception, e:
print e
return LookAtBlobResponse(Bool(False))
