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 = (25-min_radius)/20
+    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)/4
-    average_balance=average_balance+balance/4
+    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
@@ -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(4):
+    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)