class ImageDisplay: def __init__(self, device_num=0, color=True): cv.NamedWindow("Display",1) self.bridge = CvBridge() self.color = color self.device_num = device_num sub_name = 'camera_' + str(self.device_num) self.image_sub = rospy.Subscriber(sub_name,Image,self.image_callback) node_name = 'image_display' + str(self.device_num) rospy.init_node(node_name, anonymous=True) def image_callback(self,data): try: if self.color: cv_image = self.bridge.imgmsg_to_cv(data, "rgb8") else: cv_image = self.bridge.imgmsg_to_cv(data, "mono8") except CvBridgeError, e: print e cv.ShowImage("Display", cv_image) cv.WaitKey(3)
class Viewer(object): def __init__(self): self.bridge = CvBridge() self.sub_00 = message_filters.Subscriber("image_00/image_raw",Image) self.sub_01 = message_filters.Subscriber("image_01/image_raw",Image) self.sub_02 = message_filters.Subscriber("image_02/image_raw",Image) self.sub_03 = message_filters.Subscriber("image_03/image_raw",Image) tmp = [self.sub_00, self.sub_01, self.sub_02, self.sub_03] self.ts = message_filters.TimeSynchronizer(tmp,10) self.ts.registerCallback(self.callback) cv.NamedWindow("Camera 00", cv.CV_WINDOW_NORMAL) cv.NamedWindow("Camera 01", cv.CV_WINDOW_NORMAL) cv.NamedWindow("Camera 02", cv.CV_WINDOW_NORMAL) cv.NamedWindow("Camera 03", cv.CV_WINDOW_NORMAL) cv.NamedWindow("Anaglyf col", cv.CV_WINDOW_NORMAL) def callback(self,I0,I1,I2,I3): try: cv_i0 = self.bridge.imgmsg_to_cv(I0, "mono8") cv_i1 = self.bridge.imgmsg_to_cv(I1, "mono8") cv_i2 = self.bridge.imgmsg_to_cv(I2, "bgr8") cv_i3 = self.bridge.imgmsg_to_cv(I3, "bgr8") except CvBridgeError, e: print e cv.ShowImage("Camera 00", cv_i0) cv.ShowImage("Camera 01", cv_i1) cv.ShowImage("Camera 02", cv_i2) cv.ShowImage("Camera 03", cv_i3) merge=cv.CreateImage(cv.GetSize(cv_i2),8,3) makeMagic(cv_i3,cv_i2,merge) cv.ShowImage("Anaglyf col", merge) cv.WaitKey(3)
def save_images(bagfile): if not os.path.isdir("video_images"): os.mkdir("video_images") if not os.path.isdir("depth_images"): os.mkdir("depth_images") bag = rosbag.Bag(bagfile) bridge = CvBridge() count = 0 o = open('image_log.csv','w') o.write('im,secs,nsecs,extra\n') for topic, msg, t in bag.read_messages(): if topic == '/camera/rgb/image_rect_color': img = bridge.imgmsg_to_cv(msg, "bgr8") img_name = "video_images/img_%06d.png" %count o.write('im,%d,%d,\n' % (t.secs,t.nsecs)) count += 1 cv.SaveImage(img_name, img) elif topic == '/camera/depth_registered/image_rect': img = bridge.imgmsg_to_cv(msg, "bgr8") img_name = "depth_images/img_%06d.png" %count #o.write('de,%d,%d,\n' % (t.secs,t.nsecs)) #count += 1 cv.SaveImage(img_name, img)
class PersonExtracter(): def __init__(self, image_topic, labels_topic): self.bridge = CvBridge() # Synchronized subscribers sub_rgb = message_filters.Subscriber(image_topic, Image) sub_labels = message_filters.Subscriber(labels_topic, Image) ts = message_filters.TimeSynchronizer([sub_rgb, sub_labels], 1) ts.registerCallback(self.callback) def callback(self, image, labels): # Prep the images try: image_cv = self.bridge.imgmsg_to_cv(image) # to CvMat labels_cv = self.bridge.imgmsg_to_cv(labels) image_np = numpy.asarray(image_cv) # to numpy labels_np = numpy.asarray(labels_cv) except CvBridgeError, e: print e # Apply the mask mask = numpy.logical_not(labels_np) # "True" where there are NOT people for channel in range(3): # R, G, and B image_np[:, :, channel] *= mask; # in case we need to publish it try: image = self.bridge.cv2_to_imgmsg(image_np) except CvBridgeError, e: print e
def handle_detector_cb(req): vws = read_vws(req.geo_vw_sets) bridge = CvBridge() try: cv_image = bridge.imgmsg_to_cv(req.image, "bgr8") cv_mask = bridge.imgmsg_to_cv(req.mask, "bgr8") except CvBridgeError, e: print 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 ROSStereoListener: def __init__(self, topics, rate=30.0, name='stereo_listener'): self.listener = ru.GenericListener(name, [Image, Image], topics, rate) self.lbridge = CvBridge() self.rbridge = CvBridge() def next(self): lros, rros = self.listener.read(allow_duplication=False, willing_to_wait=True, warn=False, quiet=True) lcv = cv.CloneMat(self.lbridge.imgmsg_to_cv(lros, 'bgr8')) rcv = cv.CloneMat(self.rbridge.imgmsg_to_cv(rros, 'bgr8')) return lcv, rcv
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()
class main_node: def __init__(self): rospy.init_node('openCVodometry') self.cv_window_name = "OpenCV Odometry" cv.NamedWindow(self.cv_window_name, 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/rgb/image_color",\ ImageMsg, self.callback) root = Tk() app = TKWindow(master=root) try: app.mainloop() root.destroy() except KeyboardInterrupt: print "Shutting node." cv.DestroyAllWindows() def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e cv.ShowImage(self.cv_window_name, cv_image) cv.WaitKey(3)
class ImageDisplay: def __init__(self): self.image_sub = rospy.Subscriber("UndistortedImage", Image, self.image_callback) cv.NamedWindow("Display",1) self.bridge = CvBridge() self.color_max = 255 self.font = cv.InitFont(cv.CV_FONT_HERSHEY_TRIPLEX,0.5,0.5) self.images_initialized = False def initialize_images(self,cv_image): self.im_display = cv.CreateImage(cv.GetSize(cv_image),cv.IPL_DEPTH_8U,3) self.images_initialized = True def image_callback(self,data): # Convert ROS image to OpenCV image try: cv_image = cv.GetImage(self.bridge.imgmsg_to_cv(data, "passthrough")) except CvBridgeError, e: print e if not self.images_initialized: self.initialize_images(cv_image) cv.CvtColor(cv_image,self.im_display,cv.CV_GRAY2RGB) cv.ShowImage("Display", self.im_display) cv.WaitKey(3)
class DetectObjects(): def __init__(self): self.bridge = CvBridge() self.fix_count = 0 self.angle = None # init camera rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.cb_image) def cb_image(self, msg): try: frame = self.bridge.imgmsg_to_cv(msg, "bgr8") frame = np.array(frame, dtype=np.uint8) self.process_image(frame) key = cv.WaitKey(5) #print key if key == ord('r'): # restart rospy.sleep(3) except CvBridgeError, e: print e
class ardroneTracker: def __init__(self, tracker): self.point_pub = rospy.Publisher("/ardrone_tracker/found_point", Point ) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/ardrone/front/image_raw",Image,self.callback) self.image_pub = rospy.Publisher( "/ardrone_tracker/image", Image ) self.tracker = tracker def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e numpy_image = np.asarray( cv_image ) trackData = self.tracker.track( numpy_image ) if trackData: x,y,z = trackData[0],trackData[1],trackData[2] point = Point( x,y,z ) self.point_pub.publish( point ) else: self.point_pub.publish( Point( 0, 0, -1 ) ) try: vis = self.tracker.get_vis() image_message = self.bridge.cv_to_imgmsg(cv2.cv.fromarray( vis ), encoding="passthrough") self.image_pub.publish( image_message ) except CvBridgeError, e: print e
class roskoki: def __init__(self): self.image_pub = rospy.Publisher("image_topic_2",Image) cv.NamedWindow("Image window", 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("image_topic",Image,self.callback) def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e koki = PyKoki() params = CameraParams(Point2Df( cv_image.width/2, cv_image.height/2 ), Point2Df(571, 571), Point2Di( cv_image.width, cv_image.height )) print koki.find_markers( cv_image, 0.1, params ) cv.ShowImage("Image window", cv_image) cv.WaitKey(3) try: self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8")) except CvBridgeError, e: print e
class ObserveDartboard(threading.Thread): BGsample = 30 # number of frames to gather BG samples (average) from at start of capture def __init__(self, camSource): threading.Thread.__init__(self) # self.capture = cv2.VideoCapture(0) # self.ret, self.frame = self.capture.read() # width = self.capture.get(3) # height = self.capture.get(4) # print 'size = ' + str(width) + ' x ' + str(height) self.bridge = CvBridge() self.frame = 0 def receive_image_from_camera_callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") self.frame = cv_image except CvBridgeError, e: print e # cv.ShowImage("Image window", cv_image) # cv.WaitKey(3) print time.time()
class SkeletonSketcher(): def __init__(self, image_topic, skeletons_topic): self.got_skeletons = False self.skeleton_array = SkeletonArray() self.bridge = CvBridge() rospy.Subscriber(skeletons_topic, SkeletonArray, self.skeletons_callback) rospy.Subscriber(image_topic, Image, self.image_callback) def skeletons_callback(self, skeleton_array): self.skeleton_array = skeleton_array self.got_skeletons = True rospy.loginfo('Got some skeletons!') def image_callback(self, image): """ Draws joints on image as dots. """ skeleton_array = self.skeleton_array # shorthand if self.got_skeletons: image_cv = self.bridge.imgmsg_to_cv(image) for s in range(len(skeleton_array.skeletons)): for j in range(len(skeleton_array.skeletons[s].joints)): u = int(skeleton_array.skeletons[s].joints[j].uv[0]) v = int(skeleton_array.skeletons[s].joints[j].uv[1]) x = skeleton_array.skeletons[s].joints[j].xyz[0] y = skeleton_array.skeletons[s].joints[j].xyz[1] z = skeleton_array.skeletons[s].joints[j].xyz[2] cv.Circle(image_cv, (u, v), 4, (0, 0, 255), -1) print x, y, z, '-', u, v print '' cv.ShowImage('Image with Skeletons', image_cv) cv.WaitKey(5)
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 image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_topic_2",Image) cv.NamedWindow("Image window", 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/camera/image_raw",Image,self.callback) def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e (cols,rows) = cv.GetSize(cv_image) if cols > 60 and rows > 60 : cv.Circle(cv_image, (50,50), 10, 255) cv.ShowImage("Image window", cv_image) cv.WaitKey(3) try: self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8")) except CvBridgeError, e: print e
class DetectObjects(): def __init__(self): self.bridge = CvBridge() self.fix_count = 0 self.angle = None self.object_type = None # self.detector = cv2.FastFeatureDetector(16, True) #cv2.SURF() # init camera rospy.Subscriber("/camera/rgb/image_rect_color", Image, self.cb_image) self.img_pub = rospy.Publisher("/vision/image", Image) self.pose_pub = rospy.Publisher("/vision/detected_object", PoseStampedLabeled) def cb_image(self, msg): try: frame = self.bridge.imgmsg_to_cv(msg, "bgr8") frame = np.array(frame, dtype=np.uint8) self.process_image(frame) key = cv.WaitKey(5) #print key if key == ord('r'): # restart rospy.sleep(3) if key >= 48 and key <= 57: self.object_type = OBJECTS[key - 48] else: self.object_type = None except CvBridgeError, e: print e
class imageAnalyzer(object): def __init__(self): self.bridge = CvBridge() # Estimated circle center point pulisher self.circleCenterPublisher = rospy.Publisher("est_ccPos",Pose2D) def callback(self,data): try: img = self.bridge.imgmsg_to_cv(data, "mono8") except CvBridgeError, e: print e moments = cv.Moments(img) data = Pose2D() m00 = cv.GetSpatialMoment(moments, 0, 0) data.x = cv.GetSpatialMoment(moments, 1, 0) / m00 data.y = cv.GetSpatialMoment(moments, 0, 1) / m00 data.theta = 0.0 self.circleCenterPublisher.publish(data) print data.x / 500 print data.y / 500
class SnapshotFilter: ## The constructor # @param self The object pointer def __init__(self): input_topic = rospy.get_param("~input","defaultFilterInput") output_topic = rospy.get_param("~output","defaultFilterOutput") self.get_extended_params() self.name = rospy.get_name() self.bridge = CvBridge() self.input_topic = input_topic self.output_topic = output_topic self.output_pub = rospy.Publisher(self.output_topic,Snapshot) self.input_sub = rospy.Subscriber(self.input_topic,Snapshot,self.handle_input) def get_extended_params(self): #Do nothing return #Takes a snapshot as input, returns a filtered snapshot as output def handle_input(self,snapshot): try: input_cv_image = self.bridge.imgmsg_to_cv(snapshot.image, "bgr8") copy_input_cv_image = cv.CreateImage((input_cv_image.width,input_cv_image.height),8,3) cv.Copy(input_cv_image,copy_input_cv_image) except CvBridgeError, e: print e output_cv_image = self.image_filter(cv_image=input_cv_image,info=snapshot.info,copy=copy_input_cv_image) output_info = snapshot.info try: output_image = self.bridge.cv_to_imgmsg(output_cv_image, "bgr8") except CvBridgeError, e: print e
class Image_Converter: def __init__(self, camera=None): if camera is None: camera = '' self.image_source = camera + "/image_raw" self.pub_name = camera + "/image_float" self.bridge = CvBridge() #self.np_image_pub = rospy.Publisher(self.pub_name, numpy_msg(Uint16s)) self.float_image_pub = rospy.Publisher(self.pub_name, FloatArray) rospy.init_node('image_converter', anonymous=True) self.image_sub = rospy.Subscriber(self.image_source,Image,self.image_callback) def image_callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv(data, "mono8") np_image = numpy.asarray(cv_image, dtype=numpy.float32) msg = FloatArray() msg.shape = [cv_image.height, cv_image.width] msg.data = [i for i in np_image.reshape(cv_image.height*cv_image.width, 1)] self.float_image_pub.publish(msg) except CvBridgeError, e: print e
class StitchCapture(CaptureInterface): """ Bag capture is not thread-safe. """ def start(self, pano_id, goal): """ set up a new pano. """ self.bridge = CvBridge() self._bag_capture = BagCapture() self._bag_capture.start(pano_id, goal) self.img_pub = rospy.Publisher('~stitch',Image) def __call__(self, image, camera_info): if self._bag_capture.n_captures < 1: self.setupCamera(camera_info) self.setupStitch() try: cv_image = self.bridge.imgmsg_to_cv(image, "rgb8") pano_image = pcv.convertCvMat2Mat(cv_image) self.stitcher.addNewImage(pano_image) except CvBridgeError, e: print e #capture to bag for back up and later stitching self._bag_capture(image,camera_info)
class image_shower: def __init__(self,argv,show): self.show = show self.image_sub_topic = "/prosilica/image_color"; self.points_sub_topic = "/camera/depth_registered/points"; cv.NamedWindow(window_name, 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.image_sub_topic,Image,self.image_callback) self.image = None self.interval = rospy.Duration(3) self.last_image_time = None def image_callback(self,data): #print 'img' if self.last_image_time is None or (data.header.stamp - self.last_image_time > self.interval): stamp = data.header.stamp.to_time() self.image = data self.last_image_time = data.header.stamp print "got image at %s with size (%d,%d)" % (time.strftime("%Y_%m_%d_T%H_%M_%S",time.localtime(stamp)),self.image.width,self.image.height) self.image_pub.publish(self.image) if self.show: try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") cv.ShowImage(window_name, cv_image) cv.WaitKey(3) except CvBridgeError, e: print e
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 process_image(self, msg): print 'pi' bridge = CvBridge() # Use cv_bridge() to convert the ROS image to OpenCV format try: # The depth image is a single-channel float32 image depth_image = bridge.imgmsg_to_cv(msg, "32FC1") floor_distance = 0.25 mid_row_idx = depth_image.height//2 d = 4209183091839 idx = 0 for x in xrange(depth_image.width): for y in xrange(mid_row_idx-2, mid_row_idx + 2 ): tmp_depth = depth_image[y, x] if tmp_depth != 'nan' and tmp_depth < d: d = tmp_depth idx = x # print depth_image.width, depth_image.height, depth_image[depth_image.height//2, idx] theta = self.angle_calculation(depth_image.width, idx) # print theta self.twist = self.evaluate_twist(theta, d) except CvBridgeError, e: print e
def __init__(self, hmi_callback, im_server, image): self.hmi_callback = hmi_callback self.im_server = im_server # convert goal image to opencv self.frame_id = image.header.frame_id cv_bridge = CvBridge() self.image = cv_bridge.imgmsg_to_cv(image) rospy.loginfo("created BoundingBox with frame %s" % image.header.frame_id) # pixels-per-m in published markers self.ppm = rospy.get_param("~ppm", 100) # starting coordinates of ROI; need to be floats! # (This is arbitrary - just needs to be on top of the image) self.x1 = 0.25*self.image.cols/self.ppm self.x2 = 0.75*self.image.cols/self.ppm self.y1 = 0.25*self.image.rows/self.ppm self.y2 = 0.75*self.image.rows/self.ppm # used to store the result once it's been computed self.result = None self.add_image() self.add_bounding_box()
def convert(filename): ''' Creates directory (if there are images to be saved) for filename and save any images that it finds in this bag file from the spec'd topic. Args: filename (str): Full system path of bag file. ''' save_dir = filename.split('.bag')[-2] + '-images/' # print 'bag file: ', filename # print 'saving images in:', save_dir # Use a CvBridge to convert ROS images to OpenCV images so they can # be saved. bridge = CvBridge() with rosbag.Bag(filename, 'r') as bag: idx = 1 for topic, msg, t in bag.read_messages(): if topic == "/head_mount_kinect/rgb/image_color": # Wait to create image dir until we know we have one. if not os.path.exists(save_dir): print 'creating dir: ', save_dir os.makedirs(save_dir) image_name = save_dir + str(idx) + ".jpg" if not os.path.exists(image_name): print 'saving: ', image_name cv_image = bridge.imgmsg_to_cv(msg, "bgr8") cv.SaveImage(image_name, cv_image) else: pass # print 'skipping: ', image_name idx += 1
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)
def imgproc(d): bridge = CvBridge() cvimg = bridge.imgmsg_to_cv(d,"bgr8") npimg = numpy.asarray(cvimg) cv2.imshow("Output",npimg) cv2.waitKey(1)
class data_saver: def __init__(self,argv,show): self.show = show self.image_sub_topic = "/prosilica/image_color"; self.points_sub_topic = "/camera/depth_registered/points"; self.image_pub = rospy.Publisher("/google_goggles/input_image",Image) self.points_pub = rospy.Publisher("/google_goggles/input_points",PointCloud2) if self.show: cv.NamedWindow(window_name, 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber(self.image_sub_topic,Image,self.image_callback) self.points_sub = rospy.Subscriber(self.points_sub_topic,PointCloud2,self.points_callback) self.image = None self.points = None self.interval = rospy.Duration(3) self.last_image_time = None self.last_points_time = None self.name = "data" self.folder = "../data/bags" if len(argv) >= 1: self.name = argv[0] if len(argv) >= 2: self.interval = rospy.Duration(int(argv[1])) self.filename = self.folder + "/" + self.name + "_" + time.strftime("%Y_%m_%d_T%H_%M_%S",time.localtime(rospy.Time.now().to_time())) + ".bag"; #self.bag = rosbag.Bag(self.filename, 'w') #print "Saving to %s" % self.filename print "prefix: %s, interval: %d" % (self.name,self.interval.to_sec()) def image_callback(self,data): #print 'img' if self.last_image_time is None or (data.header.stamp - self.last_image_time > self.interval): stamp = data.header.stamp.to_time() self.image = data self.last_image_time = data.header.stamp print "got image at %s with size (%d,%d)" % (time.strftime("%Y_%m_%d_T%H_%M_%S",time.localtime(stamp)),self.image.width,self.image.height) self.image_pub.publish(self.image) if self.show: try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") cv.ShowImage(window_name, cv_image) cv.WaitKey(3) except CvBridgeError, e: print e
class TowerDetectViewerServer: # name of tower TOWER_LOWEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_LOWEST TOWER_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_MIDDLE TOWER_HIGHEST = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.TOWER_HIGHEST PLATE_SMALL = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_SMALL PLATE_MIDDLE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_MIDDLE PLATE_LARGE = jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.PLATE_LARGE PLATE_HEIGHT_LOWEST = 0 PLATE_HEIGHT_MIDDLE = 1 PLATE_HEIGHT_HIGHEST = 2 ROBOT0_BASE_FRAME_ID = "/R1/L0" ROBOT1_BASE_FRAME_ID = "/R2/L0" def __init__(self): # initialize the position of the tower self.tower_position = { self.TOWER_LOWEST: Point(), self.TOWER_MIDDLE: Point(), self.TOWER_HIGHEST: Point() } self.radius = rospy.get_param("radius", 0.075) self.circle0 = Drawer3DCircle("/image_marker", 1, "/cluster00", self.radius, [1, 0, 0]) self.circle1 = Drawer3DCircle("/image_marker", 2, "/cluster01", self.radius, [0, 1, 0]) self.circle2 = Drawer3DCircle("/image_marker", 3, "/cluster02", self.radius, [0, 0, 1]) self.circles = [self.circle0, self.circle1, self.circle2] # bgr self.color_indices = [[0, 0, 255], [0, 255, 0], [255, 0, 0]] self.cluster_num = -1 self.circle0.advertise() self.circle1.advertise() self.circle2.advertise() self.bridge = CvBridge() self.state = State("/browser/state") self.tf_listener = tf.TransformListener() self.browser_click_sub = rospy.Subscriber("/browser/click", Point, self.clickCB) self.browser_message_pub = rospy.Publisher("/browser/message", String) self.image_sub = rospy.Subscriber("/image_marked", Image, self.imageCB) self.cluster_num_sub = rospy.Subscriber( "/pcl_nodelet/clustering/cluster_num", Int32Stamped, self.clusterNumCB) self.check_circle_srv = rospy.Service("/browser/check_circle", CheckCircle, self.checkCircleCB) self.pickup_srv = rospy.Service("/browser/pickup", TowerPickUp, self.pickupCB) self.state.updateState(State.INITIAL) # waiting for ik server if rospy.get_param("~wait_robot_move_command", False): rospy.loginfo("waiting for robot server") rospy.wait_for_service("/browser/tower_robot_move_command") self.robot_command = rospy.ServiceProxy( "/browser/tower_robot_move_command", TowerRobotMoveCommand) rospy.loginfo("connected to robot_move server") # initialize the position of the towers from TL self.updateTowerPosition(self.TOWER_LOWEST) self.updateTowerPosition(self.TOWER_MIDDLE) self.updateTowerPosition(self.TOWER_HIGHEST) self.S_TOWER = self.TOWER_MIDDLE self.G_TOWER = None self.I_TOWER = None def towerNameToFrameId(self, tower_name): if tower_name == self.TOWER_LOWEST: return "/cluster02" elif tower_name == self.TOWER_MIDDLE: return "/cluster01" elif tower_name == self.TOWER_HIGHEST: return "/cluster00" else: raise Exception("unknown tower: %d" % (tower_name)) def resolveTowerName(self, tower_id): if tower_id == self.TOWER_LOWEST: return "TOWER_LOWEST" elif tower_id == self.TOWER_MIDDLE: return "TOWER_MIDDLE" elif tower_id == self.TOWER_HIGHEST: return "TOWER_HIGHEST" else: raise Exception("unknown tower: %d" % (tower_id)) def resolvePlateName(self, plate_id): if plate_id == self.PLATE_SMALL: return "PLATE_SMALL" elif plate_id == self.PLATE_MIDDLE: return "PLATE_MIDDLE" elif plate_id == self.PLATE_LARGE: return "PLATE_LARGE" else: raise Exception("unknown plate id: %d" % (plate_id)) def resolvePlateHeightOffset(self, height_id): """ return the offset of z-axis of `height_id' """ return 0.0 def resolvePlateHeight(self, height_id): if height_id == self.PLATE_HEIGHT_LOWEST: return "lowest" elif height_id == self.PLATE_HEIGHT_MIDDLE: return "middle" elif height_id == self.PLATE_HEIGHT_HIGHEST: return "highest" else: raise Exception("unknown plate height: %d" % (height_id)) def robotBaseFrameId(self, index): #index is 0 or 1 if index == 0: return self.ROBOT0_BASE_FRAME_ID elif index == 1: return self.ROBOT1_BASE_FRAME_ID else: raise Exception("unknown index: %d" % (index)) def updateTowerPosition(self, tower_name): frame_id = self.towerNameToFrameId(tower_name) rospy.loginfo("resolving %s" % (frame_id)) try: (trans, rot) = self.tf_listener.lookupTransform("/origin", frame_id, rospy.Time(0)) rospy.loginfo("%s => %s: (%f, %f, %f)" % ("/origin", frame_id, trans[0], trans[1], trans[2])) self.tower_position[tower_name].x = trans[0] self.tower_position[tower_name].y = trans[1] self.tower_position[tower_name].z = trans[2] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): rospy.logerr("failed to lookup transform: %s => %s" % ("/origin", frame_id)) def clusterNumCB(self, msg): self.cluster_num = msg.data def moveRobot(self, plate, from_tower, to_tower, from_height, to_height): robot_index = 0 #use the 1st robot first robot_frame_id = self.robotBaseFrameId(robot_index) rospy.loginfo( "moving: %s from %s(%s) to %s(%s)" % (self.resolvePlateName(plate), self.resolveTowerName(from_tower), self.resolvePlateHeight(from_height), self.resolveTowerName(to_tower), self.resolvePlateHeight(to_height))) from_target_position = self.tower_position[from_tower] to_target_position = self.tower_position[to_tower] self.robot_command( jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.ROBOT1, plate, from_tower, to_tower, jsk_recognition_msgs.srv.TowerRobotMoveCommandRequest.OPTION_NONE) # self.robot_server1(Header(), from_target_position, 0) # self.robot_server1(Header(), to_target_position, 1) def runMain(self): # first of all, resolve tf and store the position of the tower # but, we don't need to update `S' tower's position. # update the tf value self.updateTowerPosition(self.I_TOWER) self.updateTowerPosition(self.G_TOWER) self.state.updateState(State.MOVE_LARGE_S_G) self.moveRobot(self.PLATE_LARGE, self.S_TOWER, self.G_TOWER, self.PLATE_HEIGHT_HIGHEST, self.PLATE_HEIGHT_LOWEST) self.state.updateState(State.MOVE_MIDDLE_S_I) self.moveRobot(self.PLATE_MIDDLE, self.S_TOWER, self.I_TOWER, self.PLATE_HEIGHT_MIDDLE, self.PLATE_HEIGHT_LOWEST) self.state.updateState(State.MOVE_LARGE_G_I) self.moveRobot(self.PLATE_LARGE, self.G_TOWER, self.I_TOWER, self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_MIDDLE) self.state.updateState(State.MOVE_SMALL_S_G) self.moveRobot(self.PLATE_SMALL, self.S_TOWER, self.G_TOWER, self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_LOWEST) self.state.updateState(State.MOVE_LARGE_I_S) self.moveRobot(self.PLATE_LARGE, self.I_TOWER, self.S_TOWER, self.PLATE_HEIGHT_MIDDLE, self.PLATE_HEIGHT_LOWEST) self.state.updateState(State.MOVE_MIDDLE_I_G) self.moveRobot(self.PLATE_MIDDLE, self.I_TOWER, self.G_TOWER, self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_MIDDLE) self.state.updateState(State.MOVE_LARGE_S_G) self.moveRobot(self.PLATE_LARGE, self.S_TOWER, self.G_TOWER, self.PLATE_HEIGHT_LOWEST, self.PLATE_HEIGHT_HIGHEST) def pickupCB(self, req): target_index = req.tower_index # first of all, resolveing S, I and G name binding # S is the START tower # I is the INTERMEDIATE tower # G is the GOAL tower self.G_TOWER = req.tower_index # lookup I self.I_TOWER = ( set([self.TOWER_LOWEST, self.TOWER_MIDDLE, self.TOWER_HIGHEST]) - set([self.G_TOWER, self.S_TOWER])).pop() # update the position of the tower self.state.updateState(State.MOVE_LARGE_S_G) self.state.publish() self.runMain() self.state.updateState(State.INITIAL) # update S self.S_TOWER = self.G_TOWER return TowerPickUpResponse() def checkCircleCB(self, req): (width, height) = cv.GetSize(self.cv_image) x = int(width * req.point.x) y = int(height * req.point.y) click_index = -1 if self.checkColor(self.cv_image[y, x], self.color_indices[0]): click_index = self.TOWER_HIGHEST elif self.checkColor(self.cv_image[y, x], self.color_indices[1]): click_index = self.TOWER_MIDDLE elif self.checkColor(self.cv_image[y, x], self.color_indices[2]): click_index = self.TOWER_LOWEST if click_index == self.S_TOWER: msg = "the tower the user clicked equals to the start tower" rospy.logerr(msg) return CheckCircleResponse(False, click_index, msg) else: return CheckCircleResponse(click_index != -1, click_index, "") def checkColor(self, image_color, array_color): return (image_color[0] == array_color[0] and image_color[1] == array_color[1] and image_color[2] == array_color[2]) def clickCB(self, msg): (width, height) = cv.GetSize(self.cv_image) # msg.x and msg.y is on a relative coordinate (u, v) x = int(width * msg.x) y = int(height * msg.y) output_str = str([x, y]) + " - " + str(self.cv_image[y, x]) click_index = -1 if self.checkColor(self.cv_image[y, x], self.color_indices[0]): output_str = output_str + " cluster00 clicked" click_index = self.TOWER_HIGHEST elif self.checkColor(self.cv_image[y, x], self.color_indices[1]): output_str = output_str + " cluster01 clicked" click_index = self.TOWER_MIDDLE elif self.checkColor(self.cv_image[y, x], self.color_indices[2]): output_str = output_str + " cluster02 clicked" click_index = self.TOWER_LOWEST self.browser_message_pub.publish(String(output_str)) def imageCB(self, data): try: self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e
class CalibrateRobotCamera(metaclass.AutoReloader): class CalibrationError(Exception): def __init__(self, value, action=None): self.value = value self.action = action def __str__(self): return repr(self.value) + '; action=' + str(self.action) def __init__(self, image_raw='image_raw', pattern=None, KK=None, kc=None, timeout=4.0): self.image_raw = image_raw self.bridge = CvBridge() self.pattern = pattern px = self.pattern['corners_x'] * self.pattern['spacing_x'] py = self.pattern['corners_y'] * self.pattern['spacing_y'] self.pattern['type'] = """<KinBody name="calibration"> <Body name="calibration"> <Geom type="box"> <extents>%f %f 0.001</extents> <translation>%f %f 0</translation> <diffusecolor>0 0.5 0</diffusecolor> </Geom> <Geom type="box"> <extents>%f %f 0.002</extents> <translation>%f %f 0</translation> <diffusecolor>0 1 0</diffusecolor> </Geom> </Body> </KinBody> """ % (px * 0.5 + 2.0 * self.pattern['spacing_x'], py * 0.5 + 2.0 * self.pattern['spacing_y'], px * 0.5, py * 0.5, px * 0.5, py * 0.5, px * 0.5, py * 0.5) self.obstaclexml = """<KinBody name="obstacle"> <Body name="obstacle"> <Geom type="box"> <extents>%f %f 0.001</extents> <translation>0 0 0.001</translation> <diffusecolor>1 0 0</diffusecolor> </Geom> </Body> </KinBody>""" % (px + 0.1, py + 0.1) self.timeout = timeout self.cvKK = None if KK is None else cv.fromarray(KK) self.cvkc = None if kc is None else cv.fromarray(kc) def detect(self, cvimage): corners_x = self.pattern['corners_x'] corners_y = self.pattern['corners_y'] found, corners = cv.FindChessboardCorners( cvimage, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = (corners[0], corners[corners_x - 1], corners[(corners_y - 1) * corners_x], corners[len(corners) - 1]) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = array( cv.FindCornerSubPix( cvimage, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.01)), float64) return corners else: return None def getObjectPoints(self): object_points = zeros( (self.pattern['corners_x'] * self.pattern['corners_y'], 3), float64) X, Y = meshgrid( arange(self.pattern['corners_x']) * self.pattern['spacing_x'], arange(self.pattern['corners_y']) * self.pattern['spacing_y']) object_points[:, 0] = X.flatten() object_points[:, 1] = Y.flatten() return object_points def calibrateIntrinsicCamera(self, all_object_points, all_corners, imagesize, usenonlinoptim=True, fixprincipalpoint=False, computegradients=False): pointCounts = vstack( [len(object_points) for object_points in all_object_points]) cvKK = cv.CreateMat(3, 3, cv.CV_64F) cvkc = cv.CreateMat(5, 1, cv.CV_64F) cvrvecs = cv.CreateMat(len(pointCounts), 3, cv.CV_64F) cvtvecs = cv.CreateMat(len(pointCounts), 3, cv.CV_64F) flags = cv.CV_CALIB_FIX_PRINCIPAL_POINT if fixprincipalpoint else 0 cv.CalibrateCamera2(cv.fromarray(vstack(all_object_points)), cv.fromarray(vstack(all_corners)), cv.fromarray(pointCounts), (1024, 768), cvKK, cvkc, cvrvecs, cvtvecs, flags) rvecs = array(cvrvecs) tvecs = array(cvtvecs) KK = array(cvKK) kc = array(cvkc) Ts = [] for i in range(len(pointCounts)): T = matrixFromAxisAngle(rvecs[i]) T[0:3, 3] = tvecs[i] Ts.append(T) error = None if usenonlinoptim: # for some reason, the opencv solver is not doing as good of a job as it can... (it also uses levmarq) x0 = r_[KK[0, 0], KK[1, 1]] if not fixprincipalpoint: x0 = r_[x0, KK[0, 2], KK[1, 2]] x0 = r_[x0, kc[:, 0]] for i in range(len(pointCounts)): x0 = r_[x0, rvecs[i], tvecs[i]] N = pointCounts[0] cv_image_points = cv.CreateMat(N, 2, cv.CV_64F) cv_object_points = [cv.fromarray(x) for x in all_object_points] def errorfn(x): xoff = 2 cvKK[0, 0] = x[0] cvKK[1, 1] = x[1] if not fixprincipalpoint: cvKK[0, 2] = x[2] cvKK[1, 2] = x[3] xoff += 2 for i in range(5): cvkc[i, 0] = x[xoff + i] xoff += 5 e = zeros(len(all_object_points) * N * 2) off = 0 for i in range(len(all_object_points)): for j in range(3): cvrvecs[0, j] = x[xoff + 6 * i + j] cvtvecs[0, j] = x[xoff + 6 * i + 3 + j] cv.ProjectPoints2(cv_object_points[i], cvrvecs[0], cvtvecs[0], cvKK, cvkc, cv_image_points) image_points = array(cv_image_points) e[off:(off + len(image_points) )] = all_corners[i][:, 0] - image_points[:, 0] off += len(image_points) e[off:(off + len(image_points) )] = all_corners[i][:, 1] - image_points[:, 1] off += len(image_points) #print 'rms: ',sqrt(sum(e**2)) return e x, success = leastsq(errorfn, x0, maxfev=100000, epsfcn=1e-7) if not success: raise CalibrationError('failed to converge to answer') e = errorfn(x) abse = sqrt(sum(e**2)) e2 = reshape(e, [len(all_object_points), 2 * N])**2 error = mean(sqrt(e2[:, 0:N] + e2[:, N:]), 1) KK[0, 0] = x[0] KK[1, 1] = x[1] xoff = 2 if not fixprincipalpoint: KK[0, 2] = x[2] KK[1, 2] = x[3] xoff += 2 for i in range(5): kc[i, 0] = x[xoff + i] if computegradients: deltas = r_[0.01 * ones(2 if fixprincipalpoint else 4), 0.0001 * ones(5)] grad = [] normalization = 1.0 / (len(all_object_points) * N * 2) for i, delta in enumerate(deltas): x[i] += delta e_p = errorfn(x) abse_p = sqrt(sum(e_p**2)) x[i] -= 2 * delta e_n = errorfn(x) abse_n = sqrt(sum(e_n**2)) x[i] += delta grad.append( normalization * (abse_p + abse_n - 2 * abse) / (delta** 2)) #sum((e_p-e)**2) + sum((e-e_n)**2))/(2.0*delta)) return KK, kc, Ts, error, array(grad) return KK, kc, Ts, error def validateCalibrationData(self, all_object_points, all_corners): """Builds up linear equations using method of Zhang 1999 "Flexible Camera Calibration By Viewing a Plane From Unknown Orientations". The condition number of the linear equations can tell us how good the data is. The object data all needs to lie on the Z=0 plane (actually any plane would probably do?) """ cvH = cv.fromarray(eye(3)) B = zeros((6, 1)) B[1, 0] = 1 # skewness is 0 constraint # for every observation, solve for the homography and build up a matrix to solve for the intrinsicparameters for corners, object_points in izip(all_corners, all_object_points): if not all(object_points[:, 2] == 0): raise ValueError( 'The object data all needs to lie on the Z=0 plane.') cv.FindHomography(cv.fromarray(object_points[:, 0:2]), cv.fromarray(corners), cvH, 0) H = array(cvH) v = [] for i, j in [(0, 1), (0, 0), (1, 1)]: v.append([ H[0, i] * H[0, j], H[0, i] * H[1, j] + H[1, i] * H[0, j], H[1, i] * H[1, j], H[2, i] * H[0, j] + H[0, i] * H[2, j], H[2, i] * H[1, j] + H[1, i] * H[2, j], H[2, i] * H[2, j] ]) B = c_[B, array(v[0]), array(v[1]) - array(v[2])] U, s, Vh = linalg.svd(dot(B, transpose(B))) b = U[:, -1] A = array([[b[0], b[1], b[3]], [b[1], b[2], b[4]], [b[3], b[4], b[5]]]) # A has to be positive definite Aeigs = linalg.eigvals(A) if not all(sign(Aeigs) > 0) and not all(sign(Aeigs) < 0): return None #print 'eigenvalues: ',sqrt(s) print 'condition is: ', sqrt(s[-2] / s[-1]) if False: # compute the intrinsic parameters K = [alpha c u0; 0 beta v0; 0 0 1] b = U[:, -1] v0 = (b[1] * b[3] - b[0] * b[4]) / (b[0] * b[2] - b[1] * b[1]) lm = b[5] - (b[3] * b[3] + v0 * (b[1] * b[3] - b[0] * b[4])) / b[0] alpha = sqrt(lm / b[0]) beta = sqrt(lm * b[0] / (b[0] * b[2] - b[1] * b[1])) c = -b[1] * alpha * alpha * beta / lm u0 = c * v0 / alpha - b[3] * alpha * alpha / lm print alpha, beta, u0, v0 return sqrt(s) / len(all_object_points) def waitForRobot(self, robot): # wait for robot to stop moving lastvalues = robot.GetJointValues() print 'waiting for robot to stop...' while True: time.sleep(0.5) # sleep some time for better values difference newvalues = robot.GetJointValues() if sqrt(sum((lastvalues - newvalues)**2)) < 0.001: break lastvalues = newvalues time.sleep(0.5) # stabilize camera images? def waitForPattern(self, timeout=None, waitforkey=False, robot=None): # wait for robot to stop moving if robot: self.waitForRobot(robot) if timeout is None: timeout = self.timeout timestart = rospy.get_rostime() donecond = threading.Condition() data = [None] def detcb(imagemsg): if imagemsg.header.stamp > timestart: cvimage = self.bridge.imgmsg_to_cv(imagemsg, 'mono8') corners = self.detect(cvimage) if corners is not None: with donecond: # get the pose with respect to the attached sensor tf frame if self.cvKK is None or self.cvkc is None: KK, kc, Ts, error = self.calibrateIntrinsicCamera( [self.getObjectPoints()], [corners], (cvimage.width, cvimage.height), usenonlinoptim=False) T = Ts[0] else: cvrvec = cv.CreateMat(1, 3, cv.CV_64F) cvtvec = cv.CreateMat(1, 3, cv.CV_64F) cv.FindExtrinsicCameraParams2( cv.fromarray(self.getObjectPoints()), cv.fromarray(corners), self.cvKK, self.cvkc, cvrvec, cvtvec) T = matrixFromAxisAngle(array(cvrvec)[0]) T[0:3, 3] = array(cvtvec)[0] data[0] = { 'T': T, 'corners_raw': corners, 'image_raw': array(cvimage) } if 'type' in self.pattern: data[0]['type'] = self.pattern['type'] donecond.notifyAll() image_sub = rospy.Subscriber(self.image_raw, sensor_msgs.msg.Image, detcb, queue_size=4) try: with donecond: donecond.wait(timeout) if data[0] is not None: print 'found pattern' finally: image_sub.unregister() if waitforkey: cmd = raw_input('press any key to continue (q-quit)... ') if cmd == 'q': raise KeyboardInterrupt() return data[0] def waitForObjectDetectionMessage(self, timeout=None, waitforkey=False, robot=None): # wait for robot to stop moving if robot: self.waitForRobot(robot) if timeout is None: timeout = self.timeout timestart = rospy.get_rostime() donecond = threading.Condition() data = [None] def detcb(imagemsg, detmsg): if len(detmsg.objects) > 0 and detmsg.header.stamp > timestart: with donecond: # get the pose with respect to the attached sensor tf frame q = detmsg.objects[0].pose.orientation t = detmsg.objects[0].pose.position cvimage = self.bridge.imgmsg_to_cv(imagemsg) data[0] = { 'T': matrixFromPose([q.w, q.x, q.y, q.z, t.x, t.y, t.z]), 'type': detmsg.objects[0].type, 'image': array(cvimage) } donecond.notifyAll() image_sub = message_filters.Subscriber(self.image_raw, sensor_msgs.msg.Image) det_sub = message_filters.Subscriber( self.objectdetection, posedetection_msgs.msg.ObjectDetection) ts = message_filters.TimeSynchronizer([image_sub, det_sub], 10) ts.registerCallback(detcb) try: with donecond: donecond.wait(timeout) finally: del ts # explicitly delete if waitforkey: cmd = raw_input('press any key to continue (q-quit)... ') if cmd == 'q': raise KeyboardInterrupt() return data[0] @staticmethod def moveRobot(robot, values): basemanip = BaseManipulation(robot) with robot: robot.SetActiveDOFs(arange(robot.GetDOF())) basemanip.MoveActiveJoints(values) while not robot.GetController().IsDone(): time.sleep(0.01) def gatherCalibrationData(self, robot, sensorname, waitforkey=False, forcecalibintrinsic=False, calibextrinsic=True, maxconedist=0.1, maxconeangle=0.6, sensorrobot=None, **kwargs): waitcond = lambda: self.waitForPattern(robot=robot, waitforkey=waitforkey) origvalues = None env = robot.GetEnv() obstacle = None observations = [] try: if forcecalibintrinsic or (self.cvKK is None or self.cvkc is None): origvalues = robot.GetJointValues() averagedist = 0.03 angledelta = 0.3 while True: observations += examples.calibrationviews.CalibrationViews( robot=robot, sensorrobot=sensorrobot, sensorname=sensorname).computeAndMoveToObservations( waitcond=waitcond, usevisibility=False, angledelta=angledelta, maxconedist=maxconedist, maxconeangle=maxconeangle, averagedist=averagedist, **kwargs) pickle.dump(observations, open('observations_initial.pp', 'w')) try: KKorig, kcorig, Tcamera, info = self.calibrateFromObservations( observations, False, fixprincipalpoint=True) break except self.CalibrationError, e: print 'cannot compute stable KK, attempting more views', e self.moveRobot(robot, origvalues) averagedist = 0.02 angledelta = 0.2 print 'num observations is %d: ' % len( observations), KKorig, kcorig self.cvKK = cv.fromarray(KKorig) self.cvkc = cv.fromarray(kcorig) if not calibextrinsic: return KKorig, kcorig, None, {'observations': observations} if origvalues is not None: print 'moving robot back to original values' self.moveRobot(robot, origvalues) # add an obstacle around the pattern if self.obstaclexml is not None: data = waitcond() obstacle = env.ReadKinBodyXMLData(self.obstaclexml) env.AddKinBody(obstacle, True) obstacle.SetTransform( dot(robot.GetSensor(sensorname).GetTransform(), data['T'])) newobservations, target = examples.calibrationviews.CalibrationViews.gatherCalibrationData( robot=robot, sensorrobot=sensorrobot, sensorname=sensorname, waitcond=waitcond, **kwargs) observations += newobservations KKorig, kcorig, Tcamera, info = self.calibrateFromObservations( observations, Tcameraestimate=array( robot.GetSensor(sensorname).GetRelativeTransform(), float64)) info['observations'] = observations return KKorig, kcorig, Tcamera, info finally:
class DetectRV20(object): def __init__(self): self.bridge = CvBridge() self.depth_frame = None rospy.Subscriber("/softkinetic_camera/depth/image_raw", Image, self.cb_depth) while self.depth_frame is None: if rospy.is_shutdown(): return rospy.logwarn('RV20 waiting for first depth frame') rospy.sleep(0.1) rospy.Service("/vision/check_rv20", CheckRV20, self.cb_start_check) def cb_start_check(self, req): res = CheckRV20Response() rate = rospy.Rate(20) R20 = 0 V20 = 0 rospy.sleep(1.0) for i in xrange(NUM_FRAMES): while self.depth_frame is None: if rospy.is_shutdown(): return res rate.sleep() depth_frame = self.bridge.imgmsg_to_cv(self.depth_frame, "32FC1") self.depth_frame = None depth_frame = np.array(depth_frame, dtype=np.float32) tmp_img = np.copy(depth_frame) cv2.rectangle(tmp_img, (crop_x[0], crop_y[0]), (crop_x[1], crop_y[1]), (255, 255, 255), 2) cv2.imshow('full image', tmp_img) depth_frame = depth_frame[crop_y[0]:crop_y[1], crop_x[0]:crop_x[1]] num_circles = self.process_image(depth_frame) if num_circles > 0: R20 += 1 else: V20 += 1 print R20, V20, 'R, V' print 'MIN_SEEN_CIRCLES' + str(MIN_SEEN_CIRCLES) if R20 > MIN_SEEN_CIRCLES: res.result = 'R20' else: res.result = 'V20' return res def cb_depth(self, msg): self.depth_frame = msg def process_image(self, depth_frame): filtered = self.process_depth(depth_frame) circles = cv2.HoughCircles(filtered, cv.CV_HOUGH_GRADIENT, 1, 20, param1=50, param2=30, minRadius=0, maxRadius=0) count = 0 if circles is not None: circles = np.uint16(np.around(circles)) if VISUALIZE: for i in circles[0, :]: # draw the outer circle cv2.circle(depth_frame, (i[0], i[1]), i[2], (0, 255, 0), 2) # draw the center of the circle cv2.circle(depth_frame, (i[0], i[1]), 2, (0, 0, 255), 3) count = len(circles[0, :]) if VISUALIZE: cv2.imshow('depth', depth_frame) cv2.waitKey(5) return count def process_depth(self, depth_img): # # Filter NaN idx = np.isnan(depth_img) depth_img[idx] = 0 # # Convert to UINT8 image #print np.min(depth_img), np.max(depth_img), np.mean(depth_img) depth_img = filter_low_high(depth_img, MIN, MAX) #print np.min(depth_img), np.max(depth_img), np.mean(depth_img) depth_img = depth_img / (MAX) * 255 depth_img = np.uint8(depth_img) depth_img = cv2.medianBlur(depth_img, 9) frame_filter = cv2.adaptiveThreshold( depth_img, 255, cv2.ADAPTIVE_THRESH_GAUSSIAN_C, # cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 11, # neighbourhood 2) cv2.bitwise_not(frame_filter, frame_filter) kernel = np.ones((1, 1), 'uint8') frame_filter = cv2.dilate(frame_filter, kernel) return frame_filter
def detect_and_draw(imgmsg): global skp, sd global im_ref #print 'number of KeyPoint objects skp', len(skp) br = CvBridge() temp = br.imgmsg_to_cv(imgmsg, "bgr8") im_height = temp.height im_length = temp.width cv.ShowImage("view", temp) cv.WaitKey(10) template = np.asarray(temp) tkp = detector.detect(template) tkp, td = descriptor.compute(template, tkp) #print 'number of KeyPoint objects tkp', len(tkp) #print 'number of KeyPoint objects skp', len(skp) flann_params = dict(algorithm=1, trees=4) flann = cv2.flann_Index(sd, flann_params) idx, dist = flann.knnSearch(td, 1, params={}) del flann dist = dist[:, 0] / 2500.0 dist = dist.reshape(-1, ).tolist() idx = idx.reshape(-1).tolist() indices = range(len(dist)) indices.sort(key=lambda i: dist[i]) dist = [dist[i] for i in indices] idx = [idx[i] for i in indices] skp_final = [] for i, dis in itertools.izip(idx, dist): if dis < threshold: skp_final.append(skp[i]) else: break tkp_final = [] for i, dis in itertools.izip(range(len(idx)), dist): if dis < threshold: tkp_final.append(tkp[indices[i]]) else: break h1, w1 = im_ref.shape[:2] h2, w2 = template.shape[:2] nWidth = w1 + w2 nHeight = max(h1, h2) hdif = (h1 - h2) / 2 newimg = np.zeros((nHeight, nWidth, 3), np.uint8) newimg[hdif:hdif + h2, :w2] = template newimg[:h1, w2:w1 + w2] = im_ref tkp_final skp_final #print 'number of KeyPoint objects in skp_final', len(skp_final) #print 'number of KeyPoint objects in tkp_final', len(tkp_final) for i in range(min(len(tkp), len(skp_final))): pt_a = (int(tkp_final[i].pt[0]), int(tkp_final[i].pt[1] + hdif)) pt_b = (int(skp_final[i].pt[0] + w2), int(skp_final[i].pt[1])) cv2.circle(newimg, pt_a, int(tkp_final[i].size), (160, 32, 240), 1) cv2.circle(newimg, pt_b, int(skp_final[i].size), (160, 32, 240), 1) cv2.line(newimg, pt_a, pt_b, (255, 0, 0)) cv.ShowImage("sift", cv.fromarray(newimg)) kp_array = sift_keypoints_array() kp_array.skp = [sift_keypoint(*k.pt) for k in skp_final] kp_array.tkp = [sift_keypoint(*k.pt) for k in tkp_final] pk_pub.publish(kp_array) key = cv.WaitKey(10) & 0xFF if key == ord('d'): im_ref = template skp = tkp sd = td
class ROSProxy(object): def __init__(self): self.mans = {} self.mods = {} self.bridge = CvBridge() self.imgCls = self.msgClassFromTypeString('sensor_msgs/Image') def __GetTopics(self): return [x[0] for x in rospy.get_published_topics()] topics = property(fget=__GetTopics) def __GetServices(self): return rosservice.get_service_list() services = property(fget=__GetServices) def typeStringFromTopic(self, topic): try: return [x[1] for x in rospy.get_published_topics() if x[0] == topic][0] except: return None def typeStringFromService(self, service): try: return rosservice.get_service_type(service) except: return None def __classFromTypeString(self, typeString, subname): basemodule, itype = typeString.split('/') if not (basemodule in self.mans): try: roslib.load_manifest(basemodule) self.mans[basemodule] = True; except: return None modname = basemodule + '.'+ subname + '._' + itype if not (modname in self.mods): try: mod = __import__(modname) self.mods['modname'] = mod except: return None mod = self.mods['modname'] return getattr(getattr(getattr(mod,subname),'_' + itype), itype) def msgClassFromTypeString(self, typeString): return self.__classFromTypeString(typeString, 'msg') def srvClassFromTypeString(self, typeString): return self.__classFromTypeString(typeString, 'srv') def classFromTopic(self, topic): return self.msgClassFromTypeString(self.typeStringFromTopic(topic)) def classFromService(self, service): return self.srvClassFromTypeString(self.typeStringFromService(service)) def callService(self, service, arguments, callback=False, wait=True): def defCallback(x): pass if callback == False: callback = defCallback if wait: try: rospy.wait_for_service(service) except: callback(None) try: function = rospy.ServiceProxy(service, self.classFromService(service)) response = apply(function, arguments) callback(response) except: callback(None) def generalize(self, inst, encoding='jpg', width=160, height=120, qual=30): if isinstance(inst, self.imgCls): cvImg = self.bridge.imgmsg_to_cv(inst,"rgb8") size = cv.GetSize(cvImg) img = Image.fromstring("RGB", size, cvImg.tostring()) if width != size[0] or height != size[1]: img = img.resize((width,height),Image.NEAREST) buf = StringIO.StringIO() if encoding == 'png': img.save(buf, 'PNG', optimize=1) else: img.save(buf, 'JPEG', quality=qual) data = buf.getvalue() buf.close() return {'uri':'data:image/' + encoding + ';base64,' + standard_b64encode(data)} elif hasattr(inst,'__slots__'): obj = {} for field in inst.__slots__: obj[field] = self.generalize(getattr(inst,field), encoding, width, height, qual) return obj elif isinstance(inst,tuple) or isinstance(inst,list): return [self.generalize(x, encoding, width, height, qual) for x in inst] else: return inst braces = re.compile(r'\[[^\]]*\]') atomics = ['bool', 'int8', 'uint8', 'int16', 'uint16', 'int32', 'uint32', 'int64', 'uint64', 'float32', 'float64', 'string'] def specify(self, typeStr, obj): if isinstance(typeStr,list): lst = [] for i in xrange(len(typeStr)): lst.append(self.specify(typeStr[i],obj[i])) return lst elif typeStr != self.braces.sub('',typeStr): return [self.specify(self.braces.sub('',typeStr), x) for x in obj] elif typeStr in self.atomics: if typeStr == 'string': return obj.encode('ascii','ignore') return obj elif typeStr == 'time' or typeStr == 'duration': inst = None if typeStr == 'time': inst = roslib.rostime.Time() else: inst = roslib.rostime.Duration() inst.nsecs = obj['nsecs'] inst.secs = obj['secs'] return inst else: cls = self.msgClassFromTypeString(typeStr) inst = cls() for i in xrange(len(cls._slot_types)): field = cls.__slots__[i] typ = cls._slot_types[i] if field in obj: setattr(inst,field,self.specify(typ,obj[field])) return inst
class image_converter: def __init__(self): self.image_pub = rospy.Publisher('/image_circle',Image) self.win = "Image window" cv.NamedWindow(self.win, 1) # cv.MoveWindow(self.win, 25, 800) # cv.ResizeWindow(self.win, 160, 120) self.bridge = CvBridge() self.image_sub = rospy.Subscriber('/camera/rgb/image_raw',Image,self.callback) feature_name = 'surf' self.detector, self.matcher = init_feature(feature_name) if self.detector != None: print 'using', feature_name else: print 'unknown feature:', feature_name sys.exit(1) self.eyeimg = cv2.imread('eye1.jpg', cv.CV_LOAD_IMAGE_COLOR) self.eye_kp, self.eye_desc = self.detector.detectAndCompute(self.eyeimg, None) def callback(self,data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e # cv2.imshow("eyeimg", self.eyeimg) # cv.ShowImage("cv_image", cv_image) cv_image = np.array(cv_image) # cv2.cvtColor(cv_image, cv.CV_BGR2GRAY) kp, desc = self.detector.detectAndCompute(cv_image, None) print 'eye - %d features, frame - %d features' % (len(self.eye_kp), len(kp)) # def match_and_draw(win): def match_and_draw(): print 'matching...' raw_matches = self.matcher.knnMatch(self.eye_desc, trainDescriptors = desc, k = 2) #2 p1, p2, kp_pairs = filter_matches(self.eye_kp, kp, raw_matches) print 'found %d' % len(p1) if len(p1) >= 4: H, status = cv2.findHomography(p1, p2, cv2.RANSAC, 5.0) print '%d / %d inliers/matched' % (np.sum(status), len(status)) vis = explore_match(self.eyeimg, cv_image, kp_pairs, status, H) new_image = self.convert_image(vis) try: self.image_pub.publish(self.bridge.cv_to_imgmsg(new_image, "bgr8")) except CvBridgeError, e: print e # vis = explore_match(win, self.eyeimg, cv_image, kp_pairs, status, H) # while True: # c = cv.WaitKey(7) % 0x100 # if c == 27: # break # print 'Continue...' # else: # H, status = None, None print '%d matches found, not enough for homography estimation' % len(p1)
class ROS2OpenCV2(object): def __init__(self, node_name): self.node_name = node_name rospy.init_node(node_name) rospy.loginfo("Starting node " + str(node_name)) rospy.on_shutdown(self.cleanup) # A number of parameters to determine what gets displayed on the # screen. These can be overridden the appropriate launch file self.show_text = rospy.get_param("~show_text", True) self.show_features = rospy.get_param("~show_features", True) self.show_boxes = rospy.get_param("~show_boxes", True) self.flip_image = rospy.get_param("~flip_image", False) self.feature_size = rospy.get_param("~feature_size", False) # Initialize the Region of Interest and its publisher self.ROI = RegionOfInterest() self.roi_pub = rospy.Publisher("/roi", RegionOfInterest) # Initialize a number of global variables self.frame = None self.frame_size = None self.frame_width = None self.frame_height = None self.depth_image = None self.marker_image = None self.display_image = None self.grey = None self.prev_grey = None self.selected_point = None self.selection = None self.drag_start = None self.keystroke = None self.detect_box = None self.track_box = None self.display_box = None self.keep_marker_history = False self.night_mode = False self.auto_face_tracking = False self.cps = 0 # Cycles per second = number of processing loops per second. self.cps_values = list() self.cps_n_values = 20 self.busy = False self.resize_window_width = 0 self.resize_window_height = 0 # Create the main display window self.cv_window_name = self.node_name cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL) if self.resize_window_height > 0 and self.resize_window_width > 0: cv.ResizeWindow(self.cv_window_name, self.resize_window_width, self.resize_window_height) else: cv.ResizeWindow(self.cv_window_name, 640, 480) # Create the cv_bridge object self.bridge = CvBridge() # Set a call back on mouse clicks on the image window cv.SetMouseCallback(self.node_name, self.on_mouse_click, None) # Subscribe to the image and depth topics and set the appropriate callbacks # The image topic names can be remapped in the appropriate launch file self.image_sub = rospy.Subscriber("input_rgb_image", Image, self.image_callback) self.depth_sub = rospy.Subscriber("input_depth_image", Image, self.depth_callback) def on_mouse_click(self, event, x, y, flags, param): # This function allows the user to selection a ROI using the mouse if self.frame is None: return if event == cv.CV_EVENT_LBUTTONDOWN and not self.drag_start: self.features = [] self.track_box = None self.detect_box = None self.selected_point = (x, y) self.drag_start = (x, y) if event == cv.CV_EVENT_LBUTTONUP: self.drag_start = None self.classifier_initialized = False self.detect_box = self.selection if self.drag_start: xmin = max(0, min(x, self.drag_start[0])) ymin = max(0, min(y, self.drag_start[1])) xmax = min(self.frame_width, max(x, self.drag_start[0])) ymax = min(self.frame_height, max(y, self.drag_start[1])) self.selection = (xmin, ymin, xmax - xmin, ymax - ymin) def image_callback(self, data): # Store the image header in a global variable self.image_header = data.header # Time this loop to get cycles per second start = time.time() # Convert the ROS image to OpenCV format using a cv_bridge helper function frame = self.convert_image(data) # Some webcams invert the image if self.flip_image: frame = cv2.flip(frame, 0) # Store the frame width and height in a pair of global variables if self.frame_width is None: self.frame_size = (frame.shape[1], frame.shape[0]) self.frame_width, self.frame_height = self.frame_size # Create the marker image we will use for display purposes if self.marker_image is None: self.marker_image = np.zeros_like(frame) # Copy the current frame to the global image in case we need it elsewhere self.frame = frame.copy() # Reset the marker image if we're not displaying the history if not self.keep_marker_history: self.marker_image = np.zeros_like(self.marker_image) # Process the image to detect and track objects or features processed_image = self.process_image(frame) # If the result is a greyscale image, convert to 3-channel for display purposes """ #if processed_image.channels == 1: #cv.CvtColor(processed_image, self.processed_image, cv.CV_GRAY2BGR) #else: # Make a global copy self.processed_image = processed_image.copy() # Display the user-selection rectangle or point self.display_selection() # Night mode: only display the markers if self.night_mode: self.processed_image = np.zeros_like(self.processed_image) # Merge the processed image and the marker image self.display_image = cv2.bitwise_or(self.processed_image, self.marker_image) # If we have a track box, then display it. The track box can be either a regular # cvRect or a rotated Rect. if self.track_box is not None and self.is_rect_nonzero(self.track_box): try: (center, size, angle) = self.track_box pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2)) pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2)) if self.show_boxes: #cv.EllipseBox(cv.fromarray(self.display_image), self.track_box, cv.CV_RGB(50, 255, 50), self.feature_size) cv2.rectangle(self.display_image, pt1, pt2, cv.RGB(50, 255, 50), self.feature_size, 8, 0) except: try: x, y, w, h = self.track_box size = w, h center = x + w / 2, y + h / 2 pt1 = (int(center[0] - size[0] / 2), int(center[1] - size[1] / 2)) pt2 = (int(center[0] + size[0] / 2), int(center[1] + size[1] / 2)) cv2.rectangle(self.display_image, pt1, pt2, cv.RGB(50, 255, 50), self.feature_size, 8, 0) except: pass # Otherwise, if we have a detect box then display it elif self.detect_box is not None and self.is_rect_nonzero( self.detect_box): (pt1_x, pt1_y, w, h) = self.detect_box if self.show_boxes: cv2.rectangle(self.display_image, (pt1_x, pt1_y), (pt1_x + w, pt1_y + h), cv.RGB(50, 255, 50), self.feature_size, 8, 0) # Publish the ROI self.publish_roi() # Handle keyboard events self.keystroke = cv.WaitKey(5) # Compute the time for this loop and estimate CPS as a running average end = time.time() duration = end - start fps = int(1.0 / duration) self.cps_values.append(fps) if len(self.cps_values) > self.cps_n_values: self.cps_values.pop(0) self.cps = int(sum(self.cps_values) / len(self.cps_values)) # Display CPS and image resolution if asked to if self.show_text: font_face = cv2.FONT_HERSHEY_SIMPLEX font_scale = 0.5 """ Print cycles per second (CPS) and resolution (RES) at top of the image """ if self.frame_size[0] >= 640: vstart = 25 voffset = int(50 + self.frame_size[1] / 120.) elif self.frame_size[0] == 320: vstart = 15 voffset = int(35 + self.frame_size[1] / 120.) else: vstart = 10 voffset = int(20 + self.frame_size[1] / 120.) cv2.putText(self.display_image, "CPS: " + str(self.cps), (10, vstart), font_face, font_scale, cv.RGB(255, 255, 0)) cv2.putText( self.display_image, "RES: " + str(self.frame_size[0]) + "X" + str(self.frame_size[1]), (10, voffset), font_face, font_scale, cv.RGB(255, 255, 0)) # Update the image display cv2.imshow(self.node_name, self.display_image) # Process any keyboard commands if 32 <= self.keystroke and self.keystroke < 128: cc = chr(self.keystroke).lower() if cc == 'n': self.night_mode = not self.night_mode elif cc == 'f': self.show_features = not self.show_features elif cc == 'b': self.show_boxes = not self.show_boxes elif cc == 't': self.show_text = not self.show_text elif cc == 'q': # The has press the q key, so exit rospy.signal_shutdown("User hit q key to quit.") def depth_callback(self, data): # Convert the ROS image to OpenCV format using a cv_bridge helper function depth_image = self.convert_depth_image(data) # Some webcams invert the image if self.flip_image: depth_image = cv2.flip(depth_image, 0) # Process the depth image processed_depth_image = self.process_depth_image(depth_image) # Make global copies self.depth_image = depth_image.copy() self.processed_depth_image = processed_depth_image.copy() def convert_image(self, ros_image): # Use cv_bridge() to convert the ROS image to OpenCV format try: cv_image = self.bridge.imgmsg_to_cv(ros_image, "bgr8") return np.array(cv_image, dtype=np.uint8) except CvBridgeError, e: print e
if depth_image.header.stamp - rgb_image_color.header.stamp > rospy.Duration.from_sec(1/30.0): continue frame += 1 if frame % float(args.nth) == 0: if args.skip > 0: args.skip -= 1 else: # store messages outbag.write("/camera/depth_registered/camera_info",depth_camera_info,t) outbag.write("/camera/depth_registered/image",depth_image,t) outbag.write("/camera/rgb/camera_info",rgb_camera_info,t) outbag.write("/camera/rgb/image_color",rgb_image_color,t) # generate monochrome image from color image cv_rgb_image_mono = bridge.imgmsg_to_cv(rgb_image_color, "mono8") rgb_image_mono = bridge.cv_to_imgmsg(cv_rgb_image_mono) rgb_image_mono.header = rgb_image_color.header outbag.write("/camera/rgb/image_mono",rgb_image_mono,t) # generate depth and colored point cloud cv_depth_image = bridge.imgmsg_to_cv(depth_image, "passthrough") cv_rgb_image_color = bridge.imgmsg_to_cv(rgb_image_color, "bgr8") centerX = depth_camera_info.K[2] centerY = depth_camera_info.K[5] depthFocalLength = depth_camera_info.K[0] depth_points = sensor_msgs.msg.PointCloud2() depth_points.header = depth_image.header depth_points.width = depth_image.width depth_points.height = depth_image.height
class bag2data: def __init__(self, filename): print filename try: self.bag = rosbag.Bag(filename, 'r') except rosbag.bag.ROSBagUnindexedException: print 't' self.bag = rosbag.Bag(filename, 'w', allow_unindexed=True) print 't2' self.bag.reindex() # self.bag.close() # self.bag = rosbag.Bag(filename, 'r', allow_unindexed=False) print 'Bag reindexed' self.pub_img = rospy.Publisher('img_out', Image) self.img = None self.bridge = CvBridge() def view_depth(self): print 'start' t0 = None for topic, msg, t in self.bag.read_messages(): print t self.img = msg if self.img != None: self.pub_img.publish(self.img) print 'end' def convert(self): #Make a directory in .../kinect_tools/output/TIME to put the images out_dir_ext = time.ctime() #name output folder by time output_dir = working_dir + 'kinect_tools/output/' + out_dir_ext + '/' os.mkdir(output_dir) self.output_dir = output_dir topic_list = [] topic_frame = [0] i_frame = 0 #Go through all of the messages in the bag for topic, msg, t in self.bag.read_messages(): i_num = 0 img_num = None #Check which image it is (ie rgb or depth). Title the image differently dependend on name. Output is .../kinect_tools/output/TIME/imgNUM_FRAME.jpg # NUM is the topic index and FRAME is the message number for i_topic in topic_list: if topic == i_topic: img_num = i_num i_frame = topic_frame[i_num] break else: i_num += 1 if img_num == None: topic_list.append(topic) topic_frame.append(0) img_num = i_num i_frame = topic_frame[img_num] print topic_list try: self.img = np.array(self.bridge.imgmsg_to_cv(msg)) except: print 'Recieved non-image message!' if self.img != None: filename = output_dir + 'img' + str(img_num) + '_' + str( topic_frame[img_num]) + '.jpg' print filename try: if img_num == 1: self.img = self.img * 60 cv.SaveImage(filename, self.img) except: 'print error!!!!!!!!' self.pub_img.publish(self.bridge.cv_to_imgmsg(self.img)) topic_frame[img_num] += 1 # i_frame += 1 # print 'Output frame: ' + str(i_frame-1) # print 'Output frame: ' + str(topic_frame[img_num]) # time.sleep(1) print 'Done converting' def convert_3D(self): #Make a directory in .../kinect_tools/output/TIME to put the images out_dir_ext = time.ctime() #name output folder by time output_dir = working_dir + 'kinect_tools/output/' + out_dir_ext + '/' os.mkdir(output_dir) self.output_dir = output_dir print "Output_dir: " + str(output_dir) msg_generator = self.bag.read_messages( topics=('/camera/depth/points2', '/camera/rgb/image_color')) color_index = 0 depth_index = 0 while 1: # pdb.set_trace() # try: topic, msg, t = msg_generator.next() print topic if topic == '/camera/depth/points2': self.save_depth_img(msg, depth_index) self.save_color_img(msg, color_index) depth_index += 1 else: # self.save_color_img(msg, color_index) color_index += 1 # except: # print 'Done saving images' # break def save_depth_img(self, msg, img_num): img = np.empty((480, 640), dtype=np.float32) pts = [] # print msg.fields for p in point_cloud.read_points(msg): pts.append(p[2]) img = np.reshape(pts, (480, 640)) max_px = np.nanmax(img) min_px = np.nanmin(img) scale = int(255 / (max_px - min_px)) filename = self.output_dir + 'img_depth_' + str(img_num) + '.jpg' cv.SaveImage(filename, img * scale) def save_color_img(self, msg, img_num): img = np.empty((480, 640), dtype=np.float32) pts = [] # print msg.fields for p in point_cloud.read_points(msg): # print len(p) # pts.append(p[3]) x = 1 print 0 print p[3] x = struct.pack('f', p[3]) print x img = np.reshape(pts, (480, 640)) # max_px = np.nanmax(img) # min_px = np.nanmin(img) # scale = int(255 / (max_px-min_px)) filename = self.output_dir + 'img_color_' + str(img_num) + '.jpg' cv.SaveImage(filename, np.uint8(img))
class pr2WideDepth: def __init__(self, targetFrame=None, visual=False, tfListener=None): self.cloudTime = time.time() self.readTime = time.time() self.pointCloud = None self.visual = visual self.targetFrame = targetFrame self.updateNumber = 0 self.points3D = None if tfListener is None: self.transformer = tf.TransformListener() else: self.transformer = tfListener self.bridge = CvBridge() self.imageData = None self.spoonImageData = None # RGB Camera self.rgbCameraFrame = None self.cameraWidth = None self.cameraHeight = None self.pinholeCamera = None # Gripper self.lGripperPosition = None self.lGripperRotation = None self.lGripperTransposeMatrix = None self.lGripX = None self.lGripY = None self.micLocation = None self.grips = [] # Spoon self.spoonX = None self.spoonY = None self.spoon = None self.targetTrans = None self.targetRot = None self.gripperTrans = None self.gripperRot = None self.cloudSub = rospy.Subscriber('/wide_stereo/points2', PointCloud2, self.cloudCallback) print 'Connected to depth' self.imageSub = rospy.Subscriber('/wide_stereo/right/image_color', Image, self.imageCallback) print 'Connected to image' self.cameraSub = rospy.Subscriber('/wide_stereo/right/camera_info', CameraInfo, self.cameraRGBInfoCallback) print 'Connected to camera info' def getAllRecentPoints(self): print 'Time between read calls:', time.time() - self.readTime startTime = time.time() self.transformer.waitForTransform(self.targetFrame, self.rgbCameraFrame, rospy.Time(0), rospy.Duration(5)) try: self.targetTrans, self.targetRot = self.transformer.lookupTransform( self.targetFrame, self.rgbCameraFrame, rospy.Time(0)) except tf.ExtrapolationException: print 'TF Target Error!' pass self.transformer.waitForTransform('/l_gripper_tool_frame', self.targetFrame, rospy.Time(0), rospy.Duration(5)) try: self.gripperTrans, self.gripperRot = self.transformer.lookupTransform( '/l_gripper_tool_frame', self.targetFrame, rospy.Time(0)) except tf.ExtrapolationException: print 'TF Gripper Error!' pass print 'Read computation time:', time.time() - startTime self.readTime = time.time() return self.points3D, self.imageData, self.micLocation, self.spoon, [self.targetTrans, self.targetRot], [self.gripperTrans, self.gripperRot], \ [self.lGripX, self.lGripY], [self.spoonX, self.spoonY] def cancel(self): self.cloudSub.unregister() self.cameraSub.unregister() self.imageSub.unregister() def imageCallback(self, data): if self.lGripX is None: return try: image = self.bridge.imgmsg_to_cv(data) self.imageData = np.asarray(image[:, :]) except CvBridgeError, e: print e return
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_converter1", Image) cv.NamedWindow("Image window", 1) self.bridge = CvBridge() self.image_sub = rospy.Subscriber("ardrone/image_raw", Image, self.callback) def callback(self, data): try: cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e #(cols,rows) = cv.GetSize(cv_image) #if cols > 60 and rows > 60 : # cv.Circle(cv_image, (50,50), 10, 255) ####################################### #img1=cv.CreateImage((150,150),8,3) #cv.Rectangle(cv_image, (60,60),(70,90), (255,0,255)) #sub1=cv.GetSubRect(cv_image,(60,60,150,150)) #save1=cv.CloneMat(sub1) #sub2=cv.GetSubRect(img1,(0,0,150,150)) #cv.Copy(save1,sub2) storage = cv.CreateMemStorage(0) img = cv.CreateImage( (cv.GetSize(cv_image)[1], cv.GetSize(cv_image)[1]), 8, 3) img1 = cv.CreateImage( (cv.GetSize(cv_image)[1], cv.GetSize(cv_image)[1]), 8, 3) #img=cv.CreateImage(cv.GetSize(cv_image),8,3) img_r = cv.CreateImage(cv.GetSize(img), 8, 1) img_g = cv.CreateImage(cv.GetSize(img), 8, 1) img_b = cv.CreateImage(cv.GetSize(img), 8, 1) img_g1 = cv.CreateImage(cv.GetSize(img), 8, 1) img_g2 = cv.CreateImage(cv.GetSize(img), 8, 1) img2 = cv.LoadImage("/home/petin/catkin_ws/src/vp_ardrone2/ris1.jpg", cv.CV_LOAD_IMAGE_GRAYSCALE) sub1 = cv.GetSubRect( cv_image, (0, 0, cv.GetSize(cv_image)[1], cv.GetSize(cv_image)[1])) save1 = cv.CloneMat(sub1) sub2 = cv.GetSubRect( img, (0, 0, cv.GetSize(cv_image)[1], cv.GetSize(cv_image)[1])) cv.Copy(save1, sub2) #cv.CvtColor(img, img1, cv.CV_BGR2HSV) #cv.CvtPixToPlane(img1,img_h,img_s,img_v,None) #cv.CvtColor(img, gray, cv.CV_BGR2GRAY) #cv.Smooth(gray,gray,cv.CV_GAUSSIAN,5,5) cv.Split(img, img_b, img_g, img_r, None) # #cv.ShowImage("Image window1", img) #cv.ShowImage("Image windowb", img_b) #cv.ShowImage("Image windowg", img_g) #cv.ShowImage("Image windowr", img_r) # cv.InRangeS(img_g, cv.Scalar(180), cv.Scalar(255), img_g1) #cv.InRangeS(img_s, cv.Scalar(135), cv.Scalar(255), img_s1); #cv.InRangeS(img_b, cv.Scalar(0), cv.Scalar(61), img_b1); #cv.Invert(img_g1,img_g2,cv.CV_SVD) cv.Smooth(img2, img2, cv.CV_GAUSSIAN, 9, 9) # cv.ShowImage("Image windowh1", img_g1) #cv.ShowImage("Image windowg1", img_h1) #cv.ShowImage("Image windowr1", img_r1) #cv.ShowImage("Image gray", gray) # search circle storage = cv.CreateMat(img2.width, 1, cv.CV_32FC3) cv.ShowImage("Image window1", img2) cv.HoughCircles(img2, storage, cv.CV_HOUGH_GRADIENT, 2, 100, 100, 50, 10, 400) #rospy.loginfo(storage.width) for i in xrange(storage.width - 1): radius = storage[i, 2] center = (storage[i, 0], storage[i, 1]) rospy.loginfo('444') cv.Circle(cv_image, center, radius, (0, 0, 255), 3, 10, 200) #search_circle=cv.HoughCircles(img_g1,np.asarray(storage),3,10,150) #for res in search_circles: # p = float (cv.GetSeqElem(res)) # pt = cv.Point( cv.Round( p[0] ), cv.Round( p[1] ) ); # cv.Circle( cv_image, pt, cv.Round( p[2] ), 255 ); # #cv.And(img_g,img_r,img_a) #cv.And(img_a,img_b,img_a) # cv.ShowImage("Image window", cv_image) cv.WaitKey(3) try: self.image_pub.publish(self.bridge.cv_to_imgmsg(cv_image, "bgr8")) except CvBridgeError, e: print e
class CalibCam(script): def Initialize(self): self.bridge = CvBridge() self.image_sub_r = rospy.Subscriber("/stereo/right/image_color",Image,self.callback_r) self.cv_image_r = cv.CreateImage((1,1), 1 , 3) self.image_sub_l = rospy.Subscriber("/stereo/left/image_color",Image,self.callback_l) self.cv_image_l = cv.CreateImage((1,1), 1 , 3) self.image_sub_3d = rospy.Subscriber("/cam3d/rgb/image_color",Image,self.callback_3d) self.cv_image_3d = cv.CreateImage((1,1), 1 , 3) self.sss.init("head") self.sss.init("torso") self.sss.init("sdh") self.image_number_offset = 1 self.listener = tf.TransformListener() self.file_path = "./" def Run(self): print "start" seed() maxVal_pan = 0.125 #max: 0.125 maxVal_tilt = 0.20 #max: 0.20 nr_images = 12 #joint_names: ['torso_lower_neck_pan_joint', 'torso_lower_neck_tilt_joint', 'torso_upper_neck_pan_joint', 'torso_upper_neck_tilt_joint'] #lower right positions: [-0.10956629365682602, -0.049855709075927734, -0.16463811695575714, -0.074878953397274017] # move components to initial position self.sss.move("head","back") #self.sss.move("head","front") #self.sss.move("arm","calib") self.sss.move("torso","home") #self.sss.move("sdh","home") self.sss.wait_for_input() #self.sss.move("sdh","calib") #self.sss.wait_for_input() for j in range(1,4): if (j==1): # close (~ 0.75m) print "Move checkerboard to a close distance and strike any key when ready." self.sss.wait_for_input() maxVal_pan = 0.125 #max: 0.125 maxVal_tilt = 0.15 #max: 0.20 elif (j==2): # intermediate (~1.20m) print "Move checkerboard to an intermediate distance and strike any key when ready." self.sss.wait_for_input() maxVal_pan = 0.125 #max: 0.125 maxVal_tilt = 0.20 #max: 0.20 elif (j==3): # far (~1.90m) print "Move checkerboard to a far distance and strike any key when ready." self.sss.wait_for_input() maxVal_pan = 0.125 #max: 0.125 maxVal_tilt = 0.20 #max: 0.20 # start calbration routine for i in range(1,nr_images+1): if i==1: r1 = maxVal_pan r2 = maxVal_tilt elif i==2: r1 = 0 r2 = maxVal_tilt elif i==3: r1 = -maxVal_pan r2 = maxVal_tilt elif i==4: r1 = -maxVal_pan r2 = 0 elif i==5: r1 = 0 r2 = 0 elif i==6: r1 = maxVal_pan r2 = 0 elif i==7: r1 = maxVal_pan r2 = -maxVal_tilt elif i==8: r1 = 0 r2 = -maxVal_tilt elif i==9: r1 = -maxVal_pan r2 = -maxVal_tilt else: r1 = (random()-0.5)*2*maxVal_pan; r2 = (random()-0.5)*2*maxVal_tilt; self.sss.move("torso",[[1./3.*r1,1./3.*r2,2./3.*r1,2./3.*r2]]) self.sss.sleep(1) if not self.sss.parse: self.captureImage() self.sss.move("torso","home") print "Prepare checkerboard for further views and hit <Enter> for each image capture. Press any other key + <Enter> to finish.\n" key = self.sss.wait_for_input() if not self.sss.parse: while (key==''): self.captureImage() key = self.sss.wait_for_input() print "finished" def captureImage(self): try: (trans,rot) = self.listener.lookupTransform('/base_link', '/head_axis_link', rospy.Time(0)) rpy = euler_from_quaternion(rot) cyaw = cos(rpy[2]) syaw = sin(rpy[2]) cpitch = cos(rpy[1]) spitch = sin(rpy[1]) croll = cos(rpy[0]) sroll = sin(rpy[0]) R11 = cyaw*cpitch R12 = cyaw*spitch*sroll-syaw*croll R13 = cyaw*spitch*croll+syaw*sroll R21 = syaw*cpitch R22 = syaw*spitch*sroll+cyaw*croll R23 = syaw*spitch*croll-cyaw*sroll R31 = -spitch R32 = cpitch*sroll R33 = cpitch*croll fout = open(self.file_path+'calpic'+str(self.image_number_offset)+'.coords','w') fout.write(str(R11)+' '+str(R12)+' '+str(R13)+' '+str(trans[0]*1000)+'\n'+str(R21)+' '+str(R22)+' '+str(R23)+' '+str(trans[1]*1000)+'\n'+str(R31)+' '+str(R32)+' '+str(R33)+' '+str(trans[2]*1000)) fout.close() except (tf.LookupException, tf.ConnectivityException): print "tf exception" self.sss.sleep(1) cv.SaveImage(self.file_path+'right_'+str(self.image_number_offset)+'.png',self.cv_image_r) cv.SaveImage(self.file_path+'left_'+str(self.image_number_offset)+'.png',self.cv_image_l) cv.SaveImage(self.file_path+'cam3d_'+str(self.image_number_offset)+'.png',self.cv_image_3d) self.sss.sleep(1) self.image_number_offset = self.image_number_offset + 1 def callback_r(self,data): try: self.cv_image_r = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e
class VisibilityCheckerNode(): ''' @summary: Captures Images from one or more cameras (Image message topics) to files. Number of cameras, output folder and file names are configurable via ROS parameters. After startup call "~capture_images" ROS service to save images of all cameras to output folder. ''' def __init__(self): ''' Initializes storage, gets parameters from parameter server and logs to rosinfo ''' self.bridge = CvBridge() # set up Checkerboard and CheckerboardDetector self.board = Checkerboard((9, 6), 0.03) self.detector = CheckerboardDetector(self.board) rospy.init_node(NODE) self.counter = 0 # Get params from ros parameter server or use default self.cams = rospy.get_param("~cameras") self.camera = [self.cams["reference"]["topic"]] if self.cams.has_key("further"): rospy.loginfo("FURTHER cameras specified") for cam in self.cams["further"]: self.camera.append(cam["topic"]) else: rospy.logwarn("No FURTHER cameras specified") self.numCams = len(self.camera) # Init images self.image = [] self.image_received = [False] * self.numCams for id in range(self.numCams): self.image.append(Image()) # Subscribe to images self.imageSub = [] for id in range(self.numCams): self.imageSub.append(rospy.Subscriber( self.camera[id], Image, self._imageCallback, id)) # Wait for image messages for id in range(self.numCams): rospy.wait_for_message(self.camera[id], Image, 5) def _imageCallback(self, data, id): ''' Copy image message to local storage @param data: Currently received image message @type data: ROS Image() message @param id: Id of camera from which the image was received @type id: integer ''' #print "cb executed" self.image[id] = data self.image_received[id] = True def _checkHandle(self, req): ''' Service handle for "Capture" Service Grabs images, converts them and saves them to files @param req: service request @type req: CaptureRequest() message @return: CaptureResponse() message ''' self.image_received = [False] * self.numCams visible = [] while not all(self.image_received): print "waiting for images" rospy.sleep(1) if all(self.image_received): print "=== ALL IMAGES RECEIVED ===" self.images_received = [False] * self.numCams for id in range(self.numCams): image = self.image[id] cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8') img_raw = cv2util.cvmat2np(cvImage) try: self.detector.detect_image_points( img_raw, False, True) visible.append(True) except self.detector.NoPatternFoundException: rospy.logwarn("No cb found") visible.append(False) # grab image messages #print '%s checkerboards found --> return %s'%(sum(visible),all(visible)) response = VisibleResponse() response.every = False response.master = False if all(visible): response.every = True response.master = True #elif visible[0]: # response.master = True return response def run(self): # Start service srv = rospy.Service( '/image_capture/visibility_check', Visible, self._checkHandle) rospy.loginfo("service '/image_capture/visibility_check' started, waiting for requests...") rospy.spin()
class Ros2Python: def __init__(self, objectDetector=None, laneDetector=None, grassDetector=None): # Subscribe to PointCloud2 self.subImage = rospy.Subscriber("/left/image_rect_color",\ Image, self.cbImage, queue_size=1) self.subPoints = rospy.Subscriber("/points2",\ PointCloud2, self.cbPoints, queue_size=1) self.images = [] self.mask = None self.imageStamps = [] self.imageInd = 0 self.bridge = CvBridge() self.showDetected = False self.obstacleCloud = np.array([]) if objectDetector: self.objectDetector = objectDetector else: self.objectDetector = ObstacleDetector((240, 320)) # We assume that the normal of the plane is pointing straight up # in the vision (-Y direction) self.objectDetector.setNormalConstraint(np.array([0., -1., -0.2])) # And allow the search to deviate only 30 degrees from that self.objectDetector.angleConstraint = 30. if laneDetector: self.laneDetector = laneDetector else: self.laneDetector = LaneDetector((240, 320)) if grassDetector: self.grassDetector = grassDetector else: # Init with H,S,V thresholds self.grassDetector = GrassDetector(45, 35, 35) if VERBOSE: print "Subscribed to /left/image_rect_color" print "Subscribed to /points2" def run(self): rospy.init_node('Ros2Python', anonymous=True) try: rospy.spin() except KeyboardInterrupt: print "Shutting down liveObstacleDetection module" def cbPoints(self, point_msg): if VERBOSE: print 'Received points. Size: (', point_msg.width, ',', point_msg.height, ')' # Convert to numpy array (http://goo.gl/s6OGja) cloud = np.array(points2cloud_to_array(point_msg)) # ObDetector only recalculates everything when the cloud is updated cloud = np.dstack((cloud['x'], cloud['y'], cloud['z'])) pointT = point_msg.header.stamp.to_sec() #print 'Point Time:', pointT #print 'Image Times:', self.imageStamps image = None for i in range(len(self.imageStamps)): imageT = self.imageStamps[i] if imageT > pointT: image = self.images[i] break if image == None: image = self.images[i] self.images = self.images[i:] self.imageStamps = self.imageStamps[i:] self.objectDetector.updateImage(image) self.laneDetector.updateImage(image) self.grassDetector.updateImage(image) self.objectDetector.updatePoints(cloud) ##### Jared, these are the 2 most useful variables for you: ''' mask: is a numpy array the same size as the camera image. The elements in the mask are set to 1 if that pixel is part of the plane/ground, 2 if that pixel is an object, and 0 otherwise. ''' self.mask = self.objectDetector.getPlaneObjectMask() self.grassDetector.updateMask(self.mask == 1) grassMask = np.logical_and(self.grassDetector.findGrass(), self.mask != 2) self.mask[grassMask] = 1 model = self.objectDetector.model planeInd = model.coord2ind(np.array(np.where(self.mask == 1)).T) planeInd = planeInd.astype(int) success, coeff = model.optimiseModelCoefficients(\ planeInd,self.objectDetector.coeff) success, newInd = model.selectWithinDistance(coeff, 0.15) self.objectDetector.coeff = coeff #print newInd.shape #print newInd self.mask[model.ind2coord(newInd)] = 1 ''' obsacleCloud: a list of 2D points relative to the camera. The positive X-axis goes to the right of the camera. The positive Y-axis goes away from the camera. TODO: Double check this =S ''' self.obstacleCloud = self.objectDetector.getObstacleCloud() # Do the lane detect also detectedLanes = self.laneDetector.findLines() self.detectedLanes = np.logical_and(detectedLanes, self.mask != 2) self.laneDetector.showImage() self.mask[self.detectedLanes] = 2 self.maskall = self.mask if self.showDetected: #print 'Showing Detected' self.objectDetector.showDetected(self.mask) self.showDetected = False cv2.waitKey(10) #self.objectDetector.showDepth() # Show the plane and abjects # im = self.objectDetector.image.copy() # im[grassMask] = [255,0,0] # im[mask==2] = [0,0,255] # cv2.imshow('DetectedObjects', im) # Show the lines #self.laneDetector.showImage() # cv2.imshow('DetectedLines', detectedLanes.astype(float)*255) # cv2.waitKey(15) def getMaskAll(self): return self.maskall def cbImage(self, img_msg): image = np.asarray(self.bridge.imgmsg_to_cv(img_msg)) self.images.append(image) self.imageStamps.append(img_msg.header.stamp.to_sec())
class CalibCam(script): def Initialize(self): self.bridge = CvBridge() self.image_sub = rospy.Subscriber("/stereo/right/image_color", Image, self.callback) self.cv_image = cv.CreateImage((1, 1), 1, 3) self.sss.init("head") self.sss.init("torso") self.sss.init("sdh") def Run(self): print "start" seed() maxVal = 0.04 file_path = "./" listener = tf.TransformListener() nr_images = 14 # move components to initial position self.sss.move("head", "back") self.sss.move("arm", "calib") self.sss.move("torso", "home") self.sss.move("sdh", "home") self.sss.wait_for_input() self.sss.move("sdh", "calib") self.sss.wait_for_input() # start calbration routine for i in range(1, nr_images): if i == 1: r1 = maxVal r2 = maxVal elif i == 2: r1 = -maxVal r2 = maxVal elif i == 3: r1 = maxVal r2 = -maxVal elif i == 4: r1 = -maxVal r2 = -maxVal else: r1 = (random() - 0.5) * 2 * maxVal r2 = (random() - 0.5) * 2 * maxVal self.sss.move("torso", [[r1, r2, r1, r2]]) self.sss.sleep(1) try: (trans, rot) = listener.lookupTransform('/base_link', '/head_color_camera_r_link', rospy.Time(0)) rpy = euler_from_quaternion(rot) cyaw = cos(rpy[2]) syaw = sin(rpy[2]) cpitch = cos(rpy[1]) spitch = sin(rpy[1]) croll = cos(rpy[0]) sroll = sin(rpy[0]) R11 = cyaw * cpitch R12 = cyaw * spitch * sroll - syaw * croll R13 = cyaw * spitch * croll + syaw * sroll R21 = syaw * cpitch R22 = syaw * spitch * sroll + cyaw * croll R23 = syaw * spitch * croll - cyaw * sroll R31 = -spitch R32 = cpitch * sroll R33 = cpitch * croll fout = open(file_path + 'calpic' + str(i) + '.coords', 'w') fout.write( str(R11) + ' ' + str(R12) + ' ' + str(R13) + ' ' + str(trans[0] * 1000) + '\n' + str(R21) + ' ' + str(R22) + ' ' + str(R23) + ' ' + str(trans[1] * 1000) + '\n' + str(R31) + ' ' + str(R32) + ' ' + str(R33) + ' ' + str(trans[2] * 1000)) fout.close() except (tf.LookupException, tf.ConnectivityException): print "tf exception" self.sss.sleep(1) cv.SaveImage(file_path + 'calpic' + str(i) + '.png', self.cv_image) self.sss.sleep(1) self.sss.move("torso", "home") print "finished" def callback(self, data): try: self.cv_image = self.bridge.imgmsg_to_cv(data, "bgr8") except CvBridgeError, e: print e
def get_image_from_topic(rgb_topic=RGB_TOPIC): msg = rospy.wait_for_message(rgb_topic, Image, 4) bridge = CvBridge() img = bridge.imgmsg_to_cv(msg, 'bgr8') np_img = np.asarray(img) return np_img
class ObjectFollower(): def __init__(self): rospy.init_node("object_follower") # Set the shutdown function (stop the robot) rospy.on_shutdown(self.shutdown) # How often should we update the robot's motion? self.rate = rospy.get_param("~rate", 10) r = rospy.Rate(self.rate) # Scale the ROI by this factor to avoid background distance values around the edges self.scale_roi = rospy.get_param("~scale_roi", 0.9) # The max linear speed in meters per second self.max_linear_speed = rospy.get_param("~max_linear_speed", 0.3) # The minimum linear speed in meters per second self.min_linear_speed = rospy.get_param("~min_linear_speed", 0.02) # The maximum rotation speed in radians per second self.max_rotation_speed = rospy.get_param("~max_rotation_speed", 2.0) # The minimum rotation speed in radians per second self.min_rotation_speed = rospy.get_param("~min_rotation_speed", 0.5) # The x threshold (% of image width) indicates how far off-center # the ROI needs to be in the x-direction before we react self.x_threshold = rospy.get_param("~x_threshold", 0.1) # How far away from the goal distance (in meters) before the robot reacts self.z_threshold = rospy.get_param("~z_threshold", 0.05) # The maximum distance a target can be from the robot for us to track self.max_z = rospy.get_param("~max_z", 1.6) # The minimum distance to respond to self.min_z = rospy.get_param("~min_z", 0.5) # The goal distance (in meters) to keep between the robot and the person self.goal_z = rospy.get_param("~goal_z", 0.75) # How much do we weight the goal distance (z) when making a movement self.z_scale = rospy.get_param("~z_scale", 0.5) # How much do we weight (left/right) of the person when making a movement self.x_scale = rospy.get_param("~x_scale", 2.0) # Slow down factor when stopping self.slow_down_factor = rospy.get_param("~slow_down_factor", 0.8) # Initialize the global ROI self.roi = RegionOfInterest() # Publisher to control the robot's movement self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist) # Intialize the movement command self.move_cmd = Twist() # Get a lock for updating the self.move_cmd values self.lock = thread.allocate_lock() # We will get the image width and height from the camera_info topic self.image_width = 0 self.image_height = 0 # We need cv_bridge to convert the ROS depth image to an OpenCV array self.cv_bridge = CvBridge() self.depth_array = None # Set flag to indicate when the ROI stops updating self.target_visible = False # Wait for the camera_info topic to become available rospy.loginfo("Waiting for camera_info topic...") rospy.wait_for_message('camera_info', CameraInfo) # Subscribe to the camera_info topic to get the image width and height rospy.Subscriber('camera_info', CameraInfo, self.get_camera_info) # Wait until we actually have the camera data while self.image_width == 0 or self.image_height == 0: rospy.sleep(1) # Wait for the depth image to become available rospy.loginfo("Waiting for depth_image topic...") rospy.wait_for_message('depth_image', Image) # Subscribe to the depth image self.depth_subscriber = rospy.Subscriber("depth_image", Image, self.convert_depth_image, queue_size=1) # Subscribe to the ROI topic and set the callback to update the robot's motion rospy.Subscriber('roi', RegionOfInterest, self.set_cmd_vel) # Wait until we have an ROI to follow rospy.loginfo("Waiting for an ROI to track...") rospy.wait_for_message('roi', RegionOfInterest) rospy.loginfo("ROI messages detected. Starting follower...") # Begin the tracking loop while not rospy.is_shutdown(): # Acquire a lock while we're setting the robot speeds self.lock.acquire() try: if not self.target_visible: # If the target is not visible, stop the robot smoothly self.move_cmd.linear.x *= self.slow_down_factor self.move_cmd.angular.z *= self.slow_down_factor else: # Reset the flag to False by default self.target_visible = False # Send the Twist command to the robot self.cmd_vel_pub.publish(self.move_cmd) finally: # Release the lock self.lock.release() # Sleep for 1/self.rate seconds r.sleep() def set_cmd_vel(self, msg): # Acquire a lock while we're setting the robot speeds self.lock.acquire() try: # If the ROI has a width or height of 0, we have lost the target if msg.width == 0 or msg.height == 0: self.target_visible = False return else: self.target_visible = True self.roi = msg # Compute the displacement of the ROI from the center of the image target_offset_x = msg.x_offset + msg.width / 2 - self.image_width / 2 try: percent_offset_x = float(target_offset_x) / ( float(self.image_width) / 2.0) except: percent_offset_x = 0 # Rotate the robot only if the displacement of the target exceeds the threshold if abs(percent_offset_x) > self.x_threshold: # Set the rotation speed proportional to the displacement of the target speed = percent_offset_x * self.x_scale self.move_cmd.angular.z = -copysign( max(self.min_rotation_speed, min(self.max_rotation_speed, abs(speed))), speed) else: self.move_cmd.angular.z = 0 # Now compute the depth component # Initialize a few depth variables n_z = sum_z = mean_z = 0 # Shrink the ROI slightly to avoid the target boundaries scaled_width = int(self.roi.width * self.scale_roi) scaled_height = int(self.roi.height * self.scale_roi) # Get the min/max x and y values from the scaled ROI min_x = int(self.roi.x_offset + self.roi.width * (1.0 - self.scale_roi) / 2.0) max_x = min_x + scaled_width min_y = int(self.roi.y_offset + self.roi.height * (1.0 - self.scale_roi) / 2.0) max_y = min_y + scaled_height # Get the average depth value over the ROI for x in range(min_x, max_x): for y in range(min_y, max_y): try: # Get a depth value in millimeters z = self.depth_array[y, x] # Convert to meters z /= 1000.0 except: # It seems to work best if we convert exceptions to 0 z = 0 # Check for values outside max range if z > self.max_z: continue # Increment the sum and count sum_z = sum_z + z n_z += 1 # Stop the robot's forward/backward motion by default linear_x = 0 # If we have depth values... if n_z: mean_z = float(sum_z) / n_z # Don't let the mean fall below the minimum reliable range mean_z = max(self.min_z, mean_z) # Check the mean against the minimum range if mean_z > self.min_z: # Check the max range and goal threshold if mean_z < self.max_z and (abs(mean_z - self.goal_z) > self.z_threshold): speed = (mean_z - self.goal_z) * self.z_scale linear_x = copysign( min(self.max_linear_speed, max(self.min_linear_speed, abs(speed))), speed) if linear_x == 0: # Stop the robot smoothly self.move_cmd.linear.x *= self.slow_down_factor else: self.move_cmd.linear.x = linear_x finally: # Release the lock self.lock.release() def convert_depth_image(self, ros_image): # Use cv_bridge() to convert the ROS image to OpenCV format try: # The depth image is a single-channel float32 image depth_image = self.cv_bridge.imgmsg_to_cv(ros_image, "32FC1") except CvBridgeError, e: print e # Convert the depth image to a Numpy array self.depth_array = np.array(depth_image, dtype=np.float32)
class ImageRenderer: def __init__(self, ns): self.lock = threading.Lock() self.image_time = rospy.Time(0) self.info_time = rospy.Time(0) self.image = None self.interval = 0 self.features = None self.bridge = CvBridge() self.ns = ns self.max_interval = rospy.get_param('filter_intervals/min_duration') self.font = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.30, 1.5, thickness=2) self.font1 = cv.InitFont(cv.CV_FONT_HERSHEY_SIMPLEX, 0.10, 1, thickness=1) self.info_sub = rospy.Subscriber(ns + '/camera_info', CameraInfo, self.info_cb) self.image_sub = rospy.Subscriber(ns + '/image_throttle', Image, self.image_cb) self.interval_sub = rospy.Subscriber(ns + '/settled_interval', Interval, self.interval_cb) self.features_sub = rospy.Subscriber(ns + '/features', CalibrationPattern, self.features_cb) def info_cb(self, msg): with self.lock: self.info_time = rospy.Time.now() def image_cb(self, msg): with self.lock: self.image_time = rospy.Time.now() self.image = msg def interval_cb(self, msg): with self.lock: self.interval = (msg.end - msg.start).to_sec() def features_cb(self, msg): with self.lock: self.features = msg def render(self, window): with self.lock: if self.image and self.image_time + rospy.Duration( 8.0) > rospy.Time.now( ) and self.info_time + rospy.Duration( 8.0) > rospy.Time.now(): cv.Resize(self.bridge.imgmsg_to_cv(self.image, 'rgb8'), window) # render progress bar interval = min(1, (self.interval / self.max_interval)) cv.Rectangle( window, (int(0.05 * window.width), int(window.height * 0.9)), (int(interval * window.width * 0.9 + 0.05 * window.width), int(window.height * 0.95)), (0, interval * 255, (1 - interval) * 255), thickness=-1) cv.Rectangle( window, (int(0.05 * window.width), int(window.height * 0.9)), (int(window.width * 0.9 + 0.05 * window.width), int(window.height * 0.95)), (0, interval * 255, (1 - interval) * 255)) cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * 0.1)), self.font1, (0, 0, 255)) if self.features and self.features.header.stamp + rospy.Duration( 4.0) > self.image.header.stamp: w_scaling = float(window.width) / self.image.width h_scaling = float(window.height) / self.image.height if self.features.success: corner_color = (0, 255, 0) for cur_pt in self.features.image_points: cv.Circle(window, (int(cur_pt.x * w_scaling), int(cur_pt.y * h_scaling)), int(w_scaling * 5), corner_color) else: window = add_text(window, ["Could not detect", "checkerboard"], False) else: window = add_text( window, ["Timed out waiting", "for checkerboard"], False) else: # Generate random white noise (for fun) noise = numpy.random.rand(window.height, window.width) * 256 numpy.asarray(window)[:, :, 0] = noise numpy.asarray(window)[:, :, 1] = noise numpy.asarray(window)[:, :, 2] = noise cv.PutText(window, self.ns, (int(window.width * .05), int(window.height * .95)), self.font, (0, 0, 255))
class pipe_task: def __init__(self): self.cvbridge = CvBridge() self.cam_sub_ = rospy.Subscriber('camera/image_rect_color', Image, self.ImageCB) self.cam_info_sub_ = rospy.Subscriber('camera/camera_info', CameraInfo, self.CameraInfoCB) self.cameraInfo_initialized_ = False self.pipe_length_total_ = 0.305 self.pipe_length_little_ = 0.0 self.find_times_ = 0 self.find_times_limit_ = 30 self.pipePose_pub_ = rospy.Publisher('/pipe_pose', pipe_pose) def CameraInfoCB(self, info_msg): if not self.cameraInfo_initialized_: self.camera_info_ = info_msg self.cameraInfo_initialized_ = True def ImageCB(self, image_msg): if not self.cameraInfo_initialized_: return try: iplimg = self.cvbridge.imgmsg_to_cv(image_msg, image_msg.encoding) cvimg = np.array(iplimg, dtype=np.uint8) except CvBridgeError, e: rospy.logerr('cvbridge exception: ' + str(e)) return hsv_image = cv2.cvtColor(cvimg, cv2.cv.CV_BGR2HSV) color_min = np.array([20, 10, 20], np.uint8) color_max = np.array([90, 50, 100], np.uint8) pipe_orange_threshold = cv2.inRange(hsv_image, color_min, color_max) opening_size = 1 opening_element = cv2.getStructuringElement( cv2.MORPH_RECT, (2 * opening_size + 1, 2 * opening_size + 1)) closing_size = 5 closing_element = cv2.getStructuringElement( cv2.MORPH_RECT, (2 * closing_size + 1, 2 * closing_size + 1)) pipe_orange_threshold = cv2.morphologyEx(pipe_orange_threshold, cv2.MORPH_OPEN, opening_element) pipe_orange_threshold = cv2.morphologyEx(pipe_orange_threshold, cv2.MORPH_CLOSE, closing_element) cv2.imshow('raw_threshold', pipe_orange_threshold) white_pixel_num = np.count_nonzero(pipe_orange_threshold) copy_for_contour = pipe_orange_threshold.copy() contours, _ = cv2.findContours(copy_for_contour, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) for i in range(1, len(contours)): cv2.line(pipe_orange_threshold, tuple(contours[i - 1][0][0]), tuple(contours[i][0][0]), 255, 2) cv2.imshow('pipe_outer', pipe_orange_threshold) copy_for_contour2 = pipe_orange_threshold.copy() single_contour, _ = cv2.findContours(copy_for_contour2, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) pipe_skeleton_pose = pipe_pose() #pipe_skeleton_pose.header = iplimg.header if white_pixel_num > 200: minArea_rectangle = cv2.minAreaRect(single_contour[0]) color_rect = cv2.cvtColor(pipe_orange_threshold, cv2.cv.CV_GRAY2BGR) rect_points = cv2.cv.BoxPoints(minArea_rectangle) for j in range(4): pt1 = tuple(np.int32(rect_points[j])) pt2 = tuple(np.int32(rect_points[(j + 1) % 4])) cv2.line(color_rect, pt1, pt2, (255, 0, 0), 3, 8) pipe_skeleton_pose.detect_pipe, lowest_pt, second_lowest_pt = self.Calc_pose( rect_points, pipe_skeleton_pose) pt1 = tuple(np.int32(lowest_pt)) pt2 = tuple(np.int32(second_lowest_pt)) cv2.line(color_rect, pt1, pt2, (0, 0, 255), 3, 8) cv2.imshow('color_rect', color_rect) if pipe_skeleton_pose.detect_pipe: self.find_times_ += 1 else: self.find_times_ -= 1 pipe_skeleton_pose.detection_stable = self.find_times_ > self.find_times_limit_ self.pipePose_pub_.publish(pipe_skeleton_pose) cv2.imshow('orange_threshold', pipe_orange_threshold) cv2.imshow('pipe_outer', pipe_orange_threshold) cv2.waitKey(2)
def callback(data): global imagePub global markerPub #Convert image to CV image bridge = CvBridge() cv_image = bridge.imgmsg_to_cv(data, "mono8") # Convert the image to a Numpy array since most cv2 functionsrequire Numpy arrays. searched = np.array(cv_image, dtype=np.uint8) #Create a copy for sobel comparison searchedCopy = copy.copy(searched) searchedCopy[searchedCopy == 255] = 0 #Take Sobel Derivatives sobel_x = np.uint8( np.absolute(cv2.Sobel(searched, cv2.CV_16S, 1, 0, ksize=1))) sobel_y = np.uint8( np.absolute(cv2.Sobel(searched, cv2.CV_16S, 0, 1, ksize=1))) sobel_xy = cv2.addWeighted(sobel_x, 0.5, sobel_y, 0.5, 0) sobel_x_base = np.uint8( np.absolute(cv2.Sobel(searchedCopy, cv2.CV_16S, 1, 0, ksize=1))) sobel_y_base = np.uint8( np.absolute(cv2.Sobel(searchedCopy, cv2.CV_16S, 0, 1, ksize=1))) sobel_xy_base = cv2.addWeighted(sobel_x_base, 0.5, sobel_y_base, 0.5, 0) ret, sobel_xy_thres = cv2.threshold(sobel_xy, 0, 255, cv2.THRESH_BINARY) ret, sobel_xy_base_thres = cv2.threshold(sobel_xy_base, 0, 255, cv2.THRESH_BINARY) #Subtract Comparisons for frontiers only sobelCombined = sobel_xy_base_thres - sobel_xy_thres ret, sobelCombined_thresh = cv2.threshold(sobelCombined, 0, 255, cv2.THRESH_BINARY) #Dialate Combined dialate = cv2.dilate(sobelCombined_thresh, np.ones((3, 3), 'uint8')) #Find Contour Centorids dialateCopy = copy.copy(dialate) contours, hier = cv2.findContours(dialateCopy, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE) centroids = [] colors = [] for i in contours: if len(i) > 10: moments = cv2.moments(i) cx = int(moments['m10'] / moments['m00']) cy = int(moments['m01'] / moments['m00']) centroidPoint = Point() centroidColor = ColorRGBA() centroidPoint.x = cx / 20.0 - 20 centroidPoint.y = cy / 20.0 - 32.8 centroidPoint.z = 0 #centroidColor = (0,0,1,1) centroidColor.r = 0 centroidColor.g = 0 centroidColor.b = 1 centroidColor.a = 1 centroids.append(centroidPoint) colors.append(centroidColor) #code.interact(local=locals()) #Display Image in PLT Window '''plt.subplot(1,5,1),plt.imshow(searched,cmap='gray'),plt.title('Searched Space'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,2),plt.imshow(sobel_xy_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,3),plt.imshow(sobel_xy_base_thres,cmap='gray'),plt.title('Sobel All'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,4),plt.imshow(sobelCombined,cmap='gray'),plt.title('Frontiers Map'),plt.xticks([]), plt.yticks([]) plt.subplot(1,5,5),plt.imshow(dialate,cmap='gray'),plt.title('Dialate'),plt.xticks([]), plt.yticks([]) plt.draw() plt.pause(0.001)''' #Convert the image back to mat format and publish as ROS image frontiers = cv.fromarray(dialate) imagePub.publish(bridge.cv_to_imgmsg(frontiers, "mono8")) #Publish centorids of frontiers as marker markerMsg = Marker() markerMsg.header.frame_id = "/map" markerMsg.header.stamp = rospy.Time.now() markerMsg.ns = "" markerMsg.id = 0 markerMsg.type = 7 #Sphere List markerMsg.action = 0 #Add markerMsg.scale.x = 0.5 markerMsg.scale.y = 0.5 markerMsg.scale.z = 0.5 markerMsg.points = centroids markerMsg.colors = colors markerPub.publish(markerMsg) print 'yes'
class HumanDetector(AbstractVisionModule): def __init__(self, host, port, freq, source="camera/depth/image_raw", transform=[[0, 0, 0.63]]): super(HumanDetector, self).__init__(host, port) self.logger = logging.getLogger("Borg.Brain.Vision.HumanDetector") print "Human detection running" self.bridge = CvBridge() self.cv_image = None self.FPS_counter = 0 self.FPS_time = time.time() self.camera_to_base_transform = transform self.source = source self.freq = freq #Parameters self.__body_line = 120 self.__leg_line = 320 self.__line_height = 10 self.__max_depth = 2000 self.__depth_search_limit = 1000 self.__depth_step = 500 self.__min_width_body = 60 self.__max_width_body = 250 self.__min_width_leg = 15 self.__max_width_leg = 100 self.__search_depth_leg = 500 self.__hori_fov = 57.0 # Horizontal field of view self.__vert_fov = 43.0 # Vertical self.__hori_res = 640.0 # Horizontal resolution self.__vert_res = 480.0 # Vertical #self.vid_mem_reader = util.vidmemreader.VidMemReader([self.source]) self.Operators = [] rospy.Subscriber(source, Image, self.newImageCB) print "Subscribing to %s" % source def train(self): pass def run(self): """Start loop which simple sends a random fake observation every 5 secondes.""" t = Ticker(self.freq) while True: t.tick() #Name, used to identify the observation in memory: Operators = self.Operators if Operators: self.add_property('name', 'HumanDetector') self.add_property('Operators', Operators) self.store_observation() self.update() def computeFPS(self): self.FPS_counter += 1 if (time.time() - self.FPS_time > 1): print "FPS: ", self.FPS_counter self.FPS_time = time.time() self.FPS_counter = 0 #Filter depth in the image def filterDepth(self, depth_image, minimum, maximum): depth_image[depth_image > maximum] = 0 depth_image[depth_image < minimum] = 0 depth_image[depth_image > minimum] = 50000 return depth_image #Select image line from image def selectLine(self, image, line, height): return image[line:line + height, 0:640] #Remove noise from the image. #If there are 1 or more zeros in one column -> Entire column is set to zero def removeNoise(self, image): image[:, sum(image == 0) >= 1] = 0 return image #Remove blobs from the list def removeBlobsFromList(self, Blobs, key, comparison, value): if not Blobs == None: if comparison == '<': return [x for x in Blobs if x[str(key)] > value] elif comparison == '>': return [x for x in Blobs if x[str(key)] < value] else: print "Wrong comparison" else: return [] #Compute the depth value of the blob. Remove outliers def computeDepth(self, image_depth, x, size): temp = np.copy(image_depth[0:10, x:x + size]) return np.mean(temp[abs(temp - np.mean(temp)) < np.std(temp)]) #Sort the list with blobs def sortBlobs(self, blobs): return sorted(blobs, key=lambda k: k['width'], reverse=True) #Detect blobs in the image def detectBlobs(self, image, image_depth): detections = np.argwhere(image[0, :]) if np.size(detections) > 5: blobs = [] start_x = detections[0] for i in range(0, np.size(detections) - 5): if detections[i + 1] - detections[i] > 2: blobs.append({ 'x': int(start_x), 'width': int(detections[i] - start_x), 'depth': self.computeDepth(image_depth, int(start_x), int(detections[i] - start_x)) }) start_x = detections[i + 1] blobs.append({ 'x': int(start_x), 'width': int(detections[i + 1] - start_x), 'depth': self.computeDepth(image_depth, int(start_x), int(detections[i + 1] - start_x)) }) return self.sortBlobs(blobs) return None #Return a list with the blobs def detectOperator(self, body, legs): if len(legs) >= 2: legs = self.sortBlobs(legs) if abs(legs[0]['width'] - legs[1]['width']) < 20: return True else: #print "Legs are different" return False else: #print "Not enough possible legs detected" return False def newImageCB(self, data): self.computeFPS() Operators = [] #Create an array with depth information from the image temp = self.bridge.imgmsg_to_cv(data) image_temp = np.asarray(temp[:, :]) depth = 500 while depth < self.__max_depth: ########################## # Body Detection # ########################## #Make a copy of the original image image = np.copy(image_temp) #Select a line at x = self.__body_line with a height of self.__line_height image_body = self.selectLine(image, self.__body_line, self.__line_height) #Create an array with the depth values image_body_depth = np.copy(image_body) #Filter on depth image_body = self.filterDepth(image_body, depth, depth + self.__depth_search_limit) #Remove noise from the image image_body = self.removeNoise(image_body) #Detect blobs in image body_blobs = self.detectBlobs(image_body, image_body_depth) #Remove small blobs body_blobs = self.removeBlobsFromList(body_blobs, 'width', '<', self.__min_width_body) if depth >= 4000: body_blobs = self.removeBlobsFromList(body_blobs, 'width', '>', 150) else: body_blobs = self.removeBlobsFromList(body_blobs, 'width', '>', self.__max_width_body) #If there are possible bodies detected if not body_blobs == None: #Make a copy of the original image #image = np.copy(image_temp) #Select a line for detecting legs image_legs = self.selectLine(image, self.__leg_line, self.__line_height) #Create an array with the depth values image_legs_depth = np.copy(image_legs) for body_blob in body_blobs: #Minimum and maximum depth values for detecting legs (min_depth, max_depth) = (body_blob['depth'] - self.__search_depth_leg, body_blob['depth'] + self.__search_depth_leg) #Minimum and maximum position of legs (min_leg_pos, max_leg_pos) = (body_blob['x'] - 50, body_blob['x'] + body_blob['width'] + 50) #Filter on depth image_legs = self.filterDepth(image_legs, min_depth, max_depth) #Remove noise image_legs = self.removeNoise(image_legs) #Detect blobs in image leg_blobs = self.detectBlobs(image_legs, image_legs_depth) leg_blobs = self.removeBlobsFromList( leg_blobs, 'x', '<', min_leg_pos) leg_blobs = self.removeBlobsFromList( leg_blobs, 'x', '>', max_leg_pos) leg_blobs = self.removeBlobsFromList( leg_blobs, 'width', '<', self.__min_width_leg) leg_blobs = self.removeBlobsFromList( leg_blobs, 'width', '>', self.__max_width_leg) if self.detectOperator(body_blob, leg_blobs): Operator = { 'x': (body_blob['x'] + (0.5 * body_blob['width'])), 'depth': body_blob['depth'], 'width': body_blob['width'], 'time': time.time() } Match = False #Convert x, y values to real-world x, y values (in meters) depth_dist = body_blob['depth'] / 1000 #Angle difference per pixel hori_pp = self.__hori_fov / self.__hori_res vert_pp = self.__vert_fov / self.__vert_res #Locate x x_from_center = self.__hori_res / 2.0 - body_blob['x'] x_alpha = (x_from_center * hori_pp) / 180.0 * 3.14195 x_actual = depth_dist * math.sin(x_alpha) #Locate y y_from_center = self.__vert_res / 2.0 - self.__body_line y_alpha = y_from_center * vert_pp / 180.0 * 3.14195 y_actual = depth_dist * math.sin(y_alpha) #Transform to base_link Operator[ 'y_actual'] = x_actual + self.camera_to_base_transform[ 0][1] Operator[ 'z_actual'] = y_actual + self.camera_to_base_transform[ 0][2] Operator['x_actual'] = math.sqrt( Operator['y_actual'] * Operator['y_actual'] + depth_dist * depth_dist) for i in range(0, len(Operators)): if abs(Operators[i]['x'] - (body_blob['x'] + (0.5 * body_blob['width']))) < 30: Match = True if not Match: Operators.append(Operator) depth += self.__depth_step #image_temp[self.__body_line:(self.__body_line + self.__line_height),0:640] = 10000 #image_temp[self.__leg_line:(self.__leg_line + self.__line_height),0:640] = 10000 #cv2.imshow("Human detector 1.0", image_temp) #cv2.waitKey(3) self.Operators = Operators
class anaglyph: def __init__(self): self.pub_img = rospy.Publisher('anaglyph_img', Image) self.sub_depth = rospy.Subscriber('/kinect/depth/image_raw', Image, self.subscribe_callback, queue_size=1) self.sub_rgb = rospy.Subscriber('/kinect/rgb/image_raw', Image, self.subscribe_callback, queue_size=1) self.bridge = CvBridge() self.img_anaglyph = np.zeros((480, 640, 3), dtype=np.uint8) self.img_rgb = np.zeros((480, 640, 3), dtype=np.uint8) self.depth = np.zeros((480, 640), dtype=np.uint8) self.img_depth = np.zeros((480, 640), dtype=np.uint8) # self.r_offset = np.zeros((480, 640), dtype=np.uint8) # self.g_offset = np.zeros((480, 640), dtype=np.uint8) # self.b_offset = np.zeros((480, 640), dtype=np.uint8) # Store offsets for green/magenta channels in one matrix self.offset_indices = np.zeros((480, 640, 2), dtype=np.uint16) self.index_basis_x = np.zeros((480, 640), dtype=np.uint16) self.index_basis_y = np.zeros((480, 640), dtype=np.uint16) # Store index base for x and y dimensions -- used to vectorize anaglyph operations for i in range(0, 640): self.index_basis_y[:, i] = i for i in range(0, 480): self.index_basis_x[i, :] = i self.processing = 0 self.current_depth_time = time.time() self.current_rgb_time = time.time() self.timer_1 = time.time() self.timer_2 = time.time() self.rotation = np.array([[0.999979, 0.006497, -0.000801], [-0.006498, 0.999978, -0.001054], [0.000794, 0.001059, 0.999999]]) # self.transform = np.array([[526.6, 3.752, -201.2, 1.604e+04], # [-3.215, 526.6, -14.29, 2.429e+04], # [0.000794, 0.001059, -0.05648, 584.5]]) print 'Done initializing' def subscribe_callback(self, data): # Assign incoming Kinect images to depth/rgb images and process data #Block data if it is currently being processed if self.processing == 0: #Incoming data: #Depth if data.encoding == 'mono8': depth = np.asarray(self.bridge.imgmsg_to_cv(data)) self.img_depth = np.asarray(self.bridge.imgmsg_to_cv(data)) self.current_depth_time = time.time() #RGB elif data.encoding == 'rgb8': self.img_rgb = np.asarray(self.bridge.imgmsg_to_cv(data)) self.current_rgb_time = time.time() #If depth+rgb are both updated generate new anaglyph and publish output if np.abs(self.current_depth_time - self.current_rgb_time) < 0.1: self.generate_anaglyph() # self.pub_img.publish( self.bridge.cv_to_imgmsg(self.img_depth)) self.pub_img.publish( self.bridge.cv_to_imgmsg(self.img_anaglyph)) def generate_anaglyph(self): self.processing = 1 timer = time.time() # mask = (self.img_depth > 0) & (self.img_depth < 255) mask = np.logical_and((self.img_depth > 0), (self.img_depth < 255)) # self.depth = (5 - (self.img_depth*mask/4))*mask mod_depth = 2.5 max_depth = (self.img_depth * mask).max() / mod_depth self.depth = (max_depth - (self.img_depth / mod_depth)) * mask # self.depth = np.maximum( (max_depth -(self.img_depth/mod_depth)) , 0) * mask # print 'Min depth: ' + str(self.depth.min()) # print 'Max depth: ' + str(self.depth.max()) # print 'Min depth: ' + str((self.img_depth*mask).min()) # print 'Max depth: ' + str((self.img_depth*mask).max()) # Find length to prevent indices from being higher than the image resolution max_length = self.img_rgb.shape[1] - 1 #Find offset green/magenta indices for new image (based on depth) # Green self.offset_indices[:, :, 0] = np.maximum( np.minimum(self.index_basis_y + self.depth, max_length), 0) # Magenta self.offset_indices[:, :, 1] = np.maximum( np.minimum(self.index_basis_y - self.depth, max_length), 0) #Do lookup based on offset indices self.img_anaglyph[:, :, 0] = self.img_rgb[self.index_basis_x, self.offset_indices[:, :, 1], 2] #red self.img_anaglyph[:, :, 2] = self.img_rgb[self.index_basis_x, self.offset_indices[:, :, 1], 0] #blue self.img_anaglyph[:, :, 1] = self.img_rgb[self.index_basis_x, self.offset_indices[:, :, 0], 1] #green # self.r_offset = self.img_rgb[self.index_basis_x, self.offset_indices[:,:,1], 2] #red # self.b_offset = self.img_rgb[self.index_basis_x, self.offset_indices[:,:,1], 0] #blue # self.g_offset = self.img_rgb[self.index_basis_x, self.offset_indices[:,:,0], 1] #green print 'Framerate: ' + str(int(1 / (time.time() - timer))) # cv.MixChannels([self.r_offset, self.g_offset, self.b_offset], [self.img_anaglyph], [(0,0), (1,1), (2,2)]) self.processing = 0
class kinectDepth: def __init__(self, targetFrame=None, visual=False, tfListener=None): # ROS publisher for data points self.publisher = rospy.Publisher('visualization_marker', Marker) # List of features we are tracking self.clusterPoints = None self.nonClusterPoints = None self.dbscan = DBSCAN(eps=0.12, min_samples=10) self.cloudTime = time.time() self.pointCloud = None self.visual = visual self.targetFrame = targetFrame self.updateNumber = 0 self.points3D = None if tfListener is None: self.transformer = tf.TransformListener() else: self.transformer = tfListener self.bridge = CvBridge() self.imageData = None # RGB Camera self.rgbCameraFrame = None self.cameraWidth = None self.cameraHeight = None self.pinholeCamera = None # Gripper self.lGripperPosition = None self.lGripperRotation = None self.lGripperTransposeMatrix = None self.lGripX = None self.lGripY = None self.micLocation = None self.grips = [] # Spoon self.spoonX = None self.spoonY = None self.spoon = None self.targetTrans = None self.targetRot = None self.gripperTrans = None self.gripperRot = None self.cloudSub = rospy.Subscriber('/head_mount_kinect/depth_registered/points', PointCloud2, self.cloudCallback) print 'Connected to Kinect depth' # self.imageSub = rospy.Subscriber('/head_mount_kinect/rgb_lowres/image', Image, self.imageCallback) # print 'Connected to Kinect image' self.cameraSub = rospy.Subscriber('/head_mount_kinect/depth_lowres/camera_info', CameraInfo, self.cameraRGBInfoCallback) print 'Connected to Kinect camera info' def getAllRecentPoints(self): # print 'Time between read calls:', time.time() - self.cloudTime # startTime = time.time() self.transformer.waitForTransform(self.targetFrame, self.rgbCameraFrame, rospy.Time(0), rospy.Duration(5)) try: self.targetTrans, self.targetRot = self.transformer.lookupTransform(self.targetFrame, self.rgbCameraFrame, rospy.Time(0)) except tf.ExtrapolationException: print 'TF Target Error!' pass self.transformer.waitForTransform('/l_gripper_tool_frame', self.targetFrame, rospy.Time(0), rospy.Duration(5)) try: self.gripperTrans, self.gripperRot = self.transformer.lookupTransform('/l_gripper_tool_frame', self.targetFrame, rospy.Time(0)) except tf.ExtrapolationException: print 'TF Gripper Error!' pass # print 'Read computation time:', time.time() - startTime # self.cloudTime = time.time() return self.points3D, self.imageData, self.micLocation, self.spoon, [self.targetTrans, self.targetRot], [self.gripperTrans, self.gripperRot] # self.transformer.waitForTransform(self.targetFrame, self.rgbCameraFrame, rospy.Time(0), rospy.Duration(5)) # try: # targetTrans, targetRot = self.transformer.lookupTransform(self.targetFrame, self.rgbCameraFrame, rospy.Time(0)) # self.transMatrix = np.dot(tf.transformations.translation_matrix(targetTrans), tf.transformations.quaternion_matrix(targetRot)) # # print transMatrix # except tf.ExtrapolationException: # print 'TF Error!' # pass # points = np.c_[self.points3D, np.ones(len(self.points3D))] # values = np.dot(self.transMatrix, points.T).T[:, :3] # return values, np.dot(self.transMatrix, np.array([self.micLocation[0], self.micLocation[1], self.micLocation[2], 1.0]))[:3].tolist(), \ # np.dot(self.transMatrix, np.array([self.spoon[0], self.spoon[1], self.spoon[2], 1.0]))[:3].tolist() def cancel(self): self.publisher.unregister() self.cloudSub.unregister() self.cameraSub.unregister() # self.imageSub.unregister() def imageCallback(self, data): if self.lGripX is None: return try: image = self.bridge.imgmsg_to_cv(data) image = np.asarray(image[:,:]) except CvBridgeError, e: print e return # Crop imageGray to bounding box size lowX, highX, lowY, highY = self.boundingBox() self.imageData = image[lowY:highY, lowX:highX, :]
class DataCollector(): ''' @summary: Collects data for robot calibration. Subscribes to various topics needed (e.g. images, camera infos, joint angles) and provides a service. When service is called, a set of samples is recorded, processed (e.g. checkerboards are detected) and combined to a RobotMeasurement message which is published as /robot_measurement. ''' def __init__(self): ''' Set up subscribers, publishers and local storage ''' rospy.init_node(NODE) print "==> %s started " % NODE checkerboard = rospy.get_param("~calibration_pattern") self.checkerboard_square_size = checkerboard["square_size"] self.checkerboard_pattern_size = ( int(checkerboard["pattern_size"].split("x")[0]), int(checkerboard["pattern_size"].split("x")[1])) with open(rospy.get_param("~sensors_yaml"), 'r') as a: sensors_yaml = yaml.load(a.read()) # self._get_transformation_links(sensors_yaml) self._create_transformation_callbacks(sensors_yaml) #self.listener = tf.TransformListener() # CvBridge self.bridge = CvBridge() # initialize private storage self._images = {} self._images_received = {} self.counter = 1 # init publisher / subscriber self._robot_measurement_pub = rospy.Publisher("/robot_measurement", RobotMeasurement) # left camera rospy.Subscriber( rospy.get_param("~cameras")["reference"]["topic"], Image, self._callback_image, rospy.get_param("~cameras")["reference"]["name"]) self._images[rospy.get_param("~cameras")["reference"]["name"]] = {} self._images_received[rospy.get_param("~cameras")["reference"] ["name"]] = False for camera in rospy.get_param("~cameras")["further"]: rospy.Subscriber(camera["topic"], Image, self._callback_image, camera["name"]) self._images[camera["name"]] = {} self._images_received[camera["name"]] = True print "==> done with initialization" def _callback_image(self, image_raw, id): ''' Callback function for left camera message filter ''' # print "DEBUG: callback left" self._images[id]["image"] = image_raw # if self._left_received == False: # print "--> left sample received (this only prints once!)" self._images_received[id] = True def _create_transformation_callbacks(self, sensors_yaml): # kinematic chains self.transformations = {} self.transformations_received = {} for _chain in sensors_yaml["chains"]: rospy.Subscriber(_chain["topic"], JointTrajectoryControllerState, self._callback_jointstate, _chain["chain_id"]) self.transformations_received[_chain["chain_id"]] = False def _callback_jointstate(self, data, id): self.transformations[id] = data self.transformations[id].header.frame_id = id self.transformations_received[id] = True def run(self): ''' Main method, starts service to provide capture functionality ''' rospy.sleep(1) # Start service rospy.Service('/collect_data/capture', Capture, self._collect) rospy.loginfo( "service '/collect_data/capture' started, waiting for requests...") rospy.spin() def _collect(self, data): ''' Executed on service call. Logs and calls _capture_and_pub ''' rospy.loginfo("capturing sample %.2i" % self.counter) res = self._capture_and_pub("sample%.2i" % self.counter, CHECKERBOARD_NAME, CHECKERBOARD_CHAIN, self.checkerboard_pattern_size, self.checkerboard_square_size) self.counter += 1 return CaptureResponse(res) def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id # -------------------- # receive images # ----------------- for v in self._images_received: self._images_received[v] = False for v in self.transformations_received: self.transformations_received[v] = False print self._images_received start_time = rospy.Time.now() while (not all(self._images_received.values()) or not all(self.transformations.values())): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still # missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): for name, v in self._images_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name for name, v in self._transformations_received.iteritems(): if not v: print "--> still waiting for sample from %s" % name start_time = rospy.Time.now() print "got sample" #latest_left = self._left # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb # -------------- for name, image in self._images.iteritems(): image = image["image"] #print image.header cvImage = self.bridge.imgmsg_to_cv(image, "mono8") imagecv = cv2util.cvmat2np(cvImage) try: corners = checkerboard_detector.detect_image_points( imagecv, is_grayscale=True) except: # cb not found rospy.logwarn("No calibration pattern found for: '%s'" % name) return False else: print "cb found: %s" % name img_points = [] for (x, y) in corners.reshape(-1, 2): img_points.append(ImagePoint(x, y)) # create camera msg left # ---------------------- cam_msg = CameraMeasurement() cam_msg.camera_id = name #print "crash before" cam_msg.header.stamp = image.header.stamp #print "crash after" #cam_msg.cam_info = image["camera_info"] cam_msg.image_points = img_points cam_msg.verbose = False robot_msg.M_cam.append(cam_msg) cam_msg.image = image # print cam_msg.camera_id #print cam_msg.header # cam_msg.image_rect = latest_left["image_rect"] # cam_msg.features = # Not implemented here #---------------------- #DEBUG publish pic #----------------- #self._image_pub_left.publish(latest_left["image"]) #---------------------- # Fill robot_msg #---------------------- robot_msg.M_chain = self.transformations.values() self._robot_measurement_pub.publish(robot_msg) return True
class DataCollector(): ''' @summary: Collects data for robot calibration. Subscribes to various topics needed (e.g. images, camera infos, joint angles) and provides a service. When service is called, a set of samples is recorded, processed (e.g. checkerboards are detected) and combined to a RobotMeasurement message which is published as /robot_measurement. ''' def __init__(self): ''' Set up subscribers, publishers and local storage ''' rospy.init_node(NODE) print "==> %s started " % NODE # get joint names for arm if rospy.has_param("arm_controller/joint_names"): # real hardware self.arm_joint_names = rospy.get_param( "arm_controller/joint_names") elif rospy.has_param("arm_controller/joints"): # simulation self.arm_joint_names = rospy.get_param("arm_controller/joints") else: print "Could not get joint names for arm from parameter server. exiting..." exit(-1) # get joint names for torso if rospy.has_param("torso_controller/joint_names"): # real hardware self.torso_joint_names = rospy.get_param( "torso_controller/joint_names") elif rospy.has_param("torso_controller/joints"): # simulation self.torso_joint_names = rospy.get_param("torso_controller/joints") else: print "Could not get joint names for torso from parameter server. exiting..." exit(-1) # CvBridge self.bridge = CvBridge() # initialize private storage self._arm_joint_msg_received = False self._arm_joint_msg = None self._torso_joint_msg_received = False self._torso_joint_msg = None self._left = {} self._left_received = False self._right = {} self._right_received = False self._kinect_rgb = {} self._kinect_rgb_received = False self.counter = 1 # init publisher / subscriber self._robot_measurement_pub = rospy.Publisher("/robot_measurement", RobotMeasurement) self._image_pub_left = rospy.Publisher("/robot_measurement_image_left", Image) #DEBUG self._image_pub_right = rospy.Publisher( "/robot_measurement_image_right", Image) #DEBUG self._image_pub_kinect_rgb = rospy.Publisher( "/robot_measurement_image_kinect_rgb", Image) #DEBUG self._sub_joint_states = rospy.Subscriber("/joint_states", JointState, self._callback_joints) # left camera self._sub_left_info = message_filters.Subscriber( "/stereo/left/camera_info", CameraInfo) self._sub_left_image_color = message_filters.Subscriber( "/stereo/left/image_rect_color", Image) self._sub_left_image_rect = message_filters.Subscriber( "/stereo/left/image_rect", Image) self._sub_left = message_filters.TimeSynchronizer([ self._sub_left_info, self._sub_left_image_color, self._sub_left_image_rect ], 15) self._sub_left.registerCallback(self._callback_left) # right camera self._sub_right_info = message_filters.Subscriber( "/stereo/right/camera_info", CameraInfo) self._sub_right_image_color = message_filters.Subscriber( "/stereo/right/image_rect_color", Image) self._sub_right_image_rect = message_filters.Subscriber( "/stereo/right/image_rect", Image) self._sub_right = message_filters.TimeSynchronizer([ self._sub_right_info, self._sub_right_image_color, self._sub_right_image_rect ], 15) self._sub_right.registerCallback(self._callback_right) # kinect rgb self._sub_kinect_rgb_info = message_filters.Subscriber( "/cam3d/rgb/camera_info", CameraInfo) self._sub_kinect_rgb_image_color = message_filters.Subscriber( "/cam3d/rgb/image_color", Image) self._sub_kinect_rgb = message_filters.TimeSynchronizer( [self._sub_kinect_rgb_info, self._sub_kinect_rgb_image_color], 15) self._sub_kinect_rgb.registerCallback(self._callback_kinect_rgb) print "==> done with initialization" def _callback_left(self, camera_info, image_color, image_rect): ''' Callback function for left camera message filter ''' #print "DEBUG: callback left" self._left["camera_info"] = camera_info self._left["image_color"] = image_color self._left["image_rect"] = image_rect # if self._left_received == False: # print "--> left sample received (this only prints once!)" self._left_received = True def _callback_right(self, camera_info, image_color, image_rect): ''' Callback function for right camera message filter ''' #print "DEBUG: callback right" self._right["camera_info"] = camera_info self._right["image_color"] = image_color self._right["image_rect"] = image_rect # if self._right_received == False: # print "--> right sample received (this only prints once!)" self._right_received = True def _callback_kinect_rgb(self, camera_info, image_color): ''' Callback function for kinect rgb message filter ''' #print "DEBUG: callback kinect_rgb" self._kinect_rgb["camera_info"] = camera_info self._kinect_rgb["image_color"] = image_color # if self._kinect_rgb_received == False: # print "--> kinect sample received (this only prints once!)" self._kinect_rgb_received = True def _callback_joints(self, msg): ''' Callback function for joint angles messages ''' #print "DEBUG: callback joints" # torso if self.torso_joint_names[0] in msg.name: pos = [] header = msg.header for name in self.torso_joint_names: pos.append(msg.position[msg.name.index(name)]) # create JointState message joint_msg = JointState() joint_msg.header = msg.header joint_msg.name = self.torso_joint_names joint_msg.position = pos # safe joint state msg self._torso_joint_msg = joint_msg # if self._torso_joint_msg_received == False: # print "--> torso joint state received (this only prints once!)" self._torso_joint_msg_received = True # arm if self.arm_joint_names[0] in msg.name: pos = [] header = msg.header for name in self.arm_joint_names: pos.append(msg.position[msg.name.index(name)]) # create JointState message joint_msg = JointState() joint_msg.header = msg.header joint_msg.name = self.arm_joint_names joint_msg.position = pos # safe joint state msg self._arm_joint_msg = joint_msg # if self._arm_joint_msg_received == False: # print "--> arm joint state received (this only prints once!)" self._arm_joint_msg_received = True def run(self): ''' Main method, starts service to provide capture functionality ''' rospy.sleep(1) # Start service srv = rospy.Service('/collect_data/capture', Capture, self._collect) rospy.loginfo( "service '/collect_data/capture' started, waiting for requests...") rospy.spin() def _collect(self, data): ''' Executed on service call. Logs and calls _capture_and_pub ''' rospy.loginfo("capturing sample %.2i" % self.counter) res = self._capture_and_pub("sample%.2i" % self.counter, CHECKERBOARD_NAME, CHECKERBOARD_CHAIN, CHECKERBOARD_PATTERN_SIZE, CHECKERBOARD_SQUARE_SIZE) self.counter += 1 return CaptureResponse(res) def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size): ''' Main capturing function. Gets a set of recent messages for all needed topics. Processes messages and creates RobotMeasuerment message which is published. @param sample_id: Sample identifier (e.g. sample01) @type sample_id: string @param target_id: Name of checkerboard (e.g. cb_9x6) @type target_id: string @param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain) @type chain_id: string @param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6)) @type pattern_size: tuple(x, y) @param square_size: Size of checkerboard sqaures (im m) @type square_size: float ''' # capture measurements # -------------------- self._left_received = False self._right_received = False self._kinect_rgb_received = False start_time = rospy.Time.now() while (not self._left_received or not self._right_received or not self._kinect_rgb_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._left_received: print "--> still waiting for sample from left" if not self._right_received: print "--> still waiting for sample from right" if not self._kinect_rgb_received: print "--> still waiting for sample from kinect" start_time = rospy.Time.now() latest_left = self._left latest_right = self._right latest_kinect_rgb = self._kinect_rgb self._torso_joint_msg_received = False self._arm_joint_msg_received = False start_time = rospy.Time.now() while (not self._torso_joint_msg_received or not self._arm_joint_msg_received): rospy.sleep(0.005) # print warning every 2 seconds if one of the messages is still missing... if start_time + rospy.Duration(2.0) < rospy.Time.now(): if not self._torso_joint_msg_received: print "--> still waiting for torso joint states" if not self._arm_joint_msg_received: print "--> still waiting for srm joint states" start_time = rospy.Time.now() latest_torso = self._torso_joint_msg latest_arm = self._arm_joint_msg # set up checkerboard and checkerboard detector # --------------------------------------------- checkerboard = Checkerboard(pattern_size, square_size) checkerboard_detector = CheckerboardDetector(checkerboard) # detect cb left # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: left" img_points_left = [] for (x, y) in corners.reshape(-1, 2): img_points_left.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg left # ---------------------- cam_msg_left = CameraMeasurement() cam_msg_left.camera_id = "left" cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp cam_msg_left.cam_info = latest_left["camera_info"] cam_msg_left.image_points = img_points_left cam_msg_left.verbose = False #cam_ms_leftg.image = latest_left["image_color"] #cam_msg_left.image_rect = latest_left["image_rect"] #cam_msg_left.features = # Not implemented here # detect cb right # -------------- cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: right" img_points_right = [] for (x, y) in corners.reshape(-1, 2): img_points_right.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg right # ----------------------- cam_msg_right = CameraMeasurement() cam_msg_right.camera_id = "right" cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp cam_msg_right.cam_info = latest_right["camera_info"] cam_msg_right.image_points = img_points_right cam_msg_right.verbose = False #cam_msg_right.image = latest_right["image_color"] #cam_msg_right.image_rect = latest_right["image_rect"] #cam_msg_right.features = # Not implemented here # detect cb kinect_rgb # -------------------- cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8") image = cv2util.cvmat2np(cvImage) corners = checkerboard_detector.detect_image_points(image, is_grayscale=True) if corners != None: print "cb found: kinect_rgb" img_points_kinect_rgb = [] for (x, y) in corners.reshape(-1, 2): img_points_kinect_rgb.append(ImagePoint(x, y)) else: # cb not found return False # create camera msg kinect_rgb # ---------------------------- cam_msg_kinect_rgb = CameraMeasurement() cam_msg_kinect_rgb.camera_id = "kinect_rgb" cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb[ "camera_info"].header.stamp cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"] cam_msg_kinect_rgb.image_points = img_points_kinect_rgb cam_msg_kinect_rgb.verbose = False #cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"] #cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"] #cam_msg_kinect_rgb.features = # Not implemented here # create torso_chain msg # ---------------------- torso_chain_msg = ChainMeasurement() torso_chain_msg.header = latest_torso.header torso_chain_msg.chain_id = "torso_chain" torso_chain_msg.chain_state = latest_torso # create arm_chain msg # -------------------- arm_chain_msg = ChainMeasurement() arm_chain_msg.header = latest_arm.header arm_chain_msg.chain_id = "arm_chain" arm_chain_msg.chain_state = latest_arm # DEBUG publish pic # ----------------- self._image_pub_left.publish(latest_left["image_color"]) self._image_pub_right.publish(latest_right["image_color"]) self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"]) # create robot measurement msg and publish # ----------------- robot_msg = RobotMeasurement() robot_msg.sample_id = sample_id robot_msg.target_id = target_id robot_msg.chain_id = chain_id robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb] robot_msg.M_chain = [torso_chain_msg, arm_chain_msg] self._robot_measurement_pub.publish(robot_msg) return True
bag_file_name = arguments[0] if arguments_size > 1: topic_name = arguments[1] if arguments_size > 2: directory = arguments[2] # Open the bag file: bag = rosbag.Bag(bag_file_name) bridge = CvBridge() # Sweep through messages that match *topic_name*: index = 0 previous_time = None for topic, msg, t in bag.read_messages(topic_name): # Extract *cv_image* in RGB8 mode: index += 1 try: cv_image = bridge.imgmsg_to_cv(msg, desired_encoding="rgb8") except CvBridgeError, e: print e # Save the image cv.SaveImage("{0}/Image{1:03d}.pnm".format(directory, index), cv_image) # Print out time changes: if previous_time == None: previous_time = t dt = t - previous_time previous_time = t print("[{0}]:{1}".format(index, dt))
class cvBridgeDemo(): def __init__(self): self.node_name = "cv_bridge_demo" rospy.init_node(self.node_name) # What we do during shutdown rospy.on_shutdown(self.cleanup) # Create the OpenCV display window for the RGB image self.cv_window_name = self.node_name cv.NamedWindow(self.cv_window_name, cv.CV_WINDOW_NORMAL) cv.MoveWindow(self.cv_window_name, 25, 75) # And one for the depth image self.target_visible = False self.tl = tf.TransformListener() self.xt = -1 self.yt = -1 self.w = -1 self.i = -1 self.j = -1 self.k = -1 self.yaw = -1; self.roll= -1; self.pitch = -1; self.MarkId = -1; # Create the cv_bridge object self.bridge = CvBridge() # Subscribe to the camera image and depth topics and set # the appropriate callbacks self.image_sub = rospy.Subscriber("/camera/rgb/image_color", Image, self.image_callback) self.ar_sub = rospy.Subscriber('ar_pose_marker', AlvarMarkers, self.doSomething) self.is_visible = False; rospy.loginfo("Waiting for image topics...") def doSomething(self, msg): """ Try """ try: rospy.loginfo("trying") rospy.loginfo("tags detected: %d", len(msg.markers)) for tag in (msg.markers): rospy.loginfo("%f, %f, %f",tag.pose.pose.position.x, tag.pose.pose.position.y, tag.pose.pose.position.z); marker = msg.markers[0] #rospy.loginfo(marker.id) if not self.is_visible: rospy.loginfo("marker detected!!") self.is_visible = True; tX = marker.pose.pose.position.x tY = marker.pose.pose.position.y if sqrt((self.xt - tX)**2 + (self.yt - tY)**2) < 0.05: rospy.loginfo("distance from prev: %f", sqrt((self.xt - tX)**2 + (self.yt - tY)**2)) #preventing from relocalising the target position return self.xt = marker.pose.pose.position.x self.yt = marker.pose.pose.position.y w = marker.pose.pose.orientation.w i = marker.pose.pose.orientation.x j = marker.pose.pose.orientation.y k = marker.pose.pose.orientation.z self.yaw = atan2(2*j*w - 2*i*k, 1 - 2*j*j - 2*k*k)*180/pi self.pitch = asin(2*i*j + 2*k*w)*180/pi self.roll = atan2(2*i*w - 2*j*k, 1 - 2*i*i - 2*k*k)*180/pi rospy.loginfo("position @ (%f, %f)", self.xt, self.yt) rospy.loginfo("yaw, pitch, roll @ (%f, %f, %f)", self.yaw, self.pitch, self.roll) if self.yaw < 110 and self.yaw > 70 and self.roll > -10 and self.roll < 10: dx = 0.5; dy = self.yt; # q = Quaternion(0, 0, 0.7071, 0.7071) rospy.loginfo("!!tag faceing along x-axis!!") elif self.yaw < -70 and self.yaw > -110 and self.roll > -10 and self.roll < 10: dx = -0.5; dy = self.yt; # q = Quaternion(0, 0, 0.7071, 0.7071) rospy.loginfo("!!tag faceing against x-axis!!") elif self.yaw < 110 and self.yaw > 70 and self.roll < -70 and self.roll > -110: dx = self.xt; dy = 0.5; # q = Quaternion(0, 0, 0.7071, 0.7071) rospy.loginfo("!!tag faceing along y-axis!!") elif self.yaw < 110 and self.yaw > 70 and self.roll < 110 and self.roll > 70: dx = self.xt; dy = -0.5; # q = Quaternion(0, 0, 0.7071, 0.7071) rospy.loginfo("!!tag faceing against y-axis!!") self.MarkId = marker.id if marker.id == 3: """ try rotating according to the tag """ except: if self.is_visible: rospy.loginfo("marker not confirmed!!") self.is_visible = False return def image_callback(self, ros_image): # Use cv_bridge() to convert the ROS image to OpenCV format try: frame = self.bridge.imgmsg_to_cv(ros_image, "bgr8") except CvBridgeError, e: print e # Convert the image to a Numpy array since most cv2 functions # require Numpy arrays. frame = np.array(frame, dtype=np.uint8) # Process the frame using the process_image() function frame = self.process_image(frame) font_face = cv2. FONT_HERSHEY_PLAIN font_scale = 1 display_image = frame.copy() cv2.putText(display_image, "Marker@:" , (10,20),font_face, font_scale, cv.RGB(0, 0, 255) ) cv2.putText(display_image, "x: " + str(self.xt) , (10,40),font_face, font_scale, cv.RGB(0, 255, 0) ) cv2.putText(display_image, "y: " + str(self.yt), (10,54),font_face, font_scale, cv.RGB(0, 255, 0) ) cv2.putText(display_image, "Ort: " + str(self.yaw) + ", " + str(self.pitch) + ", " + str(self.roll), (10,68),font_face, font_scale, cv.RGB(0, 255, 0) ) cv2.putText(display_image, "iD: " + str(self.MarkId), (10,82),font_face, font_scale, cv.RGB(0, 255, 0) ) # cv2.putText(display_image, "FPS: " + str(self.fps), (10, 20), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, "Keyboard commands:", (20, int(self.frame_size[0] * 0.6)), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, " ", (20, int(self.Markx * 0.65)), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, "space - toggle pause/play", (20, int(self.frame_size[0] * 0.72)), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, " r - restart video from beginning", (20, int(self.frame_size[0] * 0.79)), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, " t - hide/show this text", (20, int(self.frame_size[0] * 0.86)), font_face, font_scale, cv.RGB(255, 255, 0)) # cv2.putText(display_image, " q - quit the program", (20, int(self.frame_size[0] * 0.93)), font_face, font_scale, cv.RGB(255, 255, 0)) # Merge the original image and the display image (text overlay) display_image = cv2.bitwise_or(frame, display_image) # Display the image. cv2.imshow(self.node_name, display_image) # Process any keyboard commands self.keystroke = cv.WaitKey(5) if 32 <= self.keystroke and self.keystroke < 128: cc = chr(self.keystroke).lower() if cc == 'q': # The user has press the q key, so exit rospy.signal_shutdown("User hit q key to quit.")
class video_processor: def __init__(self): self.sub = rospy.Subscriber('usb_cam/image_raw', Image, self.callback) self.pub = rospy.Publisher('heading', Twist) self.speed = float(1) self.bridge = CvBridge() cv.NamedWindow("Input Video") #cv.NamedWindow("Blur Video") #cv.NamedWindow("HSV Video") #cv.NamedWindow("Hue Video") #cv.NamedWindow("Saturation Video") #cv.NamedWindow("Value Video") cv.NamedWindow("Red-Orange Video") cv.NamedWindow("White Video") cv.NamedWindow("Red-Orange and White Video") #cv.WaitKey(0) def callback(self, image_in): try: input_image = self.bridge.imgmsg_to_cv(image_in,"bgr8") except CvBridgeError, e: print e cv.ShowImage("Input Video", input_image) blur_image = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC3) cv.Smooth(input_image,blur_image,cv.CV_BLUR, 10, 10) #cv.ShowImage("Blur Video", proc_image) proc_image = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC3) cv.CvtColor(blur_image, proc_image, cv.CV_BGR2HSV) #cv.ShowImage("HSV Video", proc_image) split_image = [cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1),cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1),cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1)] cv.Split(proc_image, split_image[0],split_image[1],split_image[2], None ) #hue = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) #sat = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) #val = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) #cv.Split(proc_image, hue,sat,val, None ) #cv.ShowImage("Hue Video", hue) #cv.ShowImage("Saturation Video", sat) #cv.ShowImage("Value Video", val) thresh_0 = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) thresh_1 = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) thresh_2 = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) red_orange = cv.CreateMat(input_image.rows,input_image.cols,cv.CV_8UC1) cv.Threshold(split_image[1],thresh_0, 128,255,cv.CV_THRESH_BINARY) # > 50% saturation cv.Threshold(split_image[0],thresh_1, 220,255,cv.CV_THRESH_BINARY) # > Purple cv.Threshold(split_image[0],thresh_2, 10, 255,cv.CV_THRESH_BINARY_INV) # < Yellow-Orange cv.Add(thresh_1,thresh_2,red_orange) cv.And(red_orange,thresh_0,red_orange) cv.ShowImage("Red-Orange Video",red_orange) cv.CvtColor(blur_image, proc_image, cv.CV_BGR2HLS) cv.Split(proc_image, split_image[0], split_image[1],split_image[2], None ) cv.Threshold(split_image[1],thresh_0, 204,255,cv.CV_THRESH_BINARY) # > 80% Lum cv.ShowImage("White Video",thresh_0) cv.Or(red_orange, thresh_0, thresh_0) cv.ShowImage("Red-Orange and White Video",thresh_0) cv.WaitKey(30) ang_z = 0 x = 0 for i in range(input_image.rows): y = -(input_image.cols / 2) row = cv.GetRow(thresh_0,i) for j in row.tostring(): ang_z = ang_z + (x * y *ord(j)) y = y + 1 x = x + 1 ang_z = (ang_z * pi * 2 * 2 * 4 / 255 / input_image.rows / input_image.rows / input_image.cols / input_image.cols) p = Twist() p.linear.x = self.speed p.angular.z = ang_z self.pub.publish(p)
class HistAnalyzer: def __init__(self, background_noise, mask, topic='/playpen_kinect/rgb/image_color'): rospy.Subscriber(topic, Image, self.get_img) self.check = rospy.Service("playpen_check_success_hist", Check, self.serv_success) self.train_empty = rospy.Service("playpen_train_hist", Train, self.serv_train) self.background_noise = background_noise self.h_bins = 30 self.s_bins = 32 self.h_ranges = [0, 180] self.s_ranges = [0, 255] self.ranges = [self.h_ranges, self.s_ranges] self.hist = None self.mask = mask self.avg_noise = None self.online_img = None self.bridge = CvBridge() size = cv.GetSize(background_noise[0]) self.IavgF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.IdiffF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.IprevF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.IhiF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.IlowF = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ilow1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ilow2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ilow3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ihi1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ihi2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Ihi3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) cv.Zero(self.IavgF) cv.Zero(self.IdiffF) cv.Zero(self.IprevF) cv.Zero(self.IhiF) cv.Zero(self.IlowF) self.Icount = 0.00001 self.Iscratch = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Iscratch2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Igray1 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Igray2 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Igray3 = cv.CreateImage(size, cv.IPL_DEPTH_32F, 1) self.Imaskt = cv.CreateImage(size, cv.IPL_DEPTH_8U, 1) cv.Zero(self.Iscratch) cv.Zero(self.Iscratch2) self.first = 1 def accumulateBackground(self, img): cv.CvtScale(img, self.Iscratch, 1, 0) if (not self.first): cv.Acc(self.Iscratch, self.IavgF) cv.AbsDiff(self.Iscratch, self.IprevF, self.Iscratch2) cv.Acc(self.Iscratch2, self.IdiffF) self.Icount += 1.0 self.first = 0 cv.Copy(self.Iscratch, self.IprevF) def setHighThresh(self, thresh): cv.ConvertScale(self.IdiffF, self.Iscratch, thresh) cv.Add(self.Iscratch, self.IavgF, self.IhiF) #cv.Split(self.IhiF, self.Ihi1, self.Ihi2, self.Ihi3, None) def setLowThresh(self, thresh): cv.ConvertScale(self.IdiffF, self.Iscratch, thresh) cv.Sub(self.IavgF, self.Iscratch, self.IlowF) #cv.Split(self.IlowF, self.Ilow1, self.Ilow2, self.Ilow3, None) def createModelsfromStats(self): cv.ConvertScale(self.IavgF, self.IavgF, float(1.0 / self.Icount)) cv.ConvertScale(self.IdiffF, self.IdiffF, float(1.0 / self.Icount)) cv.AddS(self.IdiffF, cv.Scalar(1.0, 1.0, 1.0), self.IdiffF) self.setHighThresh(200.0) self.setLowThresh(200.0) def backgroundDiff(self, img, Imask): print "got into backgroundDiff" cv.CvtScale(img, self.Iscratch, 1, 0) #cv.Split(self.Iscratch, self.Igray1, self.Igray2, self.Igray3, None) #cv.InRange(self.Igray1, self.Ilow1, self.Ihi1, Imask) cv.InRange(self.Iscratch, self.IlowF, self.IhiF, Imask) # cv.InRange(self.Igray2, self.Ilow2, self.Ihi2, self.Imaskt) # cv.Or(Imask, self.Imaskt, Imask) # cv.InRange(self.Igray3, self.Ilow3, self.Ihi3, self.Imaskt) # cv.Or(Imask, self.Imaskt, Imask) cv.SubRS(Imask, 255, Imask) #cv.SaveImage('/home/mkillpack/Desktop/mask.png', Imask) #cv.Erode(Imask, Imask) return Imask def get_img(self, msg): try: self.online_img = self.bridge.imgmsg_to_cv(msg, "bgr8") except CvBridgeError, e: print e
#Come up with an output file root name outfileroot = "./" + os.path.basename(bagfile).split('.')[0] #Load the yaml output of calling rosbag info on the input file. fileInfo = yaml.load( subprocess.Popen(['rosbag', 'info', '--yaml', bagfile], stdout=subprocess.PIPE).communicate()[0]) #Create a cv bridge to convert the image bridge = CvBridge() #Read the messages bag = rosbag.Bag(bagfile) totalMotion = 0 #Start at zero startTime = None with open(outfileroot + "_cup.csv", 'w') as outfile: outfile.write("StartTime,Motion\n") for topic, msg, t in bag.read_messages(topics=['/gscam/image_raw']): if startTime is None: startTime = t try: cvImage = bridge.imgmsg_to_cv(msg, "bgr8") except CvBridgeError, e: print e timestamp = str((t - startTime).to_sec()) location = getRedObject(cvImage, timestamp) # motions = msg.move.states # totalMotion += motions[1] #The Y axis, left or right on the arm outfile.write(timestamp + "," + str(location[0]) + "," + str(location[1]) + "\n")