Part 6 Object Detection Node (Complete)

Here's a full example of the object_detection.py node that you should have developed during Part 6 Exercise 2. Also included here is an illustration of how to use the cv2.circle() method to create a marker on an image illustrating the centroid of the detected feature, as discussed here.

object_detection_complete.py

#!/usr/bin/env python3

import rospy
from pathlib import Path

import cv2
from cv_bridge import CvBridge, CvBridgeError

from sensor_msgs.msg import Image

node_name = "object_detection"
rospy.init_node(node_name)
print(f"Launched the '{node_name}' node. Currently waiting for an image...")
rate = rospy.Rate(5)

base_image_path = Path.home().joinpath("myrosdata/object_detection/")
base_image_path.mkdir(parents=True, exist_ok=True)

cvbridge_interface = CvBridge()

waiting_for_image = True

def show_and_save_image(img, img_name):
    full_image_path = base_image_path.joinpath(f"{img_name}.jpg")

    print("Opening the image in a new window...")
    cv2.imshow(img_name, img)
    print(f"Saving the image to '{full_image_path}'...")
    cv2.imwrite(str(full_image_path), img)
    print(f"Saved an image to '{full_image_path}'\n"
        f"image dims = {img.shape[0]}x{img.shape[1]}px\n"
        f"file size = {full_image_path.stat().st_size} bytes")
    print("**IMPORTANT: Close the image pop-up window to continue!**")
    cv2.waitKey(0)

def camera_cb(img_data):
    global waiting_for_image  
    try:
        cv_img = cvbridge_interface.imgmsg_to_cv2(img_data, desired_encoding="bgr8")
    except CvBridgeError as e:
        print(e)

    if waiting_for_image == True:
        height, width, channels = cv_img.shape

        print(f"Obtained an image of height {height}px and width {width}px.")

        show_and_save_image(cv_img, img_name = "step1_original")

        crop_width = width - 400
        crop_height = 400
        crop_y0 = int((width / 2) - (crop_width / 2))
        crop_z0 = int((height / 2) - (crop_height / 2))
        cropped_img = cv_img[crop_z0:crop_z0+crop_height, crop_y0:crop_y0+crop_width]

        show_and_save_image(cropped_img, img_name = "step2_cropping")

        hsv_img = cv2.cvtColor(cropped_img, cv2.COLOR_BGR2HSV)
        lower_threshold = (115, 225, 100)
        upper_threshold = (130, 255, 255)
        img_mask = cv2.inRange(hsv_img, lower_threshold, upper_threshold)

        show_and_save_image(img_mask, img_name = "step3_image_mask")

        filtered_img = cv2.bitwise_and(cropped_img, cropped_img, mask = img_mask)

        # FINDING THE IMAGE CENTROID: (1)
        m = cv2.moments(img_mask) # (2)!
        cy = m['m10'] / (m['m00'] + 1e-5)
        cz = m['m01'] / (m['m00'] + 1e-5) # (3)!
        cv2.circle(
            filtered_img, 
            (int(cy), int(cz)), 
            10, (0, 0, 255), 2) # (4)!

        show_and_save_image(filtered_img, img_name = "step4_filtered_image")

        waiting_for_image = False

rospy.Subscriber("/camera/rgb/image_raw", Image, camera_cb)

while waiting_for_image:
    rate.sleep()

cv2.destroyAllWindows()

1. Everything here should be familiar to you from earlier in this exercise, except for this section...

2. Here, we obtain the moments of our colour blob by providing the boolean representation of it (i.e. the img_mask) to the cv2.moments() function.

3. Then, we are determining where the central point of this colour blob is located by calculating the cy and cz coordinates of it. This provides us with pixel coordinates relative to the top left-hand corner of the image.

4. Finally, this function allows us to draw a circle on our image at the centroid location so that we can visualise it. Into this function we pass:

   1. The image that we want the circle to be drawn on. In this case: filtered_img.
   2. The location that we want the circle to be placed, specifying the horizontal and vertical pixel coordinates respectively: (int(cy), int(cz)).
   3. How big we want the circle to be: here we specify a radius of 10 pixels.
   4. The colour of the circle, specifying this using a Blue-Green-Red colour space: (0, 0, 255) (i.e.: pure red in this case)
   5. Finally, the thickness of the line that will be used to draw the circle, in pixels.

← Back to Part 6 - Exercise 2