def cast_ray(self, point_in, tf, name): base_link_point = tf.transformPoint('/base_link', point_in) t = tf.getLatestCommonTime('/base_link', point_in.header.frame_id) pos, quat = tf.lookupTransform('/base_link', point_in.header.frame_id, t) height = pos[2] x_slope = (base_link_point.point.x - pos[0])/(pos[2]-base_link_point.point.z) y_slope = (base_link_point.point.y - pos[1])/(pos[2]-base_link_point.point.z) ground_point = np.array([0.,0.,0.]) ground_point[0] = x_slope*height + pos[0] ground_point[1] = y_slope*height + pos[1] ground_named_point = NamedPoint() ground_named_point.point.x = ground_point[0] ground_named_point.point.y = ground_point[1] ground_named_point.point.z = ground_point[2] ground_named_point.header = point_in.header ground_named_point.header.frame_id = 'base_link' ground_named_point.header.stamp = point_in.header.stamp ground_named_point.name = name odom_named_point = NamedPoint() odom_point = self.tf.transformPoint('/odom',ground_named_point) odom_named_point.point = odom_point.point odom_named_point.header = point_in.header odom_named_point.header.frame_id = "/odom" odom_named_point.header.stamp = point_in.header.stamp odom_named_point.name = name return ground_named_point, odom_named_point
def handle_monocular_img(self, Image): detections = self.find_samples(Image) self.debug_img_pub.publish(self.bridge.cv_to_imgmsg(cv2.cv.fromarray(self.debug_img),'bgr8')) for d in detections: if detections[d]['location'] is None: continue else: location = detections[d]['location'] pt=self.create_point_stamped(location) self.sample_imgpoints[d].publish(pt) named=NamedPoint() named.header=pt.header named.point=pt.point named.name=d self.namedpoints.publish(named)