Camera Image Display Example

Created by Rico Stodt, Modified on Mon, 14 Feb, 2022 at 2:24 PM by Rico Stodt

This page provides a detailed example of how to display camera images.

Contents
Introduction
Usage
Arguments
View Image Results
Code Walkthrough

Introduction

The camera display example demonstrates the display of cameras on Sawyer robot. The user can choose to display unrectified image or not, as well as the Canny edge detection image. If you would like to follow along with the actual source code for the example on GitHub, it can be found through this link for camera image display example.

Usage

Start the camera display example program, you can specify the camera name (right_hand_camera or head_camera), use of the raw image (unrectified) topic or use of streaming the Canny edge detection image. The default camera name is "head_camera". The camera image will be rectified image without Canny edge detection by default.

Run the example as following command:

$ rosrun intera_examples camera_display.py

Arguments

Important Arguments:

-c or --camera : Choose camera name for display

-r or --raw : Use the "raw" (unrectified) image streaming topic (default is rectified)

-e or --edge : Apply the Canny edge detection algorithm to streamed image

usage: camera_display.py [-h] [-c {head_camera,right_hand_camera}] [-r] [-e]

Camera Display Example
    

optional arguments:
  -h, --help            show this help message and exit
  -c {head_camera,right_hand_camera}, --camera {head_camera,right_hand_camera}
                        Setup Camera Name for Camera Display
  -r, --raw             Specify use of the raw image (unrectified) topic
  -e, --edge            Streaming the Canny edge detection image

View Image Results

Here listed sample image results by using the right_hand_camera on robot arm.

Chairsmall nr.png

The first image shows the original camera image, with typical lens distortion. The second image uses the OpenCV camera calibration parameters to rectify the image, making it undistorted. In our example, by default, streaming the rectified image instead of raw image unless you specify raw image by adding argument -r or --raw.

Chairsmall edge nr.png

The images above are streaming the Canny edge detection image, the first one showing raw image with Canny edge detection, the second one showing rectified image.

Code Walkthrough

Now, let's break down the code.

import argparse
import numpy as np

import cv2
from cv_bridge import CvBridge, CvBridgeError

import rospy
import intera_interface

This imports the intera interface for accessing the camera class.

def show_image_callback(img_data, (edge_detection, window_name)):
    """The callback function to show image by using CvBridge and cv
    """
    bridge = CvBridge()
    try:
        cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
    except CvBridgeError, err:
        rospy.logerr(err)
        return
    if edge_detection == True:
        gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
        blurred = cv2.GaussianBlur(gray, (3, 3), 0)
        # customize the second and the third argument, minVal and maxVal
        # in function cv2.Canny if needed
        get_edge = cv2.Canny(blurred, 10, 100)
        cv_image = np.hstack([get_edge])
    edge_str = "(Edge Detection)" if edge_detection else ''
    cv_win_name = ' '.join([window_name, edge_str])
    cv2.namedWindow(cv_win_name, 0)
    # refresh the image on the screen
    cv2.imshow(cv_win_name, cv_image)
cv2.waitKey(3)

An instance of the CvBridge, bridge is created. Convert the image message to cv2. If the user choose to show image edge detection, the function will convert cv_image to black/white image and blur the image by using GaussianBlur then get the image edge by implementing the Canny method. Note: the image will always refresh so close the image window will not shutdown the image window.

def main():
    """Camera Display Example
    """
    rp = intera_interface.RobotParams()
    valid_cameras = rp.get_camera_names()
    if not valid_cameras:
        rp.log_message(("Cannot detect any camera_config"
            " parameters on this robot. Exiting."), "ERROR")
        return
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__)
    parser.add_argument(
        '-c', '--camera', type=str, default="head_camera",
        choices=valid_cameras, help='Setup Camera Name for Camera Display')
    parser.add_argument(
        '-r', '--raw', action='store_true', 
        help='Specify use of the raw image (unrectified) topic')
    parser.add_argument(
        '-e', '--edge', action='store_true',
        help='Streaming the Canny edge detection image')
    args = parser.parse_args()

Three optional arguments camera, raw and edge are captured from the command line arguments.

    print("Initializing node... ")
    rospy.init_node('camera_display', anonymous=True)
    camera = intera_interface.Cameras()
    if not camera.verify_camera_exists(args.camera):
        rospy.logerr("Invalid camera name, exiting the example.")
        return
    camera.start_streaming(args.camera)
    rectify_image = not args.raw
    use_canny_edge = args.edge
    camera.set_callback(args.camera, show_image_callback,
        rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera))

    def clean_shutdown():
        print("Shutting down camera_display node.")
        cv2.destroyAllWindows()

    rospy.on_shutdown(clean_shutdown)
    rospy.loginfo("Camera_display node running. Ctrl-c to quit")
    rospy.spin()


if __name__ == '__main__':
main()

The node is initialized and an instance of the camera class is created. After verified the camera name, the camera start streaming, the callback function being set, the callback function show_image_callback will be called. Press Ctrl-C to quit the example.