2nd floor nav

poi_name_locator/scripts/MoveDoorToDoor.py

@@ -0,0 +1,119 @@
+#!/usr/bin/env python
+from __future__ import print_function
+
+import sys
+import rospy
+import actionlib
+from geometry_msgs.msg import Point
+from geometry_msgs.msg import PoseStamped
+from geometry_msgs.msg import Pose
+from std_msgs.msg import Header
+from std_msgs.msg import String
+
+from move_base_msgs.msg import MoveBaseAction
+# from move_base_msgs.msg import MoveBaseActionClient
+from move_base_msgs.msg import MoveBaseGoal
+
+from poi_name_locator.srv import PoiNameLocator
+from poi_name_locator.srv import PoiNameLocatorRequest
+from poi_name_locator.srv import PoiNameLocatorResponse
+from poi_name_locator.srv import PoiNames
+from poi_name_locator.srv import PoiNamesRequest
+from poi_name_locator.srv import PoiNamesResponse
+
+
+# A simple demonstration of using poi_name_locator to move from door to door throughout the second floor of Halligan, 
+#and stop at each one before moving on to the next
+#To be used as a test/example program to make sure the robot is working correctly  
+class Patrol:
+    def __init__(self):
+        rospy.init_node('doors_patrol')
+
+        rospy.loginfo('waiting for service poi_names')
+        rospy.wait_for_service('poi_names')
+        rospy.loginfo('waiting for service poi_names finished')
+
+        self.poi_name_locator_callable = rospy.ServiceProxy(
+            'poi_name_locator', PoiNameLocator
+        )  # type: callable(PoiNamesRequest) -> PoiNamesResponse
+
+	self.poi_names_callable = rospy.ServiceProxy('poi_names', PoiNames)
+
+        self.move_base_action_client = actionlib.SimpleActionClient('move_base', MoveBaseAction)
+        self.move_base_action_client.wait_for_server()
+
+
+    def lookup(self, poi_name):  # type: (str) -> Point
+        request = PoiNameLocatorRequest(poi_name)
+
+	rospy.loginfo('Going to requested position %s'%(poi_name))
+
+        # response cannot be None. If server tries to return None, a rospy.ServiceException will raise here.
+        response = self.poi_name_locator_callable(request)  # type: PoiNameLocatorResponse
+
+        return response.position
+
+
+    def getList(self):
+
+
+
+	locList = ['Door_202_Graduate_Offices', 'Door_204_Kitchenette','Door_205_Scheutz_Ruiter', 'Door_206_Akitaya', 'Door_207_Vu', 'Door_208_Koomson', 'Door_209_Conference_Right', 'Door_210_Hughes', 'Door_211_Foster', 'Door_212_Collab_Right', 'Door_213_Sinapov', 'Door_214_Mendelsohn', 'Door_215_Jacob', 'Door_212_Collab_Left', 'Door_Mens_Bathroom','Door_217_Janitor_Closet',  'Door_Womens_Bathroom', 'Door_220_Afsar', 'Door_221_Edwards', 'Door_222_Ramsey', 'Door_224_Sheldon', 'Door_209_Conference_Left', 'Door_226_Graduate_Offices', 'Door_228_SinapovLab_ProfKorman', 'Door_235B_Offices', 'Door_250_Graduate_Offices', 'Door_233_Closet', 'Door_235A_Hempstead', 'Door_219_Wellness_Room', 'Door_237_Guyer', 'Door_239_Souvaine', 'Door_241_Landau', 'Door_245_Main_Office', 'Door_245_Sitting_Area']
+
+	#check the length of the list
+	length = len(locList)
+	rospy.loginfo('print length %s'%(length))
+
+	return locList
+
+
+    def getItem(self, index):
+	retlist = self.getList()
+	rospy.sleep(0.1) 
+	temp = retlist[index]
+	return temp
+
+
+    def patrol(self): #moves from door to door in custom order of doors  
+
+	for x in range(34): 
+
+
+		point1 = self.lookup(self.getItem(x))  # type: Point
+
+	       	goal = MoveBaseGoal()
+		target_pose = goal.target_pose  # type: PoseStamped
+
+		header = target_pose.header  # type: Header
+		header.frame_id = "/map"
+
+		pose = target_pose.pose  # type: Pose
+
+		pose.orientation.w = 1
+
+		########### Drive to poi
+
+
+		pose.position = point1
+		header.stamp = rospy.Time.now()
+
+		self.move_base_action_client.send_goal(goal)
+		self.move_base_action_client.wait_for_result()
+
+		rospy.loginfo('arrived, now waiting 5 sec')
+		for i in range(50):
+			if rospy.is_shutdown():
+				return
+			rospy.sleep(0.1)
+
+
+	rospy.loginfo('finished door to door patrol program')
+
+
+
+
+
+
+if __name__ == "__main__":
+    patrol = Patrol()
+    patrol.patrol()

poi_name_locator/scripts/poi_name_locator_server.py

@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 from __future__ import print_function
+from collections import OrderedDict
 
 from geometry_msgs.msg import Point
 from std_msgs.msg import String 
@@ -74,12 +75,13 @@ def handle_poi_get_names(self, request):  # type: (PoiNameLocatorRequest) -> Poi
         #poi_name = request.poi_name  # type: str
 
 		retlist = []
-
-		for key in self.poi.keys(): 
+            	olist = OrderedDict(sorted(self.poi.items(), key=lambda t: t[0]))
+		for key in olist.keys(): 
 			nextString = String()
 			nextString.data = key
 			retlist.append(nextString)
 
+