def decode_image(self, frame, orig_msg): msg = Image() msg.data = frame.to_rgb().planes[0].to_bytes() msg.width = frame.width msg.height = frame.height msg.step = frame.width * 3 msg.is_bigendian = 0 msg.encoding = 'rgb8' msg.header = Header() msg.header = orig_msg.header self.pub.publish(msg)
def detect(self): # call detectron2 model outputs = self.predictor(self.img) # process pointcloud to get 3D position and size detections = self.process_points(outputs) # publish detection result obstacle_array = ObstacleArray() obstacle_array.header = self.header if self.detect_obj_pub.get_subscription_count() > 0: obstacle_array.obstacles = detections self.detect_obj_pub.publish(obstacle_array) # visualize detection with detectron API if self.detect_img_pub.get_subscription_count() > 0: v = Visualizer(self.img[:, :, ::-1], MetadataCatalog.get(self.cfg.DATASETS.TRAIN[0]), scale=1) out = v.draw_instance_predictions(outputs["instances"].to("cpu")) out_img = out.get_image()[:, :, ::-1] out_img_msg = Image() out_img_msg.header = self.header out_img_msg.height = out_img.shape[0] out_img_msg.width = out_img.shape[1] out_img_msg.encoding = 'rgb8' out_img_msg.step = 3 * out_img.shape[1] out_img_msg.data = out_img.flatten().tolist() self.detect_img_pub.publish(out_img_msg)
def record_rgb(): """Recording 2D visuals """ # Get the RGB image from the Kinect sensor # Note: There are issues with wait_for_message not getting the # most up to date message from /camera/rgb/image_raw. # This is a hack to wait for the updated message current_time = rospy.get_rostime() orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) while orig_rgb.header.stamp < current_time: orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) # Copy the recevied Image message to a new Image message copy_rgb = Image() copy_rgb.header = orig_rgb.header copy_rgb.height = orig_rgb.height copy_rgb.width = orig_rgb.width copy_rgb.encoding = orig_rgb.encoding copy_rgb.is_bigendian = orig_rgb.is_bigendian copy_rgb.step = orig_rgb.step copy_rgb.data = orig_rgb.data # Publish the new Image to be saved with the specific name rgb_pub = rospy.Publisher('rgb_image', Image, queue_size=1) rospy.loginfo('-------------------------------------------------') rospy.loginfo('Generating RGB') rospy.loginfo('-------------------------------------------------') rospy.sleep(3) rgb_pub.publish(copy_rgb)
def StreamCameraSensor(self, request, context): """ Takes in a gRPC SensorStreamingRequest containing all the data needed to create and publish a sensor_msgs/Image ROS message. """ img_string = request.data cv_image = np.fromstring(img_string, np.uint8) # NOTE, the height is specifiec as a parameter before the width cv_image = cv_image.reshape(request.height, request.width, 3) cv_image = cv2.flip(cv_image, 0) bgr_image = cv2.cvtColor(cv_image, cv2.COLOR_RGB2BGR) msg = Image() header = std_msgs.msg.Header() try: # RGB # msg = self.bridge.cv2_to_imgmsg(cv_image, 'rgb8') # BGR msg = self.bridge.cv2_to_imgmsg(bgr_image, 'bgr8') header.stamp = rospy.Time.from_sec(request.timeStamp) msg.header = header except CvBridgeError as e: print(e) camera_pubs[request.frame_id].publish(msg) return sensor_streaming_pb2.CameraStreamingResponse(success=True)
def image_cb(self, data): objArray = Detection2DArray() try: rgb_img = self.bridge.imgmsg_to_cv2(data, "bgr8") rgb_img_crop = self.get_roi(rgb_img) except CvBridgeError as e: print(e) image = cv2.cvtColor(rgb_img_crop, cv2.COLOR_BGR2RGB) # the array based representation of the image will be used later in order to prepare the # result image with boxes and labels on it. image_np = np.asarray(image) # Expand dimensions since the model expects images to have shape: [1, None, None, 3] image_np_expanded = np.expand_dims(image_np, axis=0) image_tensor = detection_graph.get_tensor_by_name('image_tensor:0') # Each box represents a part of the image where a particular object was detected. boxes = detection_graph.get_tensor_by_name('detection_boxes:0') # Each score represent how level of confidence for each of the objects. # Score is shown on the result image, together with the class label. scores = detection_graph.get_tensor_by_name('detection_scores:0') classes = detection_graph.get_tensor_by_name('detection_classes:0') num_detections = detection_graph.get_tensor_by_name('num_detections:0') #print('begin detect ...') (boxes, scores, classes, num_detections) = self.sess.run( [boxes, scores, classes, num_detections], feed_dict={image_tensor: image_np_expanded}) #print('end detect ....') #print(category_index) #print(classes) objects = vis_util.visualize_boxes_and_labels_on_image_array( image, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), category_index, use_normalized_coordinates=True, line_thickness=2) objArray.detections = [] objArray.header = data.header object_count = 1 for i in range(len(objects)): object_count += 1 objArray.detections.append( self.object_predict(objects[i], data.header, image_np, cv_image)) self.object_pub.publish(objArray) img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB) image_out = Image() try: image_out = self.bridge.cv2_to_imgmsg(img, "bgr8") except CvBridgeError as e: print(e) image_out.header = data.header self.image_pub.publish(image_out)
def camera_node(): cap = cv2.VideoCapture(Configs.video_path) fps = cap.get(cv2.CAP_PROP_FPS) rospy.init_node('camera', anonymous=True) pub = rospy.Publisher('/camera_image', Image, queue_size=10) rate = rospy.Rate(Configs.camera_rate) # how many in one minute i = 0 while not rospy.is_shutdown(): try: ret, frame = cap.read() i += 1 if not ret and Configs.video_start: cap = cv2.VideoCapture(Configs.video_path) print("*************** Restart ****************") i = 0 time.sleep(2) continue image_temp = Image() header = Header(stamp=rospy.Time.now()) header.frame_id = 'camera' image_temp.height = frame.shape[0] image_temp.width = frame.shape[1] image_temp.encoding = 'bgr8' image_temp.data = frame.tostring() image_temp.header = header image_temp.step = frame.shape[1] * 3 pub.publish(image_temp) rate.sleep() print("Frame " + str(i) + ", Time " + str(int(i / fps)) + " s, Image: " + str(frame.shape[0]) + "*" + str(frame.shape[1])) except Exception as ex: print(ex)
def hoof(self, image): try: cv_image = self.bridge.imgmsg_to_cv2(image, "bgr8") except CvBridgeError as e: print(e) self.curr_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) self.count = self.count + 1 ff = self.curr_image frame = cv2.resize(ff, None, fx=0.5, fy=0.5) height, width = frame.shape[:2] celh = int(height / self.h_num) celw = int(width / self.w_num) gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) hsv = np.zeros_like(frame) hsv[:, :, 1] = 255 bgr = frame d8 = np.zeros((self.h_num, self.w_num, 8)) d16 = np.zeros((self.h_num, self.w_num, 16)) if (self.count == 1): self.prev_image = gray self.curr_image = gray edge = cv2.Canny(self.curr_image, 300, 300) flow = cv2.calcOpticalFlowFarneback(self.prev_image, self.curr_image, None, 0.5, 3, 15, 3, 5, 1.2, 0) mag, ang = cv2.cartToPolar(flow[..., 0], flow[..., 1]) hsv[:, :, 0] = ang * 180 / np.pi / 2 hsv[:, :, 2] = cv2.min(mag * 30, 255) a2 = np.floor((ang) * 15.9999995 / (2 * np.pi)) for j in range(0, self.h_num): for i in range(0, self.w_num): for k in range(0, 16): aa = a2[(j - 1) * celh:j * celh - 1, (i - 1) * celw:i * celw - 1] mm = mag[(j - 1) * celh:j * celh - 1, (i - 1) * celw:i * celw - 1] d16[j, i, k] = np.sum(mm[np.where(aa == k)]) d8[j, i, 0] = d16[j, i, 0] + d16[j, i, 15] for k in range(1, 8): d8[j, i, k] = np.sum(d16[j, i, 2 * k - 1:2 * k]) g = hsv[:, :, 2] hsv[:, :, 2] = cv2.max(g, edge) hsv[:, :, 1] = 255 - edge bgr = cv2.cvtColor(hsv, cv2.COLOR_HSV2BGR) self.prev_image = self.curr_image self.dd = np.append(self.dd, np.ravel(d8)) for i in range(1, self.h_num): cv2.line(bgr, (0, celh * i), (width, celh * i), (0, 0, 255), 2) for j in range(1, self.w_num): cv2.line(bgr, (celw * j, 0), (celw * j, height), (0, 0, 255), 2) img = cv2.cvtColor(bgr, cv2.COLOR_BGR2RGB) image_out = Image() try: image_out = self.bridge.cv2_to_imgmsg(img, "bgr8") except CvBridgeError as e: print(e) image_out.header = image.header self.hoof_pub.publish(image_out)
def timer_callback(self): ret, frame = self.cap.read() msg = Image() header = Header() header.frame_id = str(self.counter) header.stamp = self.get_clock().now().to_msg() if not ret: self.get_logger().warning('Publishing RANDOM IMAGE "%s"' % str(header.stamp)) frame = np.random.randint(255, size=(480, 640, 3), dtype=np.uint8) if self.stream_video: cv2.imshow('frame', frame) cv2.waitKey(1) frame = cv2.resize(frame, dsize=(64, 64), interpolation=cv2.INTER_CUBIC) frame = np.flipud(frame) frame = np.fliplr(frame) msg.header = header msg.height = frame.shape[0] msg.width = frame.shape[1] msg.encoding = "bgr8" value = self.bridge.cv2_to_imgmsg(frame.astype(np.uint8)) msg.data = value.data self.publisher_.publish(msg) self.get_logger().info('Publishing Image "%s"' % str(header.stamp)) self.counter += 1
def _build_result_msg(self, msg, result): result_msg = Result() result_msg.header = msg.header for i, (y1, x1, y2, x2) in enumerate(result['rois']): box = RegionOfInterest() box.x_offset = x1#np.asscalar(x1) box.y_offset = y1#np.asscalar(y1) box.height = y2-y1#np.asscalar(y2 - y1) box.width = x2-x1#np.asscalar(x2 - x1) result_msg.boxes.append(box) class_id = result['class_ids'][i] result_msg.class_ids.append(class_id) class_name = self._class_names[class_id] result_msg.class_names.append(class_name) score = result['scores'][i] result_msg.scores.append(score) mask = Image() mask.header = msg.header mask.height = result['masks'].shape[0] mask.width = result['masks'].shape[1] mask.encoding = "mono8" mask.is_bigendian = False mask.step = mask.width mask.data = (result['masks'][:, :, i] * 255).tobytes() result_msg.masks.append(mask) return result_msg
def image_cb(self, data): #Convert Image to cv2 bgr8 cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) image_np = np.asarray(image) image_np_expanded = np.expand_dims(image_np, axis=0) image_tensor = detection_graph.get_tensor_by_name('image_tensor:0') boxes = detection_graph.get_tensor_by_name('detection_boxes:0') scores = detection_graph.get_tensor_by_name('detection_scores:0') classes = detection_graph.get_tensor_by_name('detection_classes:0') num_detections = detection_graph.get_tensor_by_name('num_detections:0') #Run model detection on Image (boxes, scores, classes, num_detections) = sess.run( [boxes, scores, classes, num_detections], feed_dict={image_tensor: image_np_expanded}) objects = vis_util.visualize_boxes_and_labels_on_image_array( image, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), category_index, use_normalized_coordinates=True, line_thickness=2) img = cv2.cvtColor(image_np, cv2.COLOR_BGR2RGB) image_out = Image() image_out = self.bridge.cv2_to_imgmsg(img, "bgr8") image_out.header = data.header self.image_pub.publish(image_out)
def publish_processed_image(self): # draw box and caption string self.rgb = cv2.rectangle(self.rgb, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), (0,0,0xFF), 2) self.rgb = cv2.putText(self.rgb, self.caption, (self.bbox[0]+2, self.bbox[1]-5), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0,0,0xFF), thickness=2) #construct sensor_msgs/Image object self.imgmsg.data = self.rgb.tobytes() # publish frame rospy.loginfo('publishing processed frame (with bbox) %s' % rospy.get_time()) self.rgb1_pub.publish(self.imgmsg) # hack to republish frame msk = np.zeros( (self.rgb.shape[0], self.rgb.shape[1])) msk = cv2.rectangle(msk, (self.bbox[0], self.bbox[1]), (self.bbox[2],self.bbox[3]), 0xFF, -1) rgb_msked = np.zeros_like(self.rgb) rgb_msked[msk==0xFF, :] = self.rgb[msk==0xFF, :] # construct new imgmsg msg = Image() msg.header = self.imgmsg.header msg.height, msg.width, msg.step = self.imgmsg.height, self.imgmsg.width, self.imgmsg.step msg.encoding = self.imgmsg.encoding msg.is_bigendian = self.imgmsg.is_bigendian msg.data = rgb_msked.tobytes() self.rgb2_pub.publish(msg) self.rate.sleep() # release GPU memory for next data del self.img
def image_cb(self, data): objectArray = Detection2DArray() try: # Recordar que es necesario el uso de cv_bridge cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB) # Representamos y guardamos la imagen como un array. Se usara mas tarde para presentar el resultado # con las cajas y etiquetas npArrayImage = np.asarray(image) npArrayImageExpanded = np.expand_dims(npArrayImage, axis=0) image_tensor = detection_graph.get_tensor_by_name('image_tensor:0') # Cada caja representa una seccion de la imagen donde un objeto fue detectado boxes = detection_graph.get_tensor_by_name('detection_boxes:0') # Obtenemos los scores o nivel de confianza de la prediccion scores = detection_graph.get_tensor_by_name('detection_scores:0') # Obtenemos la clase que tambien sera mostrada junto con el score classes = detection_graph.get_tensor_by_name('detection_classes:0') num_detections = detection_graph.get_tensor_by_name('num_detections:0') print("Detectando!") (boxes, scores, classes, num_detections) = self.sess.run( [boxes, scores, classes, num_detections], feed_dict={image_tensor: npArrayImageExpanded}) objects = vis_util.visualize_boxes_and_labels_on_image_array( image, np.squeeze(boxes), np.squeeze(classes).astype(np.int32), np.squeeze(scores), categoryIndex, use_normalized_coordinates=True, line_thickness=2) # NOTA: esto podria ser interesante. Leer el comentario largo en la funcion object_predict mas abajo. objectArray.detections = [] objectArray.header = data.header object_count = 1 for i in range(len(objects)): object_count += 1 objectArray.detections.append( self.object_predict(objects[i], data.header, npArrayImage, cv_image)) self.object_pub.publish(objectArray) img = cv2.cvtColor(npArrayImage, cv2.COLOR_BGR2RGB) image_out = Image() try: # Volvemos a convertir la imagen de OpenCV a mensaje de ROS image_out = self.bridge.cv2_to_imgmsg(img, "bgr8") except CvBridgeError as e: print(e) image_out.header = data.header self.image_pub.publish(image_out)
def default(self, ci="unused"): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data["image"] image = Image() image.header = self.get_ros_header() image.header.frame_id += "/base_image" image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = "rgba8" image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data["intrinsic_matrix"] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = "plumb_bob" camera_info.K = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], ] camera_info.R = R camera_info.P = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0, ] self.publish(image) self.topic_camera_info.publish(camera_info)
def visualizacion_ros(self, img): image_out = Image() try: image_out = bridge.cv2_to_imgmsg(img, 'bgr8') except CvBridgeError as e: print(e) image_out.header = current_image.header self.img_publisher.publish(image_out)
def copy_Image_empty_data(image_old): image_new = Image() image_new.header = copy(image_old.header) image_new.height = copy(image_old.height) image_new.width = copy(image_old.width) image_new.encoding = copy(image_old.encoding) image_new.is_bigendian = copy(image_old.is_bigendian) image_new.step = copy(image_old.step) return image_new
def record_rgb(object_name): """Recording 2D visuals Spawn the object, record the visuals and delete the object Parameters ---------- object_name : str The name of the tool or object to be recorded """ # Spawn the object rospack = rospkg.RosPack() object_urdf_path = str( rospack.get_path('affordance_experiment') + '/models/' + object_name + '.urdf') pitch = pi / 2 yaw = 0 if object_name == 'cylinder': yaw = pi / 2 spawn_object(object_name, object_urdf_path, 0.0, 0.35, 1.05, pi / 2, pitch, yaw) # Wait for the object to rest rospy.sleep(2) # Get the RGB image from the Kinect sensor # Note: There are issues with wait_for_message not getting the # most up to date message from /camera/rgb/image_raw. # This is a hack to wait for the updated message current_time = rospy.get_rostime() orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) while orig_rgb.header.stamp < current_time: orig_rgb = rospy.wait_for_message('/camera/rgb/image_raw', Image) # Copy the recevied Image message to a new Image message copy_rgb = Image() copy_rgb.header = orig_rgb.header copy_rgb.height = orig_rgb.height copy_rgb.width = orig_rgb.width copy_rgb.encoding = orig_rgb.encoding copy_rgb.is_bigendian = orig_rgb.is_bigendian copy_rgb.step = orig_rgb.step copy_rgb.data = orig_rgb.data # Publish the new Image to be saved with the specific name rgb_pub = rospy.Publisher(object_name + '_rgb', Image, queue_size=1) rospy.loginfo('-------------------------------------------------') rospy.loginfo('Generating RGB') rospy.loginfo('-------------------------------------------------') rospy.sleep(3) rgb_pub.publish(copy_rgb) # Delete object delete_object(object_name) rospy.sleep(3)
def callback_depth_img(data): global depth_data, depth_img, msg_bool depth_data = bridge.imgmsg_to_cv2(data, desired_encoding="passthrough") depth_data.setflags(write=1) depth_data[(depth_data < depth_range[0]) | (depth_data > depth_range[1])] = 0 depth_img = bridge.cv2_to_imgmsg(depth_data, encoding="passthrough") depth_img = Image() depth_img.header = copy(data.header) depth_img = copy(data) msg_bool[2] = True
def construct_image(header: Header, response: ImageResponse, encoding: str) -> Image: msg = Image() msg.header = header msg.encoding = encoding msg.height = response.height msg.width = response.width msg.data = response.image_data_uint8 if response.image_type != ImageType.DepthPlanar else response.image_data_float msg.is_bigendian = 0 msg.step = response.width * 3 return msg
def imageCallback(self, msg): '''! Recebe a imagem, seg,emta e publica ''' img = np.array(msg.data, dtype=np.uint8) img = np.array(np.split(img, msg.height)) img = np.array(np.split(img, msg.width, axis=1)) img = np.rot90(img) img = tf.keras.preprocessing.image.array_to_img(img) image_array = tf.keras.preprocessing.image.img_to_array(img) result = self.bodypix_model.predict_single(image_array) mask = result.get_mask(threshold=0.75) colored_mask = result.get_colored_part_mask(mask) mask = mask.numpy().astype(np.uint8) colored_mask = colored_mask.astype(np.uint8) maskMsg = Image() maskMsg._data = mask.flatten().tolist() maskMsg.height = mask.shape[0] maskMsg.width = mask.shape[1] maskMsg.encoding = "8UC1" maskMsg.is_bigendian = 0 maskMsg.step = maskMsg.width colMsg = Image() colMsg._data = colored_mask.flatten().tolist() colMsg.height = colored_mask.shape[0] colMsg.width = colored_mask.shape[1] colMsg.encoding = "8UC3" colMsg.is_bigendian = 0 colMsg.step = colMsg.width * 3 maskMsg.header = msg.header colMsg.header = msg.header self.pubMask.publish(maskMsg) self.colMask.publish(colMsg)
def live_depth_analysis(self): #Live stream video analysis while not rospy.is_shutdown(): #print("Looking for a frame") current_frame = self.frame current_depth_frame = self.depth_frame if (current_frame != [] and current_depth_frame != []): #print("got frame") #image = cv2.imread("/home/pmcrivet/catkin_ws/src/Mask_RCNN/script/images/frame0057.jpg") #img = numpy.asarray(frame,dtype='uint8') #cv2.imshow("image",image) #cv2.waitKey(0) #cv2.destroyAllWindows() #print image.shape #print image.dtype #Dilatation effect on masks for better feature masking dilatation = 10 threshold = 0.3 results = self.model.detect([current_frame], dilatation, threshold, verbose=1) r = results[0] #selected_class = self.class_selection(r['masks'], r['class_ids']) selected_class = r['masks'] #Comment for faster code run #result_image = visualize_cv.cv_img_masked(current_frame, r['rois'], selected_class, r['class_ids'], # self.class_names, r['scores']) result_depth_image = visualize_cv.cv_depth_img_masked( current_depth_frame, r['rois'], selected_class, r['class_ids'], self.class_names, r['scores']) DITS = Image() #ITS = Image() DITS = self.bridge.cv2_to_imgmsg(result_depth_image, '16UC1') #ITS = self.bridge.cv2_to_imgmsg(result_image,'bgr8') DITS.header = self.depth_msg_header #ITS.header = self.msg_header #self.image_pub.publish(ITS) self.depth_image_pub.publish(DITS) #print("Publishing img") else: print("Waiting for frame")
def publish_image(image, name): image_temp = Image() header = Header(stamp=rospy.Time.now()) header.frame_id = 'map' image_temp.height = image.shape[0] image_temp.width = image.shape[1] image_temp.encoding = 'rgb8' image_temp.data = np.array(image).tostring() image_temp.header = header image_temp.step = image.shape[0] * 3 image_pubulish.publish(image_temp)
def img_cb(self, data): if self.stop_line == 0 or self.ncs_set == 0: return try: print "start detection" start = time.time() #for SR300 image self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") #for compressed image #np_arr = np.fromstring(data.data, np.uint8) #self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) img_gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY) mser = cv2.MSER_create(20, 300, 20000, 0.25, 0.2, 200, 1.01, 0.003, 5) regions, boxes = mser.detectRegions(img_gray) hulls = [cv2.convexHull(p.reshape(-1, 1, 2)) for p in regions] #cv2.polylines(gray_img, hulls, 1, (255, 0, 0), 2) imgContours = self.cv_image contour_list = [] point_list = [[0, 0]] point_check = True for i, contour in enumerate(hulls): x, y, w, h = cv2.boundingRect(contour) if 2 * h < w and h * w < 10000 and h * w > 1000: point_check = True for p in point_list: #print p if p[0] - 20 < x < p[0] + 20 and p[1] - 20 < y < p[ 1] + 20: point_check = False if point_check == True: point_list.append([x, y]) cv2.rectangle(imgContours, (x, y), (x + w, y + h), (0, 255, 0), 3) img_region = self.cv_image[y:y + h, x:x + w] self.cv_img_crop.append(img_region) #publish image_with_box image_all_mser_image = Image() image_all_mser_image.header = rospy.Time.now image_all_mser_image = self.bridge.cv2_to_imgmsg( imgContours, "bgr8") self.image_pub.publish(image_all_mser_image) print "detection time:", time.time() - start #call prediction self.cnn() except CvBridgeError as e: print(e)
def publish_raw_image(publisher, header, img): if len(img.shape) < 3: img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) msg_img = Image() try: msg_img = CvBridge().cv2_to_imgmsg(img, "bgr8") except CvBridgeError as e: print(e) return msg_img.header = header publisher.publish(msg_img)
def publish_image(imgdata): image_temp = Image() header = Header(stamp=rospy.Time.now()) header.frame_id = 'map' image_temp.height = IMAGE_HEIGHT image_temp.width = IMAGE_WIDTH image_temp.encoding = 'rgb8' image_temp.data = np.array(imgdata).tostring() #print(imgdata) #image_temp.is_bigendian=True image_temp.header = header image_temp.step = 1241 * 3 image_pub.publish(image_temp)
def apriltag_callback(data): if len(data.detections) >=1: id = Image() for detect in data.detections: id.header = detect.pose.header if(detect.id == 0): id.step = detect.id break id.step = detect.id #print id.data #print " " id_pub = rospy.Publisher("/id_output", Image, queue_size=1) id_pub.publish(id)
def publish_image(imgdata): image_temp = Image() header = Header(stamp=rospy.Time.now()) header.frame_id = 'map' image_temp.height = np.shape(imgdata)[0] image_temp.width = np.shape(imgdata)[1] image_temp.encoding = 'rgb8' image_temp.data = np.array(imgdata).tostring() #print(imgdata) #image_temp.is_bigendian=True image_temp.header = header image_temp.step = np.shape(imgdata)[1] * 3 img_pub.publish(image_temp)
def map_image(values): """ Map the values generated with the hypothesis-ros image strategy to a rospy Image type. """ if not isinstance(values, _Image): raise TypeError('Wrong type. Use appropriate hypothesis-ros type.') ros_image = Image() ros_image.header = values.header ros_image.height = values.height ros_image.width = values.width ros_image.encoding = values.encoding ros_image.is_bigendian = values.is_bigendian ros_image.data = values.data return ros_image
def timer_callback(self): image = cv2.imread(str(self.images[self.counter])) msg = Image() header = Header() header.frame_id = str(self.counter) header.stamp = self.get_clock().now().to_msg() msg.header = header msg.height = image.shape[0] msg.width = image.shape[1] msg.encoding = "bgr8" value = self.bridge.cv2_to_imgmsg(image.astype(np.uint8)) msg.data = value.data self.publisher_.publish(msg) self.counter += 1
def publish_image(image_data, ts): image = bridge.cv2_to_imgmsg(image_data, encoding='bgr8') image_publisher.publish(image) return image = Image() # ts = rospy.Time.from_sec(float(ts) / 1000) ts = rospy.Time.now() header = Header(stamp=ts) header.frame_id = 'world' image.height = image_data.shape[0] image.width = image_data.shape[1] image.encoding = 'bgr8' image.data = np.array(image_data).tostring() image.header = header image_publisher.publish(image)
def image_rgb_callback(msg): frame = bridge.imgmsg_to_cv2(msg) global image_header #UD = UserData() #UD.type = 16 #UD.rows = frame.shape[0] #UD.cols = frame.shape[0] #UD.data = frame IRGBTS = Image() IRGBTS = msg IRGBTS.encoding = "bgr8" IRGBTS.header = image_header #UD_pub.publish(UD) IRGBTS_pub.publish(IRGBTS)
def img_cb(self, data): print "image received" if self.stop_line == 0: return try: print "start detection" ###caffe.set_mode_gpu() #self.start = data.header.stamp.secs start = time.time() #for SR300 image self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") #for compressed image #np_arr = np.fromstring(data.data, np.uint8) #self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) img_gray = cv2.cvtColor(self.cv_image, cv2.COLOR_BGR2GRAY) mser = cv2.MSER_create(25, 100, 250000, 0.25, 0.2, 200, 1.01, 0.003, 5) regions, _ = mser.detectRegions(img_gray) hulls = [cv2.convexHull(p.reshape(-1, 1, 2)) for p in regions] #cv2.polylines(gray_img, hulls, 1, (255, 0, 0), 2) imgContours = self.cv_image contour_list = [] for i, contour in enumerate(hulls): x, y, w, h = cv2.boundingRect(contour) #repeat = self.checkContourRepeat(contour_list, x, y, w, h) #img_region = img_cv[y:y+h, x:x+w] if 2 * h < w and h * w < 10000 and h * w > 1000: cv2.rectangle(imgContours, (x, y), (x + w, y + h), (0, 255, 0), 3) img_region = self.cv_image[y:y + h, x:x + w] self.cv_img_crop.append(img_region) self.cv_crop_region.append([x, y, w, h]) #publish image_with_box image_all_mser_image = Image() image_all_mser_image.header = rospy.Time.now image_all_mser_image = self.bridge.cv2_to_imgmsg( imgContours, "bgr8") self.image_pub.publish(image_all_mser_image) print "detection time:", time.time() - start #call prediction ###self.cnn() except CvBridgeError as e: print(e)
def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = self.encoding image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.D = [0] camera_info.K = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2] ] camera_info.R = R camera_info.P = [ intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0 ] if self.pub_tf: self.publish_with_robot_transform(image) else: self.publish(image) self.topic_camera_info.publish(camera_info)
def OccupancyGridToNavImage(self, grid_map): img = Image() img.header = grid_map.header img.height = grid_map.info.height img.width = grid_map.info.width img.is_bigendian = 1 img.step = grid_map.info.width img.encoding = "mono8" maxindex = img.height*img.width numpy.uint for i in range(0, maxindex, 1): if int(grid_map.data[i]) < 20: #img.data[i] = "0" data.append(0) else: img.data[i] = "255" return imgding
def default(self, ci='unused'): if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = self.encoding image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.D = [0] camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] if self.pub_tf: self.publish_with_robot_transform(image) else: self.publish(image) self.topic_camera_info.publish(camera_info)
def find_object(self): with self.lock_depth: image_data = self.image_data depth_data = self.depth_data pub_data = rospy.Publisher('/racecar/image/data', Float64MultiArray, queue_size=1) arrayData = Float64MultiArray() #print "received image" if image_data: newImage = Image() newImage.encoding = image_data.encoding newImage.header = image_data.header newImage.is_bigendian = image_data.is_bigendian newData = bytearray() image = image_data.data clusters = [] white = chr(255) *3 black = chr(0) * 3 red = chr(0) + chr(0) + chr(255) blue = chr(255) + chr(0) * 2 #defining color search thresholds redThreshold = 230 blueThreshold = 20 greenLowerThreshold = 50 greenUpperThreshold = 200 height = 0 starttime = time.time() for h in xrange(0, image_data.height, self.resolution): height += 1 for w in xrange(0, image_data.width, self.resolution): index = w * 3 + image_data.width * 3 * h b = ord(image[index]) g = ord(image[index + 1]) r = ord(image[index + 2]) #filtering for the color if r > redThreshold and b < blueThreshold and g <= greenUpperThreshold and g >= greenLowerThreshold: for char in white: newData.append(char) inCluster = False for cluster in clusters: if self.checkAdjacency((w, h), cluster): cluster.add((w, h)) inCluster = True if not inCluster: newCluster = set() newCluster.add((w, h)) clusters.append(newCluster) else: for char in black: newData.append(char) newImage.height = height newImage.width = len(newData)/height/3 newImage.step = len(newData)/height newImage.data = str(newData) #print 'time taken: ', time.time() - starttime clusters = self.reduceClusters(clusters) #print len(clusters), float(len(candidates_x))/len(newData) #print [len(cluster) for cluster in clusters] if len(clusters) > 1: print "multiple objects found" print [len(cluster) for cluster in clusters] if len(clusters): publishableClusters = [max(clusters, key=lambda x:len(x))] data_to_publish = [] for cluster in publishableClusters: centerX = sum(x for (x, y) in cluster)/len(cluster) centerY = sum(y for (x, y) in cluster)/len(cluster) width = max(cluster, key=lambda (x, y):x)[0] - min(cluster, key=lambda (x, y):x)[0] height = max(cluster, key=lambda (x, y):y)[1] - min(cluster, key=lambda (x, y):y)[1] relativeLocation = float(centerX)/image_data.width - 0.5 relativeAngle = relativeLocation * 100 * math.pi / 180 data_to_publish += [centerX, centerY, width, height, relativeAngle] #print 'center ', centerX, centerY, len(candidates_x) #print 'distance ', depth1, depth2 #print 'dim ', height, width '''depth_index = centerX * 2 + image_data.width * 2 * centerY depth1 = ord(depth_data.data[depth_index]) depth2 = ord(depth_data.data[depth_index + 1])''' arrayData.data = data_to_publish else: arrayData.data = [-1, -1, -1, -1, -1] print 'no cone found' pub_data.publish(arrayData) #print "publishing" pub_img = rospy.Publisher('/racecar/image/newImage', Image, queue_size=1) pub_img.publish(newImage) else: arrayData.data = [-1, -1, -1, -1] print 'image not ready'
def default(self, ci='unused'): """ Publish the data of the Camera as a ROS Image message. """ if not self.component_instance.capturing: return # press [Space] key to enable capturing image_local = self.data['image'] image = Image() image.header = self.get_ros_header() image.header.frame_id += '/base_image' image.height = self.component_instance.image_height image.width = self.component_instance.image_width image.encoding = 'rgba8' image.step = image.width * 4 # VideoTexture.ImageRender implements the buffer interface image.data = bytes(image_local) # sensor_msgs/CameraInfo [ http://ros.org/wiki/rviz/DisplayTypes/Camera ] # fill this 3 parameters to get correcty image with stereo camera Tx = 0 Ty = 0 R = [1, 0, 0, 0, 1, 0, 0, 0, 1] intrinsic = self.data['intrinsic_matrix'] camera_info = CameraInfo() camera_info.header = image.header camera_info.height = image.height camera_info.width = image.width camera_info.distortion_model = 'plumb_bob' camera_info.K = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], intrinsic[2][0], intrinsic[2][1], intrinsic[2][2]] camera_info.R = R camera_info.P = [intrinsic[0][0], intrinsic[0][1], intrinsic[0][2], Tx, intrinsic[1][0], intrinsic[1][1], intrinsic[1][2], Ty, intrinsic[2][0], intrinsic[2][1], intrinsic[2][2], 0] self.publish(image) self.topic_camera_info.publish(camera_info)