def cb_3d(self, req):
res = VisionRequestResponse()
if (self.last2d == None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = self.cam.tfFrame()
res.pose.header.stamp = self.last_found_time
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time - self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res
def _vision_cb_3D(self, req):
res = VisionRequestResponse()
if self.last_found_time_3D is None or self.image_sub.last_image_time is None:
res.found = False
return res
dt = (self.image_sub.last_image_time -
self.last_found_time_3D).to_sec()
if dt < 0 or dt > self.timeout_seconds:
res.found = False
elif (self.last3d is None or not self.enabled):
res.found = False
else:
res.pose.header.frame_id = "/map"
res.pose.header.stamp = self.last_found_time_3D
res.pose.pose.position = numpy_to_point(self.last3d[0])
res.pose.pose.orientation = numpy_to_quaternion(self.last3d[1])
res.found = True
return res