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