def publish(self): print("pi4 initiated, time: " + str(time.time())) while True: if rospy.is_shutdown(): break #with picamera.PiCamera() as camera: # camera.resolution=(1280,720) ## camera.framerate=Fraction(1,6) ## camera.shutter_speed=6000000 ## camera.exposure_mode='off' ## camera.iso=800 # camera.start_preview() # time.sleep(2) # with picamera.array.PiRGBArray(camera) as stream: # camera.capture(stream, format='bgr') # image=stream.array # bridge=CvBridge() # msg=bridge.cv2_to_compressed_imgmsg(image) # pub.publish(msg) bridge = CvBridge() _, img = self.cam.read() _, img2 = self.cam2.read() msg1 = bridge.cv2_to_compressed_imgmsg(img) msg2 = bridge.cv2_to_compressed_imgmsg(img2) self.pub.publish(msg1) self.pub2.publish(msg2) self.rate.sleep()
class image_capture: ''' Image capture and ROS publisher ''' def __init__(self, node_name="image_capture"): ''' Initialize the image capturer Parameters: N/A Returns: N/A ''' self.node_name = node_name rospy.init_node(node_name) cam_topic = rospy.get_namespace() + "cv2_capture" self.image_pub = rospy.Publisher(cam_topic, CompressedImage, queue_size=1) self.bridge = CvBridge() self.cv_capture = cv2.VideoCapture('v4l2src device=/dev/video0 ! video/x-raw, framerate=30/1, width=640, height=360 ! videoconvert ! appsink') #self.cv_capture = cv2.VideoCapture(0) print('Initiated cam connection') def run(self): try: while not rospy.is_shutdown(): ret, cv_img = self.cv_capture.read() if ret: # Publish the resulting frame try: self.image_pub.publish(self.bridge.cv2_to_compressed_imgmsg(cv_img)) except CvBridgeError as e: print(e) except rospy.ROSInterruptException: pass
def start(self): env = gym_duckietown.simulator.Simulator( seed=123, # random seed map_name="loop_empty", max_steps=500001, # we don't want the gym to reset itself domain_rand=0, camera_width=640, camera_height=480, accept_start_angle_deg=4, # start close to straight full_transparency=True, distortion=True, ) bridge = CvBridge() while (True): action = self.action observation, reward, done, misc = env.step(action) env.render() if done: env.reset() #Generate the compressed image image = cv2.cvtColor(numpy.asarray(observation), cv2.COLOR_RGB2BGR) image_message = bridge.cv2_to_compressed_imgmsg(image, dst_format='jpeg') #Publish the compressed image self.pub_img.publish(image_message)
def spin(self): time.sleep(1.0) started = time.time() counter = 0 cvim = numpy.zeros((480, 640, 3), numpy.uint8) ball_xv = 10 ball_yv = 10 ball_x = 100 ball_y = 100 cvb = CvBridge() while not rospy.core.is_shutdown(): cvim.fill(0) cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1) ball_x += ball_xv ball_y += ball_yv if ball_x in [10, 630]: ball_xv = -ball_xv if ball_y in [10, 470]: ball_yv = -ball_yv self.pub.publish(cvb.cv2_to_imgmsg(cvim, "bgr8")) self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) time.sleep(0.03)
class ROSWrapper(DTROS): def __init__(self, node_name): # initialize the DTROS parent class super(ROSWrapper, self).__init__(node_name=node_name, node_type=NodeType.GENERIC) self.bridge = CvBridge() self.pub = rospy.Publisher('/fakebot/camera_node/image/compressed', CompressedImage, queue_size=1) self.sub = rospy.Subscriber('/fakebot/wheels_driver_node/wheels_cmd', WheelsCmdStamped, self.callback) self.action = [0.1, 0.1] def imagepublisher(self, observation): #rate = rospy.Rate(5) b, g, r = cv2.split(observation) img = cv2.merge([r, g, b]) #img_msg=self.bridge.cv2_to_compressed_imgmsg(observation) img_msg = self.bridge.cv2_to_compressed_imgmsg(img) img_msg.header.stamp = rospy.Time.now() #rospy.loginfo("Get an image...") self.pub.publish(img_msg) #rospy.loginfo("Published to topic camera_node/image/compressed") #rate.sleep() def callback(self, wheelcommand): rospy.loginfo("get wheel command") self.action = [wheelcommand.vel_left, wheelcommand.vel_right]
class Gray_Scale(): def __init__(self): self.enabled = False rospy.init_node("gray_scale", anonymous=True) # services s = rospy.Service('perception_service', SetBool, self.perception_service_cb) # subscribers # subscriber for camera feed self.subVideo = rospy.Subscriber("/camera/image_raw/compressed", CompressedImage, self.video_cb) # publishers self.pub_pose = rospy.Publisher("/gray_scale_out/compressed", CompressedImage, queue_size=1) self.bridge = CvBridge() def perception_service_cb(self, req): self.enabled = req.data return SetBoolResponse(True, "Yay!") def convert_to_grayscale(self, compressed_image_msg): cv_image = self.bridge.compressed_imgmsg_to_cv2(compressed_image_msg) cv_gray_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) gray_image_msg = self.bridge.cv2_to_compressed_imgmsg(cv_gray_image) return gray_image_msg def video_cb(self, msg): if(self.enabled): gray_msg = self.convert_to_grayscale(msg) self.pub_pose.publish(gray_msg) def run(self): rate = rospy.Rate(10) # 10hz default while not rospy.is_shutdown(): rospy.loginfo_throttle(0.5,'enabled: ' + str(self.enabled)) rate.sleep()
def test_encode_decode_cv2_compressed(self): import numpy as np # from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm", "jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support cvb_en = CvBridge() cvb_de = CvBridge() for w in range(100, 800, 100): for h in range(100, 800, 100): for f in formats: for channels in ([], 1, 3): if channels == []: original = np.uint8(np.random.randint(0, 255, size=(h, w))) else: original = np.uint8(np.random.randint(0, 255, size=(h, w, channels))) compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f) newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg) self.assert_(original.dtype == newimg.dtype) if channels == 1: # in that case, a gray image has a shape of size 2 self.assert_(original.shape[:2] == newimg.shape[:2]) else: self.assert_(original.shape == newimg.shape) self.assert_(len(original.tostring()) == len(newimg.tostring()))
def main(args=None): if args is None: args = sys.argv rclpy.init(args=args) node = rclpy.create_node("Source") node_logger = node.get_logger() topic = args[1] filenames = args[2:] pub_img = node.create_publisher(sensor_msgs.msg.Image, topic) pub_img_compressed = node.create_publisher(sensor_msgs.msg.CompressedImage, topic + "/compressed") time.sleep(1.0) cvb = CvBridge() while rclpy.ok(): try: cvim = cv2.imread(filenames[0]) pub_img.publish(cvb.cv2_to_imgmsg(cvim)) pub_img_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) filenames = filenames[1:] + [filenames[0]] time.sleep(1) except KeyboardInterrupt: node_logger.info("shutting down: keyboard interrupt") break node_logger.info("test_completed") node.destroy_node() rclpy.shutdown()
def spin(self): time.sleep(1.0) started = time.time() counter = 0 cvim = numpy.zeros((480, 640, 1), numpy.uint8) ball_xv = 10 ball_yv = 10 ball_x = 100 ball_y = 100 cvb = CvBridge() while not rospy.core.is_shutdown(): cvim.fill(0) cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1) ball_x += ball_xv ball_y += ball_yv if ball_x in [10, 630]: ball_xv = -ball_xv if ball_y in [10, 470]: ball_yv = -ball_yv self.pub.publish(cvb.cv2_to_imgmsg(cvim)) self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) time.sleep(0.03)
def test_encode_decode_cv2_compressed(self): import numpy as np # from: http://docs.opencv.org/2.4/modules/highgui/doc/reading_and_writing_images_and_video.html#Mat imread(const string& filename, int flags) formats = ["jpg", "jpeg", "jpe", "png", "bmp", "dib", "ppm", "pgm", "pbm", "jp2", "sr", "ras", "tif", "tiff"] # this formats rviz is not support cvb_en = CvBridge() cvb_de = CvBridge() for w in range(100, 800, 100): for h in range(100, 800, 100): for f in formats: for channels in ([], 1, 3): if channels == []: original = np.uint8(np.random.randint(0, 255, size=(h, w))) else: original = np.uint8(np.random.randint(0, 255, size=(h, w, channels))) compress_rosmsg = cvb_en.cv2_to_compressed_imgmsg(original, f) newimg = cvb_de.compressed_imgmsg_to_cv2(compress_rosmsg) self.assert_(original.dtype == newimg.dtype) if channels == 1: # in that case, a gray image has a shape of size 2 self.assert_(original.shape[:2] == newimg.shape[:2]) else: self.assert_(original.shape == newimg.shape) self.assert_(len(original.tostring()) == len(newimg.tostring()))
class DetectionImagePublisher(object): def __init__(self, detection_images_qdic): self.detection_images_qdic = detection_images_qdic self.image_publishers = {} self.bridge = CvBridge() for camera in self.detection_images_qdic.keys(): topic = '/cameras/' + camera + '/ar_detection_images/compressed' self.image_publishers[camera] = rospy.Publisher(topic, CompressedImage, queue_size=10) self._detection_image_available = Event() def notify_detection_image_available(self): self._detection_image_available.set() def run(self): while not rospy.is_shutdown(): self._detection_image_available.wait() for camera in self.detection_images_qdic.keys(): for _ in range(self.detection_images_qdic[camera].qsize()): try: self.image_publishers[camera].publish( self.bridge.cv2_to_compressed_imgmsg( self.detection_images_qdic[camera].get())) except CvBridgeError as e: print e self._detection_image_available.clear()
def video_to_msg(): rospy.init_node("video_to_msg_node", anonymous=True) pub_image = rospy.Publisher("/axis/image_raw", Image, queue_size=10) pub_compr = rospy.Publisher("/axis/image_raw/compressed", CompressedImage, queue_size=10) pub_info = rospy.Publisher("/axis/camera_info", CameraInfo, queue_size=10) filename = sys.argv[1] video = cv2.VideoCapture(filename) if not video.isOpened(): print "Error: Can't open: " + str(filename) exit(0) print "Correctly opened, starting to publish." fps = video.get(cv2.CAP_PROP_FPS) rate = rospy.Rate(fps) count_max = int(video.get(cv2.CAP_PROP_FRAME_COUNT)) resolution = video.get(cv2.CAP_PROP_FRAME_HEIGHT) bridge = CvBridge() msg_header = std.Header() msg_header.frame_id = "/axis" if resolution == 720.0: msg_info = info_720p() elif resolution == 480.0: msg_info = info_480p() success, frame = video.read() count = 1 while success and not rospy.is_shutdown(): msg_image = bridge.cv2_to_imgmsg(np.uint8(frame), encoding="rgb8") msg_compr = bridge.cv2_to_compressed_imgmsg(frame, dst_format="jpeg") msg_header.stamp = rospy.Time.now() msg_header.seq = count msg_image.header = msg_header msg_compr.header = msg_header msg_info.header = msg_header #print msg_compr #time.sleep(10) pub_image.publish(msg_image) pub_compr.publish(msg_compr) pub_info.publish(msg_info) print str(count) + "/" + str(count_max) rate.sleep() success, frame = video.read() count += 1
def spin(self): time.sleep(1.0) started = time.time() counter = 0 cvim = numpy.zeros((480, 640, 1), numpy.uint8) ball_xv = 10 ball_yv = 10 ball_x = 100 ball_y = 100 cvb = CvBridge() cv2.namedWindow("source", cv2.WINDOW_NORMAL) src = cv2.imread( "/home/ganga/Documents/rover-control/src/opencv_tests/ball.jpg", 1) cv2.imshow("source", src) cv2.waitKey(0) while not rospy.core.is_shutdown(): cvim.fill(0) cv2.circle(cvim, (ball_x, ball_y), 10, 255, -1) ball_x += ball_xv ball_y += ball_yv if ball_x in [10, 630]: ball_xv = -ball_xv if ball_y in [10, 470]: ball_yv = -ball_yv self.pub.publish(cvb.cv2_to_imgmsg(cvim)) self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) time.sleep(0.03)
class camera_Worker(threading.Thread): def __init__(self): self.flag = False self.wait_flag = threading.Event() threading.Thread.__init__(self) self.image_pub = rospy.Publisher("/camera_topic", CompressedImage, queue_size=1) #encoding defult jpg self.bridge = CvBridge() def run(self): self.camera() def camera(self): self.cap = cv2.VideoCapture(gstreamer_pipeline(), cv2.CAP_GSTREAMER) if self.cap.isOpened(): print("Start Camera") while not rospy.is_shutdown(): if not self.flag: self.wait_flag.wait() self.ret_val, self.img = self.cap.read() self.image_pub.publish( self.bridge.cv2_to_compressed_imgmsg( self.img)) #encoding defult jpg print("Close Camera") else: print("Unable to open camera") self.cap.release() print(self.cap.isOpened())
def sync_callback(self, _img1, _img2, _img3, _img4): _time = time.time() self.is_initiated = True img1, time1 = self.callback_undistort1(_img1) img2, time2 = self.callback_undistort2(_img2) img3, time3 = self.callback_undistort3(_img3) img4, time4 = self.callback_undistort4(_img4) print("1: " + str(time.time() - _time)) im_mask_inv1, im_mask1 = self.find_mask(img1) im_mask_inv3, im_mask3 = self.find_mask(img3) print("2: " + str(time.time() - _time)) img1_masked = np.multiply(img1, im_mask_inv1) img2_masked = np.multiply(np.multiply(img2, im_mask1), im_mask3) print("3: " + str(time.time() - _time)) img3_masked = np.multiply(img3, im_mask_inv3) img4_masked = np.multiply(np.multiply(img4, im_mask1), im_mask3) summed_image = img1_masked + img2_masked + img3_masked + img4_masked print("4: " + str(time.time() - _time)) #cv2.imshow('summed.png', summed_image) #cv2.waitKey(3) bridge = CvBridge() summed_msg = bridge.cv2_to_compressed_imgmsg(summed_image) print("5: " + str(time.time() - _time)) self.sum_pub.publish(summed_msg) print("img1: " + str(time1) + ", img2: " + str(time2 - time1) + ", img3: " + str(time3 - time1) + ", img4: " + str(time4 - time1) + ", summ: " + str(time.time() - time1))
def run(self): ''' Continuously run the wrapper to map ROS interface with simulator. This method runs the node, and begins an iterative process where the image is retrieved from the simulator and published in the camera image topic, and the wheels command received by subscribing to the wheels_cmd topic is translated into actions executed in the simulator. The process iterates until the node is terminated ''' while not rospy.is_shutdown(): # Update the simulator # the action message is updated every time the wheels_cmd_cb is called # that is, every time a message arrives. observation, reward, done, misc = self.env.step(self.action) # Render the new state self.env.render() # Set again action to stop, in case no more messages are being received self.action = [0.0, 0.0] if done: # Reset the simulator when the robot goes out of the road. self.env.reset() # Transalte the observation into a CompressedImage message to publish it bgr_obs = cv2.cvtColor(observation, cv2.COLOR_RGB2BGR) bridge = CvBridge() img_msg = bridge.cv2_to_compressed_imgmsg(cvim=bgr_obs, dst_format="jpg") # Publish the image in the /image/compressed topic self.camera_pub.publish(img_msg)
def spin(self): time.sleep(1.0) started = time.time() counter = 0 # imv = np.zeros((height, width, bpp), np.uint8) cvim = numpy.zeros((500, 500, 3), numpy.uint8) ball_xv = 10 ball_yv = 10 ball_x = 100 ball_y = 100 cvb = CvBridge() while not rospy.core.is_shutdown(): # cvim.fill(0) # cv2.circle(img, (center_x, center_y), radius, (b, g, r), thickness) cv2.circle(cvim, (ball_x, ball_y), 100, (0,0,255), 1) ball_x += ball_xv ball_y += ball_yv if ball_x in [10, 630]: ball_xv = -ball_xv if ball_y in [10, 470]: ball_yv = -ball_yv self.pub.publish(cvb.cv2_to_imgmsg(cvim, 'bgr8')) self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) time.sleep(0.03)
def CreateVideoBag(videopath, bagname): '''Creates a bag file with a video file''' bag = rosbag.Bag(bagname, 'w') cap = cv2.VideoCapture(videopath) cb = CvBridge() prop_fps = cap.get(cv2.CAP_PROP_FPS) if prop_fps != prop_fps or prop_fps <= 1e-2: print "Warning: can't get FPS. Assuming 24." prop_fps = 24 ret = True frame_id = 0 while (ret): ret, frame = cap.read() if not ret: break stamp = rospy.rostime.Time.from_sec(float(frame_id) / prop_fps) frame_id += 1 # image = cb.cv2_to_imgmsg(frame, encoding='bgr8') image = cv2.resize(frame, dsize=(1280, 720), interpolation=cv2.INTER_LINEAR) image = cb.cv2_to_compressed_imgmsg(frame) image.header.stamp = stamp image.header.frame_id = "camera" bag.write(TOPIC, image, stamp) cap.release() bag.close()
class VehicleDetectionTestNode(object): def __init__(self): self.node_name = "Vehcile Detection Test" self.bridge = CvBridge() self.pub_image = rospy.Publisher("~image", CompressedImage, queue_size=1) self.sub_image = rospy.Subscriber("~corners", VehicleCorners, self.processCorners, queue_size=1) self.original_filename = rospy.get_param('~original_image_file') self.original_image = cv2.imread(self.original_filename) rospy.loginfo("Initialization of [%s] completed" % (self.node_name)) pub_period = rospy.get_param("~pub_period", 1.0) rospy.Timer(rospy.Duration.from_sec(pub_period), self.pubOrig) def pubOrig(self, args=None): np_arr = np.fromstring(np.array(cv2.imencode('.png', self.original_image)[1]).tostring(), np.uint8) cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR) img_msg = self.bridge.cv2_to_compressed_imgmsg(cv_image, "jpg") img_msg.header.stamp = rospy.Time.now() self.pub_image.publish(img_msg) rospy.loginfo("Publishing original image") def processCorners(self, corners_msg): for i in np.arange(len(corners_msg.corners)): rospy.loginfo('Corners received : (x = %.2f, y = %.2f)' % (corners_msg.corners[i].x, corners_msg.corners[i].y))
def talker(): pub = rospy.Publisher('/howard17/camera_node/image/compressed', CompressedImage, queue_size=10) rospy.init_node('aruco_talker') rate = rospy.Rate(10) # 10hz br = CvBridge() #cap = cv2.VideoCapture('test1.mp4') cap = cv2.VideoCapture(0) #print('Got the cap') #cap = cv2.VideoCapture(0) while not rospy.is_shutdown(): ret, frame = cap.read() if ret == True: #print('ret is true') # Our operations on the frame come here #gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) # Display the resulting frame #cv2.imshow('frame',gray) #if cv2.waitKey(1) & 0xFF == ord('q'): #break #rospy.loginfo(hello_str) # Convert the video to a ros Compressed image message = br.cv2_to_compressed_imgmsg(frame) pub.publish(message) #rospy.loginfo(message) #rate.sleep() # Upon shutdown cap.release() cv2.destroyAllWindows()
class FaceRecognitionNode: def __init__(self): self.__bridge = CvBridge() self.__image_pub = rospy.Publisher( 'face_recognition_node/image/compressed', CompressedImage, queue_size=1) self.__image_sub = rospy.Subscriber('raspicam_node/image/compressed', CompressedImage, self.callback) confidence_level = rospy.get_param('/face_rec_python/confidence_level', 20) rospy.loginfo("FaceRecognitionNode: Confidence level %s", str(confidence_level)) # Create the face_recognition_lib class instance self.__frc = face_recognition_lib.FaceRecognition( roslib.packages.get_pkg_dir('face_recognition', required=True), confidence_level) # Create the Action server self.__as = actionlib.SimpleActionServer('face_recognition', scan_for_facesAction, self.do_action, False) self.__as.start() def do_action(self, goal): # Scan the current image for faces recognised # The image may show more than one face. # The returned dictionary will contain the unique IDs # and Names of any subjects recognised. # If no detection/recognition dictionary will be empty image = self.__bridge.compressed_imgmsg_to_cv2(self.__current_image) # In the next call image will be altered if faces are recognised detected_dict = self.__frc.scan_for_faces(image) try: self.__image_pub.publish( self.__bridge.cv2_to_compressed_imgmsg(image)) except CvBridgeError as e: print(e) # Now post a message with the list of IDs and names ids = [] names = [] for k, v in detected_dict.items(): ids.append(k) names.append(v) # Set result for the action result = scan_for_facesResult() result.ids_detected = ids result.names_detected = names self.__as.set_succeeded(result) # Callback for new image received def callback(self, data): # Each time we receive an image we store it ready in case then asked to scan it self.__current_image = data
def spin(self): time.sleep(1.0) cvb = CvBridge() while not rospy.core.is_shutdown(): cvim = cv2.imload(self.filenames[0]) self.pub.publish(cvb.cv2_to_imgmsg(cvim)) self.pub_compressed.publish(cvb.cv2_to_compressed_imgmsg(cvim)) self.filenames = self.filenames[1:] + [self.filenames[0]] time.sleep(1)
class OpenMVCamNode: def __init__(self): rospy.init_node('openmv_cam_node', argv=sys.argv) self.device = rospy.get_param('~device', default='/dev/ttyACM0') self.topic = rospy.get_param( '~topic', default='openmv_cam/camera_main/image_raw') self.compressed = rospy.get_param('~compressed', default=False) self.image_type = Image if self.compressed: self.topic += '/compressed' self.image_type = CompressedImage try: self.openmv_cam = OpenMVCam(self.device) except Exception: rospy.logerr('Serial connection to OpenMV Cam failed: {}'.format( self.device)) exit() self.bridge = CvBridge() self.publisher = rospy.Publisher(self.topic, self.image_type, queue_size=1) self.seq = 0 rospy.loginfo("Relaying '{}' to '{}'".format(self.device, self.topic)) def read_and_publish_image(self): # Read image from camera image = self.openmv_cam.read_image() # Deduce color/grayscale from image dimensions channels = 1 if image.ndim == 2 else 3 encoding = 'bgr8' if channels == 3 else 'mono8' # Convert from BGR to RGB by reversing the channel order if channels == 3: image = image[..., ::-1] # Convert numpy image to (commpressed) ROS image message if self.compressed: image_msg = self.bridge.cv2_to_compressed_imgmsg(image) else: image_msg = self.bridge.cv2_to_imgmsg(image, encoding=encoding) # Add timestamp and sequence number (empty by default) image_msg.header.stamp = rospy.Time.now() image_msg.header.seq = self.seq self.seq += 1 self.publisher.publish(image_msg)
class ObjectDetectionNode: def __init__(self): self.__bridge = CvBridge() # Publisher to publish update image self.__image_pub = rospy.Publisher( 'tf_object_detection_node/image/compressed', CompressedImage, queue_size=1) # Subscribe to the topic which will supply the image fom the camera self.__image_sub = rospy.Subscriber('raspicam_node/image/compressed', CompressedImage, self.Imagecallback) # Read the path for models/research/object_detection directory from the parameter server or use this default object_detection_path = rospy.get_param( '/object_detection/path', '/home/ubuntu/git/models/research/object_detection') # Read the confidence level, any object with a level below this will not be used confidence_level = rospy.get_param( '/object_detection/confidence_level', 0.50) # Create the object_detection_lib class instance self.__odc = object_detection_lib.ObjectDetection( object_detection_path, confidence_level) # Create the Action server self.__as = actionlib.SimpleActionServer('object_detection', scan_for_objectsAction, self.do_action, False) self.__as.start() # Callback for new image received def Imagecallback(self, data): # Each time we receive an image we store it in case we are asked to scan it self.__current_image = data def do_action(self, goal): # Scan the current image for objects # Convert the ROS compressed image to an OpenCV image image = self.__bridge.compressed_imgmsg_to_cv2(self.__current_image) # The supplied image will be modified if known objects are detected object_names_detected = self.__odc.scan_for_objects(image) # publish the image, it may have been modified try: self.__image_pub.publish( self.__bridge.cv2_to_compressed_imgmsg(image)) except CvBridgeError as e: print(e) # Set result for the action result = scan_for_objectsResult() result.names_detected = object_names_detected self.__as.set_succeeded(result)
class ImageConverter: def __init__(self): self.image_pub = rospy.Publisher("/image/compressed", CompressedImage, queue_size=1) self.brige = CvBridge() self.image_sub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, self.callback) def callback(self, data): try: cv_image = self.brige.compressed_imgmsg_to_cv2(data, "passthrough") except CvBridgeError as e: print(e) (rows, cols, channels) = cv_image.shape if cols > 60 and rows > 60: cv2.circle(cv_image, (50, 50), 10, 255) # cv2.imshow("Image Window", cv_image) # cv2.waitKey(3) # insert jays code image = cv_image #try: image_copy = np.copy(image) # Edge Detection using Canny E.D img_canny_detection = canny_ed(image_copy) img_region_extract = region_extract(img_canny_detection) lines = cv2.HoughLinesP(img_region_extract, 2, np.pi / 180, 100, np.array([]), minLineLength=40, maxLineGap=5) # num of pixels in the bin, 1 deg, thereshold(min), placeholder, tracd below 40 lines rejected, max distance btw pixels) lines_avg = avg_slope(image_copy, lines) img_lines = line_display(image_copy, lines) #img_combined = img_canny_detection #img_combined = img_region_extract img_combined = cv2.addWeighted(image_copy, 0.7, img_lines, 1, 1) rows, cols, ch = img_combined.shape #psts1 = np.float32([[155, 240], [463, 243], [0, 290], [600, 290]]) #psts2 = np.float32([[0, 0], [640, 0], [0, 450], [640, 450]]) #M = cv2.getPerspectiveTransform(psts1, psts2) #dst = cv2.warpPerspective(img_combined, M, (640, 450)) try: # changed this from cv_image to img_combined self.image_pub.publish( self.brige.cv2_to_compressed_imgmsg(img_combined)) except CvBridgeError as e: print(e)
class image_converter: def __init__(self): self.image_pub = rospy.Publisher("image_topic", CompressedImage) self.bridge = CvBridge() try: img = cv2.imread('25.png') self.image_pub.publish(cmprsmsg = self.bridge.cv2_to_compressed_imgmsg(img, 'png')) except CvBridgeError as e: print(e)
def operator(): rospy.init_node('image_file_publisher', anonymous=True) pub_c = rospy.Publisher('image_data/compressed', CompressedImage, queue_size=10) pub_r = rospy.Publisher('image_data/raw', Image, queue_size=10) dir_name = rospy.get_param('~dir_name') file_name = rospy.get_param('~file_name') sequence_no = int(rospy.get_param('~sequence_no')) if sequence_no < 1 and file_name != "": filepath = os.path.abspath( os.path.join(os.path.dirname(dir_name), file_name)) elif sequence_no >= 1 and sequence_no <= 999 and file_name == "": qr_file_name = "MarkerData_%03d.png" % sequence_no filepath = os.path.abspath( os.path.join(os.path.dirname(dir_name), qr_file_name)) else: raise SyntaxError( "sequence_no is 1 to 999 and file_name is not empty.") print filepath output_width_px = int(rospy.get_param("~output_width_px", "320")) output_height_px = int(rospy.get_param("~output_height_px", "240")) pub_rate = int(rospy.get_param("~pub_rate", "1")) # make bridge bridge = CvBridge() rate = rospy.Rate(pub_rate) while not rospy.is_shutdown(): # read image try: im = cv2.imread(filepath, cv2.IMREAD_COLOR) tmp = im[:, :] input_height_px, input_width_px = im.shape[:2] new_img = cv2.resize(np.full((1, 1, 3), 255, np.uint8), (output_width_px, output_height_px)) # Add Margin(Side || Side) tmp = cv2.resize( tmp, dsize=None, fx=(float(output_height_px) / float(input_height_px)), fy=(float(output_height_px) / float(input_height_px))) start = int((output_width_px - tmp.shape[1]) / 2) fin = int((tmp.shape[1] + output_width_px) / 2) new_img[0:output_height_px, start:fin] = tmp # make msg msg_c = bridge.cv2_to_compressed_imgmsg(new_img, dst_format='jpg') msg_r = bridge.cv2_to_imgmsg(new_img, encoding="bgr8") pub_c.publish(msg_c) pub_r.publish(msg_r) except: pass rate.sleep()
class Camera: # DO_NOTHING_CMD = CFMotion() def __init__(self, ID): self.id = ID self.bridge = CvBridge() self.mat = None #need to facilitate a set of publishers per cf node self.image_pub = rospy.Publisher('cf/%d/image' % self.id, CompressedImage, queue_size=10) ## CALLBACKS ## ## THREADS ## def run(self): try: cap = cv2.VideoCapture( 0) # TODO: multiple vid captures in parallel cap.set(cv2.CAP_PROP_FRAME_WIDTH, 192) cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 144) # cap.set(cv2.CAP_PROP_BRIGHTNESS, 0.8) # cap.set(cv2.CAP_PROP_CONTRAST, 0.2) # cap.set(cv2.CAP_PROP_EXPOSURE, 0.08) # cap.set(cv2.CAP_PROP_AUTO_EXPOSURE, 0.25) while not rospy.is_shutdown(): ret, frame = cap.read() gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) #ret, gray = cap.read() self.image_pub.publish( self.bridge.cv2_to_compressed_imgmsg(gray)) cv2.imshow('frame', gray) if cv2.waitKey(1) & 0xFF == ord('q'): break cap.release() cv2.destroyAllWindows() except Exception as e: print "CAMERA %d STREAM FAILED -- CHECK INPUTS" % self.id print "Error: " + str(e) print " -- Camera %d Finished -- " % self.id
class CamCapture(): def __init__(self): self.camID = rospy.get_param("~camID", 0) self.width = rospy.get_param("~width", 640) self.height = rospy.get_param("~height", 480) self.rate = rospy.get_param("~rate", 30) self.flipH = rospy.get_param("~flipH", False) self.flipV = rospy.get_param("~flipV", False) rospy.loginfo("use camera id=%d, w=%d, h=%d, rate=%f" % (self.camID, self.width, self.height, self.rate)) self.cap = cv2.VideoCapture(self.camID) self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, self.width) self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, self.height) self.br = CvBridge() self.pub = rospy.Publisher("/topower_v1/camera/image_raw/compressed", CompressedImage, queue_size=1) self.sub = rospy.Subscriber("/topower_v1/camera/capture", Empty, self.CaptureImage) self.savePath = rospy.get_param("~savePath", "captureImage/") self.frame = None def CaptureImage(self, msg): if self.frame is not None: if not os.path.exists(self.savePath): os.makedirs(self.savePath) now = datetime.datetime.now() filename = self.savePath + now.strftime( "%Y-%m-%d_%H-%M-%S") + ".jpg" rospy.loginfo("save image " + filename) cv2.imwrite(filename, self.frame) def Run(self): rate = rospy.Rate(self.rate) while not rospy.is_shutdown(): ret, self.frame = self.cap.read() if self.flipH and self.flipV: self.frame = cv2.flip(self.frame, -1) elif self.flipH: self.frame = cv2.flip(self.frame, 1) elif self.flipV: self.frame = cv2.flip(self.frame, 0) msg = self.br.cv2_to_compressed_imgmsg(self.frame) self.pub.publish(msg) #cv2.imshow('frame',self.frame) #cv2.waitKey(1) rate.sleep()
class RosTensorFlow(): def __init__(self): in_img_topic = get_param('~in_img_topic', '/image_in') in_geo_img_topic = get_param('~in_geo_img_topic', '/geo_image_in') out_img_topic = get_param('~out_img_topic', '/image_detected') out_geo_img_topic = get_param('~out_geo_img_topic', '/geo_image_detected') self.m_cv_bridge = CvBridge() self.m_geo_img = GeoImageCompressed() self.m_sub_img = rospy.Subscriber(in_img_topic, Image, self.detectImgCb, queue_size=50) self.m_sub_geo_img = rospy.Subscriber(in_geo_img_topic, GeoImageCompressed, self.detectGeoImgCb, queue_size=50) self.m_pub_img = rospy.Publisher(out_img_topic, Image, queue_size=50) self.m_pub_geo_img = rospy.Publisher(out_geo_img_topic, GeoImageCompressed, queue_size=50) def detectGeoImgCb(self, geo_image_msg): self.m_geo_img = geo_image_msg # First copy the full msg and then update only the detected image later. print("Image recieved.") frame = self.m_cv_bridge.compressed_imgmsg_to_cv2( geo_image_msg.imagedata, "bgr8") #frame = cv2.resize(frame,(0,0), fx=0.2, fy=0.2) t = time.time() detected_image = detect_objects(frame, sess, detection_graph) self.m_geo_img.imagedata = self.m_cv_bridge.cv2_to_compressed_imgmsg( detected_image, dst_format='jpg') self.m_pub_geo_img.publish(self.m_geo_img) self.m_pub_img.publish( self.m_cv_bridge.cv2_to_imgmsg(detected_image, "bgr8")) print('[INFO] elapsed time: {:.2f}'.format(time.time() - t)) def detectImgCb(self, image_msg): print("Image recieved.") frame = self.m_cv_bridge.imgmsg_to_cv2(image_msg, "bgr8") #frame = cv2.resize(frame,(0,0), fx=0.2, fy=0.2) t = time.time() detected_image = detect_objects(frame, sess, detection_graph) self.m_pub_img.publish( self.m_cv_bridge.cv2_to_imgmsg(detected_image, "bgr8")) print('[INFO] elapsed time: {:.2f}'.format(time.time() - t)) def main(self): rospy.spin()
class AcaptureCameraNode(Node): def __init__(self): super().__init__('acapture_camera_node', allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) self.node_name = self.get_name() self.log = self.get_logger() self.cap = acapture.open(0) #/dev/video0 #self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, 640) #self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) #self.cap.set(cv2.CAP_PROP_FPS, 5) self.log.info("successfully opened camera device!") # init ROS opencv bridge self.bridge = CvBridge() self.log.info("sucessfully created CvBridge.") self.pub_img = self.create_publisher(CompressedImage, "image/compressed", 1) self.image_msg = CompressedImage() def start_capturing(self): self.log.info("Start capturing.") while True: try: check, frame = self.cap.read() # non-blocking if check: self.log.info("sucessfully read a frame") frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) self.image_msg = self.bridge.cv2_to_compressed_imgmsg( frame) self.image_msg.header.stamp = self.get_clock().now( ).to_msg() # self.image_msg.frame_id = "camera_optical_frame" self.pub_img.publish(self.image_msg) self.log.info("publishing image") # cv2.waitKey(100) sleep(0.05) except StopIteration: pass self.log.info("Capture Ended.")
class ImageConverter: def __init__(self): self.brige = CvBridge() self.brige2 = CvBridge() self.image_pub = rospy.Publisher("/image/compressed2", CompressedImage, queue_size=1) self.top_image_pub = rospy.Publisher("/top_image/compressed", CompressedImage, queue_size=1) #self.image_pub2 = rospy.Publisher("/image/compressed2", CompressedImage, queue_size=1) self.image_sub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, self.callback) def callback(self,data): try: cv_image = self.brige.compressed_imgmsg_to_cv2(data, "passthrough") except CvBridgeError as e: print(e) (rows, cols, channels) = cv_image.shape if cols > 60 and rows > 60: #cv2.circle(cv_image, (50,50), 10, 255) # print center of lane line bisector angle average intersection if CENTER_LANE_X <= 640 and CENTER_LANE_X>= 0: cv2.circle(cv_image, (int(CENTER_LANE_X), 480), 20, (0, 0, 255)) elif CENTER_LANE_X > 640: cv2.circle(cv_image, (640, 480), 20, (0, 0, 255)) elif CENTER_LANE_X < 0: cv2.circle(cv_image, (0, 480), 20, (0, 0, 255)) # print target cv2.circle(cv_image, (int(640/2), 480), 20, (255, 0, 0)) # insert jays code global LAST_LINES_AVG image = cv_image #try: image_copy = np.copy(image) img_combined = image_copy # Edge Detection using Canny E.D img_canny_detection = canny_ed(image_copy) img_region_extract = region_extract(img_canny_detection) img_combined = img_canny_detection try: # changed this from cv_image to img_combined self.image_pub.publish(self.brige.cv2_to_compressed_imgmsg(img_combined)) # self.top_image_pub.publish(self.brige.cv2_to_compressed_imgmsg(dst)) #self.image_pub.publish(self.brige.cv2_to_compressed_imgmsg(dst)) #self.image_pub2.publish(CvBridge().cv2_to_compressed_imgmsg(dst)) except CvBridgeError as e: print(e)