def __init__(self,
connect=True,
fps=3000,
port='/dev/ttyACM0',
buffer_size=20,
debug=True):
super(SerialThread, self).__init__()
self.fps = fps
self.connect = connect
self.debug = debug
if self.connect:
self.dev = slcan.slcanBus(port, bitrate=1000000)
self.dev.open()
print('Connection found!')
self.yaw = YAW_MID
self.pitch = PITCH_MID
self.send_yaw = 123
self.send_pitch = 123
self.sendData = []
from can.interfaces import slcan
try:
print("Closing CANBUS on ttyACM0...")
port = slcan.slcanBus("/dev/ttyACM0", bitrate=1000000)
except:
print("Can not find ttyACM0, closing ttyACM1...")
port = slcan.slcanBus("/dev/ttyACM1", bitrate=1000000)
port.close()
print("CANBUS ttyACM1 Closed")
else:
port.close()
print("CANBUS ttyACMO Closed")
def main():
default_model_dir = 'models'
default_model = '2019_05_13_whole/output_tflite_graph_1557776948_edgetpu.tflite'
default_labels = 'armor_plate_labels.txt'
# default_model = 'mobilenet_ssd_v2_face_quant_postprocess_edgetpu.tflite'
# default_labels = 'face_labels.txt'
parser = argparse.ArgumentParser()
parser.add_argument('--model',
help='.tflite model path',
default=os.path.join(default_model_dir, default_model))
parser.add_argument('--labels',
help='label file path',
default=os.path.join(default_model_dir,
default_labels))
parser.add_argument('--top_k',
type=int,
default=5,
help='number of classes with highest score to display')
parser.add_argument('--threshold',
type=float,
default=0.05,
help='class score threshold')
args = parser.parse_args()
print("Loading %s with %s labels." % (args.model, args.labels))
engine = DetectionEngine(args.model)
labels = load_labels(args.labels)
last_time = time.monotonic()
try:
dev = slcan.slcanBus(PORT[0], bitrate=1000000)
dev.open()
print('Connection found at port', PORT[0])
except:
dev = slcan.slcanBus(PORT[1], bitrate=1000000)
dev.open()
print('Connection found at port', PORT[1])
yaw = YAW_MID
pitch = PITCH_MID
def user_callback(image):
nonlocal last_time
start_time = time.monotonic()
objs = engine.DetectWithImage(image,
threshold=args.threshold,
keep_aspect_ratio=True,
relative_coord=True,
top_k=args.top_k)
end_time = time.monotonic()
obj = choose_obj(objs, start_time)
if obj:
# if labels:
# print(labels[obj.label_id], 'score = ', obj.score)
# else:
# print('score = ', obj.score)
[x1, y1, x2, y2] = obj.bounding_box.flatten().tolist()
# calculate pixel coords
pix_x = (x1 + x2) * X_PIXEL / 2 # 640/2 = 320
pix_y = (y1 + y2) * Y_PIXEL / 2 # 480/2 = 240
# calculate angles with respect to center
yaw = math.atan((pix_x - X_PIXEL/2) / CAMERA_PARAM) * \
1800 / math.pi + YAW_MID
pitch = math.atan((pix_y - Y_PIXEL/2) / CAMERA_PARAM) * \
1800 / math.pi + PITCH_MID
sendMessage(dev, yaw, pitch)
else:
print('No object detected!')
sendMessage(dev, DEFAULT_YAW, DEFAULT_PITCH)
text_lines = [
'Inference: %.2f ms' % ((end_time - start_time) * 1000),
'SPF: %.2f ms' % ((end_time - last_time) * 1000),
'FPS: %.2f fps' % (1.0 / (end_time - last_time)),
]
print(' '.join(text_lines))
last_time = end_time
# generate_svg(svg_canvas, objs, labels, text_lines)
result = gstreamer.run_pipeline(user_callback)
def run_pipeline(debug,
user_function,
src_size=(X_PIXEL, Y_PIXEL),
appsink_size=(300, 300)):
PIPELINE = 'v4l2src device=/dev/video1 ! {src_caps} ! {leaky_q} '
if debug:
if detectCoralDevBoard():
SRC_CAPS = 'video/x-raw,format=YUY2,width={width},height={height},framerate={frame_rate}/1'
PIPELINE += """ ! glupload ! tee name=t
t. ! {leaky_q} ! glfilterbin filter=glcolorscale
! {dl_caps} ! videoconvert ! {sink_caps} ! {sink_element}
t. ! {leaky_q} ! glfilterbin filter=glcolorscale
! rsvgoverlay name=overlay ! waylandsink
"""
else:
SRC_CAPS = 'video/x-raw,width={width},height={height},framerate={frame_rate}/1'
PIPELINE += """ ! tee name=t
t. ! {leaky_q} ! videoconvert ! videoscale ! {sink_caps} ! {sink_element}
t. ! {leaky_q} ! videoconvert
! rsvgoverlay name=overlay ! videoconvert ! ximagesink
"""
SINK_ELEMENT = 'appsink name=appsink sync=false emit-signals=true max-buffers=1 drop=true'
DL_CAPS = 'video/x-raw,format=RGBA,width={width},height={height}'
SINK_CAPS = 'video/x-raw,format=RGB,width={width},height={height}'
LEAKY_Q = 'queue max-size-buffers=1 leaky=downstream'
src_caps = SRC_CAPS.format(width=src_size[0],
height=src_size[1],
frame_rate=FRAME_RATE)
dl_caps = DL_CAPS.format(width=appsink_size[0], height=appsink_size[1])
sink_caps = SINK_CAPS.format(width=appsink_size[0],
height=appsink_size[1])
pipeline = PIPELINE.format(leaky_q=LEAKY_Q,
src_caps=src_caps,
dl_caps=dl_caps,
sink_caps=sink_caps,
sink_element=SINK_ELEMENT)
print(pipeline)
pipeline = Gst.parse_launch(pipeline)
overlay = pipeline.get_by_name('overlay')
appsink = pipeline.get_by_name('appsink')
appsink.connect(
'new-sample',
partial(on_new_sample,
True,
overlay=overlay,
screen_size=src_size,
appsink_size=appsink_size,
user_function=user_function))
loop = GObject.MainLoop()
else:
SRC_CAPS = 'video/x-raw,format=YUY2,width={width},height={height},framerate={frame_rate}/1'
PIPELINE += """ ! glupload ! {leaky_q} ! glfilterbin filter=glcolorscale
! videoconvert n-threads=4 ! {sink_caps} ! {sink_element}
"""
SINK_ELEMENT = 'appsink name=appsink sync=false emit-signals=true max-buffers=1 drop=true'
SINK_CAPS = 'video/x-raw,format=RGB,width={width},height={height}'
LEAKY_Q = 'queue max-size-buffers=1 leaky=downstream'
src_caps = SRC_CAPS.format(width=src_size[0],
height=src_size[1],
frame_rate=FRAME_RATE)
sink_caps = SINK_CAPS.format(width=appsink_size[0],
height=appsink_size[1])
pipeline = PIPELINE.format(leaky_q=LEAKY_Q,
src_caps=src_caps,
sink_caps=sink_caps,
sink_element=SINK_ELEMENT)
print("Preparing streamer pipeline")
print("Camera resolution", src_size[0], src_size[1], "Frame rate",
FRAME_RATE)
pipeline = Gst.parse_launch(pipeline)
appsink = pipeline.get_by_name('appsink')
appsink.connect(
'new-sample',
partial(on_new_sample,
False,
overlay=None,
screen_size=None,
appsink_size=appsink_size,
user_function=user_function))
loop = GObject.MainLoop()
# Set up a pipeline bus watch to catch errors.
bus = pipeline.get_bus()
bus.add_signal_watch()
bus.connect('message', on_bus_message, loop)
# Run pipeline.
pipeline.set_state(Gst.State.PLAYING)
try:
loop.run()
except:
pass
# Clean up.
pipeline.set_state(Gst.State.NULL)
while GLib.MainContext.default().iteration(False):
pass
# Close CAN bus
try:
print("Closing CANBUS on ttyACM0...")
port = slcan.slcanBus("/dev/ttyACM0", bitrate=1000000)
except:
print("Can not find ttyACM0, closing ttyACM1...")
port = slcan.slcanBus("/dev/ttyACM1", bitrate=1000000)
port.close()
print("CANBUS ttyACM1 Closed")
else:
port.close()
print("CANBUS ttyACMO Closed")