diff --git a/export_video.sh b/export_video.sh new file mode 100755 index 0000000..7a613a0 --- /dev/null +++ b/export_video.sh @@ -0,0 +1,7 @@ +LOGFILE=$1 +FILENAME="${LOGFILE##*/}" +NAME="${FILENAME%.*}" +python -m osgar.logger $LOGFILE --raw --stream oak.color > upload/$NAME.h26x +ffmpeg -i upload/$NAME.h26x -c copy upload/$NAME.mp4 +rm upload/$NAME.h26x + diff --git a/robotem-rovne/config/matty-redroad.json b/robotem-rovne/config/matty-redroad.json new file mode 100644 index 0000000..de074ef --- /dev/null +++ b/robotem-rovne/config/matty-redroad.json @@ -0,0 +1,91 @@ +{ + "version": 2, + "robot": { + "modules": { + "app": { + "driver": "robotem-rovne.main:RobotemRovne", + "in": ["emergency_stop", "pose2d", "nn_mask", "nmea_data", "orientation_list"], + "out": ["desired_steering"], + "init": { + "max_speed": 0.5, + "stop_dist": -1, + "terminate_on_stop": true + } + }, + "platform": { + "driver": "osgar.platforms.matty:Matty", + "in": ["esp_data"], + "out": ["esp_data"], + "init": {} + }, + "timer": { + "driver": "timer", + "in": [], + "out": ["tick"], + "init": { + "sleep": 0.1 + } + }, + "gps": { + "driver": "gps", + "in": ["raw"], + "out": ["nmea_data"], + "init": { + } + }, + "serial": { + "driver": "serial", + "in": ["raw"], + "out": ["raw"], + "init": {"port": "/dev/esp32", "speed": 115200} + }, + "oak": { + "driver": "osgar.drivers.oak_camera:OakCamera", + "init": { + "fps": 10, + "is_color": true, + "video_encoder": "h265", + "h264_bitrate": 2000, + "is_depth": true, + "is_stereo_images": false, + "laser_projector_current": 0, + "flood_light_current": 500, + "is_imu_enabled": true, + "number_imu_records": 10, + "disable_magnetometer_fusion": true, + "mono_resolution": "THE_400_P", + "color_resolution": "THE_1080_P", + "color_manual_focus": 130, + "stereo_median_filter": "KERNEL_3x3", + "stereo_mode": "HIGH_ACCURACY", + "stereo_extended_disparity": false, + "stereo_subpixel": false, + "stereo_left_right_check": true, + "sleep_on_start_sec": 3, + "model": { + "blob": "models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob" + }, + "nn_config": { + "output_format": "LayerFp16", + "NN_family": "robotourist", + "input_size": "224x224" + } + } + } + }, + "links": [ + ["app.desired_steering", "platform.desired_steering"], + ["platform.pose2d", "app.pose2d"], + ["platform.emergency_stop", "app.emergency_stop"], + ["serial.raw", "platform.esp_data"], + ["platform.esp_data", "serial.raw"], + ["platform.gps_serial", "gps.raw"], + ["timer.tick", "platform.tick"], + + ["gps.nmea_data", "app.nmea_data"], + + ["oak.nn_mask", "app.nn_mask"], + ["oak.orientation_list", "app.orientation_list"] + ] + } +} diff --git a/robotem-rovne/config/test_redroad.json b/robotem-rovne/config/test_redroad.json new file mode 100644 index 0000000..aace170 --- /dev/null +++ b/robotem-rovne/config/test_redroad.json @@ -0,0 +1,39 @@ +{ + "version": 2, + "robot": { + "modules": { + "oak": { + "driver": "osgar.drivers.oak_camera:OakCamera", + "init": { + "fps": 10, + "is_color": true, + "video_encoder": "mjpeg", + "is_depth": false, + "laser_projector_current": 0, + "flood_light_current": 500, + "is_imu_enabled": true, + "number_imu_records": 10, + "disable_magnetometer_fusion": true, + "mono_resolution": "THE_400_P", + "color_resolution": "THE_1080_P", + "color_manual_focus": 130, + "stereo_median_filter": "KERNEL_3x3", + "stereo_mode": "HIGH_ACCURACY", + "stereo_extended_disparity": false, + "stereo_subpixel": false, + "stereo_left_right_check": true, + "model": { + "blob": "models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob" + }, + "nn_config": { + "output_format": "LayerFp16", + "NN_family": "robotourist", + "input_size": "224x224" + } + } + } + }, + "links": [ + ] + } +} diff --git a/robotem-rovne/main.py b/robotem-rovne/main.py index 76d5d55..34a273b 100644 --- a/robotem-rovne/main.py +++ b/robotem-rovne/main.py @@ -80,15 +80,16 @@ def on_nmea_data(self, data): def on_nn_mask(self, data): self.last_nn_mask = data.copy() # make sure you modify only own copy - assert self.last_nn_mask.shape == (120, 160), self.last_nn_mask.shape - self.last_nn_mask[:60, :] = 0 # remove sky detections +# assert self.last_nn_mask.shape == (120, 160), self.last_nn_mask.shape + height, width = self.last_nn_mask.shape + self.last_nn_mask[:height//2, :] = 0 # remove sky detections center_y, center_x = mask_center(self.last_nn_mask) - dead = 10 + dead = width//16 # was 10 turn_angle = math.radians(20) - if center_x > 80 + dead: + if center_x > width//2 + dead: self.last_dir = -turn_angle - elif center_x < 80 - dead: + elif center_x < width//2 - dead: self.last_dir = turn_angle else: self.last_dir = 0 # straight diff --git a/robotem-rovne/models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob b/robotem-rovne/models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob new file mode 100644 index 0000000..73e4b62 Binary files /dev/null and b/robotem-rovne/models/robotourist-083-redroad-20260116-224x224-rgb-chw-1shave.blob differ diff --git a/robotem-rovne/view_mask.py b/robotem-rovne/view_mask.py index 3493da0..b0296a1 100644 --- a/robotem-rovne/view_mask.py +++ b/robotem-rovne/view_mask.py @@ -18,21 +18,23 @@ def read_h264_image(data, i_frame_only=True): - assert data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 0930')), data[ - :20].hex() - if data.startswith(bytes.fromhex('00000001 0950')): + is_h264 = data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 0930')) + is_h265 = data.startswith(bytes.fromhex('00000001 460150')) or data.startswith(bytes.fromhex('00000001 460130')) + assert is_h264 or is_h265, data[:20].hex() + + if data.startswith(bytes.fromhex('00000001 0950')) or data.startswith(bytes.fromhex('00000001 460150')): # I - key frame - with open('tmp.h264', 'wb') as f: + with open('tmp.h26x', 'wb') as f: f.write(data) - elif data.startswith(bytes.fromhex('00000001 0930')): + elif data.startswith(bytes.fromhex('00000001 0930')) or data.startswith(bytes.fromhex('00000001 460130')): # P-frame} if i_frame_only: return None - with open('tmp.h264', 'ab') as f: + with open('tmp.h26x', 'ab') as f: f.write(data) else: assert 0, f'Unexpected data {data[:20].hex()}' - cap = cv2.VideoCapture('tmp.h264') + cap = cv2.VideoCapture('tmp.h26x') image = None ret = True while ret: @@ -43,8 +45,11 @@ def read_h264_image(data, i_frame_only=True): return image -def read_logfile(logfile, writer=None, add_time=True): - nn_mask_stream = lookup_stream_id(logfile, 'oak.nn_mask') +def read_logfile(logfile, writer=None, add_time=True, threshold=None): + if threshold is None: + nn_mask_stream = lookup_stream_id(logfile, 'oak.nn_mask') + else: + nn_mask_stream = lookup_stream_id(logfile, 'oak.redroad') img_stream = lookup_stream_id(logfile, 'oak.color') pose2d_stream = lookup_stream_id(logfile, 'platform.pose2d') total_duration, total_dist = get_time_and_dist(logfile, 'platform.pose2d') @@ -59,14 +64,17 @@ def read_logfile(logfile, writer=None, add_time=True): if img is None: continue mask = deserialize(data) - assert mask.shape == (120, 160), mask.shape - mask[:60, :] = 0 # remove sky detections + if threshold is not None: + mask = (mask > threshold).astype(np.uint8) + assert mask.shape in [(120, 160), (112,112)], mask.shape + orig_height, orig_width = mask.shape +# mask[:height//2, :] = 0 # remove sky detections center_y, center_x = mask_center(mask) - scale = 12 # 160 -> 640 -> 1920 - center_x *= scale - center_y *= 9 # scale mask = cv2.resize(mask, (1920, 1080)) height, width = mask.shape + scale = width // orig_width # 160 -> 640 -> 1920 + center_x *= scale + center_y *= height // orig_height colored_mask = np.zeros((height, width, 3), dtype=np.uint8) colored_mask[mask == 1] = [0, 0, 255] overlay = cv2.addWeighted(img, 1, colored_mask, 0.7, 0) @@ -135,6 +143,7 @@ def main(): parser = argparse.ArgumentParser(description=__doc__) parser.add_argument('logfile', nargs='+', help='recorded log file(s)') parser.add_argument('--create-video', help='filename of output video') + parser.add_argument('--threshold', '-t', type=int, help='threshold value for redroad detection') args = parser.parse_args() if args.create_video is not None: @@ -148,7 +157,7 @@ def main(): writer = None # no video writer for logfile in args.logfile: - read_logfile(logfile, writer=writer) + read_logfile(logfile, writer=writer, threshold=args.threshold) if writer is not None: writer.release()