added comments to straight_line.py

src/straight_line.py

@@ -26,6 +26,7 @@ def state_callback(data):
 
 G=9.8
 def convert_to_cylindrical(point):
+    #this code converts a point to cylindrical coordinates
     recentered = point-start_point
     y=np.dot(recentered, unit_vector)
     circle_portion = recentered-y*unit_vector
@@ -37,28 +38,32 @@ def convert_to_cylindrical(point):
     return y, r, theta, circle_portion
 
 def get_error_acceleration(circle_position, r, velocity):
+    #here we take the velocity and displacement in the direction of the second 
+    #and third unit vector, and use them to calculate accelerations
+    #based off modelling them as a damped spring
     velocity_unit_2 = np.dot(velocity, unit_vector_2)
     velocity_unit_3 = np.dot(velocity, unit_vector_3)
     circle_unit_2 = np.dot(circle_position, unit_vector_2)
     circle_unit_3 = np.dot(circle_position, unit_vector_3)
     accel = -1*(b3*velocity_unit_2+k*circle_unit_2)*unit_vector_2
     accel = accel - (b*velocity_unit_3+k*circle_unit_3)*unit_vector_3
-    #velocity_in_r = np.dot(circle_position, velocity)/r
     
-    #accel = -1*(b*velocity_in_r*circle_position/r + k*circle_position)/m
     return accel
 
 def get_forward_acceleration(y, velocity):
+    #here we calculate the forward acceleration, either based off how far off
+    #the target speed we are, or based off a damped spring, depending on which
+    #is less (or if we are close to the finish)
     forward_velocity=np.dot(velocity,unit_vector)
     forward_acceleration = 2*(9.5-forward_velocity)
     forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m
     return min(forward_acceleration, forward_acceleration_2)*unit_vector
 
-    return forward_acceleration
+    
 
 
 def accel_publisher():
-
+    #first bit of code is to get the drone off the ground
     pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10)
 
     setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
@@ -105,26 +110,25 @@ def accel_publisher():
         setpointpub.publish(initial_pose)
     global start_point
     start_point=pose
-    rospy.logwarn('x: %f y: %f z: %f' %(pose[0], pose[1], pose[2]))
+    #Here is where we start doing navigation control
     while not rospy.is_shutdown():
+        #here we calculate the acceleration we need in the forward direction,
+        #and the acceleration we need that's perpendicular to that direction
         y,r,theta,circle_position=convert_to_cylindrical(pose)
         forward_accel = get_forward_acceleration(y,velocity)
         error_accel = get_error_acceleration(circle_position,r,velocity)
-        #accel = get_error_acceleration(circle_position, r, velocity) + get_forward_acceleration(y,velocity)
+        #we then add these together to get our acceleration vector
-        #accel = get_forward_acceleration(y,velocity)
-        #if(np.linalg.norm(velocity)>8) and (np.linalg.norm(forward_accel)>0):
-        #    forward_accel = -0*unit_vector
         accel = forward_accel+error_accel
+        #then we decrease it if the acceleration is over 2g
         magnitude_accel=np.linalg.norm(accel)
         if magnitude_accel>2*G:
             accel=accel/magnitude_accel*2*G
-        #if np.linalg.norm(velocity)>8:
-        #    accel = [0,0,0]
-        #accel+=get_error_acceleration(circle_position,r,velocity)
         
         to_publish=Vector3Stamped()
         to_publish.vector.x = accel[0]
         to_publish.vector.y=accel[1]
+        #we add 2 to the vertical acceleration because otherwise it sometimes
+        #just falls
         to_publish.vector.z=accel[2]+2
         pub.publish(to_publish)
         rate.sleep()
@@ -132,20 +136,22 @@ def accel_publisher():
 if __name__ == '__main__':
     pose = None
     velocity = None
+    #describe constants for acceleration
     k = 2
-    
     k2 = 0.5
-    
     m=1.5
     b3 = 1*np.sqrt(m*k)
     b = 0.4*np.sqrt(m*k)
     b2 = 2*np.sqrt(m*k2)
+    #describe the vectors that define our direction of travel
+    #the first vector is the direction we travel, the other two are two more
+    #vectors that form an orthonormal basis with the first one
     unit_vector = np.array([1,0,0])
     unit_vector_2 = np.array([0,1,0])
     unit_vector_3 = np.array([0,0,1])
     unit_vector = unit_vector/np.linalg.norm(unit_vector)
     unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2)
-    
+    #here we start creating the subscribers, and launching the drone
     state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0)
     rospy.init_node('circle_accelerations',anonymous=True)
     rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)