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.
The first image shows the original camera image, with typical lens distortio. 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.
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.
30 import argparse
31 import numpy as np
32
33 import cv2
34 from cv_bridge import CvBridge, CvBridgeError
35
36 import rospy
37 import intera_interface
This imports the intera interface for accessing the camera class.
39 def show_image_callback(img_data, (edge_detection, window_name)):
40 """The callback function to show image by using CvBridge and cv
41 """
42 bridge = CvBridge()
43 try:
44 cv_image = bridge.imgmsg_to_cv2(img_data, "bgr8")
45 except CvBridgeError, err:
46 rospy.logerr(err)
47 return
48 if edge_detection == True:
49 gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
50 blurred = cv2.GaussianBlur(gray, (3, 3), 0)
51 # customize the second and the third argument, minVal and maxVal
52 # in function cv2.Canny if needed
53 get_edge = cv2.Canny(blurred, 10, 100)
54 cv_image = np.hstack([get_edge])
55 edge_str = "(Edge Detection)" if edge_detection else ''
56 cv_win_name = ' '.join([window_name, edge_str])
57 cv2.namedWindow(cv_win_name, 0)
58 # refresh the image on the screen
59 cv2.imshow(cv_win_name, cv_image)
60 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.
62 def main():
63 """Camera Display Example
64 """
65 rp = intera_interface.RobotParams()
66 valid_cameras = rp.get_camera_names()
67 if not valid_cameras:
68 rp.log_message(("Cannot detect any camera_config"
69 " parameters on this robot. Exiting."), "ERROR")
70 return
71 arg_fmt = argparse.RawDescriptionHelpFormatter
72 parser = argparse.ArgumentParser(formatter_class=arg_fmt,
73 description=main.__doc__)
74 parser.add_argument(
75 '-c', '--camera', type=str, default="head_camera",
76 choices=valid_cameras, help='Setup Camera Name for Camera Display')
77 parser.add_argument(
78 '-r', '--raw', action='store_true',
79 help='Specify use of the raw image (unrectified) topic')
80 parser.add_argument(
81 '-e', '--edge', action='store_true',
82 help='Streaming the Canny edge detection image')
83 args = parser.parse_args()
Three optional arguments camera, raw and edge are captured from the command line arguments.
85 print("Initializing node... ")
86 rospy.init_node('camera_display', anonymous=True)
87 camera = intera_interface.Cameras()
88 if not camera.verify_camera_exists(args.camera):
89 rospy.logerr("Invalid camera name, exiting the example.")
90 return
91 camera.start_streaming(args.camera)
92 rectify_image = not args.raw
93 use_canny_edge = args.edge
94 camera.set_callback(args.camera, show_image_callback,
95 rectify_image=rectify_image, callback_args=(use_canny_edge, args.camera))
96
97 def clean_shutdown():
98 print("Shutting down camera_display node.")
99 cv2.destroyAllWindows()
100
101 rospy.on_shutdown(clean_shutdown)
102 rospy.loginfo("Camera_display node running. Ctrl-c to quit")
103 rospy.spin()
104
105
106 if __name__ == '__main__':
107 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.
