def camera_callback(self, msg):
if not self.active:
return
float_time = msg.header.stamp.to_sec()
debug_msg = LEDDetectionDebugInfo()
if self.trigger:
#rospy.loginfo('[%s] GOT TRIGGER! Starting...')
self.trigger = False
self.data = []
self.capture_finished = False
# Start capturing images
#rospy.loginfo('[%s] Start capturing frames'%self.node_name)
self.first_timestamp = msg.header.stamp.to_sec()
self.tinit = time.time()
elif self.capture_finished:
self.node_state = 0
#rospy.loginfo('[%s] Waiting for trigger...' %self.node_name)
if self.first_timestamp > 0:
# TODO sanity check rel_time positive, restart otherwise
rel_time = float_time - self.first_timestamp
# Capturing
if rel_time < self.capture_time:
self.node_state = 1
# Capture image
rgb = numpy_from_ros_compressed(msg)
rgb = cv2.cvtColor(rgb, cv2.COLOR_BGRA2GRAY)
rgb = cv2.resize(rgb, (640 * 1, 480 * 1))
rgb = 255 - rgb
#rospy.loginfo('[%s] Capturing frame %s' %(self.node_name, rel_time))
# Save image to data
#if np.size(self.data) == 0:
# self.data = rgb
#else:
# self.data = np.dstack((self.data,rgb))
self.data.append({'timestamp': float_time, 'rgb': rgb[:, :]})
debug_msg.capture_progress = 100.0 * rel_time / self.capture_time
# Start processing
elif not self.capture_finished and self.first_timestamp > 0:
#rospy.loginfo('[%s] Relative Time %s, processing' %(self.node_name, rel_time))
self.node_state = 2
self.capture_finished = True
self.first_timestamp = 0
self.sub_cam.unregister(
) # IMPORTANT! Explicitly ignore messages
# while processing, accumulates delay otherwise!
self.send_state(debug_msg)
# Process image and publish results
self.process_and_publish()
self.send_state(debug_msg) # TODO move heartbeat to dedicated thread
def camera_callback(self, msg):
if not self.active:
return
float_time = msg.header.stamp.to_sec()
debug_msg = LEDDetectionDebugInfo()
if self.trigger:
rospy.loginfo('[%s] GOT TRIGGER! Starting...')
self.trigger = False
self.data = []
self.capture_finished = False
rospy.loginfo('[%s] Start capturing frames' % self.node_name)
self.first_timestamp = msg.header.stamp.to_sec()
self.tinit = time.time()
elif self.capture_finished:
self.node_state = 0
rospy.loginfo('[%s] Waiting for trigger...' % self.node_name)
if self.first_timestamp > 0:
# TODO sanity check rel_time positive, restart otherwise
rel_time = float_time - self.first_timestamp
# Capturing
if rel_time < self.capture_time:
self.node_state = 1
rgb = numpy_from_ros_compressed(msg)
rospy.loginfo('[%s] Capturing frame %s' %
(self.node_name, rel_time))
self.data.append({
'timestamp': float_time,
'rgb': rgb[:, :, :]
})
debug_msg.capture_progress = 100.0 * rel_time / self.capture_time
# Start processing
elif not self.capture_finished and self.first_timestamp > 0:
rospy.loginfo('[%s] Relative Time %s, processing' %
(self.node_name, rel_time))
self.node_state = 2
self.capture_finished = True
self.first_timestamp = 0
self.sub_cam.unregister(
) # IMPORTANT! Explicitly ignore messages
# while processing, accumulates delay otherwise!
self.send_state(debug_msg)
self.process_and_publish()
self.send_state(debug_msg) # TODO move heartbeat to dedicated thread
def __init__(self, ploteverything=False, verbose=False, plotfinal=False, publisher=None):
self.ploteverything = ploteverything
self.verbose = verbose
self.plotfinal = plotfinal
self.publisher = publisher
self.debug_msg = LEDDetectionDebugInfo()
pass
def camera_callback(self, msg):
if not self.active:
return
float_time = msg.header.stamp.to_sec()
debug_msg = LEDDetectionDebugInfo()
if self.trigger:
rospy.loginfo('[%s] GOT TRIGGER! Starting...')
self.trigger = False
self.data = []
self.capture_finished = False
rospy.loginfo('[%s] Start capturing frames'%self.node_name)
self.first_timestamp = msg.header.stamp.to_sec()
self.tinit = time.time()
elif self.capture_finished:
self.node_state = 0
rospy.loginfo('[%s] Waiting for trigger...' %self.node_name)
if self.first_timestamp > 0:
# TODO sanity check rel_time positive, restart otherwise
rel_time = float_time - self.first_timestamp
# Capturing
if rel_time < self.capture_time:
self.node_state = 1
rgb = numpy_from_ros_compressed(msg)
rospy.loginfo('[%s] Capturing frame %s' %(self.node_name, rel_time))
self.data.append({'timestamp': float_time, 'rgb': rgb[:,:,:]})
debug_msg.capture_progress = 100.0*rel_time/self.capture_time
# Start processing
elif not self.capture_finished and self.first_timestamp > 0:
rospy.loginfo('[%s] Relative Time %s, processing' %(self.node_name, rel_time))
self.node_state = 2
self.capture_finished = True
self.first_timestamp = 0
self.sub_cam.unregister() # IMPORTANT! Explicitly ignore messages
# while processing, accumulates delay otherwise!
self.send_state(debug_msg)
self.process_and_publish()
self.send_state(debug_msg) # TODO move heartbeat to dedicated thread
def camera_callback(self, msg):
if not self.active:
return
float_time = msg.header.stamp.to_sec()
debug_msg = LEDDetectionDebugInfo()
if self.trigger:
self.trigger = False
self.data = []
self.capture_finished = False
rospy.loginfo('Start capturing frames')
self.first_timestamp = msg.header.stamp.to_sec()
self.tinit = time.time()
elif self.capture_finished:
self.node_state = 0
print('Waiting for trigger signal...')
if self.first_timestamp > 0:
# TODO sanity check rel_time positive, restart otherwise
rel_time = float_time - self.first_timestamp
# Capturing
if rel_time < self.capture_time:
rgb = numpy_from_ros_compressed(msg)
rospy.loginfo('Capturing frame %s' %rel_time)
self.data.append({'timestamp': float_time, 'rgb': rgb})
self.node_state = 1
debug_msg.capture_progress = 100.0*rel_time/self.capture_time
# Start processing
elif not self.capture_finished and self.first_timestamp > 0:
self.capture_finished = True
self.first_timestamp = 0
self.node_state = 2
self.send_state(debug_msg)
self.process_and_publish()
self.send_state(debug_msg) # TODO move heartbeat to dedicated thread
def camera_callback(self, msg):
if not self.active:
return
float_time = msg.header.stamp.to_sec()
debug_msg = LEDDetectionDebugInfo()
if self.trigger:
self.trigger = False
self.data = []
self.capture_finished = False
rospy.loginfo('Start capturing frames')
self.first_timestamp = msg.header.stamp.to_sec()
self.tinit = time.time()
elif self.capture_finished:
self.node_state = 0
print('Waiting for trigger signal...')
if self.first_timestamp > 0:
# TODO sanity check rel_time positive, restart otherwise
rel_time = float_time - self.first_timestamp
# Capturing
if rel_time < self.capture_time:
rgb = numpy_from_ros_compressed(msg)
rospy.loginfo('Capturing frame %s' % rel_time)
self.data.append({'timestamp': float_time, 'rgb': rgb})
self.node_state = 1
debug_msg.capture_progress = 100.0 * rel_time / self.capture_time
# Start processing
elif not self.capture_finished and self.first_timestamp > 0:
self.capture_finished = True
self.first_timestamp = 0
self.node_state = 2
self.send_state(debug_msg)
self.process_and_publish()
self.send_state(debug_msg) # TODO move heartbeat to dedicated thread
def publish(self, imRight, imFront, imTL, results):
# Publish image with circles
imRightCircle_msg = self.bridge.cv2_to_imgmsg(imRight,
encoding="passthrough")
imFrontCircle_msg = self.bridge.cv2_to_imgmsg(imFront,
encoding="passthrough")
imTLCircle_msg = self.bridge.cv2_to_imgmsg(imTL,
encoding="passthrough")
# Publish image
self.pub_image_right.publish(imRightCircle_msg)
self.pub_image_front.publish(imFrontCircle_msg)
self.pub_image_TL.publish(imTLCircle_msg)
# Publish results
self.pub_raw_detections.publish(results)
# Publish debug
debug_msg = LEDDetectionDebugInfo()
debug_msg.cell_size = self.cell_size
debug_msg.crop_rect_norm = self.crop_rect_norm
debug_msg.led_all_unfiltered = results
debug_msg.state = 0
self.pub_debug.publish(debug_msg)
# Loginfo (right)
if self.right != SignalsDetection.NO_CAR:
rospy.loginfo('Right: LED detected')
else:
rospy.loginfo('Right: No LED detected')
# Loginfo (front)
if self.front != SignalsDetection.NO_CAR:
rospy.loginfo('Front: LED detected')
else:
rospy.loginfo('Front: No LED detected')
# Loginfo (TL)
if self.traffic_light == SignalsDetection.STOP:
rospy.loginfo('[%s] Traffic Light: red' % (self.node_name))
elif self.traffic_light == SignalsDetection.GO:
rospy.loginfo('[%s] Traffic Light: green' % (self.node_name))
else:
rospy.loginfo('[%s] No traffic light' % (self.node_name))
#Publish
rospy.loginfo(
"[%s] The observed LEDs are:\n Front = %s\n Right = %s\n Traffic light state = %s"
% (self.node_name, self.front, self.right, self.traffic_light))
self.pub_detections.publish(
SignalsDetection(front=self.front,
right=self.right,
left=self.left,
traffic_light_state=self.traffic_light))
def heartbeat_timer(self, event):
if (time.time() - self.node_last_seen > 1.5):
win.updateState(LEDDetectionDebugInfo(), False)