class ImageDebugger:
# # Convert a ROS Image to the Numpy matrix used by cv2 functions
# def rosimg2cv(self, ros_image):
# # Convert from ROS Image to old OpenCV image
# frame = self.cvbridge.imgmsg_to_cv(ros_image, ros_image.encoding)
# # Convert from old OpenCV image to Numpy matrix
# return np.array(frame, dtype=np.uint8) #TODO: find out actual dtype
def __init__(self, topics, factor):
self.cvbridge = CvBridge()
self.topics = topics
self.factor = factor
self.yaw = 0
self.pitch = 0
self.roll = 0
for topic in topics:
outTopic = "/debug" + topic
callback = self.reducerCallback(topic, outTopic, showReticle=('/left/' in topic))
rospy.Subscriber(topic, Image, callback)
rospy.Subscriber('/euler', compass_data, self.gotAttitude)
def gotAttitude(self, msg):
self.yaw = msg.yaw
self.pitch = msg.pitch
self.roll = msg.roll
# Returns a callback for a particular Image topic which reduces the image
# and publishes it
def reducerCallback(self, imageTopic, outTopic, showReticle=False):
publisher = rospy.Publisher(outTopic, Image)
factor = self.factor
def plainCallback(rosImage):
try:
cvimg = self.cvbridge.imgmsg_to_cv(rosImage, desired_encoding="passthrough")
outimg = cv2.cv.CreateMat(int(cvimg.rows * factor), int(cvimg.cols * factor), cvimg.type)
cv2.cv.Resize(cvimg, outimg)
publisher.publish(self.cvbridge.cv_to_imgmsg(outimg, encoding="bgr8")) #TODO: figure out actual encoding
except CvBridgeError, e:
print e
def reticleCallback(rosImage):
try:
cvimg = self.cvbridge.imgmsg_to_cv(rosImage, desired_encoding="passthrough")
cvimg = np.array(cvimg, dtype=np.uint8)
cvimg = cv2.resize(cvimg, (0, 0), None, factor, factor)
outimg = self.drawReticle(cvimg)
outimg = cv2.cv.fromarray(outimg)
publisher.publish(self.cvbridge.cv_to_imgmsg(outimg, encoding="bgr8")) #TODO: figure out actual encoding
except CvBridgeError, e:
print e
def video_descriptor(obj, i, v_dictionary, v_noise):
depth = cv.LoadImage(
globals.path + 'img/' + obj + '-' + str(i) + '-depth.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
mask = cv.LoadImage(globals.path + 'mask.png', cv.CV_LOAD_IMAGE_GRAYSCALE)
if v_noise > 0:
rgb = cv2.imread(
globals.path + 'img/' + obj + '-' + str(i) + '-rgb.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
noise = np.random.randn(*rgb.shape) * v_noise
# Add this noise to image
noisy = rgb + noise
cv2.imwrite(globals.path + 'noisy.png', noisy)
rgb = cv.LoadImage(globals.path + 'noisy.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
#rgb = noisy
else:
rgb = cv.LoadImage(
globals.path + 'img/' + obj + '-' + str(i) + '-rgb.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
#cv.fromarray(rgb)
bridge = CvBridge()
rgb_image = bridge.cv_to_imgmsg(rgb)
depth_image = bridge.cv_to_imgmsg(depth)
mask_image = bridge.cv_to_imgmsg(mask)
#BASE Keypoint descriptors
keypoint_descriptors = []
rospy.wait_for_service('masked_base_descriptor_service')
try:
descriptor_request = rospy.ServiceProxy(
'masked_base_descriptor_service', masked_service)
response = descriptor_request(rgb=rgb_image,
depth=depth_image,
mask=mask_image)
base_descriptor = bridge.imgmsg_to_cv(response.descriptor)
keypoint_descriptors = np.array(base_descriptor)
keypoint_descriptors = keypoint_descriptors.tolist()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def talker():
global capture
bridge = CvBridge()
pub = rospy.Publisher('chatter', Image)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
frame = cv.QueryFrame(capture)
image = frame
image = np.asarray(image[:, :])
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_green = np.array([50, 150, 0])
upper_green = np.array([150, 255, 255])
lower_blue = np.array([80, 50, 0])
upper_blue = np.array([160, 255, 255])
lower_red = np.array([0, 160, 0])
upper_red = np.array([30, 255, 255])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask2 = cv2.inRange(hsv, lower_blue, upper_blue)
mask3 = cv2.inRange(hsv, lower_red, upper_red)
res = cv2.bitwise_and(image, image, mask=mask)
res2 = cv2.bitwise_and(image, image, mask=mask2)
res3 = cv2.bitwise_and(image, image, mask=mask3)
final = res + res2 + res3
frame = cv.fromarray(final)
pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
class CamDebug():
def __init__(self, nodeName, debugOn=True):
self.nodeName = nodeName
self.debugOn = debugOn
self.topics = {}
self.cvbridge = CvBridge()
'''
Publish an image to a stream (useful for debugging).
Expects image to be a numpy array.
'''
def publishImage(self, topicName, image):
if not self.debugOn:
return
if topicName not in self.topics:
fullTopicName = '/' + self.nodeName + '/' + topicName
self.topics[topicName] = rospy.Publisher(fullTopicName, Image)
publisher = self.topics[topicName]
try:
#outImg = array2cv(image)
outImg = cv2.cv.fromarray(image)
publisher.publish(self.cvbridge.cv_to_imgmsg(outImg,encoding='bgr8'))
except CvBridgeError, e:
print e
class CamDebug():
def __init__(self, nodeName, debugOn=True):
self.nodeName = nodeName
self.debugOn = debugOn
self.topics = {}
self.cvbridge = CvBridge()
'''
Publish an image to a stream (useful for debugging).
Expects image to be a numpy array.
'''
def publishImage(self, topicName, image):
if not self.debugOn:
return
if topicName not in self.topics:
fullTopicName = '/' + self.nodeName + '/' + topicName
self.topics[topicName] = rospy.Publisher(fullTopicName, Image)
publisher = self.topics[topicName]
try:
#outImg = array2cv(image)
outImg = cv2.cv.fromarray(image)
publisher.publish(
self.cvbridge.cv_to_imgmsg(outImg, encoding='bgr8'))
except CvBridgeError, e:
print e
class image_converter:
def __init__(self):
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.bridge = CvBridge()
out_image = rospy.get_param(rospy.search_param('out'))
image = rospy.get_param(rospy.search_param('image'))
# subscribed Topic
self.image_pub = rospy.Publisher(out_image, Image)
self.subscriber = rospy.Subscriber(image, CompressedImage, self.callback, queue_size = 1000)
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_cv = cv.fromarray(image_np)
# Publish new image
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(image_cv, "bgr8"))
except CvBridgeError, e:
print e
class ImageReducer:
# # Convert a ROS Image to the Numpy matrix used by cv2 functions
# def rosimg2cv(self, ros_image):
# # Convert from ROS Image to old OpenCV image
# frame = self.cvbridge.imgmsg_to_cv(ros_image, ros_image.encoding)
# # Convert from old OpenCV image to Numpy matrix
# return np.array(frame, dtype=np.uint8) #TODO: find out actual dtype
def __init__(self, topics, factor):
self.cvbridge = CvBridge()
self.topics = topics
self.factor = factor
for topic in topics:
outTopic = "/debug" + topic
callback = self.reducerCallback(topic, outTopic)
rospy.Subscriber(topic, Image, callback)
# Returns a callback for a particular Image topic which reduces the image
# and publishes it
def reducerCallback(self, imageTopic, outTopic):
publisher = rospy.Publisher(outTopic, Image)
factor = self.factor
def callback(rosImage):
try:
cvimg = self.cvbridge.imgmsg_to_cv(rosImage, desired_encoding="passthrough")
outimg = cv2.cv.CreateMat(int(cvimg.rows * factor), int(cvimg.cols * factor), cvimg.type)
cv2.cv.Resize(cvimg, outimg)
publisher.publish(self.cvbridge.cv_to_imgmsg(outimg, encoding="bgr8")) #TODO: figure out actual encoding
except CvBridgeError, e:
print e
return callback
def test_encode_decode_cv1(self):
import cv
fmts = [
cv.IPL_DEPTH_8U, cv.IPL_DEPTH_8S, cv.IPL_DEPTH_16U,
cv.IPL_DEPTH_16S, cv.IPL_DEPTH_32S, cv.IPL_DEPTH_32F,
cv.IPL_DEPTH_64F
]
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in fmts:
for channels in (1, 2, 3, 4):
original = cv.CreateImage((w, h), f, channels)
cv.Set(original, (1, 2, 3, 4))
rosmsg = cvb_en.cv_to_imgmsg(original)
newimg = cvb_de.imgmsg_to_cv(rosmsg)
self.assert_(
cv.GetElemType(original) == cv.GetElemType(newimg))
self.assert_(
cv.GetSize(original) == cv.GetSize(newimg))
self.assert_(
len(original.tostring()) == len(newimg.tostring()))
def OpenBlobAsSpeciesIDRequest(filename):
'''Function that opens up a blob file and converts it to a SpeciesIDRequest.'''
serializer = BlobSerializer()
# Read the blob file
f = open(filename)
try:
blobs, imgFilename = serializer.Deserialize(f, os.path.dirname(filename))
finally:
f.close()
# Now open up the image file associated with the blob
cvImg = cv2.imread(imgFilename)
if cvImg.shape[1] == 0 or cvImg.shape[0] == 0:
raise IOError("Could not open image: " + imgFilename)
if cvImg.shape[2] <> 3:
raise IOError("Image %s was not a 3 color image. It has %d channels" %
(imgFilename, cvImg.shape[2]))
if cvImg.dtype <> np.uint8:
raise IOError("Image %s was not a 8-bit image. It has %d bit depth" %
(imgFilename, cvImg.dtype))
bridge = CvBridge()
# For some reason, opencv puts images in BGR format, so label that
request = SpeciesIDRequest(image=bridge.cv_to_imgmsg(cvImg, "bgr8"))
for blob in blobs:
request.regions.append(blob.ToImageRegion())
return request
class ColorViewer():
def __init__(self, topic):
self.bridge = CvBridge()
rospy.Subscriber(topic, Image, self.image_callback)
self.pub = rospy.Publisher(topic + '_viewer', Image)
def image_callback(self, image_in):
""" Get image to which we're subscribed. """
image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
image_hsv = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
image_hsv = cv2.blur(image_hsv, (3,3))
h = [150, 480-150] # max 480
w = [200, 640-200] # max 640
cv2.rectangle(image_cv2, (w[0], h[0]), (w[1], h[1]), 255)
selected = image_hsv[h[0]:h[1], w[0]:w[1], :]
print selected.shape
print numpy.amin(numpy.amin(selected, 0), 0)
print numpy.mean(numpy.mean(selected, 0), 0)
print numpy.amax(numpy.amax(selected, 0), 0)
print ''
# Convert back to ROS Image msg
image_cv = cv.fromarray(image_cv2)
image_out = self.bridge.cv_to_imgmsg(image_cv, 'bgr8')
self.pub.publish(image_out)
class WebcamBridge:
"""
class for moving webcam images from opencv to ros
"""
def __init__(self):
self.image_pub = rospy.Publisher("/tennisballs", Image)
# self.circ_ctr_pub = rospy.Publisher("/circ_ctr", )
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/image_raw", Image, self.callback)
self.size = None
rospy.sleep(1)
def callback(self, data):
"""When the camera publishes a new frame, it sends through the cv
processing, and publishes a new output image
"""
print('In WebcamBridge.callback')
# cv.WaitKey(3)
try:
raw = self.bridge.imgmsg_to_cv(data, 'bgr8')
found = self.find_tennisball(raw)
msg = self.bridge.cv_to_imgmsg(found, 'bgr8')
self.image_pub.publish(msg)
except CvBridgeError, e:
print e
def map_segmentation_action_client_(self): ######## this function is called
mat = cv.fromarray(self.map_)
# cv.ShowImage( "map_image", mat )
# cv.WaitKey()
#creates a action client object
client = actionlib.SimpleActionClient( 'segment_map', autopnp_scenario.msg.MapSegmentationAction )
cv_image = CvBridge()
#filling the goal msg format for map segmentation action server
goal = autopnp_scenario.msg.MapSegmentationGoal( input_map = cv_image.cv_to_imgmsg( mat , "mono8"),
map_resolution = self.map_resolution_,
map_origin_x = self.map_origin_x_ ,
map_origin_y = self.map_origin_y_,
return_format_in_pixel = True )
rospy.loginfo("waiting for the map segmentation action server to start.....")
client.wait_for_server()
rospy.loginfo("map segmentation action server started, sending goal.....")
client.send_goal(goal)
finished_before_timeout = client.wait_for_result(rospy.Duration(30.0))
if finished_before_timeout:
state = client.get_state()
if state is 3:
state = 'SUCCEEDED'
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("Action did not finish before the time out.")
return client.get_result()
def talker():
global capture
bridge=CvBridge()
pub = rospy.Publisher('chatter', Image)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
frame=cv.QueryFrame(capture)
image=frame;
image=np.asarray(image[:,:])
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_green = np.array([50,150,0])
upper_green = np.array([150,255,255])
lower_blue = np.array([80,50,0])
upper_blue = np.array([160,255,255])
lower_red=np.array([0,160,0])
upper_red=np.array([30,255,255])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask2 = cv2.inRange(hsv, lower_blue, upper_blue)
mask3 = cv2.inRange(hsv, lower_red, upper_red)
res = cv2.bitwise_and(image,image, mask= mask)
res2 = cv2.bitwise_and(image,image, mask= mask2)
res3 = cv2.bitwise_and(image,image, mask= mask3)
final=res+res2+res3
frame=cv.fromarray(final)
pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
class image_converter:
def __init__(self):
'''Initialize ros publisher, ros subscriber'''
# topic where we publish
self.bridge = CvBridge()
out_image = rospy.get_param(rospy.search_param('out'))
image = rospy.get_param(rospy.search_param('image'))
# subscribed Topic
self.image_pub = rospy.Publisher(out_image, Image)
self.subscriber = rospy.Subscriber(image,
CompressedImage,
self.callback,
queue_size=1000)
def callback(self, ros_data):
'''Callback function of subscribed topic.
Here images get converted and features detected'''
#### direct conversion to CV2 ####
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_cv = cv.fromarray(image_np)
# Publish new image
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(image_cv, "bgr8"))
except CvBridgeError, e:
print e
def map_segmentation_action_client_(
self): ######## this function is called
mat = cv.fromarray(self.map_)
# cv.ShowImage( "map_image", mat )
# cv.WaitKey()
#creates a action client object
client = actionlib.SimpleActionClient(
'segment_map', autopnp_scenario.msg.MapSegmentationAction)
cv_image = CvBridge()
#filling the goal msg format for map segmentation action server
goal = autopnp_scenario.msg.MapSegmentationGoal(
input_map=cv_image.cv_to_imgmsg(mat, "mono8"),
map_resolution=self.map_resolution_,
map_origin_x=self.map_origin_x_,
map_origin_y=self.map_origin_y_,
return_format_in_pixel=True)
rospy.loginfo(
"waiting for the map segmentation action server to start.....")
client.wait_for_server()
rospy.loginfo(
"map segmentation action server started, sending goal.....")
client.send_goal(goal)
finished_before_timeout = client.wait_for_result(rospy.Duration(30.0))
if finished_before_timeout:
state = client.get_state()
if state is 3:
state = 'SUCCEEDED'
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("action finished: %s " % state)
else:
rospy.loginfo("Action did not finish before the time out.")
return client.get_result()
class ProjectorChecker:
def __init__(self):
grid_spacing = rospy.get_param('~grid_spacing')
self.bridge = CvBridge()
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('projector/get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'projector/get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('projector/set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('projector/set_projection',
projector.srv.SetProjection)
projector_info = projector_info_service().projector_info
projector_model = image_geometry.PinholeCameraModel()
projector_model.fromCameraInfo(projector_info)
# Generate grid projections
self.original_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
for row in range(0, projector_info.height, grid_spacing):
cv.Line(self.original_projection, (0, row),
(projector_info.width - 1, row), 255)
for col in range(0, projector_info.width, grid_spacing):
cv.Line(self.original_projection, (col, 0),
(col, projector_info.height - 1), 255)
predistortmap_x = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_32FC1)
InitPredistortMap(projector_model.intrinsicMatrix(),
projector_model.distortionCoeffs(), predistortmap_x,
predistortmap_y)
self.predistorted_projection = cv.CreateMat(projector_info.height,
projector_info.width,
cv.CV_8UC1)
cv.Remap(self.original_projection,
self.predistorted_projection,
predistortmap_x,
predistortmap_y,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=(0, 0, 0, 0))
self.off_projection = cv.CreateMat(projector_info.height,
projector_info.width, cv.CV_8UC1)
cv.SetZero(self.off_projection)
def project(self, projection):
projection_msg = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(projection_msg)
def test_enumerants(self):
img_msg = sensor_msgs.msg.Image()
img_msg.width = 640
img_msg.height = 480
img_msg.encoding = "rgba8"
img_msg.step = 640*4
img_msg.data = (640 * 480) * "1234"
bridge_ = CvBridge()
cvim = bridge_.imgmsg_to_cv(img_msg, "rgb8")
# A 3 channel image cannot be sent as an rgba8
self.assertRaises(CvBridgeError, lambda: bridge_.cv_to_imgmsg(cvim, "rgba8"))
# but it can be sent as rgb8 and bgr8
bridge_.cv_to_imgmsg(cvim, "rgb8")
bridge_.cv_to_imgmsg(cvim, "bgr8")
def callback(ros_data):
pub = rospy.Publisher('/chatter', Image)
bridge=CvBridge()
#print 'received image data of type:%s'% ros_data.format
np_arr = np.fromstring(ros_data.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR)
image_1=cv.fromarray(image_np)
#print type(image_1)
pub.publish(bridge.cv_to_imgmsg(image_1, "bgr8"))
def sample_descriptors(obj, i):
"""
Compute the audio and video descriptors for ith sample of object obj.
:param obj: string. Object to get descriptors from.
:param i: string. ith sample of object to get descriptors from.
"""
print obj, i
#Audio
filename = globals.path + 'wav/' + obj + '-' + str(i) + '.wav'
audio_descriptor = mfcc_descriptor(filename)
#audio_descriptor = wavelet_coefs(filename,'haar',1)
#Video
#rgb image
rgb = cv.LoadImage(globals.path + 'img/' + obj + '-' + str(i) + '-rgb.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
#depth image
depth = cv.LoadImage(
globals.path + 'img/' + obj + '-' + str(i) + '-depth.png',
cv.CV_LOAD_IMAGE_GRAYSCALE)
#mask of interest zone to retrieve descriptors
mask = cv.LoadImage(globals.path + 'mask.png', cv.CV_LOAD_IMAGE_GRAYSCALE)
bridge = CvBridge()
rgb_image = bridge.cv_to_imgmsg(rgb)
depth_image = bridge.cv_to_imgmsg(depth)
mask_image = bridge.cv_to_imgmsg(mask)
#call external visual descriptor module
rospy.wait_for_service('masked_base_descriptor_service')
try:
descriptor_request = rospy.ServiceProxy(
'masked_base_descriptor_service', masked_service)
response = descriptor_request(rgb=rgb_image,
depth=depth_image,
mask=mask_image)
base_descriptor = bridge.imgmsg_to_cv(response.descriptor)
visual_descriptor = np.array(base_descriptor)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class anaglyph:
def __init__(self):
self.pub = rospy.Publisher('anaglyph_img', Image)
self.sub_depth = rospy.Subscriber('/kinect/depth/image_raw', Image, self.cb_depth)
self.sub_rgb = rospy.Subscriber('/kinect/rgb/image_raw', Image, self.cb_rgb)
self.bridge = CvBridge()
self.img_out = cv.CreateMat(480, 640, cv.CV_8UC3)
self.img_depth = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_r = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_g = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_b = cv.CreateMat(480, 640, cv.CV_8UC1)
self.img_rgb = cv.CreateMat(480, 640, cv.CV_8UC3)
self.r_offset = cv.CreateMat(480, 640, cv.CV_8UC1)
self.g_offset = cv.CreateMat(480, 640, cv.CV_8UC1)
self.b_offset = cv.CreateMat(480, 640, cv.CV_8UC1)
def cb_depth(self, data):
self.img_depth = self.bridge.imgmsg_to_cv(data)
def cb_rgb(self, data):
rgb = self.bridge.imgmsg_to_cv(data)
cv.MixChannels([rgb], [self.img_b, self.img_g, self.img_r], [(0,0), (1,1), (2,2)])
self.generate_output()
def generate_output(self):
cv.Copy(self.img_r, self.r_offset)
cv.Copy(self.img_b, self.b_offset)
cv.Copy(self.img_g, self.g_offset)
for i in range (self.img_r.height-1):
for j in range (self.img_r.width-1):
if self.img_depth[i,j] < 255 and self.img_depth[i,j] >= 0:
disp = int(round(self.img_depth[i,j]/5))
print disp
if j-disp > 0 and j-disp < self.img_r.width-1:
self.r_offset[i,j] = self.img_r[i, j-disp]
#self.r_offset[i,j-disp] = self.img_r[i, j]
#self.b_offset[i,j-disp] = self.img_b[i, j]
self.b_offset[i,j] = self.img_b[i, j-disp]
if j+disp > 0 and j+disp < self.img_b.width-1:
#self.g_offset[i,j+disp] = self.img_g[i, j]
self.g_offset[i,j] = self.img_g[i, j+disp]
cv.MixChannels([self.r_offset, self.g_offset, self.b_offset], [self.img_out], [(0,0), (1,1), (2,2)])
#cv.MixChannels([r_offset, self.img_g, self.img_b], [self.img_out], [(0,0), (1,1), (2,2)])
img = self.bridge.cv_to_imgmsg(self.img_out)
self.pub.publish(img)
class MatFilter():
def __init__(self, topic):
self.bridge = CvBridge()
rospy.Subscriber(topic, Image, self.image_callback)
self.pub = rospy.Publisher(topic + '_filtered', Image)
def image_callback(self, image_in):
""" Get image to which we're subscribed. """
# Import and convert
image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
image_hsv = cv2.cvtColor(image_cv2, cv2.COLOR_BGR2HSV)
image_hsv = cv2.blur(image_hsv, (5, 5))
# Make binary image of pinkness
#is_pink = cv2.inRange(image_hsv, numpy.array((130, 50, 120)), numpy.array((175, 85, 200)))
is_pink = cv2.inRange(image_hsv, numpy.array((130, 50, 0)), numpy.array((175, 255, 255)))
pink = copy.deepcopy(image_cv2)
for dim in range(3): pink[:, :, dim] *= is_pink / 255
pink_avg = numpy.sum(numpy.sum(pink, 0), 0) / numpy.sum(numpy.sum(is_pink / 255, 0), 0)
pink_avg = tuple([int(pink_avg[0]), int(pink_avg[1]), int(pink_avg[2])])
print pink_avg
#code.interact(local=locals())
# Manipulate binary image
contours, hierarchy = cv2.findContours(is_pink, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE)
#code.interact(local=locals())
#print contours
max_area = 0
for index, contour in enumerate(contours):
area = cv2.contourArea(contour)
if area > max_area:
max_area = area
best_index = index
best_contour = contours[best_index]
rospy.loginfo('Best contour was contour #{0} with area of {1}'.format(best_index, max_area))
cv2.drawContours(image_cv2, contours, best_index, pink_avg, thickness=-1) # fill in the largest pink blob
#cv2.drawContours(image_cv2, contours, best_index, (0,0,0)) # draw black line around largest pink blob
# Apply binary image to full image
for dim in range(3): image_hsv[:,:,dim] *= is_pink / 255
# Convert back to ROS Image msg
#image_hsv_float = (pink.astype(float) + 1) / 256
#image_rgb = ((matplotlib.colors.hsv_to_rgb(image_hsv_float) * 256) - 1).astype('uint8')
#image_cv2 = cv2.cvtColor(image_hsv, cv2.COLOR_HSV2BGR)
image_cv = cv.fromarray(image_cv2)
image_out = self.bridge.cv_to_imgmsg(image_cv, 'bgr8')
self.pub.publish(image_out)
class image_publisher:
def __init__(self, topic_name="Camera"):
self.im_pub = rospy.Publisher(topic_name, Image)
self.bridge = CvBridge()
def send_message(self, cv_image):
try:
image_message = self.bridge.cv_to_imgmsg(cv.fromarray(cv_image), "bgr8")#,desired_encoding="passthrough")
self.im_pub.publish(image_message)
except CvBridgeError, e:
print e
def add_object_client(name, path):
rospy.wait_for_service('/hueblob/add_object')
try:
add_object = rospy.ServiceProxy('/hueblob/add_object', AddObject)
# load image, give anchor coordinates
anchor = Point(0.0,0.0,0.0)
cv_img = cv.LoadImageM(path)
bridge = CvBridge()
image = bridge.cv_to_imgmsg(cv_img)
resp1 = add_object(name, anchor, image)
return resp1.status
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def talker():
capture = cv2.VideoCapture(1)
ret, frame = capture.read()
bridge = CvBridge()
pub = rospy.Publisher('UI122xLE_C/image_raw', Image)
rospy.init_node('UI122xLE_C_Emulator')
while not rospy.is_shutdown():
ret, frame = capture.read()
frame = cv.fromarray(frame)
image_message = bridge.cv_to_imgmsg(frame, encoding="bgr8")
pub.publish(image_message)
class converter:
def __init__(self):
self.bridge = CvBridge()
def convert(self,data):
im_arr=fromstring(data, dtype=uint8)
cv_image = cv2.imdecode(im_arr,1)
try:
img = cv.fromarray(cv_image);
smaller_img = cv.CreateMat(img.rows/2, img.cols/2, img.type)
cv.Resize(img, smaller_img)
return (self.bridge.cv_to_imgmsg(smaller_img,"bgr8"))
except CvBridgeError, e:
rospy.logerr(e)
return None
class image_sender:
def __init__(self, name="Camera"):
#cv_image = bridge.imgmsg_to_cv(image_message, desired_encoding="passthrough")
self.im_pub = rospy.Publisher(name, Image)
self.bridge = CvBridge()
def send_message(self, cv_image):
try:
image_message = self.bridge.cv_to_imgmsg(cv.fromarray(cv_image),"bgr8")#,desired_encoding="passthrough")
self.im_pub.publish(image_message)
except CvBridgeError, e:
print e
class Node:
def __init__(self):
self.pub = rospy.Publisher('image_out', Image)
self.sub = rospy.Subscriber('image_in', Image, self.imgCb)
self.bridge = CvBridge()
def imgCb(self, data):
cvImage = np.asarray(self.bridge.imgmsg_to_cv(data, "bgr8"))
imageResult = imageOperation(cvImage)
self.pub.publish(self.bridge.cv_to_imgmsg(cv.fromarray(imageResult), "bgr8"))
def imageOperation(image):
# do something with the image
return image
class HoleFiller():
def __init__(self, topic_base, topic_mask, topic_out):
self.bridge = CvBridge()
self.image_out = Image()
self.have_mask = False
self.need_to_publish = False
rospy.Subscriber(topic_base, Image, self.base_callback)
rospy.Subscriber(topic_mask, Image, self.mask_callback)
self.pub = rospy.Publisher(topic_out, Image)
self.publish()
def mask_callback(self, mask):
image_cv = self.bridge.imgmsg_to_cv(mask, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
image_cv2 = image_cv2[:, :, 0]
kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (50, 50))
image_filled = cv2.morphologyEx(image_cv2, cv2.MORPH_CLOSE, kernel, borderValue=255)
image_filled.astype(bool)
image_filled = numpy.logical_not(image_filled)
self.mask = image_filled
self.have_mask = True
def base_callback(self, image_in):
if self.have_mask:
image_cv = self.bridge.imgmsg_to_cv(image_in, 'bgr8')
image_cv2 = numpy.array(image_cv, dtype=numpy.uint8)
# Filter
for dim in range(3): image_cv2[:, :, dim] *= self.mask # take out filtered pixels
image_cv2[:, :, 1] += numpy.logical_not(self.mask)*255 # turn 'em green
image_filled_cv = cv.fromarray(image_cv2)
self.image_out = self.bridge.cv_to_imgmsg(image_filled_cv, 'bgr8')
self.need_to_publish = True
def publish(self):
r = rospy.Rate(5)
while not rospy.is_shutdown():
if self.need_to_publish:
self.pub.publish(self.image_out)
self.need_to_publish = False
r.sleep()
def test_encode_decode(self):
fmts = [ cv.IPL_DEPTH_8U, cv.IPL_DEPTH_8S, cv.IPL_DEPTH_16U, cv.IPL_DEPTH_16S, cv.IPL_DEPTH_32S, cv.IPL_DEPTH_32F, cv.IPL_DEPTH_64F ]
cvb_en = CvBridge()
cvb_de = CvBridge()
for w in range(100, 800, 100):
for h in range(100, 800, 100):
for f in [ cv.IPL_DEPTH_8U ]:
for channels in (1,2,3,4):
original = cv.CreateImage((w, h), f, channels)
cv.Set(original, (1,2,3,4));
rosmsg = cvb_en.cv_to_imgmsg(original)
newimg = cvb_de.imgmsg_to_cv(rosmsg)
self.assert_(cv.GetElemType(original) == cv.GetElemType(newimg))
self.assert_(cv.GetSize(original) == cv.GetSize(newimg))
self.assert_(len(original.tostring()) == len(newimg.tostring()))
def ImageCallback(*msg_in):
br = CvBridge()
num = len(msg_in)
imgs = map(lambda msg: br.imgmsg_to_cv(msg), msg_in)
cols, rows = cv.GetSize(imgs[0])
tiles = cv.CreateMat(rows, cols * num, cv.CV_8UC3)
# Tile the individual images to form a single output.
for i in range(0, num):
tile = cv.GetSubRect(tiles, (i * cols, 0, cols, rows))
cv.Copy(imgs[i], tile)
msg_out = br.cv_to_imgmsg(tiles)
msg_out.header.stamp = msg_in[0].header.stamp
msg_out.header.frame_id = msg_in[0].header.frame_id
pub_image.publish(msg_out)
def talker():
#cv.NamedWindow("")
global height
global width
global fps
global check_fps_set
#capture =cv.CaptureFromCAM(0)
cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH, int(width))
cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_HEIGHT, int(height))
#cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FPS,2)
#rate=cv.GetCaptureProperty(capture,cv.CV_CAP_PROP_FPS)
#print rate
bridge = CvBridge()
pub = rospy.Publisher('chatter', Image)
rospy.init_node('talker', anonymous=True)
rospy.Subscriber("config", String, callback)
r = rospy.Rate(int(fps)) # 10hz
frames = 0
start_time = 0
check = 0
while not rospy.is_shutdown():
#str = "hello world %s"%rospy.get_time()
frame = cv.QueryFrame(capture)
if check == 0:
check = 1
start_time = time.time()
if check_fps_set == 1:
r = rospy.Rate(int(fps))
print "fps: " + fps
check_fps_set = 0
frames = frames + 1
if frames % 10 == 0:
curtime = time.time()
diff = curtime - start_time
fps = frames / diff
print fps
#ret,frame=capture.read()
#image=np.asarray(frame[:,:])
#a=image.shape
#print a
#rospy.loginfo(st)
r.sleep()
pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
class wireless_camera:
def __init__(self):
#IMPORTANT : Change self.CAMERA_IP to your camera IP
self.CAMERA_IP = "http://128.59.19.52:1024/img/video.mjpeg" #"http://192.168.1.2/img/video.mjpeg"
self.image_pub = rospy.Publisher("wireless_camera/image",Image)
self.bridge = CvBridge()
def open_file(self):
f = urllib.urlopen(self.CAMERA_IP)
#read data as a string
buffer_size = 65536; #2^16 = 65536 : Need to optimize
read_data = f.read(buffer_size)
EOI = [0xff,0xD9] #EOI[0] = 255, EOI[1] = 217
SOI = [0xff,0xD8] #SOI[0] = 255, SOI[1] = 216
#Find EOI and SOI
#Convert the character of read_data to the acii code by ord(). Additionally, chr(97) will show 'a'.
SOI_pos = 0
SOI_pos_prev = 0
for i in range(1,len(read_data)-1):
if SOI[0]==ord(read_data[i:i+1]) and SOI[1]==ord(read_data[i+1:i+2]):
if SOI_pos == 0:
SOI_pos=i
else:
SOI_pos_prev = SOI_pos
SOI_pos = i
break
if EOI[0]==ord(read_data[i:i+1]) and EOI[1]==ord(read_data[i+1:i+2]):
EOI_Pos=i
#Write the data into a temporary file.
f=open('temp.jpg','w')
f.write(read_data[SOI_pos_prev:SOI_pos]) # data from first SOI to the next SOI are saved to generated a JPEG file.
def publish_image(self):
#Load temporary JPEG image
cv_image=cv.LoadImage("temp.jpg")
#Convert openCV image into ROS messages
image_message = self.bridge.cv_to_imgmsg(cv_image, "bgr8")
#Publish image message using OpenCV
self.image_pub.publish(image_message)
def talker():
#cv.NamedWindow("")
global height
global width
global fps
global check_fps_set
#capture =cv.CaptureFromCAM(0)
cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FRAME_WIDTH,int(width))
cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FRAME_HEIGHT,int(height))
#cv.SetCaptureProperty(capture,cv.CV_CAP_PROP_FPS,2)
#rate=cv.GetCaptureProperty(capture,cv.CV_CAP_PROP_FPS)
#print rate
bridge=CvBridge()
pub = rospy.Publisher('chatter', Image)
rospy.init_node('talker', anonymous=True)
rospy.Subscriber("config", String, callback)
r = rospy.Rate(int(fps)) # 10hz
frames=0
start_time=0
check=0
while not rospy.is_shutdown():
#str = "hello world %s"%rospy.get_time()
frame=cv.QueryFrame(capture)
if check==0:
check=1
start_time=time.time()
if check_fps_set==1:
r = rospy.Rate(int(fps))
print "fps: " + fps
check_fps_set=0
frames=frames+1
if frames%10==0:
curtime=time.time()
diff=curtime-start_time
fps=frames/diff
print fps
#ret,frame=capture.read()
#image=np.asarray(frame[:,:])
#a=image.shape
#print a
#rospy.loginfo(st)
r.sleep()
pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
class ImageConverter:
def __init__(self):
image = rospy.get_param('~image')
self.pub = rospy.Publisher('cv_image', Image)
self.bridge = CvBridge()
self.sub = rospy.Subscriber(image, Image, self.callback)
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv(data, 'bgr8')
except CvBridgeError:
raise
# TODO: Do something with cv_image.
try:
self.pub.publish(self.bridge.cv_to_imgmsg(cv_image, 'bgr8'))
except CvBridgeError:
raise
class Filterer():
def __init__(self):
self.have_rgb = False
self.have_depth = False
self.bridge = CvBridge()
rospy.Subscriber('/camera/rgb/image_color', Image, self.callback_rgb)
rospy.Subscriber('/camera/depth_registered/hw_registered/image_rect_raw', Image, self.callback_depth)
self.pubber_filtered = rospy.Publisher('/camera/rgb/image_rect_color/filtered', Image)
def run(self):
while not rospy.is_shutdown():
if self.have_rgb and self.have_depth:
# convert image data to numpy arrays
depth_array = numpy.asarray(self.bridge.imgmsg_to_cv(self.depth, self.depth.encoding))
rgb_array = numpy.asarray(self.bridge.imgmsg_to_cv(self.rgb, self.rgb.encoding))
# apply filter via mask
mask = numpy.logical_or(depth_array > 2000, # threshold (mm)
depth_array == 0) # bad values
mask = numpy.tile(mask[:,:,numpy.newaxis], (1,1,3)) # make mask same size as rgb image
rgb_array_blurry = numpy.zeros(rgb_array.shape)
for i in range(3): # apply filter to each color channel
rgb_array_blurry[:,:,i] = gaussian_filter(rgb_array[:,:,i], sigma=7)
rgb_array[mask] = rgb_array_blurry[mask] # make far-off things blurry
# convert RGB array back to image msg and publish
rgb_filtered = copy_Image_empty_data(self.rgb) # note: timestamp unchanged
rgb_filtered.data = self.bridge.cv_to_imgmsg(cv.fromarray(rgb_array), encoding=self.rgb.encoding).data
self.pubber_filtered.publish(rgb_filtered)
def callback_rgb(self, rgb):
#rospy.loginfo('Got RGB! {0}'.format(rgb.header.stamp))
self.rgb = rgb
if not self.have_rgb:
self.have_rgb = True
def callback_depth(self, depth):
#rospy.loginfo('Got DEPTH! {0}'.format(depth.header.stamp))
self.depth = depth
if not self.have_depth:
self.have_depth = True
class node_camera_opencv:
def __init__(self):
self.image_pub = rospy.Publisher("image_raw",Image)
cv.NamedWindow("Image window", 1)
self.bridge = CvBridge()
if rospy.has_param('camera/device'):
device = rospy.get_param('camera/device')
rospy.loginfo('Device %i specified. Trying to use that.' % device)
else:
rospy.loginfo('No device specified, using default camera 0')
device = 0
#TODO: Pass camera index as a parameter:
rospy.loginfo('Creating capture object...')
try:
self.capture = cv2.VideoCapture(device)
self.capture.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH,640)
self.capture.set(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT,480)
except Exception as e:
rospy.logerr('Error creating capture object: ' + str(e))
raise e
def publish_image(self):
# try:
# cv_image = self.bridge.imgmsg_to_cv(data, "bgr8")
# except CvBridgeError, e:
# print e
try:
cv_image = self.capture.read()
except Exception as e:
rospy.logerr('Error capturing: ' + str(e))
if (cv_image[0]):
#print ('.')
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(cv.fromarray(cv_image[1]), "bgr8"))
#cv2.imshow("Image window", cv_image[1])
#cv2.waitKey(1)
except CvBridgeError, e:
rospy.logerr(e)
else:
def get_ar_marker_poses (msg, ar_markers = None, use_pc_service=True, model="", track=False):
'''
get poses according to ar_markers
if ar_markers == None, then for all ar markers appeared in the point cloud
'''
global getMarkersPC, getImageMarkers, reqPC, reqImage, bridge
if model is None:
return "No camera model provided for extraction."
if rospy.get_name() == '/unnamed':
rospy.init_node('ar_marker_poses', anonymous=True)
if use_pc_service:
if getMarkersPC is None:
getMarkersPC = rospy.ServiceProxy("getMarkers", MarkerPositions)
reqPC.pc = msg
reqPC.track = track
res = getMarkersPC(reqPC)
else:
if model not in getImageMarkers:
getImageMarkers[model] = rospy.ServiceProxy("getImageMarkers"+model, MarkerImagePositions)
if bridge is None:
bridge = CvBridge()
img = bridge.cv_to_imgmsg(msg)
reqImage.img = img
reqImage.track = track
try:
res = getImageMarkers[model](reqImage)
except Exception as e:
redprint("Something went wrong with image" )
print e
return {}
marker_tfm = {}
for marker in res.markers.markers:
if ar_markers == None or marker.id in ar_markers:
marker_tfm[marker.id] = conversions.pose_to_hmat(marker.pose.pose).tolist()
return marker_tfm
class ProjectorChecker:
def __init__(self):
grid_spacing = rospy.get_param('~grid_spacing')
self.bridge = CvBridge()
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('projector/get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy('projector/get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('projector/set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('projector/set_projection', projector.srv.SetProjection)
projector_info = projector_info_service().projector_info
projector_model = image_geometry.PinholeCameraModel()
projector_model.fromCameraInfo(projector_info)
# Generate grid projections
self.original_projection = cv.CreateMat(projector_info.height, projector_info.width, cv.CV_8UC1)
for row in range(0, projector_info.height, grid_spacing):
cv.Line(self.original_projection, (0, row), (projector_info.width - 1, row), 255)
for col in range(0, projector_info.width, grid_spacing):
cv.Line(self.original_projection, (col, 0), (col, projector_info.height - 1), 255)
predistortmap_x = cv.CreateMat(projector_info.height, projector_info.width, cv.CV_32FC1)
predistortmap_y = cv.CreateMat(projector_info.height, projector_info.width, cv.CV_32FC1)
InitPredistortMap(projector_model.intrinsicMatrix(), projector_model.distortionCoeffs(), predistortmap_x, predistortmap_y)
self.predistorted_projection = cv.CreateMat(projector_info.height, projector_info.width, cv.CV_8UC1)
cv.Remap(self.original_projection, self.predistorted_projection, predistortmap_x, predistortmap_y, flags=cv.CV_INTER_NN+cv.CV_WARP_FILL_OUTLIERS, fillval=(0, 0, 0, 0))
self.off_projection = cv.CreateMat(projector_info.height, projector_info.width, cv.CV_8UC1)
cv.SetZero(self.off_projection)
def project(self, projection):
projection_msg = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(projection_msg)
class imageGenerator(object):
def __init__(self):
self.bridge = CvBridge()
self.image_pub = rospy.Publisher("circleOut",Image)
self.imgDim = (500,500)
self.img = cv.CreateImage(self.imgDim,cv.IPL_DEPTH_8U,1)
def callback(self,data):
posX = self.imgDim[0]*data.x
posY = self.imgDim[1]*data.y
cv.Circle(self.img, (int(posX),int(posY)), int(self.imgDim[0]*0.1), 255, -1)
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(self.img, "mono8"))
except CvBridgeError, e:
print e
cv.Set(self.img,0)
class webcam:
def __init__(self):
self.image_pub = rospy.Publisher("webcam_image",Image)
self.win = "Webcam"
cv2.namedWindow(self.win)
self.c = cv2.VideoCapture(0)
self.bridge = CvBridge()
self.alive = True
def grab(self):
if not self.alive:
return
img = self.c.read()[1]
#cv2.imshow(self.win, img)
#key = cv2.waitKey(100)
img = cv2.cv.fromarray(img)
try:
self.image_pub.publish(self.bridge.cv_to_imgmsg(img, "bgr8"))
except CvBridgeError, e:
print e
class CloudNumpyer():
def __init__(self, topic):
self.bridge = CvBridge()
rospy.Subscriber(topic, Float32MultiArray, self.array_callback)
self.pub = rospy.Publisher(topic+'_image', Image)
def array_callback(self, array_in):
""" Convert unpacked point cloud (Float32MultiArray msg) to numpy array. """
self.array = numpy.asarray(array_in.data) # convert to numpy
self.array = numpy.reshape(self.array, (480, 640, 6)) # reshape to (480, 640, 6)
rospy.loginfo('Converted a cloud to numpy!')
# DEMO: Turn everything >2m away to black.
#code.interact(local=locals())
mask = self.array[:, :, 2] > 2
for i in [3, 4, 5]: self.array[:, :, i][mask] = 0
image_np = self.array[:, :, 3:].astype('uint8')
image_cv = cv.fromarray(image_np)
image_msg = self.bridge.cv_to_imgmsg(image_cv, encoding='rgb8')
self.pub.publish(image_msg)
def Analyze_Map_client(self):
mat = cv.fromarray(self.map_)
#cv.ShowImage( "map_image", mat )
#cv.WaitKey(10)
client = actionlib.SimpleActionClient(
'Analyze_Map', autopnp_scenario.msg.AnalyzeMapAction)
cv_image = CvBridge()
goal = autopnp_scenario.msg.AnalyzeMapGoal(
input_img=cv_image.cv_to_imgmsg(mat, "mono8"),
map_resolution=self.map_resolution_,
Map_Origin_x=self.map_origin_x,
Map_Origin_y=self.map_origin_y)
rospy.loginfo("Waiting for the Analyze Map Action server to start.")
client.wait_for_server()
rospy.loginfo("Analyze Map Action server started, sending goal.")
client.send_goal(goal)
finished_before_timeout = client.wait_for_result(rospy.Duration(30.0))
if finished_before_timeout:
state = client.get_state()
if state is 3:
state = 'SUCCEEDED'
rospy.loginfo("Action finished: %s " % state)
else:
rospy.loginfo("Action finished: %s " % state)
else:
rospy.loginfo("Action did not finish before the time out.")
return client.get_result()
class Blender(object):
def __init__(self, in1, in2, out):
self.in1 = in1
self.in2 = in2
self.out = out
self.im1_sub = message_filters.Subscriber(in1, Image)
self.im2_sub = message_filters.Subscriber(in2, Image)
ts = message_filters.TimeSynchronizer((self.im1_sub, self.im2_sub), 3)
ts.registerCallback(self.callback)
self.bridge = CvBridge()
self.pub = rospy.Publisher(out, Image)
self.blended_img = cv.CreateMat(480,640,cv.CV_8UC3)
def callback(self, im1, im2):
im1_cv= self.bridge.imgmsg_to_cv(im1, "bgr8")
im2_cv= self.bridge.imgmsg_to_cv(im2, "bgr8")
cv.AddWeighted(im1_cv, 0.8, im2_cv, 0.8, 0., self.blended_img)
self.pub.publish(self.bridge.cv_to_imgmsg(self.blended_img, "bgr8"))
class ImageReducer:
# # Convert a ROS Image to the Numpy matrix used by cv2 functions
# def rosimg2cv(self, ros_image):
# # Convert from ROS Image to old OpenCV image
# frame = self.cvbridge.imgmsg_to_cv(ros_image, ros_image.encoding)
# # Convert from old OpenCV image to Numpy matrix
# return np.array(frame, dtype=np.uint8) #TODO: find out actual dtype
def __init__(self, topics, factor):
self.cvbridge = CvBridge()
self.topics = topics
self.factor = factor
for topic in topics:
outTopic = "/debug" + topic
callback = self.reducerCallback(topic, outTopic)
rospy.Subscriber(topic, Image, callback)
# Returns a callback for a particular Image topic which reduces the image
# and publishes it
def reducerCallback(self, imageTopic, outTopic):
publisher = rospy.Publisher(outTopic, Image)
factor = self.factor
def callback(rosImage):
try:
cvimg = self.cvbridge.imgmsg_to_cv(
rosImage, desired_encoding="passthrough")
outimg = cv2.cv.CreateMat(int(cvimg.rows * factor),
int(cvimg.cols * factor), cvimg.type)
cv2.cv.Resize(cvimg, outimg)
publisher.publish(
self.cvbridge.cv_to_imgmsg(
outimg,
encoding="bgr8")) #TODO: figure out actual encoding
except CvBridgeError, e:
print e
return callback
class CloudConverter():
def __init__(self):
self.bridge = CvBridge()
self.pub = rospy.Publisher('/camera/depth_registered/image_filtered',
Image)
self.sub = rospy.Subscriber('/camera/depth_registered/points',
PointCloud2, self.cloud_callback)
def cloud_callback(self, cloud):
points = point_cloud2.read_points(cloud)
points_list = np.asarray(list(points))
points_arr = np.asarray(points_list)
# Unpack RGB color info
_float2rgb_vectorized = np.vectorize(_float2rgb)
r, g, b = _float2rgb_vectorized(points_arr[:, 3])
# Concatenate and Reshape
r = np.expand_dims(r,
1) # insert blank 3rd dimension (for concatenation)
g = np.expand_dims(g, 1)
b = np.expand_dims(b, 1)
points_rgb = np.concatenate((points_arr[:, 0:3], r, g, b), axis=1)
image_rgb = points_rgb.reshape(cloud.height, cloud.width, 6)
z = copy.deepcopy(image_rgb[:, :, 2]) # get depth values (I think)
image_np = copy.deepcopy(image_rgb[:, :, 3:].astype('uint8'))
#code.interact(local=locals())
# TWO-METER DISTANCE FILTER
z[np.isnan(z)] = 0.0
mask = np.logical_or(z > 2, z == 0)
for i in range(image_np.shape[2]):
image_np[:, :, i][mask] = 0
# Convert to Image msg
image_cv = cv.fromarray(image_np)
image_msg = self.bridge.cv_to_imgmsg(image_cv, encoding='bgr8')
self.pub.publish(image_msg)
class ImagePublisherSync:
def __init__(self, imagename, sync):
rospy.init_node('image_pub_sync', anonymous=True)
self.pub = rospy.Publisher('image', Image)
if sync:
self.sub = rospy.Subscriber('detections', DetectionArray,
self.callback)
self.bridge = CvBridge()
cv_image = cv.LoadImage(imagename)
self.image = self.bridge.cv_to_imgmsg(cv_image)
self.image.header.frame_id = 'custom_image'
if sync:
self.publish()
else:
self.loop_publish()
def loop_publish(self):
r = rospy.Rate(2)
while not rospy.is_shutdown():
self.publish()
r.sleep()
def publish(self):
rospy.loginfo("Publishing image")
self.image.header.stamp = rospy.rostime.get_rostime()
self.stamp = self.image.header.stamp
self.pub.publish(self.image)
def callback(self, detections):
if detections.header.stamp == self.stamp:
rospy.loginfo("Publishing image")
self.publish()
else:
rospy.logwarn("Received out of order message")
def main():
print 'Get Bridge'
br = CvBridge()
print 'Setup the ros node'
rospy.init_node('rosSegmentCloud')
print 'Setup the Acquire Object'
ac = AcquireCloud()
ac.print_test()
show_image(ac.img, 'img')
show_image(ac.img_dotted, 'dotted')
###tf_br = tf.TransformBroadcaster()
print 'publish test'
while not rospy.is_shutdown():
now = rospy.Time.now()
###tf_br.sendTransform(ac.tf_translation, ac.tf_quaternion, now, "table_camera", ac.fixed_frame)
### tf == sadness :(
ac.pub_cloud_bound()
im_msg = br.cv_to_imgmsg(ac.img_dotted)
im_msg.header.frame_id = ac.target_frame ###"table_camera"
im_msg.header.stamp = now
ac.img_pub.publish(im_msg)
cam_msg = ac.cam_msg
cam_msg.header = rospy.Header(frame_id=ac.target_frame, stamp=now)
###cam_msg.header = rospy.Header(frame_id="table_camera", stamp=now)
ac.cam_pub.publish(cam_msg)
ac.test_publish()
print 'pub'
rospy.sleep(1)
class Blob:
FOUR_CONNECTED = 0
EIGHT_CONNECTED = 1
def __init__(self, id_=-1, connectionType=FOUR_CONNECTED):
self.id = id_
self.connectionType = connectionType
self.points = set()
self.minX = float("inf")
self.minY = float("inf")
self.maxX = float("-inf")
self.maxY = float("-inf")
self.bridge = CvBridge()
def AddPoint(self, point):
'''Adds a point to the blob. Point must be a tuple of (x,y).'''
self.points.add(point)
if point[0] < self.minX:
self.minX = point[0]
if point[0] > self.maxX:
self.maxX = point[0]
if point[1] < self.minY:
self.minY = point[1]
if point[1] > self.maxY:
self.maxY = point[1]
def AddPoints(self, pointCollection):
'''Adds a set of points to the blob.'''
for point in pointCollection:
self.AddPoint(tuple(point))
def AddBox(self, minX, minY, maxX, maxY):
'''Adds a blob from (minX,minY) to (maxX,maxY) inclusive.'''
coords = np.transpose(np.meshgrid(np.arange(minX, maxX+1),
np.arange(minY, maxY+1)))
self.AddPoints(np.reshape(coords,
(coords.shape[0]*coords.shape[1],2)))
def Area(self):
return len(self.points)
def Contains(self, point):
'''Returns true if the blob contains a given point.'''
return point in self.points
def GetBoundingBox(self):
'''Returns the bounding box of the blob as (minX,minY,maxX,maxY).'''
return (self.minX, self.minY, self.maxX, self.maxY)
def __iter__(self):
'''Returns an iterator through the points in the blob.'''
return self.points.__iter__()
def __eq__(self, other):
'''Returns true if the two Blobs describe the same points.'''
return self.points == other.points
def __ne__(self, other):
return not self == other
def __str__(self):
return str(self.points)
def ToImageRegion(self):
'''Returns a reefbot_msgs::ImageRegion describing the blob.'''
# First build the bounding box
bbox = RegionOfInterest(self.minX, self.minY, self.maxY-self.minY+1,
self.maxX-self.minX+1, False)
# Now build the mask image defining which pixels are used
mask = np.zeros((bbox.height, bbox.width), dtype=np.uint8)
for x,y in self.points:
mask[y - self.minY, x - self.minX] = 1
# Create the image region object
imageRegion = ImageRegion(bounding_box=bbox,
mask=self.bridge.cv_to_imgmsg(mask, "mono8"))
return imageRegion
def ToBinaryImageMask(self, shape):
'''Returns a numpy matrix that specifies the blobs as 1 for pixel in the blobs and a 0 otherwise.'''
# Create the mask
mask = np.zeros(shape, np.uint8)
for x,y in self.points:
mask[y, x] = 1
return mask
def AsciiSerialize(self):
'''Serializes the blob into a list of locations on a line.
It is in the format "x,y;x,y;x,y"
'''
if len(self.points) > 0:
return ';'.join(['%i,%i' % x for x in self.points]) + ';\n'
return ''
@staticmethod
def CreateFromString(string):
'''Loads a description of a blob from a string output by AsciiSerialize.'''
blob = Blob()
strList = string.strip().split(';')
if len(strList) == 1 and strList[0] == '':
return blob
for pointStr in strList:
if pointStr == '':
continue
part = pointStr.partition(',')
if part[1] == '':
raise ValueError('No coordinate found in: ' + pointStr)
blob.AddPoint((int(part[0]), int(part[2])))
return blob
def talker():
global capture
bridge=CvBridge()
pub = rospy.Publisher('chatter', Image)
rospy.init_node('talker', anonymous=True)
while not rospy.is_shutdown():
#capture=cv.CaptureFromCAM(1)
frame=cv.QueryFrame(capture)
#image=cv2.imread('/home/niranjan/Desktop/Rock-Colors.JPG')
#capture=cv.CaptureFromCAM(2)
#frame1=cv.QueryFrame(capture)
image=frame;
image=np.asarray(image[:,:])
#image1=frame1;
#image1=np.asarray(image1[:,:])
#both=np.hstack((img,img1))
#bitmap = cv.CreateImageHeader((both.shape[1], both.shape[0]), cv.IPL_DEPTH_8U, 3)
#cv.SetData(bitmap, both.tostring(), both.dtype.itemsize * 3 * both.shape[1])
#frame=bitmap
#image=frame
'''medianB=np.median(image[:,:,0]);
medianG=np.median(image[:,:,1]);
medianR=np.median(image[:,:,2]);
#print median
diffB=128-medianB
diffG=128-medianG
diffR=128-medianR
image[:,:,0]=image[:,:,0]+diffB
image[:,:,1]=image[:,:,1]+diffG
image[:,:,2]=image[:,:,2]+diffR
image[image>=255]=254
image[image<=0]=1'''
#print maxi
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_blue = np.array([80,110,60])
upper_blue = np.array([130,255,255])
lower_green = np.array([50,90,0])
upper_green = np.array([75,255,255])
lower_red=np.array([170,160,60])
upper_red=np.array([180,255,255])
lower_orange=np.array([0,160,40])
upper_orange=np.array([17,255,255])
lower_yellow=np.array([23,160,60])
upper_yellow=np.array([35,255,255])
lower_violet=np.array([135,160,60])
upper_violet=np.array([155,255,255])
mask = cv2.inRange(hsv, lower_green, upper_green)
mask2 = cv2.inRange(hsv, lower_blue, upper_blue)
mask3 = cv2.inRange(hsv, lower_red, upper_red)
mask4 = cv2.inRange(hsv, lower_yellow, upper_yellow)
mask5 = cv2.inRange(hsv, lower_orange, upper_orange)
mask6 = cv2.inRange(hsv, lower_violet, upper_violet)
#a=image.shape
#print a
'''for i in range(1,a[1]):
for j in range(1,a[2]):
temp=image[i,j]
temp=temp+diff
print type(temp)
if(temp<=255 and temp>=0):
image[i,j]=temp
'''
#maxi=np.min(image)
res = cv2.bitwise_and(image,image, mask= mask)
im1=cv2.cvtColor(res,cv2.COLOR_BGR2GRAY)
contours1, hierarchy1 = cv2.findContours(im1,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours1:
x1,y1,w1,h1 = cv2.boundingRect(i)
cv2.rectangle(image,(x1,y1),(x1+w1,y1+h1),(0,255,0),2)
res2 = cv2.bitwise_and(image,image, mask= mask2)
im2=cv2.cvtColor(res2,cv2.COLOR_BGR2GRAY)
contours2, hierarchy2 = cv2.findContours(im2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours2:
x2,y2,w2,h2 = cv2.boundingRect(i)
cv2.rectangle(image,(x2,y2),(x2+w2,y2+h2),(255,0,0),2)
res3 = cv2.bitwise_and(image,image, mask= mask3)
im3=cv2.cvtColor(res3,cv2.COLOR_BGR2GRAY)
contours3, hierarchy3 = cv2.findContours(im3,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours3:
x3,y3,w3,h3 = cv2.boundingRect(i)
cv2.rectangle(image,(x3,y3),(x3+w3,y3+h3),(0,0,255),2)
res4 = cv2.bitwise_and(image,image, mask= mask4)
im4=cv2.cvtColor(res4,cv2.COLOR_BGR2GRAY)
contours4, hierarchy4 = cv2.findContours(im4,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours4:
x4,y4,w4,h4 = cv2.boundingRect(i)
cv2.rectangle(image,(x4,y4),(x4+w4,y4+h4),(0,255,255),2)
res5 = cv2.bitwise_and(image,image, mask= mask5)
im5=cv2.cvtColor(res5,cv2.COLOR_BGR2GRAY)
contours5, hierarchy5 = cv2.findContours(im5,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours5:
x5,y5,w5,h5 = cv2.boundingRect(i)
cv2.rectangle(image,(x5,y5),(x5+w5,y5+h5),(0,165,255),2)
res6 = cv2.bitwise_and(image,image, mask= mask6)
im6=cv2.cvtColor(res6,cv2.COLOR_BGR2GRAY)
contours6, hierarchy6 = cv2.findContours(im6,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
for i in contours6:
x6,y6,w6,h6 = cv2.boundingRect(i)
cv2.rectangle(image,(x6,y6),(x6+w6,y6+h6),(238,130,238),2)
#mask8 = cv2.cvtColor(mask, cv2.COLOR_HSV2BGR)
#image[mask==0]=0
#final=res+res3+res2
#final=res+res2
final=image
#frame1=cv.fromarray(final)
frame1=final
#im2=cv2.cvtColor(frame1,cv2.COLOR_BGR2GRAY)
#im3=cv2.cvtColor(im2,cv2.COLOR_GRAY2BGR)
#ret,thresh=cv2.threshold(im2,127,255,0)
#contours, hierarchy = cv2.findContours(im2,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
#for i in contours:
#x,y,w,h = cv2.boundingRect(i)
#cv2.rectangle(frame1,(x,y),(x+w,y+h),(0,255,0),2)
#frame1=im3
frame1=cv.fromarray(frame1)
#cv2.imshow('Features', im)
#frame=fin
#res = cv2.bitwise_and(image,image, mask= mask3)
#mask3=cv2.cvtColor(mask3,cv2.COLOR_GRAY2RGB)
#frame=cv.fromarray(mask3)
pub.publish(bridge.cv_to_imgmsg(frame1, "bgr8"))
class ProjectorCalibrator:
def __init__(self):
self.printed_chessboard_corners_x = rospy.get_param(
'~printed_chessboard_corners_x')
self.printed_chessboard_corners_y = rospy.get_param(
'~printed_chessboard_corners_y')
self.printed_chessboard_spacing = rospy.get_param(
'~printed_chessboard_spacing')
self.bridge = CvBridge()
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info',
sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'get_projector_info', projector.srv.GetProjectorInfo)
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection',
projector.srv.SetProjection)
rospy.loginfo("Waiting for projector info setting service...")
rospy.wait_for_service('set_projector_info')
rospy.loginfo("Projection setting service found.")
self.set_projector_info = rospy.ServiceProxy(
'set_projector_info', sensor_msgs.srv.SetCameraInfo)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.projector_info = projector_info_service().projector_info
self.blank_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.SetZero(self.blank_projection)
self.number_of_scenes = 0
self.object_points = []
self.image_points = []
def set_projected_chessboard(self, corners_x, corners_y, spacing,
horizontal_justify, vertical_justify):
self.projected_chessboard_corners_x = corners_x
self.projected_chessboard_corners_y = corners_y
self.projected_chessboard_spacing = spacing
if horizontal_justify is 0:
self.chessboard_upper_left_corner_x = 0
elif horizontal_justify is 1:
self.chessboard_upper_left_corner_x = (
self.projector_info.width -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_x + 1))) // 2
elif horizontal_justify is 2:
self.chessboard_upper_left_corner_x = (
self.projector_info.width -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_x + 1)))
if vertical_justify is 0:
self.chessboard_upper_left_corner_y = 0
elif vertical_justify is 1:
self.chessboard_upper_left_corner_y = (
self.projector_info.height -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_y + 1))) // 2
elif vertical_justify is 2:
self.chessboard_upper_left_corner_y = (
self.projector_info.height -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_y + 1)))
self.positive_chessboard_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(self.positive_chessboard_projection, 255)
for row in range(self.projected_chessboard_corners_y + 1):
for col in range(self.projected_chessboard_corners_x + 1):
if (row + col) % 2 == 0:
cv.Rectangle(
self.positive_chessboard_projection,
(self.chessboard_upper_left_corner_x +
(col * self.projected_chessboard_spacing),
self.chessboard_upper_left_corner_y +
(row * self.projected_chessboard_spacing)),
(self.chessboard_upper_left_corner_x +
((col + 1) * self.projected_chessboard_spacing) - 1,
self.chessboard_upper_left_corner_y +
((row + 1) * self.projected_chessboard_spacing) - 1),
0, cv.CV_FILLED)
self.negative_chessboard_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Not(self.positive_chessboard_projection,
self.negative_chessboard_projection)
def update_image(self, imagemsg):
with self.mutex:
if not self.image_update_flag.is_set():
self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
self.image_update_flag.set()
self.image_update_flag.clear()
def get_next_image(self):
with self.mutex:
self.image_update_flag.clear()
self.image_update_flag.wait()
with self.mutex:
return self.latest_image
def get_picture_of_projection(self, projection):
image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(image_message)
return self.get_next_image()
def preview_chessboard(self):
image_message = self.bridge.cv_to_imgmsg(
self.negative_chessboard_projection, encoding="mono8")
self.set_projection(image_message)
def get_scene_image_points(self):
scene_image_points = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC2)
for row in range(self.projected_chessboard_corners_y):
for col in range(self.projected_chessboard_corners_x):
index = (row * self.projected_chessboard_corners_x) + col
point = (self.chessboard_upper_left_corner_x +
(self.projected_chessboard_spacing * (col + 1)),
self.chessboard_upper_left_corner_y +
(self.projected_chessboard_spacing * (row + 1)))
scene_image_points[0, index] = point
return scene_image_points
def commit(self):
response = self.set_projector_info(self.projector_info)
return response.success
def calibrate(self):
image_points_mat = concatenate_mats(self.image_points)
object_points_mat = concatenate_mats(self.object_points)
print "Image Points:"
for row in range(image_points_mat.height):
for col in range(image_points_mat.width):
print image_points_mat[row, col]
print
print "Object Points:"
for row in range(object_points_mat.height):
for col in range(object_points_mat.width):
print object_points_mat[row, col]
print
point_counts_mat = cv.CreateMat(1, self.number_of_scenes, cv.CV_32SC1)
for i in range(self.number_of_scenes):
point_counts_mat[0, i] = self.image_points[i].width
intrinsics = cv.CreateMat(3, 3, cv.CV_32FC1)
distortion = cv.CreateMat(4, 1, cv.CV_32FC1)
cv.SetZero(intrinsics)
cv.SetZero(distortion)
size = (self.projector_info.width, self.projector_info.height)
tvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
rvecs = cv.CreateMat(self.number_of_scenes, 3, cv.CV_32FC1)
cv.CalibrateCamera2(object_points_mat,
image_points_mat,
point_counts_mat,
size,
intrinsics,
distortion,
rvecs,
tvecs,
flags=0)
R = cv.CreateMat(3, 3, cv.CV_32FC1)
P = cv.CreateMat(3, 4, cv.CV_32FC1)
cv.SetIdentity(R)
cv.SetZero(P)
cv.Copy(intrinsics, cv.GetSubRect(P, (0, 0, 3, 3)))
self.projector_info = create_msg(size, distortion, intrinsics, R, P)
rospy.loginfo(self.projector_info)
for col in range(3):
for row in range(self.number_of_scenes):
print tvecs[row, col],
print
def capture(self):
blank = self.get_picture_of_projection(self.blank_projection)
positive = self.get_picture_of_projection(
self.positive_chessboard_projection)
negative = self.get_picture_of_projection(
self.negative_chessboard_projection)
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positive, negative, difference)
cv.NamedWindow("blank", flags=0)
cv.ShowImage("blank", blank)
cv.WaitKey(300)
cv.NamedWindow("difference", flags=0)
cv.ShowImage("difference", difference)
cv.WaitKey(300)
camera_image_points, camera_object_points = detect_chessboard(
blank, self.printed_chessboard_corners_x,
self.printed_chessboard_corners_y, self.printed_chessboard_spacing,
self.printed_chessboard_spacing)
if camera_image_points is None:
return False
cv.UndistortPoints(camera_image_points, camera_image_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
homography = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(camera_image_points, camera_object_points,
homography)
object_points, dummy = detect_chessboard(
difference, self.projected_chessboard_corners_x,
self.projected_chessboard_corners_y, None, None)
if object_points is None:
return False
cv.UndistortPoints(object_points, object_points,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
cv.PerspectiveTransform(object_points, object_points, homography)
object_points_3d = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC3)
x = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
y = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.Split(object_points, x, y, None, None)
z = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.SetZero(z)
cv.Merge(x, y, z, None, object_points_3d)
self.object_points.append(object_points_3d)
self.image_points.append(self.get_scene_image_points())
self.number_of_scenes += 1
return True
class FloorMapper:
def __init__(self):
self.proba = None
self.info = None
self.br = CvBridge()
rospy.init_node('floor_projector')
image_size = rospy.get_param("~floor_size_pix", 1000)
image_extent = rospy.get_param("~floor_size_meter", 5.0)
self.target_frame = rospy.get_param("~target_frame", "/body")
self.horizon_offset = rospy.get_param("~horizon_offset_pix", 20)
self.floor_map = cv.CreateImage((image_size, image_size), 8, 1)
self.x_floor = 0.0
self.y_floor = self.floor_map.height / 2.0
self.floor_scale = self.floor_map.width / image_extent
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("~floor", Image)
rospy.Subscriber("~probabilities", Image, self.store_proba)
rospy.Subscriber("~info", CameraInfo, self.store_info)
rospy.loginfo("Waiting for first proba and camera info")
while (not rospy.is_shutdown()) and ((not self.info) or
(not self.proba)):
rospy.sleep(0.1)
def store_proba(self, proba):
# print "Got Image"
if not self.info:
return
# print "Processing"
self.timestamp = proba.header.stamp
I = self.br.imgmsg_to_cv(proba, "8UC1")
self.proba = cv.CloneMat(I)
cv.Threshold(I, self.proba, 0xFE, 0xFE, cv.CV_THRESH_TRUNC)
try:
# (trans,rot) = self.listener.lookupTransform(proba.header.frame_id, '/world', proba.header.stamp)
self.listener.waitForTransform(proba.header.frame_id,
self.target_frame,
proba.header.stamp,
rospy.Duration(1.0))
trans = numpy.mat(
self.listener.asMatrix(self.target_frame, proba.header))
# print "Transformation"
# print trans
dstdir = [trans * v for v in self.dirpts3d]
# print "Destination dir"
# print dstdir
origin = trans * self.origin
origin = origin / origin[3, 0]
# origin = numpy.matrix([0.0, 0.0, origin[2,0] / origin[3,0], 1.0]).T
# print "Origin"
# print origin
self.dstpts2d = cv.CreateMat(4, 2, cv.CV_32F)
for i in range(4):
self.dstpts2d[i, 0] = self.x_floor + (origin[0, 0] - dstdir[i][
0, 0] * origin[2, 0] / dstdir[i][2, 0]) * self.floor_scale
self.dstpts2d[i, 1] = self.y_floor - (origin[1, 0] - dstdir[i][
1, 0] * origin[2, 0] / dstdir[i][2, 0]) * self.floor_scale
# print numpy.asarray(self.dstpts2d)
# print "Source points"
# print numpy.asarray(self.srcpts2d)
# print "Dest points"
# print numpy.asarray(self.dstpts2d)
self.H = cv.CreateMat(3, 3, cv.CV_32F)
cv.FindHomography(self.srcpts2d, self.dstpts2d, self.H)
# print "Homography"
# print numpy.asarray(self.H)
cv.WarpPerspective(cv.GetSubRect(
self.proba, (0, self.horizon_offset, self.proba.width,
self.proba.height - self.horizon_offset)),
self.floor_map,
self.H,
flags=cv.CV_INTER_NN + cv.CV_WARP_FILL_OUTLIERS,
fillval=0xFF)
msg = self.br.cv_to_imgmsg(self.floor_map)
msg.header.stamp = proba.header.stamp
msg.header.frame_id = self.target_frame
self.pub.publish(msg)
# print "Publishing image"
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "Exception while looking for transform"
return
def store_info(self, info):
if not self.info:
# assuming no distortion
self.f = info.K[0]
self.xc = info.K[2]
self.yc = info.K[5]
print("Got camera info: f %.2f C %.2f %.2f" %
(self.f, self.xc, self.yc))
self.origin = numpy.zeros((4, 1))
self.origin[3, 0] = 1.0
self.horizon_offset = int(
math.ceil(info.height / 2. + self.horizon_offset))
srcpts = [[0,info.height-1],[info.width-1,info.height-1],\
[0,self.horizon_offset], [info.width-1,self.horizon_offset]]
self.srcpts2d = cv.CreateMat(4, 2, cv.CV_32F)
for i in range(4):
self.srcpts2d[i, 0] = srcpts[i][0]
self.srcpts2d[i, 1] = srcpts[i][1] - self.horizon_offset
self.dirpts3d = []
for i in range(4):
v3 = numpy.matrix([
-(srcpts[i][0] - self.xc) / self.f,
-(srcpts[i][1] - self.yc) / self.f, 1.0, 0.0
]).T
n = math.sqrt((v3.T * v3).sum())
self.dirpts3d.append(v3 / n)
self.info = info
# print self.dirpts3d
def run(self):
rospy.loginfo("Starting floor projection")
rospy.spin()
class ProjectorCameraCalibrator:
def __init__(self):
self.projected_chessboard_corners_x = rospy.get_param(
'~projected_chessboard_corners_x')
self.projected_chessboard_corners_y = rospy.get_param(
'~projected_chessboard_corners_y')
self.projected_chessboard_spacing = rospy.get_param(
'~projected_chessboard_spacing')
self.printed_chessboard_corners_x = rospy.get_param(
'~printed_chessboard_corners_x')
self.printed_chessboard_corners_y = rospy.get_param(
'~printed_chessboard_corners_y')
self.printed_chessboard_spacing = rospy.get_param(
'~printed_chessboard_spacing')
self.projector_camera_info_file_name = rospy.get_param(
'~projector_camera_info_file_name')
self.bridge = CvBridge()
self.mutex = threading.RLock()
self.image_update_flag = threading.Event()
rospy.Subscriber("image_stream", sensor_msgs.msg.Image,
self.update_image)
rospy.loginfo("Waiting for camera info...")
self.camera_info = rospy.wait_for_message('camera_info',
sensor_msgs.msg.CameraInfo)
rospy.loginfo("Camera info received.")
rospy.loginfo("Waiting for projector info service...")
rospy.wait_for_service('get_projector_info')
rospy.loginfo("Projector info service found.")
projector_info_service = rospy.ServiceProxy(
'get_projector_info', projector.srv.GetProjectorInfo)
self.projector_info = projector_info_service().projector_info
rospy.loginfo("Waiting for projection setting service...")
rospy.wait_for_service('set_projection')
rospy.loginfo("Projection setting service found.")
self.set_projection = rospy.ServiceProxy('set_projection',
projector.srv.SetProjection)
self.camera_model = image_geometry.PinholeCameraModel()
self.camera_model.fromCameraInfo(self.camera_info)
self.projector_model = image_geometry.PinholeCameraModel()
self.projector_model.fromCameraInfo(self.projector_info)
self.blank_projection = cv.CreateMat(self.projector_info.height,
self.projector_info.width,
cv.CV_8UC1)
cv.SetZero(self.blank_projection)
# Generate chessboard projections
self.chessboard_upper_left_corner_x = (
self.projector_info.width -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_x + 1))) // 2
self.chessboard_upper_left_corner_y = (
self.projector_info.height -
(self.projected_chessboard_spacing *
(self.projected_chessboard_corners_y + 1))) // 2
self.positive_chessboard_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Set(self.positive_chessboard_projection, 255)
for row in range(self.projected_chessboard_corners_y + 1):
for col in range(self.projected_chessboard_corners_x + 1):
if (row + col) % 2 == 0:
cv.Rectangle(
self.positive_chessboard_projection,
(self.chessboard_upper_left_corner_x +
(col * self.projected_chessboard_spacing),
self.chessboard_upper_left_corner_y +
(row * self.projected_chessboard_spacing)),
(self.chessboard_upper_left_corner_x +
((col + 1) * self.projected_chessboard_spacing) - 1,
self.chessboard_upper_left_corner_y +
((row + 1) * self.projected_chessboard_spacing) - 1),
0, cv.CV_FILLED)
self.negative_chessboard_projection = cv.CreateMat(
self.projector_info.height, self.projector_info.width, cv.CV_8UC1)
cv.Not(self.positive_chessboard_projection,
self.negative_chessboard_projection)
self.object_points = []
self.camera_image_points = []
self.projector_image_points = []
self.number_of_scenes = 0
# Show negative chessboard so user can position printed chessboard for capture
negative_chessboard_message = self.bridge.cv_to_imgmsg(
self.negative_chessboard_projection, encoding="mono8")
self.set_projection(negative_chessboard_message)
def update_image(self, imagemsg):
with self.mutex:
if not self.image_update_flag.is_set():
self.latest_image = self.bridge.imgmsg_to_cv(imagemsg, "mono8")
self.image_update_flag.set()
self.image_update_flag.clear()
def get_next_image(self):
with self.mutex:
self.image_update_flag.clear()
self.image_update_flag.wait()
with self.mutex:
return self.latest_image
def get_picture_of_projection(self, projection):
image_message = self.bridge.cv_to_imgmsg(projection, encoding="mono8")
self.set_projection(image_message)
return self.get_next_image()
def get_projector_image_points(self):
scene_image_points = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC2)
for row in range(self.projected_chessboard_corners_y):
for col in range(self.projected_chessboard_corners_x):
index = (row * self.projected_chessboard_corners_x) + col
point = (self.chessboard_upper_left_corner_x +
(self.projected_chessboard_spacing * (col + 1)),
self.chessboard_upper_left_corner_y +
(self.projected_chessboard_spacing * (row + 1)))
scene_image_points[0, index] = point
return scene_image_points
def write_projector_camera_info(self):
info_dict = {}
info_dict['projector_to_camera_translation_vector'] = mat_to_list(
self.projector_to_camera_translation_vector)
info_dict['projector_to_camera_rotation_vector'] = mat_to_list(
self.projector_to_camera_rotation_vector)
output_stream = file(self.projector_camera_info_file_name, 'w')
yaml.dump(info_dict, output_stream)
rospy.loginfo("Projector camera info successfully written to " +
self.projector_camera_info_file_name)
def calibrate(self):
self.projector_to_camera_translation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
self.projector_to_camera_rotation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
cv.SetZero(self.projector_to_camera_translation_vector)
cv.SetZero(self.projector_to_camera_rotation_vector)
for i in range(self.number_of_scenes):
camera_tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
camera_rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(self.object_points[i],
self.camera_image_points[i],
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs(),
camera_rvec, camera_tvec)
print "Camera To Board Vector:"
for row in range(camera_tvec.height):
for col in range(camera_tvec.width):
print camera_tvec[row, col],
print
print
projector_tvec = cv.CreateMat(3, 1, cv.CV_32FC1)
projector_rvec = cv.CreateMat(3, 1, cv.CV_32FC1)
cv.FindExtrinsicCameraParams2(
self.object_points[i], self.projector_image_points[i],
self.projector_model.intrinsicMatrix(),
self.projector_model.distortionCoeffs(), projector_rvec,
projector_tvec)
print "Projector To Board Vector:"
for row in range(projector_tvec.height):
for col in range(projector_tvec.width):
print projector_tvec[row, col],
print
print
camera_rmat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(camera_rvec, camera_rmat)
projector_rmat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.Rodrigues2(projector_rvec, projector_rmat)
scene_projector_to_camera_rotation_matrix = cv.CreateMat(
3, 3, cv.CV_32FC1)
cv.GEMM(camera_rmat, projector_rmat, 1, None, 0,
scene_projector_to_camera_rotation_matrix, cv.CV_GEMM_B_T)
scene_projector_to_camera_rotation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
for i in range(3):
for j in range(3):
print scene_projector_to_camera_rotation_matrix[i, j],
print
print
cv.Rodrigues2(scene_projector_to_camera_rotation_matrix,
scene_projector_to_camera_rotation_vector)
print "Scene Rotation Vector:"
for row in range(scene_projector_to_camera_rotation_vector.height):
for col in range(
scene_projector_to_camera_rotation_vector.width):
print scene_projector_to_camera_rotation_vector[row, col],
print
print
scene_projector_to_camera_translation_vector = cv.CreateMat(
3, 1, cv.CV_32FC1)
cv.GEMM(projector_rmat, projector_tvec, -1, None, 0,
scene_projector_to_camera_translation_vector,
cv.CV_GEMM_A_T)
cv.GEMM(camera_rmat, scene_projector_to_camera_translation_vector,
1, camera_tvec, 1,
scene_projector_to_camera_translation_vector, 0)
print "Scene Translation Vector:"
for row in range(
scene_projector_to_camera_translation_vector.height):
for col in range(
scene_projector_to_camera_translation_vector.width):
print scene_projector_to_camera_translation_vector[row,
col],
print
print
cv.Add(scene_projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector)
cv.Add(scene_projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector)
cv.ConvertScale(self.projector_to_camera_translation_vector,
self.projector_to_camera_translation_vector,
scale=1.0 / self.number_of_scenes)
cv.ConvertScale(self.projector_to_camera_rotation_vector,
self.projector_to_camera_rotation_vector,
scale=1.0 / self.number_of_scenes)
print "Final Translation Vector:"
for row in range(self.projector_to_camera_translation_vector.height):
for col in range(
self.projector_to_camera_translation_vector.width):
print self.projector_to_camera_translation_vector[row, col],
print
print
print "Final Rotation Vector:"
for row in range(self.projector_to_camera_rotation_vector.height):
for col in range(self.projector_to_camera_rotation_vector.width):
print self.projector_to_camera_rotation_vector[row, col],
print
def capture(self):
blank = self.get_picture_of_projection(self.blank_projection)
positive = self.get_picture_of_projection(
self.positive_chessboard_projection)
negative = self.get_picture_of_projection(
self.negative_chessboard_projection)
difference = cv.CreateMat(self.camera_info.height,
self.camera_info.width, cv.CV_8UC1)
cv.Sub(positive, negative, difference)
#cv.CmpS(difference, 0, difference, cv.CV_CMP_GT)
cv.NamedWindow("blank", flags=0)
cv.ShowImage("blank", blank)
cv.WaitKey(300)
cv.NamedWindow("difference", flags=0)
cv.ShowImage("difference", difference)
cv.WaitKey(300)
camera_image_points_printed, object_points_printed = detect_chessboard(
blank, self.printed_chessboard_corners_x,
self.printed_chessboard_corners_y, self.printed_chessboard_spacing,
self.printed_chessboard_spacing)
if camera_image_points_printed is None:
return False
cv.UndistortPoints(camera_image_points_printed,
camera_image_points_printed,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
homography = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.FindHomography(camera_image_points_printed, object_points_printed,
homography)
camera_image_points_projected, dummy = detect_chessboard(
difference, self.projected_chessboard_corners_x,
self.projected_chessboard_corners_y, None, None)
if camera_image_points_projected is None:
return False
object_points_projected = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC2)
cv.UndistortPoints(camera_image_points_projected,
object_points_projected,
self.camera_model.intrinsicMatrix(),
self.camera_model.distortionCoeffs())
cv.PerspectiveTransform(object_points_projected,
object_points_projected, homography)
object_points_3d = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC3)
x = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
y = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.Split(object_points_projected, x, y, None, None)
z = cv.CreateMat(
1, self.projected_chessboard_corners_x *
self.projected_chessboard_corners_y, cv.CV_32FC1)
cv.SetZero(z)
cv.Merge(x, y, z, None, object_points_3d)
self.object_points.append(object_points_3d)
self.camera_image_points.append(camera_image_points_projected)
self.projector_image_points.append(self.get_projector_image_points())
self.number_of_scenes += 1
return True
def __init__(self):
rospy.init_node('avi2ros', anonymous=True)
self.input = rospy.get_param("~input", "")
image_pub = rospy.Publisher("output", Image)
self.fps = rospy.get_param("~fps", 25)
self.loop = rospy.get_param("~loop", False)
self.width = rospy.get_param("~width", "")
self.height = rospy.get_param("~height", "")
self.start_paused = rospy.get_param("~start_paused", False)
self.show_text = True
rospy.on_shutdown(self.cleanup)
video = cv.CaptureFromFile(self.input)
fps = int(cv.GetCaptureProperty(video, cv.CV_CAP_PROP_FPS))
""" Bring the fps up to the specified rate """
try:
fps = int(fps * self.fps / fps)
except:
fps = self.fps
cv.NamedWindow("AVI Video", True) # autosize the display
cv.MoveWindow("AVI Video", 650, 100)
bridge = CvBridge()
self.paused = self.start_paused
self.keystroke = None
self.restart = False
# Get the first frame to display if we are starting in the paused state.
frame = cv.QueryFrame(video)
image_size = cv.GetSize(frame)
if self.width and self.height and (self.width != image_size[0]
or self.height != image_size[1]):
rospy.loginfo("Resizing! " + str(self.width) + " x " +
str(self.height))
resized_frame = cv.CreateImage((self.width, self.height),
frame.depth, frame.channels)
cv.Resize(frame, resized_frame)
frame = cv.CloneImage(resized_frame)
text_frame = cv.CloneImage(frame)
cv.Zero(text_frame)
while not rospy.is_shutdown():
""" Handle keyboard events """
self.keystroke = cv.WaitKey(1000 / fps)
""" Process any keyboard commands """
if 32 <= self.keystroke and self.keystroke < 128:
cc = chr(self.keystroke).lower()
if cc == 'q':
""" user has press the q key, so exit """
rospy.signal_shutdown("User hit q key to quit.")
elif cc == ' ':
""" Pause or continue the video """
self.paused = not self.paused
elif cc == 'r':
""" Restart the video from the beginning """
self.restart = True
elif cc == 't':
""" Toggle display of text help message """
self.show_text = not self.show_text
if self.restart:
video = cv.CaptureFromFile(self.input)
self.restart = None
if not self.paused:
frame = cv.QueryFrame(video)
if frame and self.width and self.height:
if self.width != image_size[
0] or self.height != image_size[1]:
cv.Resize(frame, resized_frame)
frame = cv.CloneImage(resized_frame)
if frame == None:
if self.loop:
self.restart = True
else:
if self.show_text:
frame_size = cv.GetSize(frame)
text_font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.2, 1,
0, 1, 8)
cv.PutText(text_frame, "Keyboard commands:",
(20, int(frame_size[1] * 0.6)), text_font,
cv.RGB(255, 255, 0))
cv.PutText(text_frame, " ",
(20, int(frame_size[1] * 0.65)), text_font,
cv.RGB(255, 255, 0))
cv.PutText(text_frame, "space - toggle pause/play",
(20, int(frame_size[1] * 0.72)), text_font,
cv.RGB(255, 255, 0))
cv.PutText(text_frame,
" r - restart video from beginning",
(20, int(frame_size[1] * 0.79)), text_font,
cv.RGB(255, 255, 0))
cv.PutText(text_frame, " t - hide/show this text",
(20, int(frame_size[1] * 0.86)), text_font,
cv.RGB(255, 255, 0))
cv.PutText(text_frame, " q - quit the program",
(20, int(frame_size[1] * 0.93)), text_font,
cv.RGB(255, 255, 0))
cv.Add(frame, text_frame, text_frame)
cv.ShowImage("AVI Video", text_frame)
cv.Zero(text_frame)
try:
image_pub.publish(bridge.cv_to_imgmsg(frame, "bgr8"))
except CvBridgeError, e:
print e
class Gate:
light = 0
target = 0
rows = 0
col = 0
velocity = 0
params = {
'satLow': 50,
'satHigh': 255,
'hueLow': 13,
'hueHigh': 38,
'valLow': 0,
'valHigh': 255,
'Kp': 10,
'Vmax': 40
}
client = None
histClass = bbHistogram(Hist_constants.TRIPLE_CHANNEL)
centroidx = list()
centroidy = list()
bridge = None
# Convert a ROS Image to the Numpy matrix used by cv2 functions
def rosimg2cv(self, ros_image):
# Convert from ROS Image to old OpenCV image
frame = self.bridge.imgmsg_to_cv(ros_image, ros_image.encoding)
# Convert from old OpenCV image to Numpy matrix
return np.array(frame, dtype=np.uint8) #TODO: find out actual dtype
def __init__(self):
self.bridge = CvBridge()
#imageTopic = rospy.get_param('~image', '/stereo_camera/left/image_color')
imageTopic = rospy.get_param(
'~image', '/stereo_camera/left/image_rect_color_remote')
cv2.namedWindow("Gate Settings", cv2.CV_WINDOW_AUTOSIZE)
image_sub = rospy.Subscriber(imageTopic, Image,
self.computeImageCallback)
self.image_pub2 = rospy.Publisher("/Vision/image_filter", Image)
self.histClass.setParams(self.params)
def paramSetter(key):
def setter(val):
self.params[key] = val
return setter
cv2.createTrackbar("Kp constant:", "Gate Settings", self.params['Kp'],
1000, paramSetter('Kp'))
cv2.createTrackbar("Vmax:", "Gate Settings", self.params['Vmax'], 100,
paramSetter('Vmax'))
#in m/second
def calcCentroid(self, moments):
return moments['mu01'] / moments['mu00']
def calcVelocity(self, pos, imgCenter):
error = imgCenter - pos
correction = self.params['Kp']
return correction
def computeImageCallback(self, data):
if (self.bridge != None):
try:
cv_image = self.rosimg2cv(data)
#cv2.imshow("Actual image", cv_image)
except CvBridgeError, e:
print e
self.rows, self.cols, val = cv_image.shape
cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
cv_image = np.array(cv_image, dtype=np.uint8)
if self.histClass != None:
self.params = self.histClass.getParams()
'''Perform Thresholding on HSV'''
COLOR_MIN = np.array([
self.params['hueLow'], self.params['satLow'],
self.params['valLow']
], np.uint8)
COLOR_MAX = np.array([
self.params['hueHigh'], self.params['satHigh'],
self.params['valHigh']
], np.uint8)
self.histClass.getTripleHist(cv_image)
cv_single = cv2.inRange(cv_image, COLOR_MIN, COLOR_MAX)
'''Perform Morphological Operations on binary image to clean it up'''
kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3))
kernel_close = cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3))
cv_single = cv2.morphologyEx(cv_single, cv2.MORPH_OPEN, kernel)
cv_single = cv2.morphologyEx(cv_single,
cv2.MORPH_CLOSE,
kernel_close,
iterations=5)
'''Find contours on binary image and identify target to home in on'''
contours, hierarchy = cv2.findContours(cv_single,
cv2.RETR_EXTERNAL,
cv2.CHAIN_APPROX_NONE)
contourIdx = -1
contourImg = np.zeros((480, 640, 3), dtype=np.uint8)
color = cv2.cv.Scalar(100, 100, 100)
centroidx_temp = list()
centroidy_temp = list()
centroidx_hack = list()
centroidy_hack = list()
for i in range(0, len(contours)):
moments = cv2.moments(contours[i], binaryImage=False)
if moments['m00'] > 400:
cv2.drawContours(contourImg,
contours,
i, (100, 255, 100),
thickness=-1)
#print len(contours)
centroidy_temp.append(moments['m01'] / moments['m00'])
centroidx_temp.append(moments['m10'] / moments['m00'])
cv2.circle(contourImg,
(int(centroidx_temp[len(centroidx_temp) - 1]),
int(centroidy_temp[len(centroidy_temp) - 1])),
2,
255,
thickness=-1)
#calculate central plane for AUV to aim towards
if (len(centroidx_temp) > 1):
centroidx_hack.append(centroidx_temp[0])
centroidy_hack.append(centroidy_temp[0])
ctr = 1
while (len(centroidx_temp) - 1 > ctr):
if (fabs(centroidx_hack[0] - centroidx_temp[ctr]) > 50):
centroidx_hack.append(centroidx_temp[ctr])
centroidy_hack.append(centroidy_temp[ctr])
break
ctr = ctr + 1
if (len(centroidx_hack) == 2):
cv2.line(contourImg,
(int(centroidx_hack[1]), int(centroidy_hack[1])),
(int(centroidx_hack[0]), int(centroidy_hack[0])),
(255, 0, 0),
thickness=1,
lineType=cv2.CV_AA)
self.target = (centroidx_hack[0] + centroidx_hack[1]) / 2
cv2.circle(contourImg,
(int(self.target), int(self.rows / 2)),
2, (0, 0, 255),
thickness=-1)
#cv2.waitKey(3)
try:
contourImg = cv2.cv.fromarray(contourImg)
if self.target != None:
pass
#self.image_pub.publish(float(self.target))
self.image_pub2.publish(
self.bridge.cv_to_imgmsg(contourImg, encoding="bgr8"))
#cv2.waitKey(1)
except CvBridgeError, e:
print e
except ROSException, e:
rospy.logwarn("Topic closed during node shutdown" + str(e))
class person_detection:
def __init__(self):
config = dict(hog_win_stride=(rospy.get_param("~hog_win_stride", 8),
rospy.get_param("~hog_win_stride", 8)),
hog_padding=(rospy.get_param("~hog_padding", 2),
rospy.get_param("~hog_padding", 2)),
hog_scale=rospy.get_param("~hog_scale", 1.075),
camera_resolution_x=640,
camera_resolution_y=480)
self.detector = PersonDetector(config)
self.detected_people_topic = rospy.get_param(
"~detected_people_topic", "person_detection/people")
self.image_topic = rospy.get_param("~image_topic",
"axis/image_raw/decompressed")
self.visualization_topic = rospy.get_param("~visualization_topic",
"person_detection/viz")
self.setup_ros_node()
self.last_detection = rospy.Time.now()
self.detection_interval = rospy.Duration(0.75)
#----------------------------------------------------
def setup_ros_node(self):
rospy.init_node('person_detection')
self.people_publisher = rospy.Publisher(self.detected_people_topic,
Polygon)
self.viz_publisher = rospy.Publisher(self.visualization_topic, Image)
rospy.Subscriber(self.image_topic, Image, self.handle_image)
self.cv_bridge = CvBridge()
#----------------------------------------------------
def no_one_listening(self):
return (self.people_publisher.get_num_connections() < 1
and self.viz_publisher.get_num_connections() < 1)
#----------------------------------------------------
def handle_image(self, data):
if self.no_one_listening():
return
begin_processing = rospy.Time.now()
if begin_processing - self.last_detection < self.detection_interval:
return # don't do anything unless enough time has elapsed
# convert from ros message to openCV image
cv2_image = self.ros_msg_to_cv2(data)
rectangles = self.detector.apply_frame(cv2_image)
# publish super_polygon
super_polygon = []
for rect in rectangles:
super_polygon.append(Point32(x=rect[0][0], y=rect[0][1], z=0))
super_polygon.append(Point32(x=rect[1][0], y=rect[1][1], z=0))
super_polygon.append(Point32(x=rect[2][0], y=rect[2][1], z=0))
super_polygon.append(Point32(x=rect[3][0], y=rect[3][1], z=0))
self.people_publisher.publish(Polygon(super_polygon))
self.publish_viz(rectangles, cv2_image)
elapsed = rospy.Time.now() - begin_processing
# adjust frame processing rate to match detector rate,
# plus a small margin
self.detection_interval = rospy.Duration(elapsed.to_sec() + 0.1)
#----------------------------------------------------
def publish_viz(self, rectangles, img):
if self.viz_publisher.get_num_connections() < 1:
return
for rect in rectangles:
cv2.rectangle(img, rect[0], rect[2], (255, 255, 255))
img = cv.fromarray(img)
msg = self.cv_bridge.cv_to_imgmsg(img, encoding="bgr8")
self.viz_publisher.publish(msg)
#----------------------------------------------------
def ros_msg_to_cv2(self, ros_msg):
try:
cv_image = self.cv_bridge.imgmsg_to_cv(ros_msg,
desired_encoding="bgr8")
except CvBridgeError, e:
print e
return np.asarray(cv_image[:, :])
class Aggregator:
def __init__(self, ns_list):
print "Creating aggregator for ", ns_list
self.lock = threading.Lock()
# image
w = 640
h = 480
self.image_out = cv.CreateMat(h, w, cv.CV_8UC3)
self.pub = rospy.Publisher('aggregated_image', Image)
self.bridge = CvBridge()
self.image_captured = get_image(["Successfully captured checkerboard"])
self.image_optimized = get_image(["Successfully ran optimization"])
self.image_failed = get_image(["Failed to run optimization"], False)
# create render windows
layouts = [ (1,1), (2,2), (2,2), (2,2), (3,3), (3,3), (3,3), (3,3), (3,3) ]
layout = layouts[len(ns_list)-1]
sub_w = w / layout[0]
sub_h = h / layout[1]
self.windows = []
for j in range(layout[1]):
for i in range(layout[0]):
self.windows.append( cv.GetSubRect(self.image_out, (i*sub_w, j*sub_h, sub_w, sub_h) ) )
# create renderers
self.renderer_list = []
for ns in ns_list:
self.renderer_list.append(ImageRenderer(ns))
# subscribers
self.capture_time = rospy.Time(0)
self.calibrate_time = rospy.Time(0)
self.captured_sub = rospy.Subscriber('robot_measurement', RobotMeasurement, self.captured_cb)
self.optimized_sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.calibrated_cb)
def captured_cb(self, msg):
with self.lock:
self.capture_time = rospy.Time.now()
beep([(400, 63, 0.2)])
def calibrated_cb(self, msg):
with self.lock:
self.calibrate_time = rospy.Time.now()
def loop(self):
r = rospy.Rate(20)
beep_time = rospy.Time(0)
while not rospy.is_shutdown():
try:
r.sleep()
except:
print "Shutting down"
with self.lock:
for window, render in zip(self.windows, self.renderer_list):
render.render(window)
if self.capture_time+rospy.Duration(4.0) > rospy.Time.now():
if self.capture_time+rospy.Duration(2.0) > rospy.Time.now():
self.pub.publish(self.bridge.cv_to_imgmsg(self.image_captured, encoding="passthrough"))
elif self.calibrate_time+rospy.Duration(20.0) > rospy.Time.now():
self.pub.publish(self.bridge.cv_to_imgmsg(self.image_optimized, encoding="passthrough"))
if beep_time+rospy.Duration(4.0) < rospy.Time.now():
beep_time = rospy.Time.now()
beep([(600, 63, 0.1), (800, 63, 0.1), (1000, 63, 0.3)])
else:
self.pub.publish(self.bridge.cv_to_imgmsg(self.image_failed, encoding="passthrough"))
if beep_time+rospy.Duration(4.0) < rospy.Time.now():
beep_time = rospy.Time.now()
beep([(400, 63, 0.1), (200, 63, 0.1), (100, 63, 0.6)])
else:
self.pub.publish(self.bridge.cv_to_imgmsg(self.image_out, encoding="passthrough"))
