From 07315995fd7fdbdeff1a24e3bf69a25cf76de11d Mon Sep 17 00:00:00 2001 From: Matas Keras Date: Fri, 12 Aug 2022 15:31:14 -0400 Subject: [PATCH] added debugging features to obstacle_detection.py --- src/obstacle_detection.py | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py index c8e6b93..aff78ca 100755 --- a/src/obstacle_detection.py +++ b/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)