diff --git a/carla-carma-integration/carla-carma-agent/agent/bridge.launch b/carla-carma-integration/carla-carma-agent/agent/bridge.launch
index 009c0954..3e785151 100644
--- a/carla-carma-integration/carla-carma-agent/agent/bridge.launch
+++ b/carla-carma-integration/carla-carma-agent/agent/bridge.launch
@@ -93,4 +93,28 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt
index 8e73a732..c876828d 100644
--- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt
+++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt
@@ -8,7 +8,9 @@ project(carla_carma_bridge)
find_package(catkin REQUIRED COMPONENTS
autoware_msgs
rospy
+ cav_srvs
cav_msgs
+
)
catkin_python_setup()
@@ -21,6 +23,10 @@ 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
+ src/carla_carma_bridge/carma_carla_guidance
+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
diff --git a/carla-carma-integration/carla_carma_bridge/config/parameters.yml b/carla-carma-integration/carla_carma_bridge/config/parameters.yml
new file mode 100644
index 00000000..a204432c
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/carla-carma-integration/carla_carma_bridge/package.xml b/carla-carma-integration/carla_carma_bridge/package.xml
index 47d102d8..18b731e5 100644
--- a/carla-carma-integration/carla_carma_bridge/package.xml
+++ b/carla-carma-integration/carla_carma_bridge/package.xml
@@ -16,6 +16,8 @@
autoware_msgs
carla_msgs
cav_msgs
+ cav_srvs
+
diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_guidance b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_guidance
new file mode 100644
index 00000000..298d6260
--- /dev/null
+++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_guidance
@@ -0,0 +1,53 @@
+#!/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
+from cav_srvs.srv import SetGuidanceActive
+
+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
+ plugins_activated = True
+
+def initialize():
+ rospy.init_node("carma_carla_guidance")
+ service = rospy.ServiceProxy('/set_active_guidance', SetGuidanceActive)
+ #While-loop monitors the route and plugin status and sets guidance to active once both are confirmed true
+
+ route_sub = rospy.Subscriber("/route_event",RouteEvent(), check_route_status)
+ plugin_sub = rospy.Subscriber("/plugin_discovery", Plugin(), check_plugin_status)
+ while rospy.is_shutdown() != True:
+ if route_selected == True and plugins_activated == True:
+ activation = service(True) #Sets guidance status to active
+ break
+ rospy.sleep(0.1)
+
+
+if __name__ == '__main__':
+ print("carma_carla_guidance")
+ initialize()
\ No newline at end of file
diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins
new file mode 100644
index 00000000..cbed3c1e
--- /dev/null
+++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins
@@ -0,0 +1,81 @@
+#!/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 .
+#
+
+import rospy
+from cav_msgs.msg import Plugin, PluginList, PluginStatus
+from cav_srvs.srv import GetRegisteredPlugins, PluginActivation
+
+selected_plugins = rospy.get_param('selected_plugins')
+activation_srv = rospy.ServiceProxy("/activate_plugin", PluginActivation)
+
+
+#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 = check_plugin_registration(plugin_msg)
+
+ #If the requested plugin is not registered, print error message
+ if requested != None:
+ print("Incoming plugin is not included in list of registered plugins")
+ elif check_selected_plugins(plugin_msg.name) == True:
+ if plugin_msg.activated != True:
+ plugin_msg.activated = True
+ activation = activation_srv(plugin_msg.name, plugin_msg.activated)
+
+
+#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:
+ return False
+
+
+def initialize():
+ global plugin_list
+ rospy.init_node("carma_carla_plugins")
+ registered_plugins_serv = rospy.ServiceProxy('/get_registered_plugins',GetRegisteredPlugins)
+ plugin_list.plugins = registered_plugins_serv()
+ plugin_sub = rospy.Subscriber('/plugin_discovery',Plugin(), plugin_status_cb)
+ rospy.spin()
+
+
+if __name__ == '__main__':
+ print("carma_carla_plugins")
+ initialize()
\ No newline at end of file
diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route
new file mode 100644
index 00000000..96f7a46a
--- /dev/null
+++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route
@@ -0,0 +1,117 @@
+#!/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
+
+"""
+
+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
+from cav_srvs.srv import GetAvailableRoutes
+from cav_srvs.srv import SetActiveRoute
+
+selected_route = rospy.get_param('selected_route')
+received_pose = False
+
+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(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):
+ service = rospy.ServiceProxy('/set_active_route', SetActiveRoute)
+
+ try:
+ #Service Call Request
+ resp = service(0, id)
+ if resp.errorStatus != 0:
+ errorDescription = resp.errorStatus
+ print(errorDescription)
+
+ else:
+ #Call succeeded: Setting Route
+ print('call set active route success!')
+
+ except:
+ print("Fail")
+
+def pose_cb(pose_msg):
+ global received_pose
+ received_pose = True
+
+
+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 selected_route
+
+def initialize():
+ rospy.init_node("carma_carla_route")
+ route_event_sub = rospy.Subscriber('/guidance/route_event', RouteEvent,route_event_callback)
+ available_routes = get_available_routes()
+
+ #Ensure that localization is complete before checking route information
+ localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped, pose_cb)
+ while rospy.is_shutdown() != True:
+ if received_pose == True:
+ 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):
+ route = selected_route
+ set_route(route.route_id)
+ break
+
+ else:
+ print("Error: Invalid selected route")
+ break
+ else:
+ print("Localization Incomplete")
+ rospy.sleep(0.1)
+
+if __name__ == '__main__':
+ print("carma_carla_route")
+ initialize()
+
+
+
+