Bug fiix import patron.xacro and append packages realsense2

src/realsense2_camera/scripts/topic_hz.py

@@ -0,0 +1,68 @@
+# Copyright 2023 RealSense, Inc. All Rights Reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import rclpy
+from rclpy.node import Node
+import sensor_msgs.msg
+from realsense2_camera_msgs.msg import RGBD
+import sys
+import time
+
+class ImageListener(Node):
+    def __init__(self, topic):
+        self.topic = topic
+        super().__init__('topic_hz')
+        if 'points' in topic:
+            self.sub = self.create_subscription(sensor_msgs.msg.PointCloud2, topic, self.imageDepthCallback, 1)
+        elif 'image' in topic:
+            self.sub = self.create_subscription(sensor_msgs.msg.Image, topic, self.imageDepthCallback, 1)
+        elif 'rgb' in topic:
+            self.sub = self.create_subscription(RGBD, topic, self.imageDepthCallback, 1)
+        else:
+            raise ('Unknown message type for topic ', topic)
+        # self.sub # prevent unused variable warning
+        self.message_times = []
+        self.max_buffer_size = 100
+        self.print_time = time.time()
+
+    def imageDepthCallback(self, data):
+        crnt_time = time.time()
+        self.message_times.append(crnt_time)
+        if len(self.message_times) > self.max_buffer_size:
+            del self.message_times[0]
+        if crnt_time - self.print_time > 1 and len(self.message_times) > 1:
+            rate = len(self.message_times) / (self.message_times[-1] - self.message_times[0])
+            print('Frame rate at time: %s: %.02f(Hz)' % (time.ctime(crnt_time), rate))
+            # sys.stdout.write('%s: Depth at center(%d, %d): %f(mm)\r' % (self.topic, pix[0], pix[1], cv_image[pix[1], pix[0]]))
+            # sys.stdout.flush()
+
+def main():
+    if len(sys.argv) < 2 or '-h' in sys.argv or '--help' in sys.argv:
+        print ()
+        print ('USAGE:')
+        print ('------')
+        print ('python3 ./topic_hz.py <topic>')
+        print ('Application to act as ros2 topic hz : print the rate of which the messages on given topic arrive.')
+        print ()
+        sys.exit(0)
+
+    topic = sys.argv[1]
+    listener = ImageListener(topic)
+    rclpy.spin(listener)
+    listener.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    rclpy.init(args=sys.argv)
+    main()