changes to obstacle avoidance

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):
+        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(x,angle)
+            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

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: