Merge release/lavida (4.5.0) branch into master (#2354)

.circleci/config.yml

@@ -1,21 +1,21 @@
 version: 2
 
 #  Copyright (C) 2018-2022 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.
-# 
+#
 
-# Configuration file for Circle CI 
+# Configuration file for Circle CI
 # CI will report failure if any executed command returns and error status
 # Operations performed are as follows
 # Build source code
@@ -28,7 +28,7 @@ jobs:
     # Pull docker image from docker hub
     # XTERM used for better catkin_make output
     docker:
-      - image: usdotfhwastol/autoware.ai:carma-system-4.4.0
+      - image: usdotfhwastol/autoware.ai:carma-system-4.5.0
         user: carma
         environment:
           TERM: xterm
@@ -57,25 +57,28 @@ jobs:
           command: |
             source ${INIT_ENV}
             ./src/CARMAPlatform/docker/checkout.bash -r ${PWD}
+            sudo ./src/multiple_object_tracking/scripts/install_dependencies.sh
       - run:
           name: Build CARMA ROS 1
           command: |
             source ${INIT_ENV}
             export ROS_PARALLEL_JOBS='-j4 -l4' # Try to reduce memory consumption on build
+            sed -i 's/colcon build /colcon build --packages-skip novatel_oem7_msgs tracetools tracetools_test /' /home/carma/.ci-image/engineering_tools/code_coverage/make_with_coverage.bash
             make_with_coverage.bash -m -e /opt/carma/ -o ./coverage_reports/gcov
       - run:
           name: Run C++ Tests ROS 1
           command: |
             source ${INIT_ENV}
             export ROS_PARALLEL_JOBS='-j4 -l4' # Try to reduce memory consumption on build
+            sed -i 's/colcon test /colcon test --packages-skip novatel_oem7_msgs tracetools tracetools_test /' /home/carma/.ci-image/engineering_tools/code_coverage/make_with_coverage.bash
             make_with_coverage.bash -t -e /opt/carma/ -o ./coverage_reports/gcov
       - run:
           name: Backup ROS1 compile_commands.json
           command: |
             mv /opt/carma/build/compile_commands.json /opt/carma/compile_commands.ros1.json
       - run:
           name: Cleanup before ROS 2 build
-          # Clear the build and install folders before building ROS 2 
+          # Clear the build and install folders before building ROS 2
           command: |
             rm -rf /opt/carma/install
             rm -rf /opt/carma/build
@@ -100,7 +103,7 @@ jobs:
       # Run SonarCloud analysis
       # PR Branches and number extracted from Circle variables and github api
       # Circle CI seems to make a change to the base branch, so we must fetch --force to ensure correct git file change stats
-      # SONAR_SCANNER_TOKEN MUST be secured as an environment variable in Circle CI NOT in this file. 
+      # SONAR_SCANNER_TOKEN MUST be secured as an environment variable in Circle CI NOT in this file.
       # The following sonar settings MUST be set in SonarCloud UI NOT in this file
       # sonar.pullrequest.provider
       # sonar.pullrequest.github.endpoint
@@ -114,7 +117,7 @@ jobs:
             if [ -z "${CIRCLE_PULL_REQUEST}" ]; then
               echo "Non-PR Build Detected. Running analysis on ${CIRCLE_BRANCH}"
               cd src/CARMAPlatform
-              sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN} 
+              sonar-scanner -Dproject.settings=.sonarqube/sonar-scanner.properties -Dsonar.login=${SONAR_SCANNER_TOKEN}
               exit 0;
             fi
             echo "PR branch ${CIRCLE_BRANCH}"
@@ -123,7 +126,7 @@ jobs:
             export PR_NUM=`echo ${CIRCLE_PULL_REQUEST} | cut -d'/' -f7`
             echo "PR number ${PR_NUM}"
             export BASE_BRANCH_URL="https://api.github.com/repos/${CIRCLE_PROJECT_USERNAME}/${CIRCLE_PROJECT_REPONAME}/pulls/${PR_NUM}"
-            export TARGET_BRANCH=$(curl "$BASE_BRANCH_URL" | jq '.base.ref' | tr -d '"') 
+            export TARGET_BRANCH=$(curl "$BASE_BRANCH_URL" | jq '.base.ref' | tr -d '"')
             echo "Target Branch = ${TARGET_BRANCH}"
             cd src/CARMAPlatform
             git fetch --force origin ${TARGET_BRANCH}:${TARGET_BRANCH}

