added comments to straight_line.py

src/straight_line.py

@@ -59,38 +59,62 @@ def get_forward_acceleration(y, velocity):
     forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-300))/m
     return min(forward_acceleration, forward_acceleration_2)*unit_vector
 
-    
+def guidance_law():
+    #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)
+    #we then add these together to get our acceleration 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
+    accelerations=Vector3Stamped()
+    accelerations.vector.x = accel[0]
+    accelerations.vector.y=accel[1]
+    #we add 2 to the vertical acceleration because otherwise it sometimes
+    #just falls
+    accelerations.vector.z=accel[2]+2
+    return to_publish        
 
 
 def accel_publisher():
     #first bit of code is to get the drone off the ground
+    #first we set up publishers and services
     pub = rospy.Publisher('mavros/setpoint_accel/accel',Vector3Stamped, queue_size=10)
-
     setpointpub = rospy.Publisher('mavros/setpoint_position/local', PoseStamped, queue_size=10)
     rospy.wait_for_service('mavros/set_mode')
     rospy.wait_for_service('mavros/cmd/arming')
     arming_srv = rospy.ServiceProxy('mavros/cmd/arming',CommandBool)
     set_mode_srv = rospy.ServiceProxy('mavros/set_mode', SetMode)
-
-
+    #next we set the rate
     rate = rospy.Rate(20)
+    #now we wait till we are connected
     while not rospy.is_shutdown() and not state.connected:
 
         rate.sleep()
+
     rospy.loginfo('connected')
+    #now we set up our initial position
     initial_pose=PoseStamped()
     initial_pose.pose.position.x = 0
     initial_pose.pose.position.y = 0
-    initial_pose.pose.position.z = 12
+    initial_pose.pose.position.z = 10
+    #we publish the initial pose a few times as there needs to be setpoints
+    #published to allow us to enter offboard mode
     for i in range(100):
         setpointpub.publish(initial_pose)
         rate.sleep()
+    #now we set up our requests to arm the drone and enter offboard mode
     last_request = rospy.get_rostime().secs
     offb_set_mode = SetModeRequest()
     offb_set_mode.custom_mode="OFFBOARD"
     arm = True
-
-    while not rospy.is_shutdown() and (pose[2]<9.5 or velocity[0]>1 or velocity[1]>1):
+    #now we keep trying to arm, enter offboard, and publish setpoints until 
+    #we are hovering at 9.5m or higher, with a stable velocity
+    while not rospy.is_shutdown() and (pose[2]<9.5 or abs( velocity[0])>1 or abs(velocity[1])>1):
         if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)):
             rospy.loginfo('setting mode')
             res=set_mode_srv(offb_set_mode)
@@ -108,29 +132,15 @@ def accel_publisher():
         initial_pose.pose.position.x =pose[0]
         initial_pose.pose.position.y=pose[1]
         setpointpub.publish(initial_pose)
+    #here we set our starting point to wherever the navigation control takes
+    #over control
     global start_point
     start_point=pose
     #Here is where we start doing navigation control
+    #we get our acceleration from the guidance law, then publish it.
     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)
-        #we then add these together to get our acceleration 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
-        
-        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)
+        accelerations = guidance_law()
+        pub.publish(accelerations)
         rate.sleep()
 
 if __name__ == '__main__':