scorpio1
/
OscillationControlWithCircle
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

added debugging features to obstacle_detection.py

This commit is contained in:
Matas Keras 2022-08-12 15:31:14 -04:00
parent a11b0673f2
commit 07315995fd
1 changed files with 10 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
src/obstacle_detection.py
View File

@ -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, #depends on the average over the past 14 measurements of the minimum radius,
#with lower minimum radius meaning a preference for obstacle avoidance #with lower minimum radius meaning a preference for obstacle avoidance
#first we calculate the balance for this cycle #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 #the balance value is moved to be between 0 and 1
if balance>1: if balance>1:
balance = 1.0 balance = 1.0
@ -45,8 +45,8 @@ def get_balance(min_radius, balance_queue, average_balance):
balance=0.0 balance=0.0
#now we update the average balance, and update the list of past balance #now we update the average balance, and update the list of past balance
#values #values
average_balance=average_balance-balance_queue.pop(0)/4 average_balance=average_balance-balance_queue.pop(0)/10
average_balance=average_balance+balance/4 average_balance=average_balance+balance/10
balance_queue.append(balance) balance_queue.append(balance)
#finally, we return the average value, and the list of past values #finally, we return the average value, and the list of past values
return balance_queue, average_balance return balance_queue, average_balance
@ -194,6 +194,9 @@ def guidance_law(balance_queue_input, balance_input):
accelerations.vector.x = accel[0] accelerations.vector.x = accel[0]
accelerations.vector.y=accel[1] accelerations.vector.y=accel[1]
accelerations.vector.z=accel[2] 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 return accelerations, balance_queue, balance
@ -257,7 +260,7 @@ def accel_publisher():
#create variables for smoothing out the change between avoidance and #create variables for smoothing out the change between avoidance and
#straight line flight #straight line flight
balance_queue = [] balance_queue = []
for x in range(4): for x in range(10):
balance_queue.append(0) balance_queue.append(0)
balance = 0 balance = 0
#Here is where we start doing navigation control #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_3 = np.array([0,0,1])
unit_vector = unit_vector/np.linalg.norm(unit_vector) unit_vector = unit_vector/np.linalg.norm(unit_vector)
unit_vector_2 = unit_vector_2/np.linalg.norm(unit_vector_2) 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) state = State(connected=False,armed=False,guided=False,mode="MANUAL",system_status=0)
rospy.init_node('circle_accelerations',anonymous=True) 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/velocity_body', TwistStamped, velocity_callback)
rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback) rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
rospy.Subscriber('mavros/state', State, state_callback) rospy.Subscriber('mavros/state', State, state_callback)