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

added comments to obstacle_test.py

This commit is contained in:
Matas Keras 2022-08-12 15:04:08 -04:00
parent b9f1964d39
commit 84a35d5067
2 changed files with 35 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
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,
#with lower minimum radius meaning a preference for obstacle avoidance
#first we calculate the balance for this cycle
balance = (75-min_radius)/50
balance = (25-min_radius)/20
#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)/4
average_balance=average_balance+balance/4
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:
@ -257,7 +257,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(4):
balance_queue.append(0)
balance = 0
#Here is where we start doing navigation control

39
src/obstacle_test.py
View File

@ -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()