def main(): rospy.init_node('flow_pub') image_pub = rospy.Publisher("/pidrone/picamera/image_raw", Image, queue_size=1, tcp_nodelay=False) camera_info_pub = rospy.Publisher("/pidrone/picamera/camera_info", CameraInfo, queue_size=1, tcp_nodelay=False) cim = camera_info_manager.CameraInfoManager( "picamera", "package://pidrone_pkg/params/picamera.yaml") cim.loadCameraInfo() if not cim.isCalibrated(): rospy.logerr("warning, could not find calibration for the camera.") try: velocity = axes_err() bridge = CvBridge() with picamera.PiCamera(framerate=90) as camera: camera.resolution = (CAMERA_WIDTH, CAMERA_HEIGHT) with AnalyzeFlow(camera) as flow_analyzer: flow_analyzer.setup(camera.resolution) phase_analyzer = AnalyzePhase(camera, bridge) camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer) print "Starting Flow" camera.start_recording(phase_analyzer, format='bgr', splitter_port=2) last_time = None while not rospy.is_shutdown(): camera.wait_recording(1 / 100.0) if phase_analyzer.prev_img is not None and phase_analyzer.prev_time != last_time: image_message = bridge.cv2_to_imgmsg( phase_analyzer.prev_img, encoding="bgr8") image_message.header.stamp = phase_analyzer.prev_rostime # print "stamp", image_message.header.stamp last_time = phase_analyzer.prev_rostime image_pub.publish(image_message) camera_info_pub.publish(cim.getCameraInfo()) camera.stop_recording(splitter_port=1) camera.stop_recording(splitter_port=2) print "Shutdown Received" sys.exit() except Exception as e: raise
if __name__ == '__main__': rospy.init_node('flow_pub') velpub = rospy.Publisher('/pidrone/plane_err', axes_err, queue_size=1) imgpub = rospy.Publisher("/pidrone/camera", Image, queue_size=1) rospy.Subscriber("/pidrone/mode", Mode, mode_callback) rospy.Subscriber("/pidrone/reset_homography", Empty, reset_callback) rospy.Subscriber("/pidrone/infrared", Range, range_callback) global current_mode global homography_started global camera global homography_analyzer try: velocity = axes_err() bridge = CvBridge() with AnalyzeFlow(camera) as flow_analyzer: camera.resolution = (320, 240) flow_analyzer.setup(camera.resolution) homography_analyzer.setup() camera.start_recording("/dev/null", format='h264', splitter_port=1, motion_output=flow_analyzer) print "Starting Homography" camera.start_recording(homography_analyzer, format='bgr', splitter_port=2) homography_started = True i = 0 while not rospy.is_shutdown(): velocity.x.err = flow_analyzer.x_motion