.github/workflows/dockerhub.yml

@@ -7,7 +7,7 @@ on:
     branches:
       - "develop"
       - "master"
-
+      - "release/*"
 jobs:
   dockerhub:
     uses: usdot-fhwa-stol/actions/.github/workflows/dockerhub.yml@main

.sonarqube/sonar-scanner.properties

@@ -1,18 +1,18 @@
 #  Copyright (C) 2018-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.
 
-# Configuration file for Sonar Scanner used for CI 
+# Configuration file for Sonar Scanner used for CI
 sonar.scanner.force-deprecated-java-version-grace-period=true
 sonar.projectKey=usdot-fhwa-stol_CARMAPlatform
 sonar.organization=usdot-fhwa-stol
@@ -41,7 +41,6 @@ sonar.modules= bsm_generator, \
   route, \
   route_following_plugin, \
   trajectory_executor, \
-  health_monitor, \
   localization_manager, \
   arbitrator, \
   plan_delegator, \
@@ -78,7 +77,8 @@ sonar.modules= bsm_generator, \
   platoon_control_ihp, \
   carma_guidance_plugins, \
   carma_cloud_client, \
-  approaching_emergency_vehicle_plugin
+  approaching_emergency_vehicle_plugin, \
+  carma_cooperative_perception
 
 guidance.sonar.projectBaseDir                  = /opt/carma/src/CARMAPlatform/guidance
 bsm_generator.sonar.projectBaseDir             = /opt/carma/src/CARMAPlatform/bsm_generator
@@ -88,7 +88,6 @@ route.sonar.projectBaseDir                     = /opt/carma/src/CARMAPlatform/ro
 route_following_plugin.sonar.projectBaseDir    = /opt/carma/src/CARMAPlatform/route_following_plugin
 trajectory_executor.sonar.projectBaseDir       = /opt/carma/src/CARMAPlatform/trajectory_executor
 localization_manager.sonar.projectBaseDir         = /opt/carma/src/CARMAPlatform/localization_manager
-health_monitor.sonar.projectBaseDir            = /opt/carma/src/CARMAPlatform/health_monitor
 arbitrator.sonar.projectBaseDir                = /opt/carma/src/CARMAPlatform/arbitrator
 plan_delegator.sonar.projectBaseDir            = /opt/carma/src/CARMAPlatform/plan_delegator
 carma_wm.sonar.projectBaseDir                  = /opt/carma/src/CARMAPlatform/carma_wm
@@ -126,6 +125,7 @@ lci_strategic_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/lci_str
 carma_guidance_plugins.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_guidance_plugins
 carma_cloud_client.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_cloud_client
 approaching_emergency_vehicle_plugin.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/approaching_emergency_vehicle_plugin
+carma_cooperative_perception.sonar.projectBaseDir = /opt/carma/src/CARMAPlatform/carma_cooperative_perception
 
 # C++ Package differences
 # Sources
