diff --git a/student_flight_controller_node.py b/student_flight_controller_node.py index 71147e3..173a36e 100755 --- a/student_flight_controller_node.py +++ b/student_flight_controller_node.py @@ -1,12 +1,19 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 -import tf +import traceback import sys import yaml + import rospy import rospkg import signal import numpy as np + +print("tf import") +import tf +print("done tf import") + + import command_values as cmds from sensor_msgs.msg import Imu from h2rMultiWii import MultiWii @@ -39,7 +46,9 @@ class FlightController(object): def __init__(self): # Connect to the flight controller board + print("getboard") self.board = self.getBoard() + print("done") # stores the current and previous modes self.curr_mode = 'DISARMED' #initialize as disarmed self.prev_mode = 'DISARMED' #initialize as disarmed @@ -51,7 +60,6 @@ def __init__(self): # store the time for angular velocity calculations self.time = rospy.Time.now() - # Initialize the Imu Message ############################ # TODO: create an Imu message with some initial values. @@ -59,10 +67,10 @@ def __init__(self): # published (you will do this in another TODO). # Hint: investigate the Header message type, the Imu # message type, and rospy.Time. - self.imu_message = ??? - self.imu_message.header = ??? + self.imu_message = ... + self.imu_message.header = ... self.imu_message.header.frame_id = 'Body' - self.imu_message.header.stamp = ??? + self.imu_message.header.stamp = ... # Initialize the Battery Message ################################ @@ -70,23 +78,26 @@ def __init__(self): # In a different method, the message will be updated and # published (you will do this in another TODO). Hint: read # the Battery message file in the pidrone_pkg msg folder. - self.battery_message = ??? - self.battery_message.??? = None - self.battery_message.??? = None + self.battery_message = ... + self.battery_message.CHANGE_FIELD_HERE = None + self.battery_message.CHANGE_FIELD_HERE = None # Adjust this based on how low the battery should discharge self.minimum_voltage = 4.5 # Accelerometer parameters ########################## + print("loading") rospack = rospkg.RosPack() path = rospack.get_path('pidrone_pkg') with open("%s/params/multiwii.yaml" % path) as f: means = yaml.load(f) + print("done") self.accRawToMss = 9.8 / means["az"] - self.accZeroX = means["ax"] * self.accRawToMss - self.accZeroY = means["ay"] * self.accRawToMss - self.accZeroZ = means["az"] * self.accRawToMss + self.accZeroX=means["ax"] * self.accRawToMss + self.accZeroY=means["ay"] * self.accRawToMss + self.accZeroZ=means["az"] * self.accRawToMss + pass # ROS subscriber callback methods: @@ -113,9 +124,9 @@ def update_imu_message(self): Compute the ROS IMU message by reading data from the board. """ - # Extract roll, pitch, heading + # extract roll, pitch, heading self.board.getData(MultiWii.ATTITUDE) - # Extract lin_acc_x, lin_acc_y, lin_acc_z + # extract lin_acc_x, lin_acc_y, lin_acc_z self.board.getData(MultiWii.RAW_IMU) # TODO: calculate roll, pitch, and yaw in radians. @@ -123,13 +134,13 @@ def update_imu_message(self): # Hint: what data is in self.board.attitude? What about # self.board.rawIMU? Is the relevant data in degrees or radians? # Hint: yaw is sometimes referred to as "heading". - roll = ??? - pitch = ??? - heading = ??? + roll = ... + pitch = ... + heading = ... # Note that at pitch angles near 90 degrees, the roll angle reading can # fluctuate a lot - - # Transform heading (yaw) to standard math conventions, which + + # transform heading (similar to yaw) to standard math conventions, which # means angles are in radians and positive rotation is CCW heading = (-heading) % (2 * np.pi) @@ -141,7 +152,7 @@ def update_imu_message(self): # and vice-versa. # Hint: investigate tf.transformations.euler_from_quaternion. previous_quaternion = self.imu_message.orientation - previous_roll, previous_pitch, previous_heading = ??? + previous_roll, previous_pitch, previous_heading = ... # Although quaternion_from_euler takes a heading in range [0, 2pi), # euler_from_quaternion returns a heading in range [0, pi] or [0, -pi). @@ -152,89 +163,70 @@ def update_imu_message(self): # Hint: recall that Euler angles can be transformed to quaternions, # and vice-versa. # Hint: investigate tf.transformations.quaternion_from_euler. - quaternion = ??? + quaternion = ... # TODO: extract the raw linear accelerations from the flight controller. # Hint: what data is in self.board.attitude? What about # self.board.rawIMU? - raw_acc_x = ??? - raw_acc_y = ??? - raw_acc_z = ??? - + raw_acc_x = ... + raw_acc_y = ... + raw_acc_z = ... + # Turn the raw linear accelerations into real accelerations lin_acc_x = raw_acc_x * self.accRawToMss - self.accZeroX lin_acc_y = raw_acc_y * self.accRawToMss - self.accZeroY lin_acc_z = raw_acc_z * self.accRawToMss - self.accZeroZ - - # TODO: extract the raw angular velicities from the flight controller. - raw_ang_vel_x = ??? - raw_ang_vel_y = ??? - raw_ang_vel_z = ??? - - # Calculate the rotational rates - - - rospack = rospkg.RosPack() - path = rospack.get_path('pidrone_pkg') - with open("%s/params/multiwii.yaml" % path) as f: - means = yaml.load(f) - self.XGyroRawToRs = 0.0010569610567923715 # rad/s/raw - self.YGyroRawToRs = 0.0010533920049110032 # rad/s/raw - self.ZGyroRawToRs = 0.0010644278634753999 # rad/s/raw - self.gyroZeroX = means["gx"] * self.XGyroRawToRs # raw * rad/s/raw = rad/s - self.gyroZeroY = means["gy"] * self.YGyroRawToRs # raw * rad/s/raw = rad/s - self.gyroZeroZ = means["gz"] * self.ZGyroRawToRs # raw * rad/s/raw = rad/s - ang_vel_x = raw_ang_vel_x * self.XGyroRawToRs - self.gyroZeroX - ang_vel_y = raw_ang_vel_y * self.YGyroRawToRs - self.gyroZeroY - ang_vel_z = raw_ang_vel_z * self.ZGyroRawToRs - self.gyroZeroZ - # Rotate the IMU frame to align with our convention for the drone's body - # frame. IMU accelerometer: x is forward, y is left, z is up. We want: x - # is right, y is forward, z is up. - # ACC + # frame. IMU: x is forward, y is left, z is up. We want: x is right, + # y is forward, z is up. lin_acc_x_drone_body = -lin_acc_y lin_acc_y_drone_body = lin_acc_x lin_acc_z_drone_body = lin_acc_z - # GYRO - ang_vel_x_drone_body = ang_vel_x - ang_vel_y_drone_body = -ang_vel_y - ang_vel_z_drone_body = ang_vel_z # Account for gravity's affect on linear acceleration values when roll # and pitch are nonzero. When the drone is pitched at 90 degrees, for # example, the z acceleration reads out as -9.8 m/s^2. This makes sense, - # since our calibration variable, accZeroZ, zeros the body-frame z-axis - # acceleration to 0 when the drone is level. However, when it's pitched + # as the IMU, when powered up / when the calibration script is called, + # zeros the body-frame z-axis acceleration to 0, but when it's pitched # 90 degrees, the body-frame z-axis is perpendicular to the force of - # gravity the IMU reads -9.8 m/s^2 along the z-axis. + # gravity, so, as if the drone were in free-fall (which was roughly + # confirmed experimentally), the IMU reads -9.8 m/s^2 along the z-axis. g = 9.8 - lin_acc_x_drone_body = lin_acc_x_drone_body + g * np.sin(roll) * np.cos(pitch) - lin_acc_y_drone_body = lin_acc_y_drone_body + g * np.cos(roll) * (-np.sin(pitch)) - lin_acc_z_drone_body = lin_acc_z_drone_body + g * (1 - np.cos(roll) * np.cos(pitch)) + lin_acc_x_drone_body = lin_acc_x_drone_body + g*np.sin(roll)*np.cos(pitch) + lin_acc_y_drone_body = lin_acc_y_drone_body + g*np.cos(roll)*(-np.sin(pitch)) + lin_acc_z_drone_body = lin_acc_z_drone_body + g*(1 - np.cos(roll)*np.cos(pitch)) + # calculate the angular velocities of roll, pitch, and yaw in rad/s time = rospy.Time.now() + dt = ... + dr = ... + dp = ... + dh = ... + angvx = self.near_zero(dr / dt) + angvy = self.near_zero(dp / dt) + angvz = self.near_zero(dh / dt) self.time = time # TODO: Update the imu_message header stamp. - self.imu_message.header.stamp = ??? + self.imu_message.header.stamp = ... # TODO: update the IMU message orientation # Hint: is the orientation a set of Euler angles or a quaternion? - self.imu_message.orientation.x = ??? - self.imu_message.orientation.y = ??? - self.imu_message.orientation.z = ??? - self.imu_message.orientation.w = ??? + self.imu_message.orientation.x = ... + self.imu_message.orientation.y = ... + self.imu_message.orientation.z = ... + self.imu_message.orientation.w = ... # TODO: update the IMU message angular velocities. - self.imu_message.??? = ang_vel_x_drone_body - self.imu_message.??? = ang_vel_y_drone_body - self.imu_message.??? = ang_vel_z_drone_body + self.imu_message.CHANGE_FIELD_HERE = ang_vel_x_drone_body + self.imu_message.CHANGE_FIELD_HERE = ang_vel_y_drone_body + self.imu_message.CHANGE_FIELD_HERE = ang_vel_z_drone_body # TODO: update the IMU message linear accelerations. - self.imu_message.linear_acceleration.x = ??? - self.imu_message.linear_acceleration.y = ??? - self.imu_message.linear_acceleration.z = ??? + self.imu_message.linear_acceleration.x = ... + self.imu_message.linear_acceleration.y = ... + self.imu_message.linear_acceleration.z = ... def update_battery_message(self): @@ -246,8 +238,8 @@ def update_battery_message(self): # TODO: Update Battery message: - self.battery_message.vbat = ??? * 0.10 - self.battery_message.amperage = ??? + self.battery_message.vbat = ... * 0.10 + self.battery_message.amperage = ... @@ -270,12 +262,12 @@ def getBoard(self): try: board = MultiWii('/dev/ttyUSB0') except SerialException as e: - print("usb0 failed: " + str(e)) + print(("usb0 failed: " + str(e))) try: board = MultiWii('/dev/ttyUSB1') except SerialException: - print '\nCannot connect to the flight controller board.' - print 'The USB is unplugged. Please check connection.' + print('\nCannot connect to the flight controller board.') + print('The USB is unplugged. Please check connection.') raise sys.exit() return board @@ -285,7 +277,7 @@ def send_cmd(self): self.board.sendCMD(8, MultiWii.SET_RAW_RC, self.command) self.board.receiveDataPacket() if (self.command != self.last_command): - print 'command sent:', self.command + print(('command sent:', self.command)) self.last_command = self.command def near_zero(self, n): @@ -294,12 +286,12 @@ def near_zero(self, n): def ctrl_c_handler(self, signal, frame): """ Disarm the drone and quits the flight controller node """ - print "\nCaught ctrl-c! About to Disarm!" + print("\nCaught ctrl-c! About to Disarm!") self.board.sendCMD(8, MultiWii.SET_RAW_RC, cmds.disarm_cmd) self.board.receiveDataPacket() rospy.sleep(1) self.modepub.publish('DISARMED') - print "Successfully Disarmed" + print("Successfully Disarmed") sys.exit() # Heartbeat Callbacks: These update the last time that data was received @@ -342,6 +334,10 @@ def shouldIDisarm(self): print('\nSafety Failure: not receiving data from the IR sensor.') print('Check the infrared node\n') disarm = True + + if self.range > 0.5: + print(('\nSafety Failure: too high: ' + str(self.range))) + disarm = True if curr_time - self.heartbeat_state_estimator > rospy.Duration.from_sec(1): print('\nSafety Failure: not receiving a state estimate.') print('Check the state_estimator node\n') @@ -354,12 +350,14 @@ def main(): # ROS Setup ########### node_name = os.path.splitext(os.path.basename(__file__))[0] + print("init") rospy.init_node(node_name) - + print("done") # create the FlightController object fc = FlightController() curr_time = rospy.Time.now() fc.heartbeat_infrared = curr_time + fc.range = None fc.heartbeat_web_interface= curr_time fc.heartbeat_pid_controller = curr_time fc.heartbeat_flight_controller = curr_time @@ -370,10 +368,10 @@ def main(): imupub = rospy.Publisher('/pidrone/imu', Imu, queue_size=1, tcp_nodelay=False) batpub = rospy.Publisher('/pidrone/battery', Battery, queue_size=1, tcp_nodelay=False) fc.modepub = rospy.Publisher('/pidrone/mode', Mode, queue_size=1, tcp_nodelay=False) - print 'Publishing:' - print '/pidrone/imu' - print '/pidrone/mode' - print '/pidrone/battery' + print('Publishing:') + print('/pidrone/imu') + print('/pidrone/mode') + print('/pidrone/battery') # Subscribers ############ @@ -393,10 +391,10 @@ def main(): while not rospy.is_shutdown(): # if the current mode is anything other than disarmed # preform as safety check - if fc.curr_mode != 'DISARMED': # Break the loop if a safety check has failed - if fc.shouldIDisarm(): - break + if fc.shouldIDisarm(): + print(("mode", fc.curr_mode)) + break # update and publish flight controller readings fc.update_battery_message() @@ -415,13 +413,14 @@ def main(): r.sleep() except SerialException: - print '\nCannot connect to the flight controller board.' - print 'The USB is unplugged. Please check connection.' - except: - print 'there was an internal error' + print('\nCannot connect to the flight controller board.') + print('The USB is unplugged. Please check connection.') + except Exception as e: + print(('there was an internal error', e)) + print((traceback.format_exc())) finally: - print 'Shutdown received' - print 'Sending DISARM command' + print('Shutdown received') + print('Sending DISARM command') fc.board.sendCMD(8, MultiWii.SET_RAW_RC, cmds.disarm_cmd) fc.board.receiveDataPacket() diff --git a/student_optical_flow_node.py b/student_optical_flow_node.py index d10aa2e..f876652 100755 --- a/student_optical_flow_node.py +++ b/student_optical_flow_node.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 -from __future__ import division + import rospy import numpy as np from geometry_msgs.msg import TwistStamped @@ -55,9 +55,7 @@ def setup(self): # TODO: subscribe to /raspicam_node/motion_vectors to extract the flow vectors for estimating velocity. # message type: MotionVectors # callback method: motion_cb - - - + pass def motion_cb(self, msg): ''' Average the motion vectors and publish the @@ -70,8 +68,8 @@ def motion_cb(self, msg): # calculate the planar and yaw motions # TODO: calculate the optical flow velocities by summing the flow vectors - opflow_x = ??? - opflow_y = ??? + opflow_x = ... + opflow_y = ... x_motion = opflow_x * self.flow_coeff * self.altitude @@ -96,8 +94,8 @@ def altitude_cb(self, msg): msg: the message publishing the altitude """ - self.altitude = ?? - self.altitude_ts = ?? + self.altitude = ... + self.altitude_ts = ... def main(): optical_flow_node = OpticalFlowNode("optical_flow_node") diff --git a/student_rigid_transform_node.py b/student_rigid_transform_node.py index c5cad9c..2be4630 100755 --- a/student_rigid_transform_node.py +++ b/student_rigid_transform_node.py @@ -1,6 +1,5 @@ #!/usr/bin/env python3 - import tf import time import cv2 @@ -13,6 +12,7 @@ from sensor_msgs.msg import Range from cv_bridge import CvBridge +CAMERA_WIDTH_PIXELS, CAMERA_HEIGHT_PIXELS = 320, 240 class RigidTransformNode(object): """ @@ -21,6 +21,12 @@ class RigidTransformNode(object): For more info, visit: https://docs.opencv.org/3.0-beta/modules/video/doc/motion_analysis_and_object_tracking.html#estimaterigidtransform + Publisher: + ~pose + + Subscribers: + ~reset_transform + ~position_control """ def __init__(self, node_name): # initialize the DTROS parent class @@ -68,17 +74,19 @@ def __init__(self, node_name): # TODO: subscribe to /pidrone/reset_transform # message type: Empty # callback method: reset_callback + self._rtsub = ... # TODO: subscribe to /pidrone/position_control # message type: Bool # callback method: position_control_callback + self._pcsub = ... # TODO: subscribe to /pidrone/state # message type: State # callback method: state_callback - + self._stsub = ... - # Subscribers + # Additional Subscribers self._isub = rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, self.image_callback, queue_size=1) self._sub_alt = rospy.Subscriber('/pidrone/range', Range, self.altitude_cb, queue_size=1) @@ -157,8 +165,8 @@ def image_callback(self, msg): # calculate the x,y, and yaw translations from the transformation translation_first, yaw_first = self.translation_and_yaw(transform_first) # use an EMA filter to smooth the position and yaw values - self.pose_msg.pose.position.x = translation_first[0] - self.pose_msg.pose.position.y = translation_first[1].altitude + self.pose_msg.pose.position.x = translation_first[0]*self.altitude + self.pose_msg.pose.position.y = translation_first[1]*self.altitude # With just a yaw, the x and y components of the # quaternion are 0 _,_,z,w = tf.transformations.quaternion_from_euler(0,0,yaw_first) @@ -168,7 +176,7 @@ def image_callback(self, msg): self.first_image_counter += 1 self.max_first_counter = max(self.max_first_counter, self.first_image_counter) self.last_first_time = rospy.get_time() - print("count:", self.first_image_counter) + print(("count:", self.first_image_counter)) # else the first image was not visible (the transformation was not succesful) : else: # try to estimate the transformation from the previous image @@ -181,17 +189,18 @@ def image_callback(self, msg): if self.last_first_time is None: self.last_first_time = rospy.get_time() time_since_first = rospy.get_time() - self.last_first_time - print("integrated", time_since_first) - print("max_first_counter: ", self.max_first_counter) - + print(("integrated", time_since_first)) + print(("max_first_counter: ", self.max_first_counter)) int_displacement, yaw_previous = self.translation_and_yaw(transform_previous) + # TODO calculate the position by adding the displacement to the previous # position of the drone. + # HINT: use self.x_position_from_state and self.y_position_from_state as the # previous position - self.pose_msg.pose.position.x = self.x_position_from_state + ??? - self.pose_msg.pose.position.y = self.y_position_from_state + ??? + self.pose_msg.pose.position.x = self.x_position_from_state + ... + self.pose_msg.pose.position.y = self.y_position_from_state + ... @@ -238,14 +247,14 @@ def translation_and_yaw(self, transform): # TODO: extract the translation information from the transform variable. Divide the # the x displacement by 320, which is the width of the camera resolution. Divide the # y displacement by 240, the height of the camera resolution. - pixel_translation_x_y = ??? + pixel_translation_x_y = ... real_translation_x_y = [0.0, 0.0] - real_translation_x_y[0] = (pixel_translation_x_y[0] / 320.0) * self.altitude - real_translation_x_y[1] = (pixel_translation_x_y[1] / 240.0) * self.altitude + real_translation_x_y[0] = (pixel_translation_x_y[0] / CAMERA_WIDTH_PIXELS) * self.altitude + real_translation_x_y[1] = (pixel_translation_x_y[1] / CAMERA_HEIGHT_PIXELS) * self.altitude # TODO: use np.arctan2 and the transform variable to calculate the yaw - yaw = ??? + yaw = ... return real_translation_x_y, yaw @@ -253,6 +262,7 @@ def translation_and_yaw(self, transform): # ROS CALLBACK METHODS: ####################### # TODO: Implement + def reset_callback(self, msg): """ Reset the current position and orientation """ print("Resetting Phase") diff --git a/student_tof_pub.py b/student_tof_pub.py index a686616..a4cc60c 100755 --- a/student_tof_pub.py +++ b/student_tof_pub.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 import rospy