def img_callback(self, left_msg, right_msg): rospy.loginfo(" Received Image message") ##Decoding image ## left_np_arr = np.fromstring(left_msg.data, np.uint8) left_image_np = cv2.imdecode(left_np_arr, cv2.IMREAD_GRAYSCALE) right_np_arr = np.fromstring(right_msg.data, np.uint8) right_image_np = cv2.imdecode(right_np_arr, cv2.IMREAD_GRAYSCALE) ##Contrast Enhancement ## left_enhanced_img = self.histenhance.apply(left_image_np) right_enhanced_img = self.histenhance.apply(right_image_np) #### Create CompressedIamge #### enhanced_left_msg = CompressedImage() enhanced_left_msg.header.stamp = left_msg.header.stamp enhanced_left_msg.format = "jpeg" enhanced_left_msg.data = np.array( cv2.imencode('.jpg', left_enhanced_img)[1]).tostring() enhanced_right_msg = CompressedImage() enhanced_right_msg.header.stamp = right_msg.header.stamp enhanced_right_msg.format = "jpeg" enhanced_right_msg.data = np.array( cv2.imencode('.jpg', right_enhanced_img)[1]).tostring() # Publish new image self.left_image_pub.publish(enhanced_left_msg) self.right_image_pub.publish(enhanced_right_msg)
def main(): cap = cv2.VideoCapture(int(sys.argv[1])) cap.set(cv2.CAP_PROP_CONVERT_RGB, False) image_pub_left = rospy.Publisher("/camera/left/compressed", CompressedImage, queue_size=10) image_pub_right = rospy.Publisher("/camera/right/compressed", CompressedImage, queue_size=10) rospy.init_node('image_feature', anonymous=True) rate = rospy.Rate(40) while not rospy.is_shutdown(): ret, frame = cap.read() left = frame[:, :, 0] right = frame[:, :, 1] #### Create CompressedIamge #### msg_left = CompressedImage() msg_right = CompressedImage() msg_left.header.stamp = rospy.Time.now() msg_right.header.stamp = rospy.Time.now() msg_left.format = "jpeg" msg_right.format = "jpeg" msg_left.data = np.array(cv2.imencode('.jpg', left)[1]).tostring() msg_right.data = np.array(cv2.imencode('.jpg', right)[1]).tostring() # Publish new image image_pub_left.publish(msg_left) image_pub_right.publish(msg_right) rate.sleep()
def image_callback(self, ros_data): if self.input_compressed: np_arr = np.fromstring(ros_data.data, np.uint8) img_input = cv2.imdecode(np_arr, 1) else: img_input = bridge.imgmsg_to_cv2(ros_data) with Tick('prediction'): img_resized = self.warpper.resize_keeping_aspect_ratio(img_input) semantic = self.warpper.predict( self.warpper.project(img_resized))[0] detections = self.warpper.resize_back( voc.get_label_colormap(semantic)) print(voc.semantic_report(semantic), end='') overlap = cv2.addWeighted(detections, 0.5, img_input, 0.5, 20) cv2.imshow('overlap', overlap) cv2.waitKey(1) if self.output_compressed: msg = CompressedImage() msg.header = ros_data.header msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', detections)[1]).tostring() self.pub.publish(msg) else: msg = Image() msg.header = ros_data.header msg.data = detections.tostring() self.pub.publish(msg)
def _image_cb(self, imageMsg): # Load in image np_arr = np.fromstring(imageMsg.data, np.uint8) original_img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # Process image canny_mask = self._canny_filter(original_img) #filtered_img = canny_mask temp_img = cv2.bitwise_and(original_img, original_img, mask=canny_mask) filtered_img = self._hsv_vision_image_filter(temp_img) # Publish image after filtering imgFMsg = CompressedImage() imgFMsg.header.stamp = rospy.Time.now() imgFMsg.format = "jpeg" imgFMsg.data = np.array(cv2.imencode('.jpg', filtered_img)[1]).tostring() self.img_f_pub.publish(imgFMsg) # detect bloks from image final_img, poses = self._contour_based_detection( temp_img, filtered_img) # Publish object poses poseMsg = BlockPose2DArray() poseMsg.blocks = poses self.pose_pub.publish(poseMsg) # Publish final image imgCMsg = CompressedImage() imgCMsg.header.stamp = rospy.Time.now() imgCMsg.format = "jpeg" imgCMsg.data = np.array(cv2.imencode('.jpg', final_img)[1]).tostring() self.img_c_pub.publish(imgCMsg)
def pub_img(): '''Callback function of subscribed topic. Here images get converted and features detected''' global stamp # cap = cv2.VideoCapture(1) pipe = rs.pipeline() config = rs.config() width = 640; height = 480; config.enable_stream(rs.stream.depth, width, height, rs.format.z16, 30) config.enable_stream(rs.stream.color, width, height, rs.format.rgb8, 30) profile = pipe.start(config) align_to = rs.stream.color align = rs.align(align_to) pub_rgb = rospy.Publisher("/output/image_raw/compressed_rgb", CompressedImage, queue_size=1) pub_depth = rospy.Publisher("/output/image_raw/compressed_depth", CompressedImage, queue_size=1) print('Hey! I started publishing images.') while(True): temp = pipe.wait_for_frames() aligned_frames = align.process(temp) aligned_depth_frame = aligned_frames.get_depth_frame() # aligned_depth_frame is a 640x480 depth image color_frame = aligned_frames.get_color_frame() if not aligned_depth_frame or not color_frame: pass buffer = np.zeros((height,width,4)) buffer[:,:,0:3] = (np.asanyarray(color_frame.get_data(),dtype=np.uint8)) buffer[:,:,3] = np.asanyarray(aligned_depth_frame.get_data(),dtype=np.uint8) #### Create CompressedImage #### msg_rgb = CompressedImage() msg_depth = CompressedImage() msg_rgb.header.stamp = rospy.Time.now() msg_depth.header.stamp = msg_rgb.header.stamp#rospy.Time.now() msg_rgb.format = "rgb"#f'{stamp}' msg_depth.format = "depth" msg_rgb.data = np.array(cv2.imencode('.jpg', buffer[:,:,0:3])[1]).tostring() msg_depth.data = np.array(cv2.imencode('.jpg', buffer[:,:,3])[1]).tostring() # Publish depth frame # pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1) # r = rospy.Rate(10) # try: pub_rgb.publish(msg_rgb) pub_depth.publish(msg_depth) # r.sleep() # except: # print('Could not publish') stamp += 1 if stamp >= 100000: stamp = 0
def procesarImagen_(self, imagen_entrada): np_array = np.fromstring(imagen_entrada.data, np.uint8) np_image = cv2.imdecode(np_array, cv2.IMREAD_COLOR) # Crear y pulicar imagen procesada msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', np_image)[1]).tostring() self.pub_imagen_procesada.publish(msg) # Detecciones ## Deteccion de bordes ## imagen_bordes = cv2.Canny(np_image, 100, 200) # Crear y pulicar imagen de bordes msg2 = CompressedImage() msg2.header.stamp = rospy.Time.now() msg2.format = "jpeg" msg2.data = np.array(cv2.imencode('.jpg', imagen_bordes)[1]).tostring() self.pub_detecciones.publish(msg2) ## Deteccion de colores ## imagen_color = cv2.resize(np_image, (128, 96), interpolation=cv2.INTER_AREA) hsv_img = cv2.cvtColor(imagen_color, cv2.COLOR_RGB2HSV) #Se extraen por separado cada color en distintas mascaras blanco = cv2.inRange(hsv_img, BLANCO_MIN, BLANCO_MAX) amarillo = cv2.inRange(hsv_img, AMARILLO_MIN, AMARILLO_MAX) rojo1 = cv2.inRange(hsv_img, ROJO_MIN, ROJO_MAX) rojo2 = cv2.inRange(hsv_img, ROJO2_MIN, ROJO2_MAX) rojo = rojo1 + rojo2 resultado = blanco + amarillo + rojo #Resultados resultado_amarillo = cv2.bitwise_and(imagen_color, imagen_color, mask=amarillo) resultado_blanco = cv2.bitwise_and(imagen_color, imagen_color, mask=blanco) resultado_rojo = cv2.bitwise_and(imagen_color, imagen_color, mask=rojo) resultado_final = cv2.bitwise_and(imagen_color, imagen_color, mask=resultado) # Crear y pulicar imagen de colores detectados msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', resultado_final)[1]).tostring() self.pub_detecciones_color.publish(msg)
def callback_right_with_3D(self, ros_data): '''Callback function of subscribed topic. Here images get converted and features detected''' self.right_header = ros_data.header.stamp.secs #### direct conversion to CV2 #### np_arr = np.fromstring(ros_data.data, np.uint8) image_np = cv2.imdecode(np_arr, 1) # convert np image to grayscale gray_image = cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY) if self.right_header == self.left_header and self.group_4_recieved: s_t = time.clock() print self.right_header, self.left_header self.group_4_recieved = False left_gray = cv2.cvtColor(self.left_image, cv2.COLOR_BGR2GRAY) group_4_out = self.group_4 # PointCloudPose() # group_4_out.header = self.group_4.header #std_msgs/Int16 pose_id #std_msgs/Int16 pose_id_max #sensor_msgs/CompressedImage image_left #sensor_msgs/CompressedImage image_right #geometry_msgs/Pose spin_center_pose #sensor_msgs/PointCloud2 carmine_pointcloud #geometry_msgs/Pose carmine_pose #sensor_msgs/PointCloud2 bumblebee_pointcloud #geometry_msgs/Pose bumblebee_pose_left #geometry_msgs/Pose bumblebee_pose_right msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', left_gray)[1]).tostring() # Publish new image group_4_out.image_left = msg msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', gray_image)[1]).tostring() # Publish new image group_4_out.image_right = msg #group_4_out.bumblebee_pointcloud = pcloud self.pub_group.publish(group_4_out) print "save time", time.clock() - s_t
def callback(self, ros_data): '''Callback function of subscribed topic. Here images get converted and features detected''' if VERBOSE: print 'received image of type: "%s"' % ros_data.format #### direct conversion to CV2 #### np_arr = np.fromstring(ros_data.data, np.uint8) # image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0: if self.line == None: self.line = Line(img) time1 = rospy.Time.now() try: cte, outImg, roiImg = self.line.find_offset(img) except: cte = 0 outImg = img roiImg = img time2 = rospy.Time.now() dur = time2 - time1 # if VERBOSE : # print '%s detector found: %s points in: %s sec.'%(method, # len(featPoints),time2-time1) # Debug info rospy.loginfo("Cross track error: %s", cte) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', outImg)[1]).tostring() # Publish new image self.image_pub.publish(msg) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', roiImg)[1]).tostring() # Publish new image self.image_roi_pub.publish(msg) twist = Twist() twist.linear.x = 1.0 twist.angular.z = -cte * 0.015 self.twist_pub.publish(twist)
def _Img_callback(ros_data): global center, size, flage, pub_detections np_arr = np.fromstring(ros_data.data, np.uint8) frame = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) try: # while True: # window_name = 'Image' if flage == True: flage = False for ii in range(len(center[0, :])): left = int((center[0, ii] - size[0, ii] / 2.0)) right = int((center[0, ii] + size[0, ii] / 2.0)) top = int((center[1, ii] - size[1, ii] / 2.0)) bottom = int((center[1, ii] + size[1, ii] / 2.0)) start_point = (left, top) end_point = (right, bottom) color = (255, 0, 0) thickness = 2 image = cv2.rectangle(frame, start_point, end_point, color, thickness) circle_thickness = -1 radius = 15 circle_center = (int(center[0, ii]), int(center[1, ii])) image = cv2.circle(frame, circle_center, radius, color, circle_thickness) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image)[1]).tostring() pub_detections.publish(msg) rospy.sleep(0.2) # cv2.imshow(window_name, image) # cv2.waitKey(1) # #cv2.destroyAllWindows() # break else: msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring() pub_detections.publish(msg) except: msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring() pub_detections.publish(msg)
def publish(self, raw_img): msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode(".jpg", raw_img)[1]).tostring() # TODO compile sensor_msg error , use String instead self.image_pub.publish(msg.data)
def __init__(self): # Get the ~private namespace parameters from command line or launch file. self.UDP_IP = rospy.get_param('~UDP_IP', '192.168.50.37') #get_ip.get_local() #if self.UDP_IP != 'localhost': # self.UDP_IP = int(self.UDP_IP) self.UDP_PORT = int(rospy.get_param('~UDP_PORT', '49155')) # Create a dynamic reconfigure server. #self.server = DynamicReconfigureServer(ConfigType, self.reconfigure) # create ros::Publisher to send Odometry messages cam_pub = rospy.Publisher('image_raw/compressed', CompressedImage) #crappy test t = True while not rospy.is_shutdown(): #crappy test continued if t: print "Camera Running" t = False udp_string = self.receive_packet() jpg_string = self.parse_string(udp_string) cam_msg = CompressedImage() cam_msg.header.stamp = rospy.Time.now() #YOYOYOYO - should be from robot time cam_msg.format = "jpeg" cam_msg.data = jpg_string cam_pub.publish(cam_msg)
def cv2_to_compressed_imgmsg(cv_image): # Create CompressedImage msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', cv_image)[1]).tostring() return msg
def grabAndPublish(self,stream,publisher): while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown(): yield stream # Construct image_msg # Grab image from stream stamp = rospy.Time.now() stream.seek(0) stream_data = stream.getvalue() # Generate compressed image image_msg = CompressedImage() image_msg.format = "jpeg" image_msg.data = stream_data image_msg.header.stamp = stamp image_msg.header.frame_id = self.frame_id publisher.publish(image_msg) # Clear stream stream.seek(0) stream.truncate() if not self.has_published: rospy.loginfo("[%s] Published the first image." %(self.node_name)) self.has_published = True rospy.sleep(rospy.Duration.from_sec(0.001))
def ros_compressed_from_numpygray(np_arr): msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" np_arr = 255*(1.0*np_arr/np.max(np_arr)) msg.data = np.array(cv2.imencode('.jpg', np.array(np_arr, dtype = np.uint8))[1]).tostring() return msg
def webcam_run(): """ Run people detection on webcam and publish the data """ video_capture = cv2.VideoCapture(0) ros_publisher = rospy.Publisher("/people_detection", String, queue_size=10) processed_image = rospy.Publisher("/processed_image", CompressedImage, queue_size=10) people_detect = PeopleDetection() while (True): # Get image from webcam ret, frame = video_capture.read() details = people_detect.detect(frame) if len(details) == 0: continue rospy.loginfo(details) ros_publisher.publish(json.dumps(details)) # frame = cv2.resize(frame, (0,0), fx=0.3, fy=0.3) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring() processed_image.publish(msg)
def publish_debug_image(self, img, pub): img_msg = CompressedImage() img_msg.header.stamp = rospy.Time.now() img_msg.format = ".jpeg" img_msg.data = RiptideVision().compressed_img_msg_data(".jpeg", img) pub.publish(img_msg)
def callback(self, ros_data): #### direct conversion to CV2 #### np_arr = np.fromstring(ros_data.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) #(rows,cols,channels) = image_np.shape #image_np = image_np[0:64, 0:480] #equ = cv2.equalizeHist(image_np) #res = np.hstack((image_np,equ)) #### Feature detectors using CV2 #### # "","Grid","Pyramid" + # "FAST","GFTT","HARRIS","MSER","ORB","SIFT","STAR","SURF", "GridFAST" #method = "GridFAST" #feat_det = cv2.FeatureDetector_create(method) #time1 = time.time() # convert np image to grayscale #featPoints = feat_det.detect( cv2.cvtColor(image_np, cv2.COLOR_BGR2GRAY)) #time2 = time.time() #if VERBOSE : # print '%s detector found: %s points in: %s sec.'%(method, len(featPoints),time2-time1) #for featpoint in featPoints: # x,y = featpoint.pt # cv2.circle(image_np,(int(x),int(y)), 3, (0,0,255), -1) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring() # Publish new image self.image_pub.publish(msg)
def zed_camera(): rospy.init_node("camera_publisher", anonymous=True) rate = rospy.Rate(10) device = rospy.get_param("~device") ns = rospy.get_namespace() cap = cv2.VideoCapture(device) publisher_left = rospy.Publisher(ns + "left_image", CompressedImage, queue_size=10) publisher_right = rospy.Publisher(ns + "right_image", CompressedImage, queue_size=10) while not rospy.is_shutdown(): _, frame = cap.read() left_right_image = np.split(frame, 2, axis=1) for i in range(2): msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "png" msg.data = np.array(cv2.imencode('.png', cv2.resize(left_right_image[i],(672,188)))[1]).tostring() [publisher_left,publisher_right][i].publish(msg) rate.sleep()
def get_next_stream(self): """Generates a new 'stream' sequence and then waits for data. Once the data is received, it is Published and a new 'stream' sequence is generated.""" while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown( ): yield self.stream # publish a new 'stream' sequence to capture the next image into self.stream.seek( 0) # reset the stream to extract the captured image # Generate compressed image image_msg = CompressedImage() image_msg.header.stamp = rospy.Time.now() image_msg.header.frame_id = self.frame_id image_msg.format = "jpeg" image_msg.data = self.stream.getvalue() self.pub_img.publish(image_msg) # Clear stream self.stream.seek(0) self.stream.truncate() if not self.has_published: rospy.loginfo("[%s] Published the first image." % (self.node_name)) self.has_published = True rospy.sleep(rospy.Duration.from_sec(0.001))
def image_publisher(device_id=0, frame_rate=10, width=320, height=240, brightness=0.5): topic_name = "python_image" + str(device_id) node_name = 'python_image_publisher' + str(device_id) pub1 = rospy.Publisher(topic_name, Image, queue_size=10) pub2 = rospy.Publisher(topic_name + "/compressed", CompressedImage, queue_size=10) rospy.init_node(node_name) rate = rospy.Rate(frame_rate) cap = cv2.VideoCapture(device_id) cap.set(cv2.CAP_PROP_FRAME_WIDTH, width) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height) cap.set(cv2.CAP_PROP_FPS, frame_rate) cap.set(cv2.CAP_PROP_BRIGHTNESS, brightness) # cap.set(cv2.CAP_PROP_WHITE_BALANCE, False) br = CvBridge() while not rospy.is_shutdown(): status, img = cap.read() if status == True: pub1.publish(br.cv2_to_imgmsg(img)) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" # "jpeg" or "png" msg.data = np.array(cv2.imencode(".jpg", img)[1]).tostring() pub2.publish(msg) rate.sleep()
def main(): image_pub = rospy.Publisher("/output/image_raw/compressed", CompressedImage, queue_size=1) rospy.init_node('ROSPiCam') rate = rospy.Rate(100) # 10hz # Set publisher node to 15 Hz # Publisher node loop while not rospy.is_shutdown(): msg = CompressedImage() msg.format = "jpeg" # Open new BytesIO stream stream = io.BytesIO() # Capture frame from camera camera.capture(stream, format='jpeg') # camera.capture("/home/pi/temp.jpg msg.header.stamp = rospy.Time.now() msg.data = np.array(Image.open(stream)).tostring() # Publish image image_pub.publish(msg) # Close stream stream.close() rate.sleep()
def handle_pkt(pkt=None): ts_begin_loc = pkt.find(begin_timestamp_marker) ts_end_loc = pkt.find(end_timestamp_marker) if ts_begin_loc == -1 or ts_end_loc == -1: #something's fishy, discard jpeg print "JPEG discarded, malformed data" return ts = float(pkt[ts_begin_loc+len(begin_timestamp_marker):ts_end_loc]) # if ts > last_ts: # last_ts = ts # else: # return pkt = pkt[ts_end_loc+len(end_timestamp_marker):] print "{} bytes of JPEG recvd".format(len(pkt)) msg = CompressedImage() msg.header.stamp = rospy.Time(tango_clock_offset + float(ts)) msg.header.frame_id = camera_name msg.data = pkt msg.format = 'jpeg' pub_camera.publish(msg)
def subscribe_callback(data): # called whenever get subscribed data # get current time rospytimenow = rospy.Time.now() # CompressedImage (ros msg) to cv2 np_arr = np.fromstring(data.data, np.uint8) img = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # detect door pred, img_with_bboxresult = detector.detect_image(img) # convert img_with_bboxresult to ros msg img_msg = CompressedImage() img_msg.header.stamp = rospytimenow img_msg.format = "jpg" img_msg.data = np.array(cv2.imencode('.jpg', img_with_bboxresult)[1]).tostring() pub_img.publish(img_msg) # convert detection result (pred) to ros msg det_msg = BoundingBoxes() det_msg.header.stamp = rospytimenow bounding_boxes = [] for i, (x1, y1, x2, y2, conf) in enumerate(pred): one_det_msg = BoundingBox() one_det_msg.id = int(i) one_det_msg.xmin = int(x1) one_det_msg.xmax = int(x2) one_det_msg.ymin = int(y1) one_det_msg.ymax = int(y2) one_det_msg.probability = conf bounding_boxes.append(one_det_msg) det_msg.bounding_boxes = bounding_boxes pub_det.publish(det_msg)
def pubIm(im): img_show = np.array(im, np.uint8) msg1 = CompressedImage() msg1.format = "jpeg" msg1.header.stamp = rospy.Time.now() msg1.data = np.array(cv2.imencode('.jpg', img_show)[1]).tostring() pub.publish(msg1)
def grabAndPublish(self,stream,publisher): # Grab image from stream stream.seek(0) img_data = stream.getvalue() if self.uncompress: # Publish raw image data = np.fromstring(img_data, dtype=np.uint8) image = cv2.imdecode(data, 1) image_msg = self.bridge.cv2_to_imgmsg(image) else: # Publish compressed image only image_msg = CompressedImage() image_msg.data = img_data image_msg.format = "jpeg" image_msg.header.stamp = rospy.Time.now() # Publish publisher.publish(image_msg) # Clear stream stream.seek(0) stream.truncate() if not self.has_published: rospy.loginfo("[%s] Published the first image." %(self.node_name)) self.has_published = True
def _publish_img(self, obs, wait_for_callback=False): """ Publishes the image to the compressed_image topic, which triggers the lane following loop For training: if wait_for_callback is true, we'll also publish the camera info, and then wait until the action has been calculated """ # XXX: make this into a function (there were a few of these conversions around...) img_msg = CompressedImage() time = rospy.get_rostime() img_msg.header.stamp.secs = time.secs img_msg.header.stamp.nsecs = time.nsecs img_msg.format = "jpeg" contig = cv2.cvtColor(np.ascontiguousarray(obs), cv2.COLOR_BGR2RGB) img_msg.data = np.array(cv2.imencode(".jpg", contig)[1]).tostring() self.callback_processed = False self.latest_obs = obs self.cam_pub.publish(img_msg) if wait_for_callback: self.r = rospy.Rate(1000) self._publish_info() while not self.callback_processed: from time import sleep sleep( 0.0001 ) # TODO if you, yes YOU!, dear coder, have a better idea to synchronize the callback
def grabAndPublish(self, stream, publisher): while not self.update_framerate and not self.is_shutdown and not rospy.is_shutdown( ): yield stream # Construct image_msg # Grab image from stream stamp = rospy.Time.now() stream.seek(0) stream_data = stream.getvalue() # Generate compressed image image_msg = CompressedImage() image_msg.format = "jpeg" image_msg.data = stream_data image_msg.header.stamp = stamp image_msg.header.frame_id = self.frame_id publisher.publish(image_msg) # Clear stream stream.seek(0) stream.truncate() if not self.has_published: rospy.loginfo("[%s] Published the first image." % (self.node_name)) self.has_published = True rospy.sleep(rospy.Duration.from_sec(0.001))
def kcfTracker(self, frame): # setting initial value of bbox_obtained bbox_obtained = self.bbox # if inital flag is true then set the initial frame in the tracker if (self.init_flag): track_status = self.tracker.init(frame, self.bbox) self.init_flag = False if not track_status: rospy.logwarn("Unable to initalize tracker") else: # updating the frames in tracker track_status, bbox_obtained = self.tracker.update(frame) if track_status: p1 = (int(bbox_obtained[0]), int(bbox_obtained[1])) p2 = (int(bbox_obtained[0] + bbox_obtained[2]), int(bbox_obtained[1] + bbox_obtained[3])) cv2.rectangle(frame, p1, p2, (255, 0, 0), 2, 1) else: cv2.putText(frame, "Tracking failure detected", (60, 80), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) # Publish new feature image msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', frame)[1]).tostring() self.image_pub.publish(msg)
def stream(self): url = 'http://%s:%s@%s/mjpg/video.mjpg' % (self.axis.username, self.axis.password, self.axis.hostname) #url = url + "?resolution=%dx%d" % (self.axis.width, self.axis.height) fp = urllib.urlopen(url) while True: boundary = fp.readline() header = {} while 1: line = fp.readline() if line == "\r\n": break line = line.strip() parts = line.split(": ", 1) header[parts[0]] = parts[1] content_length = int(header['Content-Length']) img = fp.read(content_length) line = fp.readline() msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.axis_frame_id msg.format = "jpeg" msg.data = img self.axis.pub.publish(msg) """
def callback1(self, ros_data): '''Callback function of subscribed topic. Here images get converted and features detected''' #print('received image of type: "%s"' % ros_data.encoding) #### direct conversion to CV2 #### #np_arr = np.fromstring(ros_data.data, np.uint8) #image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) #image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0: #try: # image_np = self.bridge.imgmsg_to_cv2(ros_data, "u16") #except CvBridgeError as e: # print(e) image_np = self.bridge.imgmsg_to_cv2(ros_data) rmax = np.quantile(image_np, 0.95) image_u8 = image_np.copy() image_u8[image_u8 > rmax] = rmax image_u8 = (image_u8 * (256. / rmax)).astype('uint8') im_color = cv2.applyColorMap(image_u8, cv2.COLORMAP_JET) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', im_color)[1]).tobytes() # Publish new image self.image_pub1.publish(msg)
def startCapturing(self): rospy.loginfo("[%s] Start capturing." % (self.node_name)) while not self.is_shutdown and not rospy.is_shutdown(): #gen = self.grabAndPublish(self.stream,self.pub_img) try: image = self.camera.read() self.stream = np.array(bgr8_to_jpeg(image)).tostring() stamp = rospy.Time.now() stream_data = self.stream # Generate compressed image image_msg = CompressedImage() image_msg.format = "jpeg" image_msg.data = stream_data image_msg.header.stamp = stamp image_msg.header.frame_id = self.frame_id self.pub_img.publish(image_msg) if not self.has_published: rospy.loginfo("[%s] Published the first image." % (self.node_name)) self.has_published = True rospy.sleep(rospy.Duration.from_sec(0.001)) except StopIteration: pass #print("updating framerate") self.capture_fps = self.capture_fps self.update_framerate = False #self.camera.close() rospy.loginfo("[%s] Capture Ended." % (self.node_name))
def callback(self, ros_data): '''Callback function of subscribed topic. Here images get converted and features detected''' img = self.bridge.imgmsg_to_cv2(ros_data) if VERBOSE: pass # print('received image of type: "%s"' % ros_data.format) #### direct conversion to CV2 #### image_np = np.array(img[:, :, :3], np.uint8) # image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) # image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) # OpenCV >= 3.0: print(image_np.shape) res_img = self.find_ars(image_np) print(res_img.shape) cv2.imshow("found stones", res_img) if cv2.waitKey(1) & 0xFF == ord('q'): pass msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring() # Publish new image self.image_pub.publish(msg)
def imageCallback(msg): global imageSum, count, zero # rospy.loginfo("imagaCallback called") #### direct conversion to CV2 #### np_arr = np.fromstring(msg.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) image_float = image_np.astype(np.float) count += 1 if count == 1: imageSum = image_float # imageSum = imageSum else: beta = 1 / count alpha = 1 - beta # print "for count = %s, (alpha, beta) = (%s, %s)"%(count, alpha, beta) print "----" imageSum = cv2.addWeighted(imageSum, alpha, image_float, beta, 0) imageAverage = imageSum print "first imageSum pixel: (%s, %s, %s)" % tuple(imageSum[0][0]) # print "averaged" image_np_out = array(imageAverage) #### Create CompressedIamge #### out = CompressedImage() out.header.stamp = rospy.Time.now() out.format = "jpeg" np_arr_out = cv2.imencode('.jpg', image_np_out)[1] out.data = np.array(np_arr_out).tostring() # Publish new image publisher.publish(out)
def publish(self, raw_img): msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = 'jpeg' msg.data = np.array(cv2.imencode('.jpg', raw_img)[1]).tostring() #TODO compile sensor_msg error , use String instead self.image_pub.publish(msg.data)
def publish_image(self): """ publish images by publishing_names """ # # rospy.loginfo('publishers: {}, {}'%(self.publishers, self.publishers['original'])) # msg = CompressedImage() # msg.header.stamp = rospy.Time.now() # msg.format = "jpeg" # for src in self.publishing_names: # # rospy.loginfo("data on : " + str(src)) # msg.data = np.array(cv2.imencode(".jpg", self.images[src])[0]).tostring() # self.publishers[str(src)].publish(msg) # rospy.loginfo("published") # for src in self.publishing_names: # msg = CvImage(Header(), "bgr8", self.images[src]).toImageMsg() # rospy.loginfo(msg) # try: # cv_image = self.bridge.cv2_to_imgmsg(self.images['original'], "bgr8") # self.images['original'].publish(cv_image) # for src in self.publishing_names: # cv_image = self.bridge.imgmsg_to_cv2(self.images[src], "bgr8") # print("successfully published") # except CvBridgeError as e: # print(e) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" for str in self.publishing_names: if not str in self.images: continue msg.data = np.array(cv2.imencode('.jpg', self.images[str])[1]).tostring() self.publishers[str].publish(msg)
def displayvideo(image_np, mask, cnts, center): pts = deque(maxlen=64) # Overlays the identified contours on the received image frame. if len(cnts) > 0: cv2.drawContours(image_np, [cnts], -1, (0, 255, 0), 3) epsilon = 0.08 * cv2.arcLength(cnts, True) approx = cv2.approxPolyDP(cnts, epsilon, True) cv2.drawContours(image_np, [approx], -1, (255, 255, 0), 3) ((x, y), radius) = cv2.minEnclosingCircle(cnts) if radius > 10: cv2.circle(image_np, (int(x), int(y)), int(radius), (0, 255, 255), 2) cv2.circle(image_np, center, 5, (0, 0, 255), -1) # Marks the center point of the contours on the frame. pts.appendleft(center) for i in xrange(1, len(pts)): if pts[i - 1] is None or pts[i] is None: continue thickness = int(np.sqrt(64 / float(i + 1)) * 2.5) cv2.line(image_np, pts[i - 1], pts[i], (0, 0, 255), thickness) # Constructs compressed image msg to be sent to roscore. msg_vid = CompressedImage() msg_vid.header.stamp = rospy.Time.now() msg_vid.format = "jpeg" msg_vid.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring() pub_debug_vid.publish(msg_vid)
def pubOriginal(self, msg): compressed_img_msg = CompressedImage() compressed_img_msg.format = "png" compressed_img_msg.data = np.array(cv2.imencode(".png", self.testImageOrig)[1]).tostring() # compressed_img_msg.header.stamp = msg.header.stamp # compressed_img_msg.header.frame_id = msg.header.frame_id self.pub_raw.publish(compressed_img_msg)
def sendTestImage(self): img_name = '0'+str(self.sent_image_count)+'_orig.'+self.test_image_format test_img = cv2.imread(self.test_image_path+img_name) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = self.test_image_format msg.data = np.array(cv2.imencode('.'+self.test_image_format, test_img)[1]).tostring() self.pub_test_image.publish(msg)
def stream(self): """ Reads and process the streams from the camera """ try: #If flag self.enable_auth is 'True' then use the user/password to access the camera. Otherwise use only self.url if self.enable_auth: req = urllib2.Request(self.url, None, {"Authorization": self.auth }) else: req = urllib2.Request(self.url) fp = urllib2.urlopen(req, timeout = self.timeout) #fp = urllib2.urlopen(req) while self.run_camera and not rospy.is_shutdown(): boundary = fp.readline() header = {} while not rospy.is_shutdown(): line = fp.readline() #print 'read line %s'%line if line == "\r\n": break line = line.strip() #print line parts = line.split(": ", 1) header[parts[0]] = parts[1] content_length = int(header['Content-Length']) #print 'Length = %d'%content_length img = fp.read(content_length) line = fp.readline() msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.axis_frame_id msg.format = "jpeg" msg.data = img # publish image self.compressed_image_publisher.publish(msg) cimsg = self.cinfo.getCameraInfo() cimsg.header.stamp = msg.header.stamp cimsg.header.frame_id = self.axis_frame_id # publish camera info self.caminfo_publisher.publish(cimsg) #print self.real_fps self.last_update = datetime.datetime.now() self.error_reading = False self.image_pub_freq.tick() except urllib2.URLError,e: self.error_reading = True self.error_reading_msg = e rospy.logerr('Axis:stream: Camera %s (%s:%d). Error: %s'%(self.camera_id, self.hostname, self.camera_number, e))
def pubOrig(self, args=None): image_msg_out = CompressedImage() image_msg_out.header.stamp = rospy.Time.now() image_msg_out.format = "png" image_msg_out.data = np.array(cv2.imencode('.png', self.original_image)[1]).tostring() self.pub_image.publish(image_msg_out) rospy.loginfo("Publishing original image")
def pubImage(self): filename = '0'+str(self.num_sent)+'_orig.png' image = cv2.imread(self.im_dir+filename) msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = 'png' msg.data = np.array(cv2.imencode('.png', image)[1]).tostring() self.pub_fake_images.publish(msg) rospy.loginfo("[%s] Image %s published" %(self.node_name,filename))
def cv2_to_compressed_ros(self, img): try: msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', img)[1]).tostring() return msg except CvBridgeError as e: rospy.logerr(e)
def publish_image(pub, image_handle): image = CompressedImage() image.data = image_handle.read() image.format = 'jpeg' image.header.seq = 0 image.header.stamp.secs = math.floor(time.time()) image.header.stamp.nsecs = 0 image.header.frame_id = "0" pub.publish(image)
def processImage(self, image_msg): image_cv = self.bridge.imgmsg_to_cv2(image_msg) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_cv)[1]).tostring() self.pub_image_comp.publish(msg)
def stream(self): url = 'http://%s/mjpg/video.mjpg' % self.axis.hostname url = url + "?fps=0&resolultion=%dx%d" % (self.axis.width, self.axis.height) rospy.logdebug('opening ' + str(self.axis)) # only try to authenticate if user/pass configured if self.axis.password != '' and self.axis.username != '': # create a password manager password_mgr = urllib2.HTTPPasswordMgrWithDefaultRealm() # Add the username and password, use default realm. top_level_url = "http://" + self.axis.hostname password_mgr.add_password(None, top_level_url, self.axis.username, self.axis.password) handler = urllib2.HTTPBasicAuthHandler(password_mgr) # create "opener" (OpenerDirector instance) opener = urllib2.build_opener(handler) # ...and install it globally so it can be used with urlopen. urllib2.install_opener(opener) fp = urllib2.urlopen(url) while True: boundary = fp.readline() header = {} while 1: line = fp.readline() if line == "\r\n": break line = line.strip() parts = line.split(": ", 1) header[parts[0]] = parts[1] content_length = int(header['Content-Length']) img = fp.read(content_length) line = fp.readline() msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.header.frame_id = self.axis.frame_id msg.format = "jpeg" msg.data = img self.axis.pub.publish(msg) cimsg = self.axis.cinfo.getCameraInfo() cimsg.header.stamp = msg.header.stamp cimsg.header.frame_id = self.axis.frame_id cimsg.width = self.axis.width cimsg.height = self.axis.height self.axis.caminfo_pub.publish(cimsg)
def flipImageNP(self,msg): np_array = np.fromstring(msg.data, np.uint8) image_np = cv2.imdecode(np_array, cv2.CV_LOAD_IMAGE_COLOR) #flip_arr = np.fliplr(np_arr) imageflip_np=cv2.flip(image_np,1) flip_im = CompressedImage() flip_im.data = np.array(cv2.imencode('.jpg', imageflip_np)[1]).tostring() #flip_im.data = flip_arr.tostring() flip_im.header.stamp = msg.header.stamp self.pub_image.publish(flip_im)
def callback(self,ros_data): np_arr = np.fromstring(ros_data.data, np.uint8) image_np = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) image_np = cv2.flip(image_np, self.flip_code) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_np)[1]).tostring() self.image_pub.publish(msg)
def publish(self): img = cv2.imread("/home/qian/dev/catkin_ws/src/smart_ar/script/aurora.png", cv2.CV_LOAD_IMAGE_COLOR) M = cv2.getRotationMatrix2D((img.shape[1]/2,img.shape[0]/2),self.ga*100, 1.0 / self.dist) img = cv2.warpAffine(img,M,(img.shape[1], img.shape[0])) bridge = CvBridge() msg = CompressedImage() msg.format = "jpeg" msg.data = np.array(cv2.imencode(".jpg",img)[1]).tostring() msg.header.stamp = rospy.Time.now() msg.header.frame_id = 'image_compressed_publisher' msg.header.seq = self.seq self.seq += 1 self.imgPublisher.publish(msg) print self.seq, 'image published'
def cropCallback(msg): time_stamp = msg.header.stamp img_bytes = BytesIO(msg.data) img = PIL.Image.open(img_bytes) resize = (80, 60) img = img.resize(resize, resample=PIL.Image.BILINEAR) np_img = np.asarray(img) msg = CompressedImage() msg.header.stamp = time_stamp msg.format = "png" msg.data = np.array(cv2.imencode('.png', np_img)[1]).tostring() publish_cropimg.publish(msg)
def __init__(self): self.node_name = "Virtual Mirror Nbuckman Tester" self.fake_pub_image = rospy.Publisher("/ernie/camera_node/image/compressed", CompressedImage,queue_size=1) img_file = rospy.get_param("~img_file","/home/noambuckman/test_images/01_horz.jpg") rospy.loginfo(img_file) im_cv = cv2.imread(img_file) img_str = cv2.imencode('.jpg', im_cv)[1].tostring() pub_im=CompressedImage() pub_im.header=rospy.Time.now() pub_im.data = img_str #pub_im.data = np.array(cv2.imencode('.jpg', im_np)[1]).tostring() #flip_im.data = flip_arr.tostring() #flip_im.header.stamp = msg.header.stamp self.fake_pub_image.publish(pub_im)
def receive_image(self): """ Retrives image data coming from the rover and then transforms it to numpy and string data for further manupulation with OpenCV and Kivy and returns the final data """ data = '' ldata = array.array('c') image_buffer = CompressedImage() image_buffer.format = "jpeg" # start = '' found_start = False found_end = False start_pos = 0 #current position in ldata #position of the end of frame in the current array of data end_pos = 0 while (not found_end): data = self.video_socket.recv(self.max_tcp_buffer) if(data == ''): continue #check for the message start token 'MO_V' for i in range(0, len(data)-2): if (data[i:(i+4)] == 'MO_V'): if not found_start: found_start = True start_pos = i #crop the data only include stuff after the #start token. data = data[start_pos:len(data)] break elif not found_end: found_end = True end_pos = i break #if you have found the start but not the end (in the #middle of a image frame) if (found_start and not found_end): #add the recent data to ldata ldata.extend(list(data)) if found_end: ldata.extend(list(data[0:end_pos])) data = '' l_len = len(ldata) #### Create CompressedIamge #### # convert to numpy to use with OpenCV, etc. image_buffer.header.stamp = rospy.Time.now() image_buffer.data = np.array(ldata[36:l_len]).tostring() return image_buffer
def cbImage(self, img): # decode image np_arr = np.fromstring(img.data, np.uint8) color = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) self.overlayInfo(color) #### Create CompressedIamge #### msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', color)[1]).tostring() # Publish new image self.image_pub.publish(msg)
def publishTestImage(self): original_img = cv2.imread(self.original_image_file, cv2.CV_LOAD_IMAGE_COLOR) #### Create CompressedIamge #### out = CompressedImage() out.header.stamp = rospy.Time.now() out.format = "png" np_arr_out = cv2.imencode('.png', original_img)[1] out.data = np.array(np_arr_out).tostring() # Publish new image rospy.sleep(1) self.pub_mirror.publish(out) rospy.loginfo("[%s] published image"%self.node_name)
def cam_callback(msg): np_arr = np.fromstring(msg.data, np.uint8) # Decode image_rgb = cv2.imdecode(np_arr, cv2.CV_LOAD_IMAGE_COLOR) # Mirror image laterally image_rgb_rev = image_rgb[:,::-1,:] msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_rgb_rev)[1]).tostring() publisher.publish(msg) rospy.loginfo("Re Published ")
def set_scene(index): image = CompressedImage() # image.header.frame_id = '/odom_combined' # If you ever want to drive the base, you will want to use the line above, or /map or something. # Using /base_link means the robot will always stay exactly in the middle of the backdrop. image.header.frame_id = '/base_link' image.format = 'jpeg' try: scene = scenes[index] except TypeError: index = main_window.scene_comboBox.findText(index) scene = scenes[index] image.data = open(scene).read() image.header.stamp = rospy.Time.now() backdrop_publisher.publish(image)
def _publish_image(self, header, image, timestamp): """Publish JPG image as a ROS message. :param header: The HTTP-like header read from the stream. :type header: dict :param image: The video frame in JPG. :type image: :py:obj:`basestring` :param timestamp: The time when the frame was captured (or its best estimation). :type timestamp: :py:class:`rospy.Time` """ msg = CompressedImage() msg.header.stamp = timestamp msg.header.frame_id = self._axis._frame_id msg.format = "jpeg" msg.data = image self._axis._video_publisher.publish(msg)
def on_image(self, ros_data): compressed_in = np.fromstring(ros_data.data, np.uint8) image_in = cv2.imdecode(compressed_in, cv2.CV_LOAD_IMAGE_COLOR) if self.prvs == None : self.prvs = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height)) else: next = cv2.resize(cv2.cvtColor(image_in,cv2.COLOR_BGR2GRAY), (width,height)) image_out = cv2.resize(image_in, (width,height)) if self.should_be_controlled() : self.pub.publish(self.apply_control(next, image_out)) self.prvs = next msg = CompressedImage() msg.header.stamp = rospy.Time.now() msg.format = "jpeg" msg.data = np.array(cv2.imencode('.jpg', image_out)[1]).tostring() self.ipub.publish(msg)