ROS and OpenCV: sending single images, image type, and receiving laptop camera stream
WARNING: super beginner
For a week or so, me and a few others were working with openCV outside of ROS and we got some things up and running. It has fallen upon me to figure out how to get it all translated into a form run-able via ROS. Using the example node from this tutorial as a template, I created what I hope is a functional node that isolates pixels with certain B, G, or R values. I'll post the code for said node at the bottom (it's short).
On to my actual questions though:
- How can I send it a single image? This is probably just a nomenclature misunderstanding, but the tutorial indicates that the node will require an image stream as input but to date I've only been using individual image files.
- What is the type for ROS images? I may need to create my own message type so I can add more parameters to the code.
- How can I get an input stream from my laptop's webcam? I don't have frequent access to the Husky that we will eventually be using, so it would be nice to be able to use that.
Below is the code for my node. It shouldn't be necessary to answer, so feel free to skip it.
#!/usr/bin/env python
import roslib; roslib.load_manifest('ports')
import sys
import rospy
import cv
import numpy as np
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
class isolateColorServer:
def __init__(self):
self.image_pub = rospy.Publisher("image_topic_2", Image)
cv.NamedWindow("Image Window", 1)
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("image_topic", Image, self.callback)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
except CvBridgeError, e:
print e
color = 'R'
value = 128
if (color == 'R'):
cv_image[np.where((cv_image >= [0, 0, value]).all(axis=2))] = [0, 0, 255]
cv_image[np.where((cv_image <= [255, 255, value]).all(axis=2))] = [255, 255, 255]
elif (color == 'G'):
cv_image[np.where((cv_image >= [0, value, 0]).all(axis=2))] = [0, 255, 0]
cv_image[np.where((cv_image <= [255, value, 255]).all(axis=2))] = [255, 255, 255]
elif (color == 'B'):
cv_image[np.where((cv_image >= [value, 0, 0]).all(axis=2))] = [255, 0, 0]
cv_image[np.where((cv_image <= [value, 255, 255]).all(axis=2))] = [255, 255, 255]
else:
print("You picked an incorrect value for the color field. You need to pick R, G, or B.")
cv.ShowImage("Image Window", cv_image)
cv.WaitKey(3)
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError, e:
print e
def main(args):
ics = isolateColorServer()
rospy.init_node('isolateColorServer', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print "Shutting Down"
cv.DestroyAllWindows()
if __name__ == '__main__':
main(sys.argv);
Have you done the beginner tutorials http://ros.org/wiki/ROS/Tutorials ?