Feature/carla carma state integration

carla-carma-integration/carla_carma_bridge/CMakeLists.txt

@@ -8,7 +8,10 @@ project(carla_carma_bridge)
 find_package(catkin REQUIRED COMPONENTS
   autoware_msgs
   rospy
+  cav_srvs
   cav_msgs
+  health_monitor
+
 )
 
 catkin_python_setup()
@@ -21,6 +24,8 @@ catkin_install_python(PROGRAMS
     src/carla_carma_bridge/carla_to_carma_localization
     src/carla_carma_bridge/carma_to_carla_ackermann_cmd
     src/carla_carma_bridge/carla_to_carma_external_objects
+    src/carla_carma_bridge/carma_carla_route
+    src/carla_carma_bridge/carma_carla_plugins
     DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

carla-carma-integration/carla_carma_bridge/config/parameters.yml

@@ -0,0 +1,9 @@
+#String: Route to be initialized
+selected_route: '/opt/carma/routes/tfhrc_test_route.csv'
+
+# List of String: Required plugins for the platform to be functional
+selected_plugins:
+  - RouteFollowing
+  - InLaneCruisingPlugin
+  - StopAndWaitPlugin
+  - Pure Pursuit

carla-carma-integration/carla_carma_bridge/package.xml

@@ -16,6 +16,9 @@
   <exec_depend>autoware_msgs</exec_depend>
   <exec_depend>carla_msgs</exec_depend>
   <exec_depend>cav_msgs</exec_depend>
+  <exec_depend>cav_srvs</exec_depend>
+  <exec_depend>health_monitor</exec_depend>
+
   <export>
   </export>
 </package>

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

@@ -0,0 +1,82 @@
+#!/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.
+#
+#
+# This file is loosely based on the reference architecture developed by Intel Corporation for Leidos located here
+# https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/health_monitor/src/plugin_manager.cpp
+#
+# That file has the following license and some code snippets from it may be present in this file as well and are under the same license.
+#
+# Copyright (c) 2018-2019 Intel Corporation
+#
+# This work is licensed under the terms of the MIT license.
+# For a copy, see <https://opensource.org/licenses/MIT>.
+#
+
+import rospy
+from cav_msgs.msg import Plugin, PluginList, PluginStatus
+from cav_srvs.srv import GetRegisteredPlugins
+
+plugin_pub = rospy.Publisher('/guidance/plugin_discovery', Plugin())
+selected_plugins = rospy.get_param('selected_plugins')
+
+#List to contain the registered plugins from the service call in initialize()
+plugin_list = PluginList()
+
+
+#Check if the plugin is included in the list of required plugins
+def check_selected_plugins(name):
+    for i in selected_plugins:
+        if i == name:
+            return True
+        else:
+            return False
+
+def plugin_status_cb(plugin_msg):
+    print("Received status from: " ++ plugin_msg.name)
+    requested = get_plugin(plugin_msg)
+    plugin = Plugin()
+    plugin.available = plugin_msg.available
+    plugin.name = plugin_msg.name
+    plugin.capability = plugin_msg.capability
+    plugin.type = plugin_msg.type
+    activation_srv = rospy.ServiceProxy("/activate_plugin")
+
+    #If the requested plugin is registered, don't change the activation status
+    if requested != None:
+        plugin.activated = plugin_msg.activated
+    elif check_selected_plugins(plugin_msg.name) == True:
+        plugin.activated = True 
+    activation_srv.call(plugin.name, plugin.activated)
+
+
+#Search through list of registered plugins. If it exists, return the requested plugin
+def get_plugin(plugin):
+    if plugin in plugin_list.plugins:
+        return plugin
+    else:
+        return None
+
+
+def initialize():
+    registered_plugins_serv = rospy.ServiceProxy('/get_registered_plugins')
+    plugin_list.plugins = registered_plugins_serv.call()
+    plugin_sub = rospy.Subscriber('/plugin_discovery',Plugin(), plugin_status_cb)
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    print("carma_carla_plugins")
+    initialize()

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

@@ -0,0 +1,146 @@
+#!/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.
+"""
+Call Services from CARMA: 
+    Service: /guidance/get_available_routes
+             /guidance/set_active_route
+
+Publish to CARMA :cav_msgs::Route
+
+"""
+from asyncio.windows_events import NULL
+from typing import Iterator
+from xml.dom.minidom import Document
+import rospy
+import os
+from geometry_msgs.msg import PoseStamped
+from cav_msgs.msg import RouteState
+from cav_msgs.msg import Route
+from cav_msgs.msg import RouteEvent, GuidanceState
+from cav_srvs.srv import GetAvailableRoutes
+from cav_srvs.srv import SetActiveRoute
+
+localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped)
+state_pub = rospy.Publisher('/guidance/state', GuidanceState)
+selected_route = rospy.get_param('selected_route')
+
+class ErrorStatus:
+    def __init__(self, value, text):
+        self.value = value
+        self.text = text
+
+#Get the routes to be set after localization
+def get_available_routes():
+    service = rospy.ServiceProxy('/get_available_routes', GetAvailableRoutes)     
+    try:
+        resp = service.call(0)
+        available_routes = GetAvailableRoutes()
+        available_routes.availableRoutes = resp.availableRoutes
+
+    except:
+        print("Invalid: /guidance/get_available_routes failed.")
+    else:
+        print("Available Routes acquired")
+        return available_routes.availableRoutes
+
+
+
+def set_route(id):
+    state = GuidanceState()
+    service = rospy.ServiceProxy('/set_active_route', SetActiveRoute)
+
+    NO_ERROR = ErrorStatus(0, "NO_ERROR")
+    NO_ROUTE = ErrorStatus(1, "NO_ROUTE")
+    ALREADY_FOLLOWING_ROUTE = ErrorStatus(2, "ALREADY_FOLLOWING_ROUTE")
+    ROUTE_FILE_ERROR = ErrorStatus(3, "ROUTE_FILE_ERROR")
+    ROUTING_FAILURE = ErrorStatus(4, "ROUTING_FAILURE")
+    TRANSFORM_ERROR = ErrorStatus(5, "TRANSFORM_ERROR")
+
+    try:
+        #Service Call Request
+        resp = service(0, id)
+        if resp.errorStatus != NO_ERROR.value:
+            errorDescription = ""
+            if resp.errorStatus == NO_ROUTE.value:
+                errorDescription = NO_ROUTE.text
+
+            elif resp.errorStatus == ALREADY_FOLLOWING_ROUTE.value:
+                errorDescription = ALREADY_FOLLOWING_ROUTE.text
+
+            elif resp.errorStatus == ROUTE_FILE_ERROR.value:
+                errorDescription = ROUTE_FILE_ERROR.text
+
+            elif resp.errorStatus == ROUTING_FAILURE.value:
+                errorDescription = ROUTING_FAILURE.text
+
+            elif resp.errorStatus == TRANSFORM_ERROR.value:
+                errorDescription = TRANSFORM_ERROR.text
+
+            else:
+                errorDescription = resp.errorStatus
+
+        else:
+                #Call succeeded: Setting Route
+                print('call set active route success!')
+                #Change Guidance from READY to ACTIVE
+                #set_guidance_active()
+
+
+
+    except:
+            print("Fail")
+
+    #RouteEvent
+    route_event_sub = rospy.Subscriber('/guidance/route_event', RouteEvent,route_event_callback)
+
+
+def route_event_callback(event):
+    route_event = RouteEvent()
+    route_event.event = event
+    if route_event.event == 6:
+        print("ROUTE_GEN_FAILED")
+
+def check_selected_route(available_routes):
+    if selected_route in available_routes:
+        return True
+    else:
+        return False
+
+"""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():
+    available_routes = get_available_routes()
+    if available_routes == None:
+        rospy.spin()
+    else:
+        """Check if the selected route is one of the available routes. If it isn't,then use the first of the available 
+        routes received from the service call"""
+        if check_selected_route(available_routes) == True:
+            route = selected_route
+        else:
+            route = available_routes.availableRoutes[0]
+        set_route(route.route_id)
+        rospy.spin()
+
+
+if __name__ == '__main__':
+    print("")
+    initialize()
+
+
+
+