-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathROS_ImageSubscriber.py
59 lines (44 loc) · 1.98 KB
/
ROS_ImageSubscriber.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
# Import the necessary libraries
import rclpy # Python library for ROS 2
from rclpy.qos import qos_profile_sensor_data
from rclpy.node import Node # Handles the creation of nodes
from sensor_msgs.msg import Image # Image is the message type
from cv_bridge import CvBridge, CvBridgeError # Package to convert between ROS and OpenCV Images
from functools import partial #std::bind for python
from threading import Thread
class ImageSubscriber(Node):
def __init__(self):
# Initiate the Node class's constructor and give it a name
super().__init__('ImageSubscriber')
self._callbacks = {}
def subscribe(self, ros_topic_name, callback):
self._callbacks[ros_topic_name] = callback
subscriber_callback_bound = partial(self.subscriber_callback, ros_topic_name_=ros_topic_name)
# Create the subscriber. This subscriber will receive an Image from the topic. The queue size is 10 messages.
self.create_subscription(
Image,
ros_topic_name,
subscriber_callback_bound,
qos_profile_sensor_data)
self.get_logger().info('Subscribed on ' + ros_topic_name)
def subscriber_callback(self, data, ros_topic_name_):
try:
# Convert ROS Image message to OpenCV/numpy image
imageNumpy = CvBridge().imgmsg_to_cv2(data)
self.get_logger().info('Received video frame dimensions {0}x{1}'.format(imageNumpy.shape[1], imageNumpy.shape[0]))
callback = self._callbacks.get(ros_topic_name_)
if (callback is not None):
callback(imageNumpy)
except CvBridgeError as exc:
self.get_logger().error(exc)
class ImageSubscriberWrapper:
def __init__(self):
rclpy.init()
self.node = ImageSubscriber()
self.thread = Thread(target=self._threadFunc)
self.thread.start()
def subscribe(self, ros_topic_name, callback = None):
self.node.subscribe(ros_topic_name, callback)
def _threadFunc(self):
rclpy.spin(self.node)
rclpy.shutdown()