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)