From 266573f2c151710201aa0257566b47834da550f5 Mon Sep 17 00:00:00 2001
From: Matas Keras <matas.keras@spirirobotics.com>
Date: Fri, 29 Jul 2022 14:33:45 -0400
Subject: [PATCH] fixes for obstacle avoidance nodes

---
 src/obstacle_detection.py | 16 +++++++++-------
 src/obstacle_test.py      | 12 +++++++-----
 2 files changed, 16 insertions(+), 12 deletions(-)

diff --git a/src/obstacle_detection.py b/src/obstacle_detection.py
index 8a0487f..404dfba 100755
--- a/src/obstacle_detection.py
+++ b/src/obstacle_detection.py
@@ -66,9 +66,9 @@ def avoidance(weights):
     if velocity[0]<0:
         angle_velocity+=np.pi
     if go_left:
-        angle_acceleration = angle_velocity+np.pi/2
-    else:
         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
@@ -113,7 +113,7 @@ def get_forward_acceleration(y, velocity):
     #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 = -1*(b2*forward_velocity+k2*(y-100))/m
+    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():
@@ -124,6 +124,10 @@ def guidance_law():
     error_accel = get_error_acceleration(circle_position,r,velocity)
     #we then add these together to get our acceleration vector
     accel_straight = forward_accel+error_accel
+    magnitude_accel=np.linalg.norm(accel_straight)
+    if magnitude_accel>2*G:
+        accel_straight=accel_straight/magnitude_accel*2*G
+
     #next we deal with the avoidance component
     balance = get_balance(weights)
     avoidance_accel = avoidance(weights)
@@ -134,9 +138,7 @@ def guidance_law():
     accel[2] = accel[2]/(1-balance)
     #then we decrease it if the acceleration is over 2g
     
-    magnitude_accel=np.linalg.norm(accel)
-    if magnitude_accel>2*G:
-        accel=accel/magnitude_accel*2*G
+    
     accelerations=Vector3Stamped()
     accelerations.vector.x = accel[0]
     accelerations.vector.y=accel[1]
@@ -211,7 +213,7 @@ def accel_publisher():
 
 if __name__ == '__main__':
     G=9.8
-    one_degree=np.pi/180
+    one_degree=5*np.pi/180
     pose = None
     velocity = None
     #describe constants for acceleration
diff --git a/src/obstacle_test.py b/src/obstacle_test.py
index 73dcdb9..b0aee79 100755
--- a/src/obstacle_test.py
+++ b/src/obstacle_test.py
@@ -27,13 +27,13 @@ def intersection(line1, line2):
 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(np.all(intersect>bound1 and intercet<bound2)):
+    if not (intersect[0]>bound1[0] and intersect[1]>bound1[1] and intersect[0]<bound2[0] and intersect[1]<bound2[1]):
         return float('inf')
     recentered_intersect = intersect-center1
-    distance = recentered_intersec[0]/vector1[0]
+    distance = recentered_intersect[0]/vector1[0]
     #if the distance is negative, the point is behind, so we return infinity to show nothing ahead
     if distance<0:
-        return float('inf')
+        return float('NaN')
 
     return distance
 
@@ -41,6 +41,8 @@ def calculate_distance_array():
     distances = np.zeros(21)
     center = np.array([pose.x,pose.y])
     first_angle = np.arctan(center_vector[1]/center_vector[0])-10*one_degree
+    if center[0]<0:
+        first_angle+=np.pi
     for x in range(21):
         angle = first_angle+x*one_degree
         vector = np.array([np.cos(angle),np.sin(angle)])
@@ -77,9 +79,9 @@ def distance_publisher():
         rate.sleep
 
 if __name__ == '__main__':
-    one_degree = np.pi/180
+    one_degree = 5*np.pi/180
     pose = None
-    wall_lines = []
+    wall_lines = [([1,100],[-10000,-10000],[10000,10000])]
     try:
         distance_publisher()
     except rospy.ROSInterruptException: