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"))