From 74930d70361b9a45da46d250ab32ec746c64ba29 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 4 Jun 2024 11:27:08 +0200 Subject: [PATCH 1/6] trifinger_data_backend: Get camera rate from settings --- scripts/trifinger_data_backend.py | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/scripts/trifinger_data_backend.py b/scripts/trifinger_data_backend.py index 774080c..151f6eb 100644 --- a/scripts/trifinger_data_backend.py +++ b/scripts/trifinger_data_backend.py @@ -3,6 +3,7 @@ Runs the leader multi-processing robot/sensor data and loggers. """ + import argparse import sys @@ -11,12 +12,14 @@ import robot_interfaces from robot_fingers.ros import NotificationNode +from trifinger_cameras import camera # make sure camera time series covers at least one second CAMERA_TIME_SERIES_LENGTH = 15 ROBOT_TIME_SERIES_LENGTH = 1000 +ROBOT_RATE_Hz = 1000 def main(): @@ -92,17 +95,18 @@ def main(): robot_logger.start() if cameras_enabled and args.camera_logfile: - camera_fps = 10 - robot_rate_hz = 1000 + camera_settings = camera.Settings() + camera_fps = camera_settings.get_tricamera_driver_settings().frame_rate_fps + logger.debug("Loaded camera frame rate from settings: %f fps", camera_fps) + # make the logger buffer a bit bigger as needed to be on the safe side buffer_length_factor = 1.5 - episode_length_s = args.max_number_of_actions / robot_rate_hz - # Compute camera log size based on number of robot actions plus some - # buffer + episode_length_s = args.max_number_of_actions / ROBOT_RATE_Hz + # Compute camera log size based on number of robot actions plus some buffer log_size = int(camera_fps * episode_length_s * buffer_length_factor) - logger.info("Initialize camera logger with buffer size %d" % log_size) + logger.info("Initialize camera logger with buffer size %d", log_size) camera_logger = tricamera.Logger(camera_data, log_size) logger.info("Data backend is ready") @@ -128,11 +132,11 @@ def main(): logger.debug("Received shutdown signal") if cameras_enabled and args.camera_logfile: - logger.info("Save recorded camera data to file %s" % args.camera_logfile) + logger.info("Save recorded camera data to file %s", args.camera_logfile) camera_logger.stop_and_save(args.camera_logfile) if args.robot_logfile: - logger.info("Save robot data to file %s" % args.robot_logfile) + logger.info("Save robot data to file %s", args.robot_logfile) robot_logger.stop_and_save( args.robot_logfile, robot_interfaces.trifinger.Logger.Format.BINARY ) From d05af4a831d2574c9f1d37e2b5c2521c9c3baccc Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 4 Jun 2024 11:38:09 +0200 Subject: [PATCH 2/6] refactor: Move argparse to separate function Move the argument parsing to a separate function to reduce complexity of the main function. --- scripts/trifinger_data_backend.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) mode change 100644 => 100755 scripts/trifinger_data_backend.py diff --git a/scripts/trifinger_data_backend.py b/scripts/trifinger_data_backend.py old mode 100644 new mode 100755 index 151f6eb..85ac4ea --- a/scripts/trifinger_data_backend.py +++ b/scripts/trifinger_data_backend.py @@ -22,7 +22,7 @@ ROBOT_RATE_Hz = 1000 -def main(): +def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser(description=__doc__) parser.add_argument( "--max-number-of-actions", @@ -61,6 +61,15 @@ def main(): ) args = parser.parse_args() + if args.camera_logfile and not (args.cameras or args.cameras_with_tracker): + parser.error("--camera-logfile requires --cameras or --cameras-with-tracker") + + return args + + +def main() -> int: + args = parse_args() + rclpy.init() node = NotificationNode("trifinger_data") From 8cfcf7ac4661104dc095bced4455e91d66e370ac Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 4 Jun 2024 11:48:13 +0200 Subject: [PATCH 3/6] Dynamically compute camera time series length Compute camera time series length dynamically to cover the full robot time series window. This is to ensure that for each robot observation in the buffer there is also a camera observation. --- scripts/trifinger_data_backend.py | 28 +++++++++++++++++----------- 1 file changed, 17 insertions(+), 11 deletions(-) diff --git a/scripts/trifinger_data_backend.py b/scripts/trifinger_data_backend.py index 85ac4ea..7d6685e 100755 --- a/scripts/trifinger_data_backend.py +++ b/scripts/trifinger_data_backend.py @@ -15,11 +15,8 @@ from trifinger_cameras import camera -# make sure camera time series covers at least one second -CAMERA_TIME_SERIES_LENGTH = 15 - ROBOT_TIME_SERIES_LENGTH = 1000 -ROBOT_RATE_Hz = 1000 +ROBOT_RATE_HZ = 1000 def parse_args() -> argparse.Namespace: @@ -86,8 +83,21 @@ def main() -> int: if cameras_enabled: logger.info("Start camera data") + camera_settings = camera.Settings() + camera_rate_hz = camera_settings.get_tricamera_driver_settings().frame_rate_fps + logger.debug("Loaded camera frame rate from settings: %f fps", camera_rate_hz) + + # make sure camera time series covers at least the same duration as the robot + # time series (add some margin to avoid problems) + time_series_length_seconds = ROBOT_TIME_SERIES_LENGTH / ROBOT_RATE_HZ + length_margin_ratio = 1.5 + camera_time_series_length = int( + camera_rate_hz * time_series_length_seconds * length_margin_ratio + ) + logger.debug("Set camera time series length to %d", camera_time_series_length) + camera_data = tricamera.MultiProcessData( - "tricamera", True, CAMERA_TIME_SERIES_LENGTH + "tricamera", True, camera_time_series_length ) logger.info("Start robot data") @@ -104,16 +114,12 @@ def main() -> int: robot_logger.start() if cameras_enabled and args.camera_logfile: - camera_settings = camera.Settings() - camera_fps = camera_settings.get_tricamera_driver_settings().frame_rate_fps - logger.debug("Loaded camera frame rate from settings: %f fps", camera_fps) - # make the logger buffer a bit bigger as needed to be on the safe side buffer_length_factor = 1.5 - episode_length_s = args.max_number_of_actions / ROBOT_RATE_Hz + episode_length_s = args.max_number_of_actions / ROBOT_RATE_HZ # Compute camera log size based on number of robot actions plus some buffer - log_size = int(camera_fps * episode_length_s * buffer_length_factor) + log_size = int(camera_rate_hz * episode_length_s * buffer_length_factor) logger.info("Initialize camera logger with buffer size %d", log_size) camera_logger = tricamera.Logger(camera_data, log_size) From d2387f8e467e5a0d7bf7feb9c47b0f397a5bae79 Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 25 Jun 2024 09:38:51 +0200 Subject: [PATCH 4/6] fix: ROS logger doesn't support formatting --- scripts/trifinger_data_backend.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/trifinger_data_backend.py b/scripts/trifinger_data_backend.py index 7d6685e..6b83cb3 100755 --- a/scripts/trifinger_data_backend.py +++ b/scripts/trifinger_data_backend.py @@ -85,7 +85,7 @@ def main() -> int: camera_settings = camera.Settings() camera_rate_hz = camera_settings.get_tricamera_driver_settings().frame_rate_fps - logger.debug("Loaded camera frame rate from settings: %f fps", camera_rate_hz) + logger.debug("Loaded camera frame rate from settings: %f fps" % camera_rate_hz) # make sure camera time series covers at least the same duration as the robot # time series (add some margin to avoid problems) @@ -94,7 +94,7 @@ def main() -> int: camera_time_series_length = int( camera_rate_hz * time_series_length_seconds * length_margin_ratio ) - logger.debug("Set camera time series length to %d", camera_time_series_length) + logger.debug("Set camera time series length to %d" % camera_time_series_length) camera_data = tricamera.MultiProcessData( "tricamera", True, camera_time_series_length @@ -121,7 +121,7 @@ def main() -> int: # Compute camera log size based on number of robot actions plus some buffer log_size = int(camera_rate_hz * episode_length_s * buffer_length_factor) - logger.info("Initialize camera logger with buffer size %d", log_size) + logger.info("Initialize camera logger with buffer size %d" % log_size) camera_logger = tricamera.Logger(camera_data, log_size) logger.info("Data backend is ready") @@ -147,11 +147,11 @@ def main() -> int: logger.debug("Received shutdown signal") if cameras_enabled and args.camera_logfile: - logger.info("Save recorded camera data to file %s", args.camera_logfile) + logger.info("Save recorded camera data to file %s" % args.camera_logfile) camera_logger.stop_and_save(args.camera_logfile) if args.robot_logfile: - logger.info("Save robot data to file %s", args.robot_logfile) + logger.info("Save robot data to file %s" % args.robot_logfile) robot_logger.stop_and_save( args.robot_logfile, robot_interfaces.trifinger.Logger.Format.BINARY ) From 937d3b731f26593981ccce5a9b0c45d29d6c70ed Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Mon, 2 Sep 2024 14:31:24 +0200 Subject: [PATCH 5/6] trifinger_backend: Get camera rate from driver Instead of hard-coding it in the script, use the new `get_sensor_info()` method of the driver to get the camera frame rate. The frame rate is specified for each of the three cameras, but since they are syncronised, they are all the same, so we can simply take the value from the first camera in the list. --- scripts/trifinger_backend.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/scripts/trifinger_backend.py b/scripts/trifinger_backend.py index 2f8c911..b1ad093 100644 --- a/scripts/trifinger_backend.py +++ b/scripts/trifinger_backend.py @@ -14,8 +14,6 @@ import robot_fingers -# Frame rate of the cameras -CAMERA_FPS = 10 # Control rate of the robot ROBOT_RATE_HZ = 1000 @@ -128,7 +126,7 @@ def parse_arguments() -> argparse.Namespace: # not possible to decide the logger buffer size) if (args.robot_logfile or args.camera_logfile) and not args.max_number_of_actions: parser.error( - "--max-number-of-actions must be specified when using data" " logging." + "--max-number-of-actions must be specified when using data logging." ) if not args.config_dir.is_dir(): @@ -172,14 +170,17 @@ def main() -> int: if cameras_enabled: logging.info("Start camera backend") + camera_driver = CameraDriver("camera60", "camera180", "camera300") + # all cameras have the same frame rate, so just use the first one + camera_fps = camera_driver.get_sensor_info().camera[0].frame_rate_fps + # make sure camera time series covers at least one second (add some # margin to avoid problems) - CAMERA_TIME_SERIES_LENGTH = int(CAMERA_FPS * 1.5) + camera_time_series_length = int(camera_fps * 1.5) camera_data = tricamera.MultiProcessData( - "tricamera", True, CAMERA_TIME_SERIES_LENGTH + "tricamera", True, camera_time_series_length ) - camera_driver = CameraDriver("camera60", "camera180", "camera300") camera_backend = tricamera.Backend(camera_driver, camera_data) logging.info("Camera backend ready.") @@ -229,7 +230,7 @@ def main() -> int: episode_length_s = args.max_number_of_actions / ROBOT_RATE_HZ # Compute camera log size based on number of robot actions plus a 10% # buffer - log_size = int(CAMERA_FPS * episode_length_s * buffer_length_factor) + log_size = int(camera_fps * episode_length_s * buffer_length_factor) logging.info("Initialize camera logger with buffer size %d", log_size) camera_logger = tricamera.Logger(camera_data, log_size) From fae465ff150c354ef36fa4005811a2fc0ee7292c Mon Sep 17 00:00:00 2001 From: Felix Kloss Date: Tue, 17 Sep 2024 17:33:51 +0200 Subject: [PATCH 6/6] Update CHANGELOG --- CHANGELOG.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 4d6ec37..4b65a78 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -30,6 +30,9 @@ - Disable position limit checks during shutdown trajectory. This allows setting the rest position of the TriFingerPro to a more suitable position which is outside of the operation limits. +- Camera frame rate is not hard-coded anymore in `trifinger_data_backend` and + `trifinger_backend`. Instead it is fetched from the camera settings + (`trifinger_data_backend`) or the camera driver directly (`trifinger_backend`). ### Fixed - Update demo_data_logging to changed interface of the RobotLogger class.