added debugging features to obstacle_detection.py

src/obstacle_detection.py

@@ -37,7 +37,7 @@ def get_balance(min_radius, balance_queue, average_balance):
     #depends on the average over the past 14 measurements of the minimum radius,
     #with lower minimum radius meaning a preference for obstacle avoidance
     #first we calculate the balance for this cycle
-    balance = (75-min_radius)/50
+    balance = (40-min_radius)/25
     #the balance value is moved to be between 0 and 1
     if balance>1:
         balance = 1.0
@@ -45,8 +45,8 @@ def get_balance(min_radius, balance_queue, average_balance):
         balance=0.0
     #now we update the average balance, and update the list of past balance
     #values
-    average_balance=average_balance-balance_queue.pop(0)/14
-    average_balance=average_balance+balance/14
+    average_balance=average_balance-balance_queue.pop(0)/10
+    average_balance=average_balance+balance/10
     balance_queue.append(balance)
     #finally, we return the average value, and the list of past values
     return balance_queue, average_balance
@@ -113,7 +113,7 @@ def avoidance(weights):
     #next we calculate the magnitude of the velocity in the xy direction,
     #and the magnitude of the acceleration vector needed
     magnitude_velocity = np.sqrt(velocity[1]*velocity[1]+velocity[0]*velocity[0])
-    magnitude_acceleration = 1*(magnitude_velocity*magnitude_velocity)/(min_radius)
+    magnitude_acceleration = 0.1*(magnitude_velocity*magnitude_velocity)/(min_radius)
     #if the velocity is too low (below 5m/s) we also have an additional forward
     #acceleration
     if magnitude_velocity<5:
@@ -194,6 +194,9 @@ def guidance_law(balance_queue_input, balance_input):
     accelerations.vector.x = accel[0]
     accelerations.vector.y=accel[1]
     accelerations.vector.z=accel[2]
+    #we also publish the minimum radius, and the balance for debugging purposes
+    balance_pub.publish(balance)
+    min_radius_pub.publish(min_radius)
     return accelerations, balance_queue, balance
 
 
@@ -257,7 +260,7 @@ def accel_publisher():
     #create variables for smoothing out the change between avoidance and
     #straight line flight
     balance_queue = []
-    for x in range(14):
+    for x in range(10):
         balance_queue.append(0)
     balance = 0
     #Here is where we start doing navigation control
@@ -291,9 +294,11 @@ if __name__ == '__main__':
     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
+    #here we start creating the subscribers, some publishers, and launching the drone
     state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0)
     rospy.init_node('circle_accelerations',anonymous=True)
+    balance_pub = rospy.Publisher('balance', Float32, queue_size=10)
+    min_radius_pub = rospy.Publisher('min_radius', Float32, queue_size=10)
     rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)
     rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
     rospy.Subscriber('mavros/state', State, state_callback)

src/obstacle_test.py

@@ -10,8 +10,8 @@ from sensor_msgs.msg import LaserScan
 from std_msgs.msg import Header
 
 def velocity_callback(data):
-    global center_vector
-    center_vector = (data.twist.linear.x,data.twist.linear.y)
+    global velocity
+    velocity = (data.twist.linear.x,data.twist.linear.y)
     
 
 def pose_callback(data):
@@ -19,15 +19,16 @@ def pose_callback(data):
     pose = data.pose.position
 
 def intersection(line1, line2):
-    #get y intercept from  y = b2-b1/m1-m2
+    #get x of intercept from  x = b2-b1/m1-m2
     x = (line2[1]-line1[1])/(line1[0]-line2[0])
+    #get y of intercept from y = mx+b (for either line
     y = x*line1[0]+line1[1]
     return np.array([x,y])
 
 def distance_to_line(line1,vector1, center1, line2, bound1, bound2):
     intersect = intersection(line1,line2)
     #if the intersection point is outside where the line actually exists, return inifinity
-    if not (intersect[0]>bound1[0] and intersect[1]>bound1[1] and intersect[0]<bound2[0] and intersect[1]<bound2[1]):
+    if not (intersect[0]>bound1 and intersect[0]<bound2):
         return float('inf')
     recentered_intersect = intersect-center1
     distance = recentered_intersect[0]/vector1[0]
@@ -36,6 +37,7 @@ def distance_to_line(line1,vector1, center1, line2, bound1, bound2):
     #if the distance is negative, the point is behind, so we return infinity to show nothing ahead
     if distance<0:
         return float('NaN')
+    #if the drone gets too close of the wall, say the drone crashed
     if distance<0.5:
         raise Exception('crashed')
 
@@ -49,22 +51,36 @@ def calculate_distance_array():
     distances = np.zeros(61)
     #and the drones current pose
     center = np.array([pose.x,pose.y])
-    #next we de
-    first_angle = np.arctan(center_vector[1]/center_vector[0])-30*unit_degree
-    if center_vector[0]<0:
+    #next we calculate the first angle, which is the angle of the first item in
+    #the directions array. This is calculated as the direction the drone is 
+    #travelling minus 30 times the angle between each element of the array
+    first_angle = np.arctan(velocity[1]/velocity[0])-30*unit_degree
+    if velocity[0]<0:
         first_angle+=np.pi
+    #next we populate the array
     for x in range(61):
+        #for each element, we first calculate the angle that the distance will
+        #be for
         angle = first_angle+x*unit_degree
+        #next we calculate a unit vector in that direction
         vector = np.array([np.cos(angle),np.sin(angle)])
+        #next we calculate the equation for the line.
         line = [0,0]
+        #the m value is calculated as m = delta y/delta x, with delta x and y 
+        #being from the vector we just calculated
         line[0] = vector[1]/vector[0]
+        #the b value is calculated as b = y-mx. Here we use pose of the drone
+        #for x and y values
         line[1] = center[1]-line[0]*center[0]
+        #next for each wall, we calculate the distance to the wall, and we take
+        #the smallest distance
         distance = float('inf')
         for line_boundary_set in wall_lines:
             temp_distance = distance_to_line(line,vector, center,line_boundary_set[0],line_boundary_set[1],line_boundary_set[2])
             if temp_distance<distance:
                 distance = temp_distance
         distances[x] = distance
+    #after we calculate the distances for each wall, we return them
     return distances
 
 def distance_publisher():
@@ -99,8 +115,13 @@ if __name__ == '__main__':
     #unit_degree defines the amount the change in angle between scans
     unit_degree = 3*np.pi/180
     pose = None
-    #wall_lines is a variable that defines all of the lines that make up obstacles, as well as their boundaries 
-    wall_lines = [ ([-0.5,200],[-500,-10000],[500,10000])]
+    #wall_lines is a variable that defines all of the lines that make up 
+    #obstacles, as well as their boundaries wall lines is a list of tuples. 
+    #Each defines a wall, and is written in the form
+    #([m,b],lower bound for x, upper bound for x)
+    #where the line is defined by y=mx+b, but only exists when x is between
+    #the lower and upper bounds.
+    wall_lines = [ ([-1.5,100],-500,500),([-0.5,200],-500,500)]
 
     try:
         distance_publisher()