Code cleanup in maps.py and robot.py

src/cs4313_utilities/maps.py

@@ -9,6 +9,7 @@ import cs4313_utilities.geometry as geometry
 import cs4313_utilities.robot_math as rm
 import cs4313_utilities.shared_robot_classes as src
 
+import rospy
 
 class Map(object):
     ''' Abstract class for defining a map for robot navigation use.  Class
@@ -371,12 +372,14 @@ class GeometryMap(Map):
         @param sensor: sensor reading (should be a LaserData object)
         @return model PDF value corresponding to P( z | x, m )
         '''
+        # If the "state" isn't in free space, the likelihood is 0
+        if not self.is_free_space(state):
+            return 0.0
         result = 1.0
-        # For efficiency purposes, only check interval of continuity mid pts
+        # For efficiency, only check interval of continuity closest pts
         for interval in sensor.intervals:
-            test_rtn = abs(interval[1] - interval[0]) // 2
-            rng = sensor.ranges[test_rtn]
-            theta_sense = sensor.rayBearing(test_rtn)
+            rng = sensor.ranges[interval[2]]
+            theta_sense = sensor.rayBearing(interval[2])
             test_pt = \
                 rm.projectSensorReturnToWorld(state, self._sensor_posit, \
                                               rng, theta_sense)

src/cs4313_utilities/robot.py

@@ -40,6 +40,7 @@ class SimpleRobot(object):
       mapToOdomTF: ROS listener for map transforms
       timer: ROS timer for control loop iteration
       odomLock: lock object to prevent concurrent access to the odometry
+      laserLock: lock object to prevent concurrent access to the laser scan
 
     Class methods:
       spinOnce: runs a single iteration of the vehicle's control loop
@@ -97,6 +98,8 @@ class SimpleRobot(object):
         self.timer = rospy.Rate(10)
         self.cmdPub = rospy.Publisher(cmdTopic, geometry_msgs.Twist, queue_size=1)
         self.odomLock = threading.RLock()
+        self.laserLock = threading.RLock()
+
 
     def setControlConstants(self, newCaptureDist=1.5, newMaxXDot=1.0, \
                             newMaxThetaDot=0.77, newXCmdC1=2.0, newXCmdC2=1.0, \
@@ -123,7 +126,8 @@ class SimpleRobot(object):
         ''' ROS callback for updating laser data upon receipt of new scan data
         @param scanData: sensor_msgs/LaserScan object containing scan data
         '''
-        self.laserData.update(scanData)
+        with self.laserLock:
+            self.laserData.update(scanData)
 
 
     def processGroundTruth(self, groundTruthData):