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))