Added logic to calculate theta, and publish steering output.

src/geo_controller.py

@@ -4,6 +4,8 @@
 
 import rospy
 from sensor_msgs.msg import LaserScan
+from ac_msgs.msg import drive_params
+import numpy as np
 from numpy import interp
 
 
@@ -20,14 +22,30 @@ def get_range(data, theta):
     return index_float
 
 
-def laser_subscriber():
-    pass
+def laser_subscriber(msg):
+    theta = [-1*math.pi/4, -15*math.pi/16,
+              math.pi/4, 15*math.pi/16]
+    ranges = get_range(msg, theta)
+    cr = np.sqrt(ranges[2]**2 + ranges[3]**2 - (ranges[2]*ranges[3]*np.cos(theta[2]-theta[3]))) 
+    cl = np.sqrt(ranges[0]**2 + ranges[1]**2 - (ranges[0]*ranges[1]*np.cos(theta[0]-theta[1])))
+    wr = np.arcsin((np.sin(theta[2]-theta[3])*ranges[3])/cr)
+    wl = np.arcsin((np.sin(theta[0]-theta[1])*ranges[1])/cl)
+    thetar = (math.pi/4) - wr
+    thetal = (math.pi/4) - wl
+    avgtheta = (thetar+thetal)/2
+    steering_input = interp(avgtheta, [np.radians(-20),np.radians(20)], [-100,100])
+    pub = rospy.Publisher("drive_parameters", drive_params)
+    message = drive_params()
+    message.angle = steering_input
+    message.velocity = 15
+    pub.publish(message)
 
 
 def main():
-    # Listen to LaserScan
-    # Spin
-    pass
+    print("Geometric Controller Initialized")
+    rospy.init_node('geometric_controller_filter')
+    laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber)
+    rospy.spin()
 
 
 if __name__=='__main__':