Various fixes

src/geo_controller.py

@@ -13,28 +13,26 @@ def get_range(data, theta):
     # Input: 	data: LiDAR scan data
     # theta: The angle at which the distance is required
     # OUTPUT: distance of scan at angle theta
-    # Find the index of the array that corresponds to angle theta in degrees.
-    # 'interp' linearly maps one interval to another interval
-    # The following line finds the index of theta, using the assumption of regularly spaced scans along the range of the
-    # LiDAR sensor. This is the y-value. This seems like an expensive way to do this lookup. Because the lookup never ch
-    # anges, I propose that we use a dictionary, or other data structure with constant lookup time to obtain indices.-NT
     index_float = interp(theta, [data.angle_min, data.angle_max], [0, len(data.ranges) - 1])
-    return index_float
-
-
-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)
+    index = int(index_float)
+    print("range: " + str(data.ranges[index]))
+    # Return the LiDAR scan value at that index
+    # Do some error checking for NaN and ubsurd values
+    ## Your code goes here
+    return data.ranges[index]
+
+
+def laser_subscriber(msg, pub):
+    RIGHTTOP=1
+    RIGHTBOT=0
+    theta = [math.pi/2, 11*math.pi/24]
+    ranges = [get_range(msg, t) for t in theta]
+    print(ranges)
+    cr = np.sqrt(ranges[RIGHTBOT]**2 + ranges[RIGHTTOP]**2 - (2*ranges[RIGHTBOT]*ranges[RIGHTTOP]*np.cos(theta[RIGHTBOT]-theta[RIGHTTOP])))
+    wr = np.arcsin((np.sin(theta[RIGHTBOT]-theta[RIGHTTOP])*ranges[RIGHTTOP])/cr)
+    thetar = (math.pi/2) - wr
+    steering_input = interp(thetar, [np.radians(-20),np.radians(20)], [-100,100])
+    print("ThetaR:",thetar, "|Steering Angle:", steering_input)
     message = drive_params()
     message.angle = steering_input
     message.velocity = 15
@@ -44,9 +42,29 @@ def laser_subscriber(msg):
 def main():
     print("Geometric Controller Initialized")
     rospy.init_node('geometric_controller_filter')
-    laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber)
-    rospy.spin()
-
+    pub = rospy.Publisher("drive_parameters", drive_params)
+    # laser_sub = rospy.Subscriber("scan", LaserScan, laser_subscriber, pub)
+    # 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)
+        cr = np.sqrt(ranges[RIGHTBOT]**2 + ranges[RIGHTTOP]**2 - (2*ranges[RIGHTBOT]*ranges[RIGHTTOP]*np.cos(theta[RIGHTBOT]-theta[RIGHTTOP])))
+        wr = np.arcsin((np.sin(theta[RIGHTBOT]-theta[RIGHTTOP])*ranges[RIGHTTOP])/cr)
+        thetar = (math.pi/2) - wr
+        steering_input = interp(thetar, [np.radians(-20),np.radians(20)], [-100,100])
+        print("ThetaR:",thetar, "|Steering Angle:", steering_input)
+        message = drive_params()
+        message.angle = steering_input
+        message.velocity = 15
+        pub.publish(message)
 
 if __name__=='__main__':
     main()