diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py index 404dfba..e04e8b5 100755 --- a/src/obstacle_detection.py +++ b/src/obstacle_detection.py @@ -31,7 +31,7 @@ def obstacle_callback(data): def get_balance(weights): distance=min(weights) - balance = (-1*distance+30)/27 + balance = (-1*distance+80)/87 if balance<0: balance=0 return balance @@ -44,7 +44,7 @@ def get_min_radius(weights): radii_left = np.zeros(length) radii_right = np.zeros(length) for x in range(length): - angle = (length-10)*one_degree + angle = (x-30)*one_degree radii_left[x],radii_right[x] = get_radius(x,angle) index_left = np.argmin(radii_left) index_right = np.argmin(radii_right) @@ -55,8 +55,8 @@ def get_min_radius(weights): def get_radius(weight, angle): - radius_left = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2-angle)) - radius_right = (weight*weight-9)/(6+2*weight*np.cos(np.pi/2+angle)) + 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 @@ -69,9 +69,15 @@ def avoidance(weights): angle_acceleration = angle_velocity-np.pi/2 else: angle_acceleration = angle_velocity+np.pi/2 - magnitude_acceleration = 1.2*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/min_radius - acceleration_x = np.cos(angle_acceleration)*magnitude_acceleration - acceleration_y = np.sin(angle_acceleration)*magnitude_acceleration + magnitude_acceleration = 200*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/(min_radius) + magnitude_velocity = np.sqrt(velocity[1]*velocity[1]+velocity[0]*velocity[0]) + if magnitude_velocity<5: + magnitude_forward_accel = 5-magnitude_velocity + else: + magnitude_forward_accel = 0 + 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 + return np.array([acceleration_x,acceleration_y,0]) @@ -98,11 +104,11 @@ def get_error_acceleration(circle_position, r, velocity): circle_unit_3 = np.dot(circle_position, unit_vector_3) accel_2_1 = -1*(b3*velocity_unit_2+k*circle_unit_2) - accel_2_2 = 2*(9.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2) + accel_2_2 = 2*(4.5-abs(velocity_unit_2))*velocity_unit_2/abs(velocity_unit_2) accel_3_1 = -1*(b*velocity_unit_3+k*circle_unit_3) - accel_3_2 = 2*(9.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3) + accel_3_2 = 2*(4.5-abs(velocity_unit_3))*velocity_unit_3/abs(velocity_unit_3) error_accel = min(velocity_unit_2*accel_2_1,velocity_unit_2*accel_2_2)*unit_vector_2/velocity_unit_2 error_accel += min(velocity_unit_3*accel_3_1,velocity_unit_3*accel_3_2)*unit_vector_3/velocity_unit_3 return error_accel @@ -112,7 +118,7 @@ def get_forward_acceleration(y, velocity): #the target speed we are, or based off a damped spring, depending on which #is less (or if we are close to the finish) forward_velocity=np.dot(velocity,unit_vector) - forward_acceleration = 2*(9.5-abs(forward_velocity))*forward_velocity/abs(forward_velocity) + forward_acceleration = 2*(4.5-abs(forward_velocity))*forward_velocity/abs(forward_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 @@ -136,7 +142,7 @@ def guidance_law(): accel = balance*avoidance_accel+(1-balance)*accel_straight #vertical component is not affected by avoidance, so we undo the changes accel[2] = accel[2]/(1-balance) - #then we decrease it if the acceleration is over 2g + accelerations=Vector3Stamped() @@ -226,8 +232,8 @@ if __name__ == '__main__': #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 - unit_vector = np.array([1,5,0]) - unit_vector_2 = np.array([-5,1,0]) + unit_vector = np.array([0,1,0]) + unit_vector_2 = np.array([1,0,0]) 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) diff --git a/src/obstacle_test.py b/src/obstacle_test.py index b0aee79..a31b226 100755 --- a/src/obstacle_test.py +++ b/src/obstacle_test.py @@ -38,12 +38,12 @@ def distance_to_line(line1,vector1, center1, line2, bound1, bound2): return distance def calculate_distance_array(): - distances = np.zeros(21) + distances = np.zeros(61) center = np.array([pose.x,pose.y]) - first_angle = np.arctan(center_vector[1]/center_vector[0])-10*one_degree + first_angle = np.arctan(center_vector[1]/center_vector[0])-30*one_degree if center[0]<0: first_angle+=np.pi - for x in range(21): + for x in range(61): angle = first_angle+x*one_degree vector = np.array([np.cos(angle),np.sin(angle)]) line = [0,0] @@ -67,8 +67,8 @@ def distance_publisher(): while not rospy.is_shutdown(): distances = calculate_distance_array() to_publish = LaserScan() - to_publish.angle_min = -10*one_degree - to_publish.angle_max = 10*one_degree + to_publish.angle_min = -30*one_degree + to_publish.angle_max = 30*one_degree to_publish.angle_increment = one_degree to_publish.time_increment = 0 to_publish.scan_time = 0.1 @@ -81,7 +81,7 @@ def distance_publisher(): if __name__ == '__main__': one_degree = 5*np.pi/180 pose = None - wall_lines = [([1,100],[-10000,-10000],[10000,10000])] + wall_lines = [ ([5,150],[-10000,-10000],[10000,10000])] try: distance_publisher() except rospy.ROSInterruptException: