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
def setup(self, camera_wh=(320, 240), pub=None, flow_scale=16.5):
self.get_z_filter(camera_wh)
self.get_yaw_filter(camera_wh)
self.ang_vx = 0
self.ang_vy = 0
self.prev_time = time.time()
self.ang_coefficient = 1.0 # the amount that roll rate factors in
self.x_motion = 0
self.y_motion = 0
self.z_motion = 0
self.yaw_motion = 0
self.max_flow = camera_wh[0] / 16.0 * camera_wh[1] / 16.0 * 2**7
self.norm_flow_to_cm = flow_scale # the conversion from flow units to cm
self.flow_coeff = self.norm_flow_to_cm / self.max_flow
self.pub = pub
self.alpha = 0.3
if self.pub is not None:
self.velocity = axes_err()
def __init__(self):
# Initial setpoint for the z-position of the drone
self.initial_set_z = 30.0
# Setpoint for the z-position of the drone
self.set_z = self.initial_set_z
# Current z-position of the drone according to sensor data
self.current_z = 0
# Tracks x, y velocity error and z-position error
self.error = axes_err()
# PID controller
self.pid = PID()
# Setpoint for the x-velocity of the drone
self.set_vel_x = 0
# Setpoint for the y-velocity of the drone
self.set_vel_y = 0
# Time of last heartbeat from the web interface
self.last_heartbeat = None
# Commanded yaw velocity of the drone
self.cmd_yaw_velocity = 0
# Tracks previous drone attitude
self.prev_angx = 0
self.prev_angy = 0
# Time of previous attitude measurement
self.prev_angt = None
# Angle compensation values (to account for tilt of drone)
self.mw_angle_comp_x = 0
self.mw_angle_comp_y = 0
self.mw_angle_alt_scale = 1.0
self.mw_angle_coeff = 10.0
self.angle = TwistStamped()
# Desired and current mode of the drone
self.commanded_mode = self.DISARMED
self.current_mode = self.DISARMED
# Flight controller that receives commands to control motion of the drone
self.board = MultiWii("/dev/ttyUSB0")
import yaml
# stefie10: High-level comments: 1) Make a class 2) put everything
# inside a main method and 3) no global variables.
landing_threshold = 9.
initial_set_z = 0.13
set_z = initial_set_z
init_z = 0
smoothed_vel = np.array([0, 0, 0])
alpha = 1.0
ultra_z = 0
pid = PID()
first = True
error = axes_err()
cmds = [1500, 1500, 1500, 900]
current_mode = 4
set_vel_x = 0
set_vel_y = 0
errpub = rospy.Publisher('/pidrone/err', axes_err, queue_size=1)
modepub = rospy.Publisher('/pidrone/mode', Mode, queue_size=1)
flow_height_z = 0.000001
reset_pid = True
pid_is = [0,0,0,0]
last_heartbeat = None
cmd_velocity = [0, 0]
cmd_yaw_velocity = 0
mw_angle_comp_x = 0
homography_analyzer.lr_pid._i = 0
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
vrpn_curr[1] = data.pose.position.y
vrpn_curr[2] = data.pose.position.z
vrpn_speed = (vrpn_curr - vrpn_prev) / dt
vrpn_prev_time = data.header.stamp.to_sec()
def flow_callback(data):
global flow_speed, vrpn_curr
flow_speed[0] = data.x.err * vrpn_curr[2]
flow_speed[1] = data.y.err * vrpn_curr[2]
flow_speed[2] = data.z.err * vrpn_curr[2]
if __name__ == "__main__":
global flow_speed, vrpn_speed
rospy.init_node("flow_coefficient_tester")
rospy.Subscriber("/pidrone/plane_err", axes_err, flow_callback)
rospy.Subscriber("/pidrone/vrpn_pos", PoseStamped, vrpn_callback)
coeffpub = rospy.Publisher("/pidrone/flow_coeff", axes_err, queue_size=1)
r = rospy.Rate(100)
counter = 0
tot = np.array([0, 0, 0], dtype='float')
coeff = axes_err()
while not rospy.is_shutdown():
c = vrpn_speed / flow_speed
for i in range(3):
if np.absolute(c[i]) > 5000.0: c[i] = 0
(coeff.x.err, coeff.y.err, coeff.z.err) = c
coeffpub.publish(coeff)
r.sleep()