@@ -145,8 +145,6 @@ trajectory_executor.sonar.sources       = src
 trajectory_executor.sonar.exclusions  =test/**
 localization_manager.sonar.sources         = src
 localization_manager.sonar.exclusions  =test/**
-health_monitor.sonar.sources            = src
-health_monitor.sonar.exclusions  =test/**
 arbitrator.sonar.sources                = src
 arbitrator.sonar.exclusions  =test/**
 plan_delegator.sonar.sources            = src
@@ -221,6 +219,8 @@ carma_cloud_client.sonar.sources = src
 carma_cloud_client.sonar.exclusions  =test/**
 approaching_emergency_vehicle_plugin.sonar.sources = src
 approaching_emergency_vehicle_plugin.sonar.exclusions  =test/**
+carma_cooperative_perception.sonar.sources = src
+carma_cooperative_perception.sonar.exclusions = test/**
 
 # Tests
 # Note: For C++ setting this field does not cause test analysis to occur. It only allows the test source code to be evaluated.
@@ -231,17 +231,16 @@ pure_pursuit_wrapper.sonar.tests    = test
 route_generator.sonar.tests         = test
 trajectory_executor.sonar.tests     = test
 localization_manager.sonar.tests       = test
-health_monitor.sonar.tests          = test
 arbitrator.sonar.tests              = test
 carma_wm.sonar.tests                = test
 carma_wm_ctrl.sonar.tests           = test
 mpc_follower_wrapper.sonar.tests    = test
 truck_inspection_client.sonar.tests = test
-roadway_objects.sonar.tests         = test 
+roadway_objects.sonar.tests         = test
 platooning_strategic_IHP.sonar.tests    = test
-platooning_tactical_plugin.sonar.tests         = test 
+platooning_tactical_plugin.sonar.tests         = test
 platoon_control_plugin.sonar.tests         = test
-mobilitypath_publisher.sonar.tests         = test 
+mobilitypath_publisher.sonar.tests         = test
 mock_lightbar_driver.sonar.tests    = test
 port_drayage_plugin.sonar.tests   = test
 rosbag_mock_drivers.sonar.tests   = test
@@ -271,3 +270,4 @@ lci_strategic_plugin.sonar.sources = test
 carma_guidance_plugins.sonar.sources = test
 carma_cloud_client.sonar.sources = test
 approaching_emergency_vehicle_plugin.sonar.sources = test
+carma_cooperative_perception.tests = test

Dockerfile

@@ -33,7 +33,7 @@
 # Stage 1 - Acquire the CARMA source as well as any extra packages
 # /////////////////////////////////////////////////////////////////////////////
 
-FROM usdotfhwastol/autoware.ai:carma-system-4.4.0 AS base-image
+FROM usdotfhwastol/autoware.ai:carma-system-4.5.0 AS base-image
 
 FROM base-image AS source-code
 

arbitrator/include/arbitrator.hpp

@@ -32,11 +32,11 @@
 #include "planning_strategy.hpp"
 #include "capabilities_interface.hpp"
 
-namespace arbitrator 
+namespace arbitrator
 {
     /**
      * Primary work class for the Arbitrator package
-     * 
+     *
      * Governs the interactions of plugins during the maneuver planning phase of
      * the CARMA planning process. Utilizes a generic planning interface to allow
      * for reconfiguration with other paradigms in the future.
@@ -53,27 +53,27 @@ namespace arbitrator
              * \param min_plan_duration The minimum acceptable length of a plan
              * \param planning_frequency The frequency at which to generate high-level plans when engaged
              * \param wm pointer to an inialized world model.
-             */ 
+             */
             Arbitrator(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh,
-                std::shared_ptr<ArbitratorStateMachine> sm, 
-                std::shared_ptr<CapabilitiesInterface> ci, 
+                std::shared_ptr<ArbitratorStateMachine> sm,
+                std::shared_ptr<CapabilitiesInterface> ci,
                 std::shared_ptr<PlanningStrategy> planning_strategy,
                 rclcpp::Duration min_plan_duration,
                 double planning_period,
-                carma_wm::WorldModelConstPtr wm): 
+                carma_wm::WorldModelConstPtr wm):
                 sm_(sm),
                 nh_(nh),
+                min_plan_duration_(min_plan_duration),
+                time_between_plans_(planning_period),
                 capabilities_interface_(ci),
                 planning_strategy_(planning_strategy),
                 initialized_(false),
-                min_plan_duration_(min_plan_duration),
-                time_between_plans_(planning_period),
                 wm_(wm),
                 tf2_buffer_(nh_->get_clock()) {};
-            
+
             /**
              * \brief Begin the operation of the arbitrator.
-             * 
+             *
              * Loops internally via rclcpp::Duration sleeps and spins
              */
             void run();
@@ -93,7 +93,7 @@ namespace arbitrator
              * \brief Initialize transform Lookup from front bumper to map
              */
             void initializeBumperTransformLookup();
