Merge branch 'usdot-fhwa-stol:develop' into develop

.circleci/config.yml

@@ -62,12 +62,14 @@ jobs:
           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

.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

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

@@ -53,7 +53,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 +62,33 @@ 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_;
 
             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"
 

arbitrator/include/capabilities_interface.tpp

@@ -22,37 +22,43 @@
 #include <string>
 #include <functional>
 #include <carma_planning_msgs/srv/plan_maneuvers.hpp>
+#include <rclcpp/exceptions/exceptions.hpp>
 
-namespace arbitrator 
+namespace arbitrator
 {
     template<typename MSrvReq, typename MSrvRes>
-    std::map<std::string, std::shared_ptr<MSrvRes>> 
+    std::map<std::string, std::shared_ptr<MSrvRes>>
         CapabilitiesInterface::multiplex_service_call_for_capability(const std::string& query_string, std::shared_ptr<MSrvReq> msg)
     {
-        std::vector<std::string> topics = get_topics_for_capability(query_string);
-
         std::map<std::string, std::shared_ptr<MSrvRes>> responses;
 
-        for (auto i = topics.begin(); i != topics.end(); i++) 
+        for (const auto & topic : get_topics_for_capability(query_string))
         {
-            auto topic = *i;
-            auto sc = nh_->create_client<carma_planning_msgs::srv::PlanManeuvers>(topic);
-            RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << topic);
-
-            std::shared_future<std::shared_ptr<MSrvRes>> resp = sc->async_send_request(msg);
-
-            auto future_status = resp.wait_for(std::chrono::milliseconds(500));
-
-            if (future_status == std::future_status::ready) {
-                responses.emplace(topic, resp.get());
-            }
-            else
-            {
-                RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << topic);
+            try {
+                using std::literals::chrono_literals::operator""ms;
+
+                const auto client = nh_->create_client<carma_planning_msgs::srv::PlanManeuvers>(topic);
+                RCLCPP_DEBUG_STREAM(rclcpp::get_logger("arbitrator"), "found client: " << topic);
+
+                const auto response = client->async_send_request(msg);
+
+                switch (const auto status{response.wait_for(500ms)}; status) {
+                    case std::future_status::ready:
+                        responses.emplace(topic, response.get());
+                        break;
+                    case std::future_status::deferred:
+                    case std::future_status::timeout:
+                        RCLCPP_WARN_STREAM(rclcpp::get_logger("arbitrator"), "failed...: " << topic);
+                }
+            } catch(const rclcpp::exceptions::RCLError& error) {
+                RCLCPP_ERROR_STREAM(rclcpp::get_logger("arbitrator"),
+                    "Cannot make service request for service '" << topic << "': " << error.what());
+                continue;
             }
         }
+
         return responses;
     }
-};
+}
 
 #endif

arbitrator/include/cost_function.hpp

@@ -48,6 +48,6 @@ namespace arbitrator
              */
             virtual ~CostFunction(){};
     };
-};
+}
 
-#endif //__ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__
+#endif //__ARBITRATOR_INCLUDE_COST_FUNCTION_HPP__