Feature/carla carma state integration

carla-carma-integration/carla_carma_bridge/CMakeLists.txt

@@ -25,6 +25,8 @@ catkin_install_python(PROGRAMS
     src/carla_carma_bridge/carla_to_carma_external_objects
     src/carla_carma_bridge/carma_carla_route
     src/carla_carma_bridge/carma_carla_plugins
+    src/carla_carma_bridge/carma_carla_guidance
+
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

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

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+# Copyright (C) 2021 LEIDOS.
+#
+# Licensed under the Apache License, Version 2.0 (the "License"); you may not
+# use this file except in compliance with the License. You may obtain a copy of
+# the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations under
+# the License.
+
+import rospy
+from cav_msgs.msg import RouteEvent
+from cav_msgs.msg import Plugin
+
+route_selected = False
+plugins_activated = False
+
+def check_route_status(route_event_msg):
+    if route_event_msg.event == 1:
+        print("Route Selected")
+        route_selected = True
+
+def check_plugin_status(plugin_msg):
+    if plugin_msg.activated == True:
+        print("Plugins Activated")
+        #NOTE: May have to add logic to parse through every individual plugin and determine their status
+        plugins_activated = True
+
+def initialize():
+    service = rospy.ServiceProxy('/set_active_guidance')
+    route_sub = rospy.Subscriber("/route_event",RouteEvent(), check_route_status)
+    plugin_sub = rospy.Subscriber("/plugin_discovery", Plugin(), check_plugin_status)
+    if route_selected == True && plugins_activated == True:
+        service.call()
+    else:
+        rospy.spin()
+
+if __name__ == '__main__':
+    print("carma_carla_guidance")
+    initialize()

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

@@ -112,10 +112,6 @@ def check_selected_route(available_routes):
     if selected_route in available_routes:
         return selected_route
 
-"""TODO: Listen to a topic indicating that CARLA Sim has started before seting guidance to active
-def set_guidance_active():
-    service = rospy.ServiceProxy('/set_active_guidance')    """
-
 def initialize():
     route_event_sub = rospy.Subscriber('/guidance/route_event', RouteEvent,route_event_callback)
     available_routes = get_available_routes()
@@ -134,13 +130,15 @@ def initialize():
             else:
                 print("Error: Invalid selected route")
                 rospy.spin()
+
+
     else:
         print("Localization Incomplete")
         rospy.spin()
 
 
 if __name__ == '__main__':
-    print("")
+    print("carma_carla_route")
     initialize()