diff --git a/src/circle_ b/src/circle_ new file mode 100644 index 0000000..e69de29 diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py index e04e8b5..5037c64 100755 --- a/src/obstacle_detection.py +++ b/src/obstacle_detection.py @@ -10,7 +10,7 @@ from geometry_msgs.msg import Vector3Stamped from mavros_msgs.msg import State from mavros_msgs.srv import SetMode, SetModeRequest, CommandBool from sensor_msgs.msg import LaserScan - +from std_msgs.msg import Float32 def state_callback(data): global state @@ -30,10 +30,15 @@ def obstacle_callback(data): def get_balance(weights): - distance=min(weights) - balance = (-1*distance+80)/87 + + balance = (50-min(weights))/25 + if balance>1: + balance = 1 if balance<0: balance=0 + + #if balance>0.5: + # raise Exception("already above 0.5") return balance @@ -44,8 +49,18 @@ def get_min_radius(weights): radii_left = np.zeros(length) radii_right = np.zeros(length) for x in range(length): - angle = (x-30)*one_degree - radii_left[x],radii_right[x] = get_radius(x,angle) + if weights[x] == float('inf'): + radii_left[x] = 100000 + radii_right[x] = 100000 + 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 + + index_left = np.argmin(radii_left) index_right = np.argmin(radii_right) if radii_left[index_left]>radii_right[index_right]: @@ -55,8 +70,8 @@ def get_min_radius(weights): def get_radius(weight, 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)) + 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,7 +84,7 @@ def avoidance(weights): angle_acceleration = angle_velocity-np.pi/2 else: angle_acceleration = angle_velocity+np.pi/2 - magnitude_acceleration = 200*(velocity[0]*velocity[0]+velocity[1]*velocity[1])/(min_radius) + magnitude_acceleration = 1*(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 @@ -78,7 +93,7 @@ def avoidance(weights): 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]) + return np.array([acceleration_x,acceleration_y,0]),min_radius def convert_to_cylindrical(point): @@ -135,14 +150,16 @@ 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) - avoidance_accel = avoidance(weights) #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 - #vertical component is not affected by avoidance, so we undo the changes - accel[2] = accel[2]/(1-balance) - + 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) accelerations=Vector3Stamped() @@ -175,7 +192,7 @@ def accel_publisher(): initial_pose=PoseStamped() initial_pose.pose.position.x = 0 initial_pose.pose.position.y = 0 - initial_pose.pose.position.z = 10 + initial_pose.pose.position.z = 20 #we publish the initial pose a few times as there needs to be setpoints #published to allow us to enter offboard mode for i in range(100): @@ -188,7 +205,7 @@ def accel_publisher(): arm = True #now we keep trying to arm, enter offboard, and publish setpoints until #we are hovering at 9.5m or higher, with a stable velocity - while not rospy.is_shutdown() and (pose[2]<9.5 or abs( velocity[0])>1 or abs(velocity[1])>1): + while not rospy.is_shutdown() and (pose[2]<19.5 or abs( velocity[0])>1 or abs(velocity[1])>1): if(state.mode != "OFFBOARD" and (rospy.get_rostime().secs-last_request>5)): rospy.loginfo('setting mode') res=set_mode_srv(offb_set_mode) @@ -229,6 +246,8 @@ if __name__ == '__main__': b3 = 0.4*np.sqrt(m*k) b = 0.4*np.sqrt(m*k) 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) #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 diff --git a/src/obstacle_test.py b/src/obstacle_test.py index a31b226..30f6970 100755 --- a/src/obstacle_test.py +++ b/src/obstacle_test.py @@ -31,6 +31,8 @@ def distance_to_line(line1,vector1, center1, line2, bound1, bound2): return float('inf') recentered_intersect = intersect-center1 distance = recentered_intersect[0]/vector1[0] + if distance-recentered_intersect[1]/vector1[1]>0.01: + raise Exception('distance not the same') #if the distance is negative, the point is behind, so we return infinity to show nothing ahead if distance<0: return float('NaN') @@ -81,7 +83,7 @@ def distance_publisher(): if __name__ == '__main__': one_degree = 5*np.pi/180 pose = None - wall_lines = [ ([5,150],[-10000,-10000],[10000,10000])] + wall_lines = [ ([2,150],[-10000,-10000],[10000,10000])] try: distance_publisher() except rospy.ROSInterruptException: