From c8b23197bd768ebb9ef7a727b7dbd0f34c1d91a0 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 15 Feb 2022 14:57:53 +0000 Subject: [PATCH 01/26] Commit initial edits --- .../carla_carma_bridge/CMakeLists.txt | 2 ++ .../carla_carma_bridge/carla_to_carma_plugins | 26 +++++++++++++++++++ .../carla_carma_bridge/carla_to_carma_route | 26 +++++++++++++++++++ 3 files changed, 54 insertions(+) create mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins create mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index 8e73a732..ebb0e009 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -21,6 +21,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/carla_to_carma_route + src/carla_carma_bridge/carla_to_carma_plugins DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins new file mode 100644 index 00000000..f1f28c2e --- /dev/null +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins @@ -0,0 +1,26 @@ +#!/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/41623134/carla-autoware/blob/master/catkin_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/odometry_to_posestamped +# +# 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 . +# diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route new file mode 100644 index 00000000..f1f28c2e --- /dev/null +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route @@ -0,0 +1,26 @@ +#!/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/41623134/carla-autoware/blob/master/catkin_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/odometry_to_posestamped +# +# 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 . +# From ac4297fb07d59e301e3666c4d908ea5bf6c72c8d Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 15 Feb 2022 19:55:02 +0000 Subject: [PATCH 02/26] commit --- .../carla_carma_bridge/CMakeLists.txt | 1 + .../carla_carma_bridge/package.xml | 2 ++ .../carla_carma_bridge/carla_to_carma_route | 32 +++++++++++++++++++ 3 files changed, 35 insertions(+) diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index ebb0e009..f0f8bc5a 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(catkin REQUIRED COMPONENTS autoware_msgs rospy cav_msgs + cav_srvs ) catkin_python_setup() 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/carla_to_carma_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route index f1f28c2e..b7168daf 100644 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route @@ -24,3 +24,35 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . # +""" +Subscribe from CARMA:geometry_msgs::PoseStamped + Topic: /localization/gnss_pose; + /localization/current_pose + +Subscribe from CARMA: + Service: /guidance/get_available_routes + +Publish to CARMA :cav_msgs::Route + +""" +from hashlib import new +import rospy +from geometry_msgs.msg import PoseStamped +from cav_msgs.msg import RouteState +from cav_srvs.srv import GetAvailableRoutes + + +route_state_pub = rospy.Publisher('/guidance/route_state', RouteState, queue_size=1) +#available_routes_serv = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) + +def available_routes_callback(available_routes): + try: + available_routes_serv = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) + except rospy.ServiceException as exc: + print("Service did not process request: /guidance/get_available_routes ") + + + + + +#def route_state_callback(state): From f48fb0a7b643a48c8b26219a63089188877cff6e Mon Sep 17 00:00:00 2001 From: Isaiah Date: Fri, 18 Feb 2022 20:50:49 +0000 Subject: [PATCH 03/26] Commit changes to carla-carma-integration --- .../carla_carma_bridge/CMakeLists.txt | 7 +- .../carla_carma_bridge/carla_to_carma_route | 58 -------- ...o_carma_plugins => carma_to_carla_plugins} | 0 .../carla_carma_bridge/carma_to_carla_route | 130 ++++++++++++++++++ 4 files changed, 134 insertions(+), 61 deletions(-) delete mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route rename carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/{carla_to_carma_plugins => carma_to_carla_plugins} (100%) create mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index f0f8bc5a..a190de49 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -8,8 +8,9 @@ project(carla_carma_bridge) find_package(catkin REQUIRED COMPONENTS autoware_msgs rospy - cav_msgs cav_srvs + cav_msgs + ) catkin_python_setup() @@ -22,8 +23,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/carla_to_carma_route - src/carla_carma_bridge/carla_to_carma_plugins + src/carla_carma_bridge/carma_to_carla_route + src/carla_carma_bridge/carma_to_carla_plugins DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route deleted file mode 100644 index b7168daf..00000000 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_route +++ /dev/null @@ -1,58 +0,0 @@ -#!/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/41623134/carla-autoware/blob/master/catkin_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/odometry_to_posestamped -# -# 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 . -# -""" -Subscribe from CARMA:geometry_msgs::PoseStamped - Topic: /localization/gnss_pose; - /localization/current_pose - -Subscribe from CARMA: - Service: /guidance/get_available_routes - -Publish to CARMA :cav_msgs::Route - -""" -from hashlib import new -import rospy -from geometry_msgs.msg import PoseStamped -from cav_msgs.msg import RouteState -from cav_srvs.srv import GetAvailableRoutes - - -route_state_pub = rospy.Publisher('/guidance/route_state', RouteState, queue_size=1) -#available_routes_serv = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) - -def available_routes_callback(available_routes): - try: - available_routes_serv = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) - except rospy.ServiceException as exc: - print("Service did not process request: /guidance/get_available_routes ") - - - - - -#def route_state_callback(state): diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins similarity index 100% rename from carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carla_to_carma_plugins rename to carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route new file mode 100644 index 00000000..08e7332f --- /dev/null +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route @@ -0,0 +1,130 @@ +#!/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 +import rospy +from geometry_msgs.msg import PoseStamped +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 + + +localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped) +selected_route = Route() + +class ErrorStatus: + def __init__(self, value, text): + self.value = value + self.text = text + + +def get_available_routes(): + service = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) + #Make sure that the vehicle is localized + if localized_sub: + 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 + else: + print("Localization Incomplete") + return + + + +def set_route(id, route_name): + service = rospy.ServiceProxy('/guidance/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!') + selected_route.route_id = resp.routeID + selected_route.route_name = resp.routeName + + + + 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 initialize(): + available_routes = get_available_routes() + route = available_routes.availableRoutes[0] + set_route(route.route_id, route.route_name) + rospy.spin() + + +if __name__ == '__main__': + print("") + initialize() + + + + From 069df9db6af016ebebffcd94d8caa7d08191e22f Mon Sep 17 00:00:00 2001 From: Isaiah Date: Wed, 23 Feb 2022 06:28:03 +0000 Subject: [PATCH 04/26] Commit --- .../carla_carma_bridge/config/parameters.yml | 1 + .../carla_carma_bridge/carma_to_carla_route | 48 +++++++++++-------- 2 files changed, 28 insertions(+), 21 deletions(-) create mode 100644 carla-carma-integration/carla_carma_bridge/config/parameters.yml 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..666a7f7f --- /dev/null +++ b/carla-carma-integration/carla_carma_bridge/config/parameters.yml @@ -0,0 +1 @@ +route_id: /opt/carma/routes/tfhrc_test_route.csv diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route index 08e7332f..1ef6c108 100644 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route @@ -21,16 +21,18 @@ Publish to CARMA :cav_msgs::Route """ from asyncio.windows_events import NULL +from typing import Iterator +from xml.dom.minidom import Document import rospy from geometry_msgs.msg import PoseStamped from cav_msgs.msg import RouteState from cav_msgs.msg import Route -from cav_msgs.msg import RouteEvent +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 = Route() class ErrorStatus: @@ -40,26 +42,22 @@ class ErrorStatus: def get_available_routes(): - service = rospy.ServiceProxy('/guidance/get_available_routes', GetAvailableRoutes) - #Make sure that the vehicle is localized - if localized_sub: - 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 + service = rospy.ServiceProxy('/guidance/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("Localization Incomplete") - return + print("Available Routes acquired") + return available_routes.availableRoutes def set_route(id, route_name): + state = GuidanceState() service = rospy.ServiceProxy('/guidance/set_active_route', SetActiveRoute) NO_ERROR = ErrorStatus(0, "NO_ERROR") @@ -97,7 +95,10 @@ def set_route(id, route_name): print('call set active route success!') selected_route.route_id = resp.routeID selected_route.route_name = resp.routeName - + #Change Guidance from READY to ACTIVE + state.state = 3 + state_pub(state) + #f = open('/opt/carma/routes') except: @@ -116,9 +117,14 @@ def route_event_callback(event): def initialize(): available_routes = get_available_routes() - route = available_routes.availableRoutes[0] - set_route(route.route_id, route.route_name) - rospy.spin() + if available_routes == None: + rospy.spin() + else: + route = available_routes.availableRoutes[0] + #Insert route from /opt/carma/routes/ folder + #route = Document.load('/opt/carma/routes/tfhrc_test_route.csv') + set_route(route.route_id, route.route_name) + rospy.spin() if __name__ == '__main__': From 34a1dabd3eb456a44a18c5c1c251a9b7491493ce Mon Sep 17 00:00:00 2001 From: Isaiah Date: Fri, 25 Feb 2022 16:48:46 +0000 Subject: [PATCH 05/26] Commit additions to carma_to_carla_plugins --- .../carla_carma_bridge/config/parameters.yml | 2 +- .../carla_carma_bridge/carma_to_carla_plugins | 34 +++++++++++++++++++ .../carla_carma_bridge/carma_to_carla_route | 8 ++++- 3 files changed, 42 insertions(+), 2 deletions(-) diff --git a/carla-carma-integration/carla_carma_bridge/config/parameters.yml b/carla-carma-integration/carla_carma_bridge/config/parameters.yml index 666a7f7f..e9ea7008 100644 --- a/carla-carma-integration/carla_carma_bridge/config/parameters.yml +++ b/carla-carma-integration/carla_carma_bridge/config/parameters.yml @@ -1 +1 @@ -route_id: /opt/carma/routes/tfhrc_test_route.csv +routes: '/opt/carma/routes/' diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins index f1f28c2e..7d11cbe8 100644 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins @@ -24,3 +24,37 @@ # This work is licensed under the terms of the MIT license. # For a copy, see . # + +from pickle import TRUE +import rospy +from cav_msgs.msg import Plugin, PluginList, PluginStatus +from cav_srvs.srv import GetRegisteredPlugins + +plugin_pub = rospy.Publisher('/guidance/plugin_discovery', Plugin()) + +def get_guidance_plugins(): + """Call Service to get registered plugins""" + + serv = rospy.ServiceProxy('guidance/get_registered_plugins', PluginList) + plugins = serv.call() + plugin = Plugin() + + #If the plugins are listed as AVAILABLE, make sure that they are both active and running + for x in plugins: + if x.available == TRUE: + plugin.activated = TRUE + plugin.name = x.name + plugin.capability = x.capability + plugin.versionId = x.versionID + plugin.type = x.type + #Publish the now active plugin message to /guidance/plugin_discovery + plugin_pub.publish(plugin) + +def initialize(): + get_guidance_plugins() + rospy.spin() + + +if __name__ == '__main__': + print("carma_to_carla_plugins") + initialize() \ No newline at end of file diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route index 1ef6c108..55717f5a 100644 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route @@ -24,6 +24,7 @@ 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 @@ -98,7 +99,12 @@ def set_route(id, route_name): #Change Guidance from READY to ACTIVE state.state = 3 state_pub(state) - #f = open('/opt/carma/routes') + + ##If resp.ID & resp.routeName return null, use the route file specified in the config/parameters.yml file + #f = open('config/parameters.yml') + #direc = f.read() + #routes = os.listdir(direc) + #selected_route.route_id = routes[0]. except: From 1fc0220213ed1ac0261429961fba7300a0fc593c Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 1 Mar 2022 20:26:56 +0000 Subject: [PATCH 06/26] Commit Changes to carla-carma-integration node --- .../carla_carma_bridge/CMakeLists.txt | 4 +- .../carla_carma_bridge/config/parameters.yml | 8 ++ .../carla_carma_bridge/carma_carla_plugins | 100 ++++++++++++++++++ ...carma_to_carla_route => carma_carla_route} | 0 .../carla_carma_bridge/carma_to_carla_plugins | 60 ----------- 5 files changed, 110 insertions(+), 62 deletions(-) create mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins rename carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/{carma_to_carla_route => carma_carla_route} (100%) delete mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index a190de49..7220e49d 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -23,8 +23,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_to_carla_route - src/carla_carma_bridge/carma_to_carla_plugins + src/carla_carma_bridge/carma_carla_route + src/carla_carma_bridge/carma_carla_plugins 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 index e9ea7008..b659afe7 100644 --- a/carla-carma-integration/carla_carma_bridge/config/parameters.yml +++ b/carla-carma-integration/carla_carma_bridge/config/parameters.yml @@ -1 +1,9 @@ +#String: Route to be initialized routes: '/opt/carma/routes/' + +# List of String: Required plugins for the platform to be functional +required_plugins: + - RouteFollowing + - InLaneCruisingPlugin + - StopAndWaitPlugin + - Pure Pursuit \ 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..c557d3cf --- /dev/null +++ b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_plugins @@ -0,0 +1,100 @@ +#!/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 . +# + +from pickle import FALSE, TRUE +import rospy +from cav_msgs.msg import Plugin, PluginList, PluginStatus +from cav_srvs.srv import GetRegisteredPlugins + + +plugin_pub = rospy.Publisher('/guidance/plugin_discovery', Plugin()) +required_plugins = rospy.get_param('required_plugins') +plugin_list = PluginList() + +def get_registered_plugins(resp): + """ Service to get registered plugins""" + + plugins = plugin_list.plugins + plugin = Plugin() + + #If the plugins are listed as AVAILABLE, make sure that they are both active and running + for x in plugins: + plugin.available == x.available + plugin.activated = x.activated + plugin.name = x.name + plugin.capability = x.capability + plugin.versionId = x.versionID + plugin.type = x.type + plugin.activated = TRUE + resp.plugins.append(plugin) + #Publish the now active plugin message to /guidance/plugin_discovery + #plugin_pub.publish(plugin) + + +#Check if the plugin is included in the list of required plugins +def check_required_plugins(name): + for i in required_plugins: + if i == name: + return TRUE + else: + return FALSE + +def update_plugin_status(plugin_msg): + print("Received status from: " ++ plugin_msg.name) + requested_name = get_plugin_name(plugin_msg.name) + plugin = Plugin() + plugin.available = plugin_msg.available + plugin.name = plugin_msg.name + plugin.capability = plugin_msg.capability + plugin.type = plugin_msg.type + + #If the requested plugin exists, don't change the activation status + if requested_name != FALSE: + plugin.activated = plugin_msg.activated + elif check_required_plugins(plugin_msg.name) == TRUE: + plugin.activated = TRUE + + plugin_list.plugins.append(plugin) + + +def get_plugin_name(name): + for i in required_plugins: + if i == name: + return i + else: + return FALSE + +def initialize(): + plugin_sub = rospy.Subscriber('plugin/plugin_discovery',Plugin(), update_plugin_status) + #registered_plugins_serv = rospy.ServiceProxy('carla/get_registered_plugins',get_registered_plugins) + registered_plugins_serv = rospy.Service('carla/get_registered_plugins',GetRegisteredPlugins,get_registered_plugins) + rospy.spin() + + +if __name__ == '__main__': + print("carma_to_carla_plugins") + initialize() \ No newline at end of file diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route similarity index 100% rename from carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_route rename to carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_route diff --git a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins b/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins deleted file mode 100644 index 7d11cbe8..00000000 --- a/carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_to_carla_plugins +++ /dev/null @@ -1,60 +0,0 @@ -#!/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/41623134/carla-autoware/blob/master/catkin_ws/src/carla_autoware_bridge/src/carla_autoware_bridge/odometry_to_posestamped -# -# 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 . -# - -from pickle import TRUE -import rospy -from cav_msgs.msg import Plugin, PluginList, PluginStatus -from cav_srvs.srv import GetRegisteredPlugins - -plugin_pub = rospy.Publisher('/guidance/plugin_discovery', Plugin()) - -def get_guidance_plugins(): - """Call Service to get registered plugins""" - - serv = rospy.ServiceProxy('guidance/get_registered_plugins', PluginList) - plugins = serv.call() - plugin = Plugin() - - #If the plugins are listed as AVAILABLE, make sure that they are both active and running - for x in plugins: - if x.available == TRUE: - plugin.activated = TRUE - plugin.name = x.name - plugin.capability = x.capability - plugin.versionId = x.versionID - plugin.type = x.type - #Publish the now active plugin message to /guidance/plugin_discovery - plugin_pub.publish(plugin) - -def initialize(): - get_guidance_plugins() - rospy.spin() - - -if __name__ == '__main__': - print("carma_to_carla_plugins") - initialize() \ No newline at end of file From 5b1f1f3e7846c8d3b62c9359ee063c3f35b18755 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Wed, 2 Mar 2022 20:15:54 +0000 Subject: [PATCH 07/26] Commit changes to carma_carla_plugins --- .../carla_carma_bridge/CMakeLists.txt | 1 + .../carla_carma_bridge/config/parameters.yml | 4 +- .../carla_carma_bridge/package.xml | 1 + .../carla_carma_bridge/carma_carla_plugins | 68 +++++++------------ 4 files changed, 29 insertions(+), 45 deletions(-) diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index 7220e49d..8d9ff040 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED COMPONENTS rospy cav_srvs cav_msgs + health_monitor ) diff --git a/carla-carma-integration/carla_carma_bridge/config/parameters.yml b/carla-carma-integration/carla_carma_bridge/config/parameters.yml index b659afe7..a68289ae 100644 --- a/carla-carma-integration/carla_carma_bridge/config/parameters.yml +++ b/carla-carma-integration/carla_carma_bridge/config/parameters.yml @@ -1,8 +1,8 @@ #String: Route to be initialized -routes: '/opt/carma/routes/' +selected_routes: '/opt/carma/routes/' # List of String: Required plugins for the platform to be functional -required_plugins: +selected_plugins: - RouteFollowing - InLaneCruisingPlugin - StopAndWaitPlugin diff --git a/carla-carma-integration/carla_carma_bridge/package.xml b/carla-carma-integration/carla_carma_bridge/package.xml index 18b731e5..62850733 100644 --- a/carla-carma-integration/carla_carma_bridge/package.xml +++ b/carla-carma-integration/carla_carma_bridge/package.xml @@ -17,6 +17,7 @@ carla_msgs cav_msgs cav_srvs + health_monitor 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 index c557d3cf..cceaea9c 100644 --- 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 @@ -25,73 +25,55 @@ # For a copy, see . # -from pickle import FALSE, TRUE import rospy from cav_msgs.msg import Plugin, PluginList, PluginStatus from cav_srvs.srv import GetRegisteredPlugins - plugin_pub = rospy.Publisher('/guidance/plugin_discovery', Plugin()) -required_plugins = rospy.get_param('required_plugins') -plugin_list = PluginList() +selected_plugins = rospy.get_param('selected_plugins') -def get_registered_plugins(resp): - """ Service to get registered plugins""" - - plugins = plugin_list.plugins - plugin = Plugin() +#List to contain the registered plugins from the service call in initialize() +plugin_list = PluginList() - #If the plugins are listed as AVAILABLE, make sure that they are both active and running - for x in plugins: - plugin.available == x.available - plugin.activated = x.activated - plugin.name = x.name - plugin.capability = x.capability - plugin.versionId = x.versionID - plugin.type = x.type - plugin.activated = TRUE - resp.plugins.append(plugin) - #Publish the now active plugin message to /guidance/plugin_discovery - #plugin_pub.publish(plugin) - #Check if the plugin is included in the list of required plugins -def check_required_plugins(name): - for i in required_plugins: +def check_selected_plugins(name): + for i in selected_plugins: if i == name: - return TRUE + return True else: - return FALSE + return False -def update_plugin_status(plugin_msg): +def plugin_status_cb(plugin_msg): print("Received status from: " ++ plugin_msg.name) - requested_name = get_plugin_name(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 exists, don't change the activation status - if requested_name != FALSE: + #If the requested plugin is registered, don't change the activation status + if requested != None: plugin.activated = plugin_msg.activated - elif check_required_plugins(plugin_msg.name) == TRUE: - plugin.activated = TRUE - - plugin_list.plugins.append(plugin) + elif check_selected_plugins(plugin_msg.name) == True: + plugin.activated = True + activation_srv.call(plugin.name, plugin.activated) -def get_plugin_name(name): - for i in required_plugins: - if i == name: - return i - else: - return FALSE +#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(): - plugin_sub = rospy.Subscriber('plugin/plugin_discovery',Plugin(), update_plugin_status) - #registered_plugins_serv = rospy.ServiceProxy('carla/get_registered_plugins',get_registered_plugins) - registered_plugins_serv = rospy.Service('carla/get_registered_plugins',GetRegisteredPlugins,get_registered_plugins) + 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() From 8a7247df3e65e9192e5b3d70f1ef6cfee98b3660 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Wed, 2 Mar 2022 20:18:27 +0000 Subject: [PATCH 08/26] Commit --- .../src/carla_carma_bridge/carma_carla_plugins | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index cceaea9c..eb88f208 100644 --- 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 @@ -78,5 +78,5 @@ def initialize(): if __name__ == '__main__': - print("carma_to_carla_plugins") + print("carma_carla_plugins") initialize() \ No newline at end of file From fa2a439c805078522337b2d367d55db154a9d645 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Thu, 3 Mar 2022 18:47:25 +0000 Subject: [PATCH 09/26] Commit changes to carma_carla_route --- .../carla_carma_bridge/config/parameters.yml | 2 +- .../src/carla_carma_bridge/carma_carla_route | 44 ++++++++++--------- 2 files changed, 25 insertions(+), 21 deletions(-) diff --git a/carla-carma-integration/carla_carma_bridge/config/parameters.yml b/carla-carma-integration/carla_carma_bridge/config/parameters.yml index a68289ae..a204432c 100644 --- a/carla-carma-integration/carla_carma_bridge/config/parameters.yml +++ b/carla-carma-integration/carla_carma_bridge/config/parameters.yml @@ -1,5 +1,5 @@ #String: Route to be initialized -selected_routes: '/opt/carma/routes/' +selected_route: '/opt/carma/routes/tfhrc_test_route.csv' # List of String: Required plugins for the platform to be functional selected_plugins: 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 index 55717f5a..75b35649 100644 --- 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 @@ -34,16 +34,16 @@ from cav_srvs.srv import SetActiveRoute localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped) state_pub = rospy.Publisher('/guidance/state', GuidanceState) -selected_route = Route() +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('/guidance/get_available_routes', GetAvailableRoutes) + service = rospy.ServiceProxy('/get_available_routes', GetAvailableRoutes) try: resp = service.call(0) available_routes = GetAvailableRoutes() @@ -57,9 +57,9 @@ def get_available_routes(): -def set_route(id, route_name): +def set_route(id): state = GuidanceState() - service = rospy.ServiceProxy('/guidance/set_active_route', SetActiveRoute) + service = rospy.ServiceProxy('/set_active_route', SetActiveRoute) NO_ERROR = ErrorStatus(0, "NO_ERROR") NO_ROUTE = ErrorStatus(1, "NO_ROUTE") @@ -94,17 +94,9 @@ def set_route(id, route_name): else: #Call succeeded: Setting Route print('call set active route success!') - selected_route.route_id = resp.routeID - selected_route.route_name = resp.routeName #Change Guidance from READY to ACTIVE - state.state = 3 - state_pub(state) - - ##If resp.ID & resp.routeName return null, use the route file specified in the config/parameters.yml file - #f = open('config/parameters.yml') - #direc = f.read() - #routes = os.listdir(direc) - #selected_route.route_id = routes[0]. + #set_guidance_active() + except: @@ -119,17 +111,29 @@ def route_event_callback(event): 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: - route = available_routes.availableRoutes[0] - #Insert route from /opt/carma/routes/ folder - #route = Document.load('/opt/carma/routes/tfhrc_test_route.csv') - set_route(route.route_id, route.route_name) + """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() From 55cbd87ee8ba9bf3eb0cffea5ded66f12c7cbbee Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 7 Mar 2022 16:17:24 +0000 Subject: [PATCH 10/26] Commit changes to PR --- .../carla_carma_bridge/CMakeLists.txt | 1 - .../carla_carma_bridge/package.xml | 1 - .../carla_carma_bridge/carma_carla_plugins | 25 ++++---- .../src/carla_carma_bridge/carma_carla_route | 59 ++++++++++--------- 4 files changed, 41 insertions(+), 45 deletions(-) diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index 8d9ff040..7220e49d 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt @@ -10,7 +10,6 @@ find_package(catkin REQUIRED COMPONENTS rospy cav_srvs cav_msgs - health_monitor ) diff --git a/carla-carma-integration/carla_carma_bridge/package.xml b/carla-carma-integration/carla_carma_bridge/package.xml index 62850733..18b731e5 100644 --- a/carla-carma-integration/carla_carma_bridge/package.xml +++ b/carla-carma-integration/carla_carma_bridge/package.xml @@ -17,7 +17,6 @@ carla_msgs cav_msgs cav_srvs - health_monitor 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 index eb88f208..df6683d5 100644 --- 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 @@ -29,8 +29,9 @@ 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') +activation_srv = rospy.ServiceProxy("/activate_plugin") + #List to contain the registered plugins from the service call in initialize() plugin_list = PluginList() @@ -46,28 +47,22 @@ def check_selected_plugins(name): 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") + requested = check_plugin_registration(plugin_msg) - #If the requested plugin is registered, don't change the activation status + #If the requested plugin is not registered, print error message if requested != None: - plugin.activated = plugin_msg.activated + print("Incoming plugin is not included in list of registered plugins") elif check_selected_plugins(plugin_msg.name) == True: - plugin.activated = True - activation_srv.call(plugin.name, plugin.activated) + plugin_msg.activated = True + activation_srv.call(plugin_msg.name, plugin_msg.activated) #Search through list of registered plugins. If it exists, return the requested plugin -def get_plugin(plugin): +def check_plugin_registration(plugin): if plugin in plugin_list.plugins: - return plugin + return True else: - return None + return False def initialize(): 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 index 75b35649..49dcf3af 100644 --- 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 @@ -20,19 +20,15 @@ Call Services from CARMA: 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 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_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') @@ -40,6 +36,13 @@ class ErrorStatus: def __init__(self, value, text): self.value = value self.text = text + +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") #Get the routes to be set after localization def get_available_routes(): @@ -58,15 +61,7 @@ def get_available_routes(): 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 @@ -90,6 +85,8 @@ def set_route(id): else: errorDescription = resp.errorStatus + + print(errorDescription) else: #Call succeeded: Setting Route @@ -102,8 +99,7 @@ def set_route(id): except: print("Fail") - #RouteEvent - route_event_sub = rospy.Subscriber('/guidance/route_event', RouteEvent,route_event_callback) + def route_event_callback(event): @@ -114,26 +110,33 @@ def route_event_callback(event): def check_selected_route(available_routes): if selected_route in available_routes: - return True - else: - return False + 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() - 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 + + #Ensure that localization is complete before checking route information + localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped) + if localized_sub: + if available_routes == None: + print("No routes available.") else: - route = available_routes.availableRoutes[0] - set_route(route.route_id) + """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 + set_route(route.route_id) + + else: + print("Error: Invalid selected route") + rospy.spin() + else: + print("Localization Incomplete") rospy.spin() From cb324e151b89dbc15cfba1b354af81672576682a Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 7 Mar 2022 16:22:16 +0000 Subject: [PATCH 11/26] Commit --- .../src/carla_carma_bridge/carma_carla_route | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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 index 49dcf3af..6f38d8b6 100644 --- 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 @@ -126,8 +126,7 @@ def initialize(): 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 use the first of the available - routes received from the service call""" + """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) From be2c7bca196bf70a569fbd4aee7427a7da3db087 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 7 Mar 2022 22:07:39 +0000 Subject: [PATCH 12/26] Commit carma_carla_guidance --- .../carla_carma_bridge/CMakeLists.txt | 2 + .../carla_carma_bridge/carma_carla_guidance | 45 +++++++++++++++++++ .../src/carla_carma_bridge/carma_carla_route | 8 ++-- 3 files changed, 50 insertions(+), 5 deletions(-) create mode 100644 carla-carma-integration/carla_carma_bridge/src/carla_carma_bridge/carma_carla_guidance diff --git a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt b/carla-carma-integration/carla_carma_bridge/CMakeLists.txt index 7220e49d..c876828d 100644 --- a/carla-carma-integration/carla_carma_bridge/CMakeLists.txt +++ b/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} ) 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..0de909e6 --- /dev/null +++ b/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() \ 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 index 6f38d8b6..a5588e65 100644 --- 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 @@ -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() From 4b81b8691a606eb3e6ebe736f94054eb79cf3d63 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 7 Mar 2022 22:14:33 +0000 Subject: [PATCH 13/26] Commit --- .../src/carla_carma_bridge/carma_carla_guidance | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) 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 index 0de909e6..20fadd19 100644 --- 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 @@ -35,8 +35,9 @@ 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() + if route_selected == True: + if plugins_activated == True: + service.call() else: rospy.spin() From 9b06ce9bb2a885ddfd43228eb2112bca4da7d2fc Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 8 Mar 2022 16:44:01 +0000 Subject: [PATCH 14/26] Commit changes to carma_carla files --- .../carla_carma_bridge/carma_carla_guidance | 18 +++++++++++------- .../src/carla_carma_bridge/carma_carla_plugins | 1 + .../src/carla_carma_bridge/carma_carla_route | 1 + 3 files changed, 13 insertions(+), 7 deletions(-) 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 index 20fadd19..39e94078 100644 --- 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 @@ -32,14 +32,18 @@ def check_plugin_status(plugin_msg): plugins_activated = True def initialize(): + rospy.init_node("carma_carla_guidance") 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: - if plugins_activated == True: - service.call() - else: - rospy.spin() + #While-loop monitors the route and plugin status and sets guidance to active once both are confirmed true + while rospy.is_shutdown() != True: + route_sub = rospy.Subscriber("/route_event",RouteEvent(), check_route_status) + plugin_sub = rospy.Subscriber("/plugin_discovery", Plugin(), check_plugin_status) + if route_selected == True: + if plugins_activated == True: + service.call(True) #Sets guidance status to active + break + + rospy.spin() if __name__ == '__main__': print("carma_carla_guidance") 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 index df6683d5..0a786d29 100644 --- 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 @@ -66,6 +66,7 @@ def check_plugin_registration(plugin): def initialize(): + rospy.init_node("carma_carla_plugins") 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) 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 index a5588e65..36ffe073 100644 --- 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 @@ -113,6 +113,7 @@ def check_selected_route(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() From 9a8dabaf23f4d79484ffec49114527d0386f143b Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 8 Mar 2022 20:21:51 +0000 Subject: [PATCH 15/26] Commit --- .../src/carla_carma_bridge/carma_carla_guidance | 11 +++++------ .../src/carla_carma_bridge/carma_carla_route | 9 ++++++--- 2 files changed, 11 insertions(+), 9 deletions(-) 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 index 39e94078..fd76eb64 100644 --- 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 @@ -35,15 +35,14 @@ def initialize(): rospy.init_node("carma_carla_guidance") service = rospy.ServiceProxy('/set_active_guidance') #While-loop monitors the route and plugin status and sets guidance to active once both are confirmed true - while rospy.is_shutdown() != True: - route_sub = rospy.Subscriber("/route_event",RouteEvent(), check_route_status) - plugin_sub = rospy.Subscriber("/plugin_discovery", Plugin(), check_plugin_status) - if route_selected == True: - if plugins_activated == 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: service.call(True) #Sets guidance status to active break - rospy.spin() if __name__ == '__main__': print("carma_carla_guidance") 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 index 36ffe073..ab7edab1 100644 --- 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 @@ -31,6 +31,7 @@ from cav_srvs.srv import SetActiveRoute state_pub = rospy.Publisher('/guidance/state', GuidanceState) selected_route = rospy.get_param('selected_route') +local_pos = PoseStamped() class ErrorStatus: def __init__(self, value, text): @@ -99,7 +100,9 @@ def set_route(id): except: print("Fail") - +def pose_cb(pose_msg): + local_pos.header = pose_msg.pose + local_pos.pose = pose_msg.pose def route_event_callback(event): @@ -118,8 +121,8 @@ def initialize(): available_routes = get_available_routes() #Ensure that localization is complete before checking route information - localized_sub = rospy.Subscriber('/localization/current_pose', PoseStamped) - if localized_sub: + 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: From 581a5f226def6c2dd9f7f4f2621d211e90863857 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 8 Mar 2022 20:23:56 +0000 Subject: [PATCH 16/26] Commit --- .../src/carla_carma_bridge/carma_carla_guidance | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 index fd76eb64..3666a9d7 100644 --- 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 @@ -40,8 +40,8 @@ def initialize(): plugin_sub = rospy.Subscriber("/plugin_discovery", Plugin(), check_plugin_status) while rospy.is_shutdown() != True: if route_selected == True and plugins_activated == True: - service.call(True) #Sets guidance status to active - break + service.call(True) #Sets guidance status to active + break if __name__ == '__main__': From 912bc6301fae8de39fa2608b1929fc6c175016ff Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 8 Mar 2022 20:55:43 +0000 Subject: [PATCH 17/26] Commit changes to carla-carma-integration files --- .../carla_carma_bridge/carma_carla_guidance | 2 ++ .../carla_carma_bridge/carma_carla_plugins | 1 + .../src/carla_carma_bridge/carma_carla_route | 36 +++++++++---------- 3 files changed, 21 insertions(+), 18 deletions(-) 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 index 3666a9d7..4a4254c6 100644 --- 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 @@ -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 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 index 0a786d29..fbd850a6 100644 --- 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 @@ -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: 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 index ab7edab1..58d47e0f 100644 --- 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 @@ -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,8 +91,6 @@ def set_route(id): else: #Call succeeded: Setting Route print('call set active route success!') - #Change Guidance from READY to ACTIVE - #set_guidance_active() @@ -101,6 +98,7 @@ def set_route(id): 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__': From 366b72cfac10006717eee59f7f08d89a48ea8456 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 8 Mar 2022 21:31:10 +0000 Subject: [PATCH 18/26] Commit --- .../src/carla_carma_bridge/carma_carla_route | 22 +++++-------------- 1 file changed, 6 insertions(+), 16 deletions(-) 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 index 58d47e0f..40131b6c 100644 --- 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 @@ -30,7 +30,7 @@ from cav_srvs.srv import GetAvailableRoutes from cav_srvs.srv import SetActiveRoute selected_route = rospy.get_param('selected_route') -local_pos = PoseStamped() +received_pose = False class ErrorStatus: def __init__(self, value, text): @@ -58,8 +58,6 @@ def get_available_routes(): print("Available Routes acquired") return available_routes.availableRoutes - - def set_route(id): service = rospy.ServiceProxy('/set_active_route', SetActiveRoute) @@ -92,15 +90,12 @@ def set_route(id): #Call succeeded: Setting Route print('call set active route success!') - - except: print("Fail") def pose_cb(pose_msg): - global local_pos - local_pos.header = pose_msg.pose - local_pos.pose = pose_msg.pose + global received_pose + received_pose = True def route_event_callback(event): @@ -121,25 +116,20 @@ def initialize(): #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 local_pos.pose.position.x != 0 and local_pos.pose.position.y != 0: + 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) == True: + if check_selected_route(available_routes): route = selected_route set_route(route.route_id) break else: - print("Error: Invalid selected route") - rospy.spin() - - + print("Error: Invalid selected route") else: print("Localization Incomplete") - rospy.spin() - if __name__ == '__main__': print("carma_carla_route") From 9f35bfe33311438679e53652253b53763f3d8be2 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Wed, 9 Mar 2022 16:38:30 +0000 Subject: [PATCH 19/26] Commit PR changes --- .../src/carla_carma_bridge/carma_carla_guidance | 5 +++-- .../src/carla_carma_bridge/carma_carla_route | 3 ++- 2 files changed, 5 insertions(+), 3 deletions(-) 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 index 4a4254c6..66cdf4c4 100644 --- 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 @@ -16,6 +16,7 @@ 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 @@ -35,14 +36,14 @@ def check_plugin_status(plugin_msg): def initialize(): rospy.init_node("carma_carla_guidance") - service = rospy.ServiceProxy('/set_active_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: - service.call(True) #Sets guidance status to active + activation = service.call(True) #Sets guidance status to active break 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 index 40131b6c..32c03c8f 100644 --- 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 @@ -127,7 +127,8 @@ def initialize(): break else: - print("Error: Invalid selected route") + print("Error: Invalid selected route") + break else: print("Localization Incomplete") From bd4747a0cedb24ad5e582978113792a5f969d20a Mon Sep 17 00:00:00 2001 From: Isaiah Date: Wed, 9 Mar 2022 16:54:24 +0000 Subject: [PATCH 20/26] Commit --- .../src/carla_carma_bridge/carma_carla_guidance | 2 +- .../src/carla_carma_bridge/carma_carla_plugins | 10 +++++----- .../src/carla_carma_bridge/carma_carla_route | 2 +- 3 files changed, 7 insertions(+), 7 deletions(-) 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 index 66cdf4c4..4780b85d 100644 --- 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 @@ -43,7 +43,7 @@ def initialize(): 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.call(True) #Sets guidance status to active + activation = service(True) #Sets guidance status to active break 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 index fbd850a6..44b65b2c 100644 --- 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 @@ -27,10 +27,10 @@ import rospy from cav_msgs.msg import Plugin, PluginList, PluginStatus -from cav_srvs.srv import GetRegisteredPlugins +from cav_srvs.srv import GetRegisteredPlugins, PluginActivation selected_plugins = rospy.get_param('selected_plugins') -activation_srv = rospy.ServiceProxy("/activate_plugin") +activation_srv = rospy.ServiceProxy("/activate_plugin", PluginActivation) #List to contain the registered plugins from the service call in initialize() @@ -54,7 +54,7 @@ def plugin_status_cb(plugin_msg): print("Incoming plugin is not included in list of registered plugins") elif check_selected_plugins(plugin_msg.name) == True: plugin_msg.activated = True - activation_srv.call(plugin_msg.name, plugin_msg.activated) + activation = activation_srv(plugin_msg.name, plugin_msg.activated) #Search through list of registered plugins. If it exists, return the requested plugin @@ -68,8 +68,8 @@ def check_plugin_registration(plugin): def initialize(): rospy.init_node("carma_carla_plugins") - registered_plugins_serv = rospy.ServiceProxy('/get_registered_plugins') - plugin_list.plugins = registered_plugins_serv.call() + 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() 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 index 32c03c8f..f8df1e26 100644 --- 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 @@ -48,7 +48,7 @@ TRANSFORM_ERROR = ErrorStatus(5, "TRANSFORM_ERROR") def get_available_routes(): service = rospy.ServiceProxy('/get_available_routes', GetAvailableRoutes) try: - resp = service.call(0) + resp = service(0) available_routes = GetAvailableRoutes() available_routes.availableRoutes = resp.availableRoutes From 81b8c891c1d440c383adcab1f1f5b4bec4a995b9 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 14 Mar 2022 17:27:36 +0000 Subject: [PATCH 21/26] Commit changes to bridge.launch --- .../carla-carma-agent/agent/bridge.launch | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) diff --git a/carla-carma-integration/carla-carma-agent/agent/bridge.launch b/carla-carma-integration/carla-carma-agent/agent/bridge.launch index 009c0954..78ed646f 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 @@ + + + + + + + + + + + + + + + From 27e0c5fe32aaad43036c699342e02fbb6ac656b4 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Mon, 14 Mar 2022 21:02:36 +0000 Subject: [PATCH 22/26] Commit changes to carla_carma_bridge --- .../carla-carma-agent/agent/bridge.launch | 4 +--- .../src/carla_carma_bridge/carma_carla_plugins | 6 ++++-- .../src/carla_carma_bridge/carma_carla_route | 1 - 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/carla-carma-integration/carla-carma-agent/agent/bridge.launch b/carla-carma-integration/carla-carma-agent/agent/bridge.launch index 78ed646f..c6a02496 100644 --- a/carla-carma-integration/carla-carma-agent/agent/bridge.launch +++ b/carla-carma-integration/carla-carma-agent/agent/bridge.launch @@ -98,7 +98,7 @@ Set the vehicle route after localization. --> - + - - 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 index 44b65b2c..cbed3c1e 100644 --- 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 @@ -53,8 +53,9 @@ def plugin_status_cb(plugin_msg): if requested != None: print("Incoming plugin is not included in list of registered plugins") elif check_selected_plugins(plugin_msg.name) == True: - plugin_msg.activated = True - activation = activation_srv(plugin_msg.name, plugin_msg.activated) + 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 @@ -67,6 +68,7 @@ def check_plugin_registration(plugin): 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() 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 index f8df1e26..a0ea6bf8 100644 --- 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 @@ -65,7 +65,6 @@ def set_route(id): #Service Call Request resp = service(0, id) if resp.errorStatus != NO_ERROR.value: - errorDescription = "" if resp.errorStatus == NO_ROUTE.value: errorDescription = NO_ROUTE.text From 248ed7ccd7ec891da9ec6685a1fb80a6bb8b34f3 Mon Sep 17 00:00:00 2001 From: Isaiah Date: Tue, 15 Mar 2022 14:12:28 -0400 Subject: [PATCH 23/26] Commit --- carla-carma-integration/carla-carma-agent/agent/bridge.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/carla-carma-integration/carla-carma-agent/agent/bridge.launch b/carla-carma-integration/carla-carma-agent/agent/bridge.launch index c6a02496..8db31bc8 100644 --- a/carla-carma-integration/carla-carma-agent/agent/bridge.launch +++ b/carla-carma-integration/carla-carma-agent/agent/bridge.launch @@ -98,7 +98,7 @@ Set the vehicle route after localization. --> - + - + + +