Revision history [back]

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:

1. You would have to write some ground truth object position which would find where the object's position is with respect to your robot base_link and publish those messages into a topic
2. Now, your robot base_link is generally the "world coordinate" and you would have to find the transformation from base_link to the camera_frame. have a look at slide 7
3. After transforming it to the camera coordinate, now you should project the 3d image into 2d image plane and this can be done easily using the image geometry pinholecameramodel
4. You should now get the pixel coordinate of the object's position with respect to the camera
5. If your robot has a camera plugin, you can get the live camera feed by subscribing to the any topics with an image type

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!

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:

1. You would have to write some ground truth object position which would find where the object's position is with respect to your robot base_link and publish those messages into a topic
2. Now, your robot base_link is generally the "world coordinate" and you would have to find the transformation from base_link to the camera_frame. have a look at slide 7
3. After transforming it to the camera coordinate, now you should project the 3d image into 2d image plane and this can be done easily using the image geometry pinholecameramodel
4. You should now get the pixel coordinate of the object's position with respect to the camera
5. If your robot has a camera plugin, you can get the live camera feed by subscribing to the any topics with an image type

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!