fixes and comments for obstacle avoidance
This commit is contained in:
parent
ad4b34e3b8
commit
b9f1964d39
|
|
@ -29,70 +29,102 @@ def obstacle_callback(data):
|
|||
weights = np.array(data.ranges)
|
||||
|
||||
|
||||
def get_balance(weights):
|
||||
|
||||
balance = (50-min(weights))/25
|
||||
def get_balance(min_radius, balance_queue, average_balance):
|
||||
#this method calculates a value that corresponds to whether avoidance or
|
||||
#straight line flight should be prioritized
|
||||
#here min_radius is the radius of a turn needed to stay 5m away from
|
||||
#obstacles at all time. Whether or not obstacle avoidance is prefered
|
||||
#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
|
||||
#the balance value is moved to be between 0 and 1
|
||||
if balance>1:
|
||||
balance = 1
|
||||
balance = 1.0
|
||||
if balance<0:
|
||||
balance=0
|
||||
|
||||
#if balance>0.5:
|
||||
# raise Exception("already above 0.5")
|
||||
return 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
|
||||
balance_queue.append(balance)
|
||||
#finally, we return the average value, and the list of past values
|
||||
return balance_queue, average_balance
|
||||
|
||||
|
||||
|
||||
|
||||
def get_min_radius(weights):
|
||||
#this function calculates the minimum radius of a turn needed to remain
|
||||
#5 meters away from any obstacles
|
||||
#first we initialize some variables
|
||||
length = weights.size
|
||||
radii_left = np.zeros(length)
|
||||
radii_right = np.zeros(length)
|
||||
#next for each different length measurement, we calculate a radius we would
|
||||
#need if we turned left, and if we turned right. If the distance is
|
||||
#infinity, we just set the radius to be infinity, otherwise we use the get
|
||||
#radius function
|
||||
for x in range(length):
|
||||
if weights[x] == float('inf'):
|
||||
radii_left[x] = 100000
|
||||
radii_right[x] = 100000
|
||||
radii_left[x] = float('inf')
|
||||
radii_right[x] = float('inf')
|
||||
else:
|
||||
angle = (x-30)*one_degree
|
||||
radii_left[x],radii_right[x] = get_radius(weights[x],angle)
|
||||
if radii_left[x]<=0:
|
||||
radii_left[x]=100000
|
||||
if radii_right[x]<=0:
|
||||
radii_right[x]=1000000
|
||||
|
||||
|
||||
#once we have calculated all of the radii, we get the minimum radius for
|
||||
#both turning left and right
|
||||
index_left = np.argmin(radii_left)
|
||||
index_right = np.argmin(radii_right)
|
||||
if radii_left[index_left]>radii_right[index_right]:
|
||||
#if the minimum radius is higher if we turn right, we turn right. Otherwise
|
||||
#we turn left
|
||||
if radii_left[index_left]<radii_right[index_right]:
|
||||
return index_right, radii_right[index_right], False
|
||||
else:
|
||||
return index_left, radii_left[index_left],True
|
||||
|
||||
|
||||
def get_radius(weight, angle):
|
||||
#here we calculate the radius needed to stay 5m from an obstacle. We do so
|
||||
#using the cosine law.
|
||||
radius_left = (weight*weight-25)/(10-2*weight*np.cos(np.pi/2-angle))
|
||||
radius_right = (weight*weight-25)/(10-2*weight*np.cos(np.pi/2+angle))
|
||||
return radius_left,radius_right
|
||||
return abs(radius_left),abs(radius_right)
|
||||
|
||||
|
||||
def avoidance(weights):
|
||||
#this function uses the different length measurements to calculate the
|
||||
#accelerations needed for avoidance
|
||||
#first we call the get min radius function to get the minimum radius,
|
||||
#as well as a boolean that determines whether we go left or right
|
||||
index, min_radius, go_left = get_min_radius(weights)
|
||||
#next we calculate the angle of the velocity, so we know which direction
|
||||
#would be straight ahead
|
||||
angle_velocity=np.arctan(velocity[1]/velocity[0])
|
||||
if velocity[0]<0:
|
||||
angle_velocity+=np.pi
|
||||
#if we want to turn left, we add 90 degrees to the angle, otherwise
|
||||
#we subtract 90 degrees
|
||||
if go_left:
|
||||
angle_acceleration = angle_velocity-np.pi/2
|
||||
else:
|
||||
angle_acceleration = angle_velocity+np.pi/2
|
||||
magnitude_acceleration = 1*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/(min_radius)
|
||||
#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)
|
||||
#if the velocity is too low (below 5m/s) we also have an additional forward
|
||||
#acceleration
|
||||
if magnitude_velocity<5:
|
||||
magnitude_forward_accel = 5-magnitude_velocity
|
||||
else:
|
||||
magnitude_forward_accel = 0
|
||||
#finally we calculate the acceleration in the x and the y directions
|
||||
#from the magnitude of the accelerations, and the angles
|
||||
acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration+np.cos(angle_velocity)*magnitude_forward_accel
|
||||
acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration+np.sin(angle_velocity)*magnitude_forward_accel
|
||||
|
||||
#and we return the acceleration vector, along with the minimum radius
|
||||
return np.array([acceleration_x,acceleration_y,0]),min_radius
|
||||
|
||||
|
||||
|
|
@ -137,7 +169,7 @@ def get_forward_acceleration(y, velocity):
|
|||
forward_acceleration_2 = -1*(b2*forward_velocity+k2*(y-1000))/m
|
||||
return min(forward_acceleration*forward_velocity, forward_acceleration_2*forward_velocity)*unit_vector/forward_velocity
|
||||
|
||||
def guidance_law():
|
||||
def guidance_law(balance_queue_input, balance_input):
|
||||
#here we calculate the acceleration we need in the forward direction,
|
||||
#and the acceleration we need that's perpendicular to that direction
|
||||
y,r,theta,circle_position=convert_to_cylindrical(pose)
|
||||
|
|
@ -150,24 +182,19 @@ def guidance_law():
|
|||
accel_straight=accel_straight/magnitude_accel*2*G
|
||||
|
||||
#next we deal with the avoidance component
|
||||
|
||||
avoidance_accel,min_radius = avoidance(weights)
|
||||
balance = get_balance(weights)
|
||||
#and we get the values that determine the balance between straight line
|
||||
#flight and obstacle avoidance
|
||||
balance_queue, balance = get_balance(min_radius, balance_queue_input, balance_input)
|
||||
#and we add the avoidance and the straight line accelerations together,
|
||||
#weighted by balance, which is how close we are to something
|
||||
accel = balance*avoidance_accel+(1-balance)*accel_straight/5
|
||||
##vertical component is not affected by avoidance, so we undo the changes
|
||||
#accel[2] = accel[2]/(1-balance)
|
||||
min_radius_pub.publish(min_radius)
|
||||
balance_pub.publish(balance)
|
||||
|
||||
|
||||
accel = balance*avoidance_accel+(1-balance)*accel_straight
|
||||
#finally, we turn our accelerations into a message we can publish
|
||||
accelerations=Vector3Stamped()
|
||||
accelerations.vector.x = accel[0]
|
||||
accelerations.vector.y=accel[1]
|
||||
|
||||
accelerations.vector.z=accel[2]
|
||||
return accelerations
|
||||
return accelerations, balance_queue, balance
|
||||
|
||||
|
||||
|
||||
|
|
@ -227,16 +254,22 @@ def accel_publisher():
|
|||
#over control
|
||||
global start_point
|
||||
start_point=pose
|
||||
#create variables for smoothing out the change between avoidance and
|
||||
#straight line flight
|
||||
balance_queue = []
|
||||
for x in range(14):
|
||||
balance_queue.append(0)
|
||||
balance = 0
|
||||
#Here is where we start doing navigation control
|
||||
#we get our acceleration from the guidance law, then publish it.
|
||||
while not rospy.is_shutdown():
|
||||
accelerations = guidance_law()
|
||||
accelerations, balance_queue, balance = guidance_law(balance_queue, balance)
|
||||
pub.publish(accelerations)
|
||||
rate.sleep()
|
||||
|
||||
if __name__ == '__main__':
|
||||
G=9.8
|
||||
one_degree=5*np.pi/180
|
||||
one_degree=3*np.pi/180
|
||||
pose = None
|
||||
velocity = None
|
||||
#describe constants for acceleration
|
||||
|
|
@ -248,6 +281,8 @@ if __name__ == '__main__':
|
|||
b2 = 0.4*np.sqrt(m*k2)
|
||||
min_radius_pub = rospy.Publisher('min_radius', Float32, queue_size = 10)
|
||||
balance_pub = rospy.Publisher('balance', Float32, queue_size = 10)
|
||||
balance_pub2 = rospy.Publisher('balance2', Float32, queue_size = 10)
|
||||
|
||||
#describe the vectors that define our direction of travel
|
||||
#the first vector is the direction we travel, the other two are two more
|
||||
#vectors that form an orthonormal basis with the first one
|
||||
|
|
|
|||
|
|
@ -36,17 +36,25 @@ 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 distance<0.5:
|
||||
raise Exception('crashed')
|
||||
|
||||
return distance
|
||||
|
||||
def calculate_distance_array():
|
||||
#this method calculates the distances to walls in 61 different directions
|
||||
#with the center (31st) direction being straight ahead, and each direction being
|
||||
#seperated by 3 degrees
|
||||
#first, we initialize the array
|
||||
distances = np.zeros(61)
|
||||
#and the drones current pose
|
||||
center = np.array([pose.x,pose.y])
|
||||
first_angle = np.arctan(center_vector[1]/center_vector[0])-30*one_degree
|
||||
if center[0]<0:
|
||||
#next we de
|
||||
first_angle = np.arctan(center_vector[1]/center_vector[0])-30*unit_degree
|
||||
if center_vector[0]<0:
|
||||
first_angle+=np.pi
|
||||
for x in range(61):
|
||||
angle = first_angle+x*one_degree
|
||||
angle = first_angle+x*unit_degree
|
||||
vector = np.array([np.cos(angle),np.sin(angle)])
|
||||
line = [0,0]
|
||||
line[0] = vector[1]/vector[0]
|
||||
|
|
@ -60,18 +68,25 @@ def calculate_distance_array():
|
|||
return distances
|
||||
|
||||
def distance_publisher():
|
||||
#first we set up our publishers
|
||||
pub = rospy.Publisher('obstacle_distances',LaserScan, queue_size = 10)
|
||||
rospy.init_node('obstacle_publisher',anonymous=True)
|
||||
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped, velocity_callback)
|
||||
rospy.Subscriber('mavros/local_position/pose', PoseStamped, pose_callback)
|
||||
#we set up the publishing rate
|
||||
rate = rospy.Rate(10)
|
||||
#we sleep to make sure that we get our first pose/velocity measurements
|
||||
#before we try and determine distances
|
||||
rospy.sleep(10)
|
||||
#finally, we start publishing
|
||||
while not rospy.is_shutdown():
|
||||
#first we calculate the distances
|
||||
distances = calculate_distance_array()
|
||||
#then we set up the message, and publish it
|
||||
to_publish = LaserScan()
|
||||
to_publish.angle_min = -30*one_degree
|
||||
to_publish.angle_max = 30*one_degree
|
||||
to_publish.angle_increment = one_degree
|
||||
to_publish.angle_min = -30*unit_degree
|
||||
to_publish.angle_max = 30*unit_degree
|
||||
to_publish.angle_increment = unit_degree
|
||||
to_publish.time_increment = 0
|
||||
to_publish.scan_time = 0.1
|
||||
to_publish.range_min = 0
|
||||
|
|
@ -81,9 +96,12 @@ def distance_publisher():
|
|||
rate.sleep
|
||||
|
||||
if __name__ == '__main__':
|
||||
one_degree = 5*np.pi/180
|
||||
#unit_degree defines the amount the change in angle between scans
|
||||
unit_degree = 3*np.pi/180
|
||||
pose = None
|
||||
wall_lines = [ ([2,150],[-10000,-10000],[10000,10000])]
|
||||
#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])]
|
||||
|
||||
try:
|
||||
distance_publisher()
|
||||
except rospy.ROSInterruptException:
|
||||
|
|
|
|||
Loading…
Reference in New Issue