ROS OpenCV hello

ros kinetic work with openCV 3.1

sudo apt install ros-kinetic-opencv3

• after install check with python

> import cv2
> cv2.__version__
'3.3.1-dev'

• Install helper utils and packages

sudo apt install ros-kinetic-usb-cam
sudo apt install ros-kinetic-image-view

CvBridge

Convert ROS image messages (sensor_msgs/Images) and OpenCv images (cv::Mat)

Demo

• Use as Gazebo camera plugin post as Image source
• Create package named cv_demo run the source code as rosnode cvnode.py package dependencies: sensor_msgs, opencv2, cv_bridge, rospy
• The node create OpenCv image window
• Use rqt to view publish image image_topic

Demo setup

• Terminal 1 (gazebo as image source)

roslaunch gazebo_ros empty_world.launch

• Terminal 2 (spawn camera)

rosrun gazebo_ros spawn_model -file camera.urdf -urdf -model bot

• Termianl 3 (run our node)

rosrun cv_demo cvnode.py

• Termianl 4 (rqt)

rqt

Reference

• Converting between ROS images and OpenCV images

Source code

#!/usr/bin/env python

import rospy
import cv2

from cv_bridge import CvBridge, CvBridgeError
import numpy as np
from sensor_msgs.msg import Image, CameraInfo

class CvBridgeDemo(object):
    NODE_NAME = "cv_demo"
    WIN_NAME = "cv_demo_win"

    def __init__(self):
        
        self._bridge = CvBridge()
        self.image_sub = rospy.Subscriber("/bot/camera/image_raw", Image, self.image_callback)
        self.image_pub = rospy.Publisher("image_topic", Image)
        rospy.loginfo("watting for image")

    def image_callback(self, ros_image):
        # Use cv_bridge() to convert the ROS image to OpenCV format
        bridge = self._bridge
        try:
            frame = bridge.imgmsg_to_cv2(ros_image, "bgr8")
        except CvBridgeError, e:
            rospy.logerr("fail convert ros image to cv: {}".format(e))
        
        # Convert the image to a Numpy array for cv2 functions
        frame = np.array(frame, dtype=np.uint8)
        
        # Process the frame using the process_image() function
        display_image = self.process_image(frame)
                       
        # Display the image.
        cv2.imshow(CvBridgeDemo.WIN_NAME, display_image)
        
        # Process any keyboard commands
        cv2.waitKey(3)

        try:
            self.image_pub.publish(self._bridge.cv2_to_imgmsg(display_image, "mono8"))
        except CvBridgeError as e:
            rospy.logerr("fail convert cv image to rosmsg: {}".format(e))

    def process_image(self, frame):
        img = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        font = cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(img,'OpenCV',(10,500), font, 4,(255,255,255),2,cv2.LINE_AA)
        return img

def on_shutdown():
    print "Shutting down cvnode."
    cv2.destroyAllWindows()

def main(): 
    rospy.init_node(CvBridgeDemo.NODE_NAME)      
    rospy.on_shutdown(on_shutdown)
    
    while not rospy.is_shutdown():
        CvBridgeDemo()
        rospy.loginfo("wait for shutdown")
        #The spin() code simply sleeps until the is_shutdown() flag is True. 
        # It is mainly used to prevent your Python Main thread from exiting.
        rospy.spin()
        

if __name__ == '__main__':
    main()