Merge pull request #49 from usdot-fhwa-stol/feature/carla-carma_state…

…_integration

Feature/carla carma state integration

carla-carma-integration/carla-carma-agent/agent/bridge.launch

@@ -93,4 +93,28 @@
     <param name='max_steering_degree' type="double" value="$(arg max_steering_degree)"/>
   </node>
 
+  <!--
+    # route #
+    Set the vehicle route after localization.
+  -->
+  <node pkg='carla_carma_bridge' type='carma_carla_route' name='carma_carla_route' output='screen'>
+    <rosparam file="$(find carla_carma_bridge)/config/parameters.yaml" command="load" />
+  </node>
+
+  <!--
+    # plugins #
+    Activate all registered plugins.
+  -->
+  <node pkg='carla_carma_bridge' type='carma_carla_plugins' name='carma_carla_plugins' output='screen'>
+    <rosparam file="$(find carla_carma_bridge)/config/parameters.yaml" command="load" />
+
+  </node>
+
+  <!--
+    # guidance #
+    Set guidance to active once the plugins have been activated and the route has been selected.
+  -->
+  <node pkg='carla_carma_bridge' type='carma_carla_guidance' name='carma_carla_guidance' output='screen'>
+  </node>
+
 </launch>

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}
 )
 

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,8 @@
   <exec_depend>autoware_msgs</exec_depend>
   <exec_depend>carla_msgs</exec_depend>
   <exec_depend>cav_msgs</exec_depend>
+  <exec_depend>cav_srvs</exec_depend>
+
   <export>
   </export>
 </package>

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()

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 <https://opensource.org/licenses/MIT>.
+#
+
+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()

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()
+
+
+
+