Added multi-robot driver program to run multiple robots from 1 program

Waypoints work with the ied_search_project/ied_field.launch
with the second robot uncommented.

src/cs4313_utilities/multi_robot_driver.py

+#!/usr/bin/env python3
+
+import threading
+import sys
+import rospy
+import cs4313_utilities.robot as robot
+
+
+def robot_thread(rbt, waypoints):
+    ''' Function to use as a thread entry point to run a single robot
+    @param rbt: a robot object to run in the thread
+    @param waypoints: an iterable object containing waypoints for the robot
+    '''
+    rbt.spinOnce()  # Let it process at least one round of callbacks
+    for waypoint in waypoints:
+        print(rbt.namespace + " setting waypoint (" + str(waypoint[0]) + \
+              ", " + str(waypoint[1]) + ")")
+        rbt.setWaypoint(waypoint[0], waypoint[1])
+
+        while not rbt.isAtWaypoint:
+            rbt.spinOnce()
+
+    print(rbt.namespace + " reached final waypoint, quitting")
+
+# Main Program--this is a shell for testing that drives the robot through
+# a fixed series of waypoints (any "smart" behavior such as collision
+# avoidance or path planning has to be implemented in the Robot class
+if __name__ == "__main__":
+    rospy.init_node("robot_driver")
+    name1 = "robot1"
+    name2 = "robot2"
+
+    rbt1 = robot.SimpleRobot(namespace=name1)
+    rbt2 = robot.SimpleRobot(namespace=name2)
+
+    # Just some canned waypoints for testing
+    waypointList1 = [ [23.0, 0.0], [25.0, 15.0], [35.0, 25.0], \
+                      [55.0, 46.0], [72.0, 50.0], [80.0, 50.0] ]
+    waypointList2 = [ [75.0, 50.0], [57.0, 46.0], [40.0, 25.0], \
+                      [25.0, 15.0], [23.0, 0.0], [0.0, 0.0] ]
+
+    robotThread1 = threading.Thread(target=robot_thread, \
+                                    args=(rbt1, waypointList1), name=name1)
+    robotThread2 = threading.Thread(target=robot_thread, \
+                                    args=(rbt2, waypointList2), name=name1)
+
+    robotThread1.start()
+    robotThread2.start()
+
+    robotThread1.join()
+    robotThread2.join()
+
+    print("Multi-robot driver finished")

src/cs4313_utilities/robot.py

@@ -64,6 +64,7 @@ class SimpleRobot(object):
         @param laserTopic: topic name to subscribe to tfor laser scan data
         @param cmdTopic: topic name to publish velocity commands to
         '''
+        self.namespace = namespace
         self.setControlConstants()
         self.state = src.RobotState()
         self.laserData = src.LaserData()

src/cs4313_utilities/robot_driver.py

@@ -25,6 +25,6 @@ if __name__ == "__main__":
         print("Setting waypoint (" + str(waypoint[0]) + ", " + str(waypoint[1]) + ")")
         rbt.setWaypoint(waypoint[0], waypoint[1])
 
-        while not robot.isAtWaypoint:
+        while not rbt.isAtWaypoint:
             rbt.spinOnce()