Added Profiling code

aria_data_loaders/aria_data_utils/human_sensor_data_streamer_interface.py

@@ -10,42 +10,50 @@
 import cv2
 import numpy as np
 import rospy
-import sophus as sp
+
+try:
+    import sophuspy as sp
+except Exception as e:
+    print(f"Cannot import sophuspy due to {e}. Import sophus instead")
+    import sophus as sp
+
+# ROS imports
+from geometry_msgs.msg import Pose, PoseStamped
+from nav_msgs.msg import Odometry
 from perception_and_utils.perception.detector_wrappers.april_tag_detector import (
     AprilTagDetectorWrapper,
 )
-from perception_and_utils.perception.detector_wrappers.object_detector import (
-    ObjectDetectorWrapper,
-)
 from perception_and_utils.perception.detector_wrappers.human_motion_detector import (
     HumanMotionDetector,
 )
-
+from perception_and_utils.perception.detector_wrappers.object_detector import (
+    ObjectDetectorWrapper,
+)
+from perception_and_utils.utils.data_frame import DataFrame
 from spot_rl.utils.utils import ros_frames as rf
-
-# ROS imports
-from geometry_msgs.msg import Pose, PoseStamped
-from nav_msgs.msg import Odometry
 from std_msgs.msg import String
 from tf2_ros import StaticTransformBroadcaster
 from visualization_msgs.msg import Marker
 
-from perception_and_utils.utils.data_frame import DataFrame
 
 class CameraParams:
     """
     CameraParams class to store camera parameters like focal length, principal point etc.
     It stores the image, timestamp and frame number of each received image
     """
-    def __init__(self, focal_lengths: Tuple[float, float], principal_point: Tuple[float, float]):
+
+    def __init__(
+        self, focal_lengths: Tuple[float, float], principal_point: Tuple[float, float]
+    ):
         self._focal_lengths = focal_lengths  # type: Optional[Tuple[float, float]]
         self._principal_point = principal_point  # type: Optional[Tuple[float, float]]
 
+
 class HumanSensorDataStreamerInterface:
     def __init__(self, verbose: bool = False) -> None:
         self.verbose = verbose
 
-        self._is_connected = False # TODO : Maybe not needed
+        self._is_connected = False  # TODO : Maybe not needed
 
         # TODO: Update this later with PerceptionPipeline
         # Create all detectors
@@ -57,10 +65,13 @@ def __init__(self, verbose: bool = False) -> None:
         self.static_tf_broadcaster = StaticTransformBroadcaster()
 
         self.human_activity_history_pub = rospy.Publisher(
-            "/human_activity_history", String, queue_size=10)
+            "/human_activity_history", String, queue_size=10
+        )
 
         # TODO: Define DEVICE FRAME as a visual frame of reference i.e. front facing frame of reference
