69 lines
2.6 KiB
Python
69 lines
2.6 KiB
Python
# 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()
|