Feature/carla carma state integration

carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_guidance

@@ -21,11 +21,13 @@ route_selected = False
 plugins_activated = False
 
 def check_route_status(route_event_msg):
+    global route_selected
     if route_event_msg.event == 1:
         print("Route Selected")
         route_selected = True
 
 def check_plugin_status(plugin_msg):
+    global plugins_activated
     if plugin_msg.activated == True:
         print("Plugins Activated")
         #NOTE: May have to add logic to parse through every individual plugin and determine their status

carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins

@@ -59,6 +59,7 @@ def plugin_status_cb(plugin_msg):
 
 #Search through list of registered plugins. If it exists, return the requested plugin
 def check_plugin_registration(plugin):
+    global plugin_list
     if plugin in plugin_list.plugins:
         return True
     else:

carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route

@@ -25,11 +25,10 @@ import rospy
 from geometry_msgs.msg import PoseStamped, Pose
 from cav_msgs.msg import RouteState
 from cav_msgs.msg import Route
-from cav_msgs.msg import RouteEvent, GuidanceState
+from cav_msgs.msg import RouteEvent
 from cav_srvs.srv import GetAvailableRoutes
 from cav_srvs.srv import SetActiveRoute
 
-state_pub = rospy.Publisher('/guidance/state', GuidanceState)
 selected_route = rospy.get_param('selected_route')
 local_pos = PoseStamped()
 
@@ -92,15 +91,14 @@ def set_route(id):
         else:
                 #Call succeeded: Setting Route
                 print('call set active route success!')
-                #Change Guidance from READY to ACTIVE
-                #set_guidance_active()
 
 
 
     except:
             print("Fail")
 
 def pose_cb(pose_msg):
+    global local_pos
     local_pos.header = pose_msg.pose
     local_pos.pose = pose_msg.pose
 
@@ -122,23 +120,25 @@ def initialize():
 
     #Ensure that localization is complete before checking route information
     localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped, pose_cb)
-    if local_pos.pose.position.x != 0 and local_pos.pose.position.y != 0:
-        if available_routes == None:
-            print("No routes available.")
-        else:
-            """Check if the selected route is one of the available routes. If it isn't,then print error statement"""
-            if check_selected_route(available_routes) == True:
-                route = selected_route
-                set_route(route.route_id)
-
+    while rospy.is_shutdown() != True:  
+        if local_pos.pose.position.x != 0 and local_pos.pose.position.y != 0:
+            if available_routes == None:
+                print("No routes available.")
             else:
-                print("Error: Invalid selected route")
-                rospy.spin()
+                """Check if the selected route is one of the available routes. If it isn't,then print error statement"""
+                if check_selected_route(available_routes) == True:
+                    route = selected_route
+                    set_route(route.route_id)
+                    break
+
+                else:
+                    print("Error: Invalid selected route")
+                    rospy.spin()
 
 
-    else:
-        print("Localization Incomplete")
-        rospy.spin()
+        else:
+            print("Localization Incomplete")
+            rospy.spin()
 
 
 if __name__ == '__main__':