Merge pull request #176 from open-dynamic-robot-initiative/fkloss/cam…

…era_settings

Do not hard-code camera frame rate

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.

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)

scripts/trifinger_data_backend.py

@@ -3,6 +3,7 @@
 
 Runs the leader multi-processing robot/sensor data and loggers.
 """
+
 import argparse
 import sys
 
@@ -11,15 +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():
+def parse_args() -> argparse.Namespace:
     parser = argparse.ArgumentParser(description=__doc__)
     parser.add_argument(
         "--max-number-of-actions",
@@ -58,6 +58,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")
 
@@ -74,8 +83,21 @@ def main():
     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")
@@ -92,15 +114,12 @@ def main():
         robot_logger.start()
 
     if cameras_enabled and args.camera_logfile:
-        camera_fps = 10
-        robot_rate_hz = 1000
         # 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
-        log_size = int(camera_fps * episode_length_s * buffer_length_factor)
+        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_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)