ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange |
1 | initial version |
I have done something similar in one of my projects but due to confidential reasons, I cant simply give you the code but I'll structure a general idea of how to implement it. I hope that this might be what you and other people are looking for since I was also struggling with this back then:
Here is how you can get the live camera feed from your camera plugin:
#!/usr/bin/env python
import rclpy
import time
import message_filters
import cv2
from cv_bridge import CvBridge, CvBridgeError
from rclpy.qos import qos_profile_sensor_data, QoSProfile
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from image_geometry import PinholeCameraModel
from eufs_msgs.msg import BoundingBoxes
class BoundingBox():
def __init__(self):
self.cone_list = {'blue':[],'yellow':[],'orange':[], 'big orange':[], 'unknown':[]}
class DrawBoundingBox(Node):
def __init__(self):
super().__init__('bounding_box_node')
self.declare_parameter('camera_image_topic', 'zed/left/image_rect_color')
self.camera_image_topic = self.get_parameter('camera_image_topic').get_parameter_value().string_value
qos_profile = qos_profile_sensor_data
# For image topic, you need to set it to equal to qos_profile sensor data because we are using
# the data from the zed camera
self.image_sub = message_filters.Subscriber(
self,
Image,
self.camera_image_topic,
qos_profile=qos_profile)
self.bounding_box_sub = message_filters.Subscriber(
self,
BoundingBoxes,
'/simulate_bounding_boxes')
sync_ = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.bounding_box_sub], 10, 0.5)
sync_.registerCallback(self.image_callback)
def image_callback(self, image_msg, bounding_box_msg):
debug = False
try:
self.bridge = CvBridge()
cv_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8")
for cone in bounding_box_msg.bounding_boxes:
if debug:
cv2.circle(cv_image, (int(cone.xmin), int(cone.ymin)), 3, (255,0,0))
else:
cv2.rectangle(cv_image, (int(cone.xmin), int(cone.ymax)), (int(cone.xmax),int(cone.ymin)), (255,0,0))
cv2.imshow(f'From {self.camera_image_topic} ', cv_image)
cv2.waitKey(1)
except CvBridgeError as e:
print(e)
def main(args=None):
rclpy.init(args=args)
bounding_box_node = DrawBoundingBox()
try:
while rclpy.ok():
rclpy.spin(bounding_box_node)
except KeyboardInterrupt:
pass
cv2.destroyAllWindows()
bounding_box_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Hope it helps!
2 | No.2 Revision |
I have done something similar in one of my projects but due to confidential reasons, I cant simply give you the code but I'll structure a general idea of how to implement it. I hope that this might be what you and other people are looking for since I was also struggling with this back then:
Here is how you can get the live camera feed from your camera plugin:
#!/usr/bin/env python
import rclpy
import time
import message_filters
import cv2
from cv_bridge import CvBridge, CvBridgeError
from rclpy.qos import qos_profile_sensor_data, QoSProfile
from rclpy.node import Node
from sensor_msgs.msg import Image, CameraInfo
from image_geometry import PinholeCameraModel
from eufs_msgs.msg import BoundingBoxes
class BoundingBox():
def __init__(self):
self.cone_list = {'blue':[],'yellow':[],'orange':[], 'big orange':[], 'unknown':[]}
class DrawBoundingBox(Node):
def __init__(self):
super().__init__('bounding_box_node')
self.declare_parameter('camera_image_topic', 'zed/left/image_rect_color')
self.camera_image_topic = self.get_parameter('camera_image_topic').get_parameter_value().string_value
qos_profile = qos_profile_sensor_data
# For image topic, you need to set it to equal to qos_profile sensor data because we are using
# the data from the zed camera
self.image_sub = message_filters.Subscriber(
self,
Image,
self.camera_image_topic,
qos_profile=qos_profile)
self.bounding_box_sub = message_filters.Subscriber(
self,
BoundingBoxes,
'/simulate_bounding_boxes')
sync_ = message_filters.ApproximateTimeSynchronizer([self.image_sub, self.bounding_box_sub], 10, 0.5)
sync_.registerCallback(self.image_callback)
def image_callback(self, image_msg, bounding_box_msg):
debug = False
try:
self.bridge = CvBridge()
cv_image = self.bridge.imgmsg_to_cv2(image_msg, desired_encoding="bgr8")
for cone in bounding_box_msg.bounding_boxes:
if debug:
cv2.circle(cv_image, (int(cone.xmin), int(cone.ymin)), 3, (255,0,0))
else:
cv2.rectangle(cv_image, (int(cone.xmin), int(cone.ymax)), (int(cone.xmax),int(cone.ymin)), (255,0,0))
cv2.imshow(f'From {self.camera_image_topic} ', cv_image)
cv2.waitKey(1)
except CvBridgeError as e:
print(e)
def main(args=None):
rclpy.init(args=args)
bounding_box_node = DrawBoundingBox()
try:
while rclpy.ok():
rclpy.spin(bounding_box_node)
except KeyboardInterrupt:
pass
cv2.destroyAllWindows()
bounding_box_node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
Hope it helps!
Note: Please be aware that the axis when trying to use the image geometry pinholecameramodel is a bit different than gazebo's axes.