def publish(self, img_right, img_front, img_tl):
"""
Publishes the results of the detection, in case of high verbosity, it publishes debug images of the
detection, otherwise just the detection result.
Args:
img_right (:obj:`numpy array`): Debug image
img_front (:obj:`numpy array`): Debug image
img_tl (:obj:`numpy array`): Debug image
"""
# Publish image with circles if verbose is > 0
if self.params['~verbose'] > 0:
img_right_circle_msg = self.bridge.cv2_to_compressed_imgmsg(img_right) # , encoding="bgr8")
img_front_circle_msg = self.bridge.cv2_to_compressed_imgmsg(img_front) # , encoding="bgr8")
img_tl_circle_msg = self.bridge.cv2_to_compressed_imgmsg(img_tl) # , encoding="bgr8")
# Publish image
self.pub_image_right.publish(img_right_circle_msg)
self.pub_image_front.publish(img_front_circle_msg)
self.pub_image_TL.publish(img_tl_circle_msg)
# Log results to the terminal
rospy.loginfo("[%s] The observed LEDs are: Front = [%s] Right = [%s] Traffic light state = [%s]"
,self.node_name,self.front, self.right, self.traffic_light)
# Publish detections
detections_msg = SignalsDetection(front=self.front,
right=self.right,
left=self.left,
traffic_light_state=self.traffic_light)
self.pub_detections.publish(detections_msg)
def publish(self):
if self.mode is not None:
self.mode_pub.publish(FSMState(state=self.mode))
self.signals_detection_pub.publish(
SignalsDetection(front=self.opposite_veh,
right=self.right_veh,
traffic_light_state=self.traffic_light))
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 Interpreter(self, msg):
rospy.loginfo("[%s] Read a message from Detector" % (self.node_name))
# initialize the standard output message
self.front = SignalsDetection.NO_CAR
self.right = SignalsDetection.NO_CAR
self.left = SignalsDetection.NO_CAR
self.traffic_light_state = SignalsDetection.NO_TRAFFIC_LIGHT
# case with a traffic light
if self.trafficLightIntersection:
for item in msg.detections:
rospy.loginfo(
"[%s]:\n pixel = %f\n top bound = %f\n measured frequence=%f\n go frequency =%f"
% (self.node_name, item.pixels_normalized.y,
self.label['top'], item.frequency, self.lightGo))
if item.pixels_normalized.y < self.label['top']:
if abs(item.frequency - self.lightGo) < 0.1:
self.traffic_light_state = SignalsDetection.GO
break
else:
self.traffic_light_state = SignalsDetection.STOP
break
# case with stop sign intersection
else:
for item in msg.detections:
rospy.loginfo(
"[%s]:\n pixel = %f\n right bound = %f\n left bound =%f\n measured frequence=%f\n"
%
(self.node_name, item.pixels_normalized.x,
self.label['right'], self.label['left'], item.frequency))
# check if front vehicle detection
if item.pixels_normalized.x < self.label[
'right'] and item.pixels_normalized.y > self.label[
'top']:
# check signal of that vehicle
detected_freq = item.frequency
for i in range(len(self.signalFrequencies)):
if abs(self.signalFrequencies[i] -
detected_freq) < 0.1:
self.front = self.vehicleSignals[i]
break
# check if right vehicle detection
if item.pixels_normalized.x > self.label[
'right'] and item.pixels_normalized.y > self.label[
'top']:
# check signal of that vehicle
detected_freq = item.frequency
for i in range(len(self.signalFrequencies)):
if abs(self.signalFrequencies[i] -
detected_freq) < 0.1:
self.right = self.vehicleSignals[i]
break
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_state))
self.pub_interpret.publish(
SignalsDetection(front=self.front,
right=self.right,
left=self.left,
traffic_light_state=self.traffic_light_state))
def publish_signals(self):
self.signals_detection_pub.publish(
SignalsDetection(front=self.opposite_veh,
right=self.right_veh,
traffic_light_state=self.traffic_light))