Updated equations and added left side evaluation

src/geo_controller.py

@@ -26,46 +26,56 @@ def laser_subscriber(msg, thetas):
     """
     I've hijacked this method to provide a visual of the thetas we're using
     """
-    #publisher = rospy.Publisher("editscan", LaserScan, queue_size=10)
-    #indices = interp(thetas, [msg.angle_min, msg.angle_max], [0, len(msg.ranges) - 1])
-    #map(lambda x: int(x), indices)
+    # publisher = rospy.Publisher("editscan", LaserScan, queue_size=10)
+    # indices = interp(thetas, [msg.angle_min, msg.angle_max], [0, len(msg.ranges) - 1])
+    # map(lambda x: int(x), indices)
     ## Drop everything but what we want.
-    #for i in range(len(msg.ranges)):
+    # for i in range(len(msg.ranges)):
     #    if i not in indices:
     #        msg.ranges[i] = 0
     #        msg.intensities = 0
-    #publisher.publish(msg)
+    # publisher.publish(msg)
     pass
 
+
 def main():
     print("Geometric Controller Initialized")
     rospy.init_node('geometric_controller_filter')
     pub = rospy.Publisher("drive_parameters", drive_params)
-    #laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber, [-math.pi/2, -11*math.pi/24])
+    # laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber, [-math.pi/2, -11*math.pi/24])
     # rospy.spin()
     counter = 0
-    while(not rospy.core.is_shutdown()):
-        msg = rospy.client.wait_for_message('scan',LaserScan)
-        print(msg.angle_min, msg.angle_max, msg.angle_increment)	
-        RIGHTTOP=1
-        RIGHTBOT=0
-        theta = [-math.pi/2, -11*math.pi/24]
-        ranges = [get_range(msg, t) for t in theta]
-        print(counter)
-        counter += 1        
-        print(ranges)
-        delx = (math.sin(theta[RIGHTBOT])*ranges[RIGHTBOT]) - (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP])
-        if(theta[RIGHTBOT]==(-math.pi/2) or theta[RIGHTBOT]==(-math.pi/2)):
-            dely = (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP])
-        else:
-            dely = (math.sin(theta[RIGHTTOP])*ranges[RIGHTTOP]) - (math.sin(theta[RIGHTBOT])*ranges[RIGHTBOT])
-        thetar = -1* np.arctan(dely/delx) + np.pi/2
-        steering_input = interp(thetar, [np.radians(-20),np.radians(20)], [-100,100])
-        print("ThetaR:",thetar, "|Steering Angle:", steering_input)
+    while (not rospy.core.is_shutdown()):
+        msg = rospy.client.wait_for_message('scan', LaserScan)
+        print(msg.angle_min, msg.angle_max, msg.angle_increment)
+        TOP = 1
+        BOT = 0
+        thetas = 3*math.pi/24
+        thetar = [-math.pi / 2, (-math.pi / 2)+thetas]
+        rangesr = [get_range(msg, t) for t in thetar]
+        thetal = [math.pi / 2, (math.pi / 2)-thetas]
+        rangesl = [get_range(msg, t) for t in thetal]
+        #print(counter)
+        #counter += 1
+        #print(ranges)
+        d1r = rangesr[BOT]
+        d2r = rangesr[TOP]
+        d3r = math.sqrt(d1r**2 + d2r**2 - 2*d1r*d2r*math.cos(thetas))
+        deltaxr = d2r*math.cos(thetas) - d1r
+        thetadr = math.asin(deltaxr/d3r)
+        d1l = rangesl[BOT]
+        d2l = rangesl[TOP]
+        d3l = math.sqrt(d1l**2 + d2l**2 - 2*d1l*d2l*math.cos(thetas))
+        deltaxl = (d2l*math.cos(thetas) - d1l)
+        thetadl = math.asin(deltaxl/d3l)
+        thetad = thetadl + thetadr
+        steering_input = interp(thetad, [np.radians(-20), np.radians(20)], [-100, 100])
+        print("ThetaR:", thetad, "|Steering Angle:", steering_input)
         message = drive_params()
         message.angle = steering_input
         message.velocity = 15
         pub.publish(message)
 
-if __name__=='__main__':
+
+if __name__ == '__main__':
     main()