-            
+
         protected:
             /**
              * \brief Function to be executed during the initial state of the Arbitrator
@@ -129,9 +129,9 @@ namespace arbitrator
             void guidance_state_cb(carma_planning_msgs::msg::GuidanceState::UniquePtr msg);
 
         private:
-            
+
             VehicleState vehicle_state_; // The current state of the vehicle for populating planning requests
-            
+
             std::shared_ptr<ArbitratorStateMachine> sm_;
             carma_ros2_utils::PubPtr<carma_planning_msgs::msg::ManeuverPlan> final_plan_pub_;
             carma_ros2_utils::SubPtr<carma_planning_msgs::msg::GuidanceState> guidance_state_sub_;
@@ -152,6 +152,6 @@ namespace arbitrator
             tf2::Stamped<tf2::Transform> bumper_transform_;
             bool planning_in_progress_ = false;
     };
-};
+}
 
 #endif

arbitrator/include/arbitrator_config.hpp

@@ -25,7 +25,7 @@
 #include <string>
 
 
-namespace arbitrator 
+namespace arbitrator
 {
     // Stream operator for map data structure
     std::ostream &operator<<(std::ostream &output, const std::map<std::string, double> &map)
@@ -37,25 +37,25 @@ namespace arbitrator
         }
         output << "}";
         return output;
-    };
+    }
 
     /**
      * \brief Config struct
      */
     struct Config
     {
-        double min_plan_duration = 6.0; // The minimum amount of time in seconds that an arbitrated plan must cover for the 
+        double min_plan_duration = 6.0; // The minimum amount of time in seconds that an arbitrated plan must cover for the
                                         // system to proceed with execution
-        double target_plan_duration = 15.0; // The nominal amount of time in seconds that an arbitrated plan should cover for the 
+        double target_plan_duration = 15.0; // The nominal amount of time in seconds that an arbitrated plan should cover for the
                                         // system to operate at best performance
         double planning_frequency = 1.0; // The planning frequency (hz) for generation for arbitrated plans
-        int beam_width = 3; // The width of the search beam to use for arbitrator planning, 1 = 
+        int beam_width = 3; // The width of the search beam to use for arbitrator planning, 1 =
                                   // greedy search, as it approaches infinity the search approaches breadth-first search
-        bool use_fixed_costs = false; // Use fixed priority cost function over using the cost system for 
+        bool use_fixed_costs = false; // Use fixed priority cost function over using the cost system for
                                      // evaluating maneuver plans
-        std::map<std::string, double> plugin_priorities = {}; // The priorities associated with each plugin during the planning 
+        std::map<std::string, double> plugin_priorities = {}; // The priorities associated with each plugin during the planning
                                                                // process, values will be normalized at runtime and inverted into costs
-       
+
         // Stream operator for this config
         friend std::ostream &operator<<(std::ostream &output, const Config &c)
         {
@@ -73,4 +73,4 @@ namespace arbitrator
 
 }   // namespace abitrator
 
-#endif
+#endif

arbitrator/include/capabilities_interface.hpp

@@ -25,6 +25,8 @@
 #include <string>
 #include <carma_planning_msgs/srv/plugin_list.hpp>
 #include <carma_planning_msgs/srv/get_plugin_api.hpp>
+#include <carma_planning_msgs/srv/plan_maneuvers.hpp>
+
 
 
 namespace arbitrator
@@ -53,7 +55,7 @@ namespace arbitrator
             /**
              * \brief Get the list of topics that respond to the capability specified by
              *      the query string
-             * 
+             *
              * \param query_string The string name of the capability to look for
              * \return A list of all responding topics, if any are found.
              */
@@ -62,33 +64,34 @@ namespace arbitrator
 
             /**
              * \brief Template function for calling all nodes which respond to a service associated
-             *      with a particular capability. Will send the service request to all nodes and 
+             *      with a particular capability. Will send the service request to all nodes and
              *      aggregate the responses.
-             * 
+             *
              * \tparam MSrvReq The typename of the service message request
              * \tparam MSrvRes The typename of the service message response
-             * 
+             *
              * \param query_string The string name of the capability to look for
              * \param The message itself to send
              * \return A map matching the topic name that responded -> the response
              */
             template<typename MSrvReq, typename MSrvRes>
             std::map<std::string, std::shared_ptr<MSrvRes>> multiplex_service_call_for_capability(const std::string& query_string, std::shared_ptr<MSrvReq> msg);
-            
+
 
             const static std::string STRATEGIC_PLAN_CAPABILITY;
         protected:
         private:
             std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh_;
+            std::unordered_map<std::string,carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::PlanManeuvers>> registered_strategic_plugins_;
 
             carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::GetPluginApi> sc_s_;
-            std::unordered_set <std::string> capabilities_ ; 
+            std::unordered_set <std::string> capabilities_ ;
+
 
 
-
     };
 
-};
+}
 
 #include "capabilities_interface.tpp"