# whatever it really is. http://wiki.ros.org/msg#Constants. It
# is okay if they are integer constants but they should be
# referenced by name (e.g., Mode.DISARMED) in the code. I had to
# inspect the code to figure out what mode was what, and still got
# it wrong when writing the keyboard controller - it added a whole
# bunch of extra time and needless errors. Even if ROS didn't
# support enums, this code could have used them by defining them
# as variable constants.
mode_to_pub = Mode()
while not rospy.is_shutdown():
mode_to_pub.mode = current_mode
modepub.publish(mode_to_pub)
errpub.publish(error)
mw_data = board.getData(MultiWii.ATTITUDE)
analog_data = board.getData(MultiWii.ANALOG)
publishRos(board, imupub, markerpub, markerarraypub, statepub)
if current_mode != 4: # stefie10: ENUM ENUM ENUM!
# angle compensation calculations
try:
# stefie10: Put this code inside of a method or several methods.
new_angt = time.time()
new_angx = mw_data['angx']/180.0*np.pi
new_angy = mw_data['angy']/180.0*np.pi
mw_angle_comp_x = np.tan((new_angx - prev_angx) * (new_angt - prev_angt)) * mw_angle_coeff
mw_angle_comp_y = np.tan((new_angy - prev_angy) * (new_angt - prev_angt)) * mw_angle_coeff
d_theta_x = new_angx - prev_angx
def main():
board = MW('/dev/ttyUSB0')
data = board.getData(MW.ANALOG)
print 'BATTERY VOLTAGE IS\t{}v'.format(voltage(data))
board.close()
with picamera.PiCamera(framerate=90) as camera:
with AnalyzeFlow(camera) as flow_analyzer:
rate = rospy.Rate(90)
camera.resolution = (320, 240)
flow_analyzer.setup(camera.resolution)
output = SplitFrames(width, height)
camera.start_recording('/dev/null',
format='h264',
motion_output=flow_analyzer)
prev_angx = 0
prev_angy = 0
prev_time = time.time()
while not rospy.is_shutdown():
mw_data = board.getData(MultiWii.ATTITUDE)
curr_angx = mw_data['angx'] / 180.0 * np.pi
curr_angy = mw_data['angy'] / 180.0 * np.pi
curr_time = time.time()
flow_analyzer.set_angular_velocity(
(curr_angx - prev_angx) / (curr_time - prev_time),
(curr_angy - prev_angy) / (curr_time - prev_time))
prev_angx = curr_angx
prev_angy = curr_angy
prev_time = curr_time
camera.wait_recording(0)
if len(output.images) == 0:
continue
ts, image = output.images[-1]
if ts == last_ts:
continue
time.sleep(0.01)
time.sleep(1)
print board.getData(MultiWii.MOTOR)
board.disarm()
# print "power down"
# for i in range(20):
# board.sendCMD(16,MultiWii.SET_RAW_RC, [1500, 1500, 1500, 988,
# 1500, 1500, 1500, 1500])
# time.sleep(0.01)
# time.sleep(0.5)
print board.getData(MultiWii.MOTOR)
print board.getData(MultiWii.IDENT)
print board.getData(MultiWii.MOTOR)
print board.getData(MultiWii.RAW_IMU)
# Must receive after calibrate.
import rospkg
rospack = rospkg.RosPack()
path = rospack.get_path('pidrone_pkg')
board.calibrate("%s/params/multiwii.yaml" % path)
board.arm()
print "box ids"
print board.getData(MultiWii.BOXIDS)
from h2rMultiWii import MultiWii as MW
def voltage(board):
return float(data['vbat'])/10.0
if __name__ == '__main__':
board = MW('/dev/ttyUSB0')
data = board.getData(MW.ANALOG)
print 'BATTERY VOLTAGE IS\t{}v'.format(voltage(board))
board.close()
class AnalyzePhase(picamera.array.PiMotionAnalysis):
def __init__(self, camera, bridge):
picamera.array.PiMotionAnalysis.__init__(self, camera)
self.bridge = bridge
self.br = tf.TransformBroadcaster()
rospy.Subscriber("/pidrone/set_mode", Mode, self.mode_callback)
rospy.Subscriber("/pidrone/reset_transform", Empty,
self.reset_callback)
rospy.Subscriber("/pidrone/toggle_transform", Empty,
self.toggle_callback)
rospy.Subscriber("/pidrone/infrared", Range, self.range_callback)
self.pospub = rospy.Publisher('/pidrone/set_mode_vel',
Mode,
queue_size=1)
self.first_image_pub = rospy.Publisher("/pidrone/picamera/first_image",
Image,
queue_size=1,
latch=True)
self.prev_img = None
self.prev_kp = None
self.prev_des = None
self.first = True
self.lr_err = ERR()
self.fb_err = ERR()
self.prev_time = None
self.prev_rostime = None
self.pos = [0, 0]
self.z = 0.075
self.smoothed_yaw = 0.0
self.yaw_observed = 0.0
self.iacc_yaw = 0.0
self.transforming = False
self.last_first_time = None
self.target_x = 0
self.target_y = 0
self.first_counter = 0
self.max_first_counter = 0
self.mode = Mode()
self.i = 1
self.board = MultiWii("/dev/ttyUSB0")
def write(self, data):
curr_img = np.reshape(np.fromstring(data, dtype=np.uint8),
(CAMERA_HEIGHT, CAMERA_WIDTH, 3))
curr_rostime = rospy.Time.now()
curr_time = curr_rostime.to_sec()
self.prev_img = curr_img
self.prev_time = curr_time
self.prev_rostime = curr_rostime
def range_callback(self, data):
if data.range != -1:
self.z = data.range
def reset_callback(self, data):
print "Taking picture"
cv2.imwrite("img" + str(self.i) + ".jpg", self.prev_img)
self.i += 1
mw_data = self.board.getData(MultiWii.ATTITUDE)
curr_angx = mw_data['angx'] / 180.0 * np.pi
curr_angy = mw_data['angy'] / 180.0 * np.pi
print curr_angx, curr_angy, self.z
def toggle_callback(self, data):
self.transforming = not self.transforming
print "Position hold", "enabled." if self.transforming else "disabled."
def mode_callback(self, data):
self.mode.mode = data.mode
if not self.transforming or data.mode == 4 or data.mode == 3:
print "VELOCITY"
self.pospub.publish(data)
else:
# TODO 4 is used for what ? Should the target be accumulated ?
self.target_x = data.x_velocity * 4.
self.target_y = data.y_velocity * 4.
print "POSITION", self.target_x, self.target_y