-        self.device_T_camera: Optional[sp.SE3] = None # TODO: Should be initialized by implementations of this class
+        self.device_T_camera: Optional[
+            sp.SE3
+        ] = None  # TODO: Should be initialized by implementations of this class
 
         # Maintain a list of all poses where qr code is detected (w.r.t deviceWorld)
         self.marker_positions_list = (
@@ -155,8 +166,7 @@ def initialize_object_detector(
         raise NotImplementedError
 
     def initialize_human_motion_detector(self, outputs: dict = {}):
-        """
-        """
+        """ """
         raise NotImplementedError
 
     ##### ROS Publishers #####
@@ -176,10 +186,12 @@ def publish_human_pose(self, data_frame: DataFrame):
         pose_stamped.header.stamp = rospy.Time().now()
         pose_stamped.header.seq = data_frame._frame_number
         pose_stamped.header.frame_id = rf.QUEST3_CAMERA
-        pose_stamped.pose = data_frame._deviceWorld_T_camera_rgb # TODO: Use a VIZ frame which is forward facing
+        pose_stamped.pose = (
+            data_frame._deviceWorld_T_camera_rgb
+        )  # TODO: Use a VIZ frame which is forward facing
         self.human_pose_publisher.publish(pose_stamped)
 
-    def publish_marker(self, data_frame: DataFrame, marker_scale: float=0.1):
+    def publish_marker(self, data_frame: DataFrame, marker_scale: float = 0.1):
         """
         Publishes marker at pose of interest
 
@@ -212,7 +224,7 @@ def publish_marker(self, data_frame: DataFrame, marker_scale: float=0.1):
 
         self.pose_of_interest_marker_publisher.publish(marker)
 
-    def publish_human_activity_history(self, human_history: List[Tuple[float,str]]):
+    def publish_human_activity_history(self, human_history: List[Tuple[float, str]]):
         """
         Publishes human activity history as a string
 
@@ -227,7 +239,9 @@ def publish_human_activity_history(self, human_history: List[Tuple[float,str]]):
         # Iterate over human history and create a string
         human_history_str = ""
         for timestamp, activity in human_history:
-            human_history_str += f"{timestamp},{activity}|"  # str = "time1,Standing|time2,Walking"
+            human_history_str += (
+                f"{timestamp},{activity}|"  # str = "time1,Standing|time2,Walking"
+            )
 
         # Publish human activity history string
         self.human_activity_history_pub.publish(human_history_str)

aria_data_loaders/aria_data_utils/quest3_data_streamer.py

@@ -19,14 +19,19 @@
 
 import logging
 import time
+import traceback
 from typing import Any, Dict, List, Optional, Tuple
 
 import click
 import cv2
 import matplotlib.pyplot as plt
 import numpy as np
 import rospy
-import sophus as sp
+try:
+    import sophuspy as sp
+except Exception as e:
+    print(f"Cannot import sophuspy due to {e}. Import sophus instead")
+    import sophus as sp
 from perception_and_utils.utils.conversions import (
     sophus_SE3_to_ros_PoseStamped,
     sophus_SE3_to_ros_TransformStamped,
@@ -47,7 +52,10 @@ def __init__(self, verbose: bool = False) -> None:
         handler.setFormatter(formatter)
         self.logger.addHandler(handler)
 
-        self.frame_rate_counter = FrameRateCounter()
+        self.frc_all = FrameRateCounter()
+        self.frc_qrd = FrameRateCounter()
+        self.frc_hmd = FrameRateCounter()
+        self.frc_od = FrameRateCounter()
 
         self.unified_quest3_camera = UnifiedQuestCamera()
         self.rgb_cam_params: CameraParams = None
@@ -60,14 +68,14 @@ def connect(self):
         Connect to Device
         """
         self._is_connected = True
-        self.logger("Quest3DataStreamer :: Connected")
+        self.logger.info("Quest3DataStreamer :: Connected")
 
     def disconnect(self):
         """
         Disconnects from Device cleanly
         """
         self._is_connected = False
-        self.logger("Quest3DataStreamer :: Disconnected")
+        self.logger.info("Quest3DataStreamer :: Disconnected")
 
     def is_connected(self) -> bool:
         return (
@@ -167,10 +175,11 @@ def get_latest_data_frame(self) -> Optional[DataFrame]:
         data_frame._aligned_depth_frame = None  # TODO: Add this logic
 
         # Update frame rate counter
-        self.frame_rate_counter.update()
+        # self.frame_rate_counter.update()
         data_frame._avg_rgb_fps = self.unified_quest3_camera.get_avg_fps_rgb()
         data_frame._avg_depth_fps = self.unified_quest3_camera.get_avg_fps_depth()
-        data_frame._avg_data_frame_fps = self.frame_rate_counter.avg_value()
+        # data_frame._avg_data_frame_fps = self.frame_rate_counter.avg_value()
+        data_frame._avg_data_frame_fps = 0.0
 
         return data_frame
 
@@ -183,15 +192,32 @@ def process_frame(
         detect_human_motion: bool = False,
         object_label="milk bottle",
     ) -> Tuple[np.ndarray, dict]:
+        rate_all = 0.0
+        rate_qrd = 0.0
+        rate_hmd = 0.0
+        rate_od = 0.0
+
+        self.frc_all.start()
+
         # Initialize object_scores to empty dict for current image frame
         # object_scores = {}  # type: Dict[str, Any]
 
         # Get rgb image from frame
         viz_img = data_frame._rgb_frame
         if viz_img is None:
-            rospy.logwarn("No image found in frame")
+            rospy.logwarn("No image found in frame")  # This gets over-written.. WASTE!
+
+        if detect_objects:
+            self.frc_od.start()
+            viz_img, object_scores = self.object_detector.process_frame(data_frame)
+            # if object_label in object_scores.keys():
+            #     if self.verbose:
+            #         plt.imsave(f"frame_{self.frame_number}.jpg", viz_img)
+            #     self.publish_pose_of_interest(frame.get("ros_pose"))
+            rate_od = self.frc_od.stop()
 
         if detect_qr:
+            self.frc_qrd.start()
             (viz_img, camera_T_marker) = self.april_tag_detector.process_frame(frame=data_frame)  # type: ignore
 
             deviceWorld_T_camera = data_frame._deviceWorld_T_camera_rgb
@@ -240,7 +266,10 @@ def process_frame(
                     viz_img=viz_img,
                 )
 
+            rate_qrd = self.frc_qrd.stop()
+
         if detect_human_motion:
+            self.frc_hmd.start()
             activity_str, avg_velocity = self.human_motion_detector.process_frame(
                 frame=data_frame
             )
@@ -253,14 +282,24 @@ def process_frame(
             self.publish_human_activity_history(
                 self.human_motion_detector.get_human_motion_history()
             )
+            rate_hmd = self.frc_hmd.stop()
 
         if detect_objects:
+            self.frc_od.start()
             viz_img, object_scores = self.object_detector.process_frame(viz_img)
             if object_label in object_scores.keys():
                 if self.verbose:
                     plt.imsave(f"frame_{self.frame_number}.jpg", viz_img)
                 # self.publish_pose_of_interest(frame.get("ros_pose"))
 
+            rate_od = self.frc_od.stop()
+
+        rate_all = self.frc_all.stop()
+
+        outputs["process_all"] = rate_all
+        outputs["process_qr"] = rate_qrd
+        outputs["process_od"] = rate_od
+        outputs["process_hmd"] = rate_hmd
         return viz_img, outputs
 
     def initialize_april_tag_detector(self, outputs: dict = {}):
@@ -345,12 +384,21 @@ def main(do_update_iptables: bool, debug: bool, hz: int):
     rospy.init_node("quest3_data_streamer", log_level=_log_level)
     rospy.loginfo("Starting quest3_data_streamer node")
 
+    # Logger for profiling
+    loggerp = logging.getLogger("Profiler-Quest3ProcessFrame")
+    loggerp.setLevel(logging.INFO)
+    csv_handlerp = logging.FileHandler("quest3_time_profiles_sec.txt")
+    formatterp = logging.Formatter("%(asctime)s, %(message)s")
+    csv_handlerp.setFormatter(formatterp)
+    loggerp.addHandler(csv_handlerp)
+
     if do_update_iptables:
         update_iptables_quest3()
     else:
         rospy.logwarn("Not updating iptables")
 
-    # rate = rospy.Rate(hz)
+    outer_frc = FrameRateCounter()
+    rate = rospy.Rate(hz)
     outputs: Dict[str, Any] = {}
     data_streamer = None
     cv2.namedWindow("Image", cv2.WINDOW_NORMAL)
@@ -364,6 +412,7 @@ def main(do_update_iptables: bool, debug: bool, hz: int):
         data_streamer.initialize_human_motion_detector()
         # data_streamer.connect()
         while not rospy.is_shutdown():
+            outer_frc.start()
             data_frame = data_streamer.get_latest_data_frame()
             if data_frame is not None:
                 if debug:
@@ -380,9 +429,16 @@ def main(do_update_iptables: bool, debug: bool, hz: int):
                 cv2.waitKey(1)
             else:
                 print("No data frame received.")
-    except Exception as e:
+
+
+            outer_rate = outer_frc.stop()
+            msg_str = f"Fetch+AllDetection:{outer_rate},All_detectors={outputs.get('process_all',0.0)},QR={outputs.get('process_qr',0.0)},OD={outputs.get('process_od',0.0)},HMD={outputs.get('process_hmd',0.0)}"
+            loggerp.info(msg_str)
+            print(msg_str)
+
+    except Exception:
         print("Ending script.")
-        rospy.logwarn(f"Exception: {e}")
+        rospy.logwarn(f"Exception: {traceback.print_exc()}")
         if data_streamer is not None:
             data_streamer.disconnect()
 

aria_data_loaders/aria_data_utils/unified_quest_camera_interface.py

@@ -3,7 +3,12 @@
 # LICENSE file in the root directory of this source tree.
 
 from typing import Optional, Tuple
-import sophus as sp
+
+try:
+    import sophuspy as sp
+except Exception as e:
+    print(f"Cannot import sophuspy due to {e}. Import sophus instead")
+    import sophus as sp
 import numpy as np
 
 """
@@ -51,7 +56,7 @@ def get_device_T_depthCamera(self) -> Optional[sp.SE3]:
 
     def get_avg_fps_rgb(self):
         raise NotImplementedError
-    
+
     def get_avg_fps_depth(self):
         raise NotImplementedError