Modified file for easier debugging.

src/geo_controller.py

@@ -22,40 +22,37 @@ def get_range(data, theta):
     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
-    pub.publish(message)
-
+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)
+    # Drop everything but what we want.
+    for i in range(len(msg.ranges)):
+        if i not in indices:
+            msg.ranges[i] = 0
+            msg.intensities = 0
+    publisher.publish(msg)
 
 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, pub)
+    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
+        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)
+        print(counter)
         counter += 1        
-	print(ranges)
+        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