Merge branch 'develop' into feature/ros2_tracing_support

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__

arbitrator/include/cost_system_cost_function.hpp

@@ -28,9 +28,9 @@ namespace arbitrator
 {
     /**
      * \brief Implementation of the CostFunction interface
-     * 
+     *
      * Implements costs by utilizing a ROS service call to the CARMA cost plugin
-     * system. Passes on the input maneuver plan to the Cost Plugin System for 
+     * system. Passes on the input maneuver plan to the Cost Plugin System for
      * computation of the actual cost and cost per unit distance.
      */
     class CostSystemCostFunction : public CostFunction
@@ -44,9 +44,9 @@ namespace arbitrator
             /**
              * Initialize the CostSystemCostFunction to communicate over the network.
              * Sets up any ROS service clients needed to interact with the cost plugin system.
-             * 
+             *
              * Must be called before using this cost function implementation.
-             * 
+             *
              * \param nh A publicly namespaced nodehandle
              */
             void init(std::shared_ptr<carma_ros2_utils::CarmaLifecycleNode> nh);
@@ -55,7 +55,7 @@ namespace arbitrator
              * \brief Compute the unit cost over distance of a given maneuver plan
              * \param plan The plan to evaluate
              * \return double The total cost divided by the total distance of the plan
-             * 
+             *
              * \throws std::logic_error if not initialized
              */
             double compute_total_cost(const carma_planning_msgs::msg::ManeuverPlan& plan);
@@ -68,10 +68,9 @@ namespace arbitrator
              */
             double compute_cost_per_unit_distance(const carma_planning_msgs::msg::ManeuverPlan& plan);
         private:
-            carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::ComputePlanCost> cost_system_sc_; 
+            carma_ros2_utils::ClientPtr<carma_planning_msgs::srv::ComputePlanCost> cost_system_sc_;
             bool initialized_ = false;
     };
-};
+}
 
 #endif //__ARBITRATOR_INCLUDE_COST_SYSTEM_COST_FUNCTION_HPP__
-

arbitrator/include/fixed_priority_cost_function.hpp

@@ -25,12 +25,12 @@ namespace arbitrator
 {
     /**
      * \brief Implementation of the CostFunction interface
-     * 
+     *
      * Implements costs by associating a fixed priority number with each plugin
      * (as specified by configuration). This priority is then normalized across
-     * all plugins, and then an inverse is computed to arrive at the cost per 
+     * all plugins, and then an inverse is computed to arrive at the cost per
      * unit distance for that plugins.
-     * 
+     *
      * e.g. Three plugins with priority 20, 10, and 5 will respectively have
      * costs 0, 0.5, 0.75 per unit distance.
      */
@@ -59,6 +59,6 @@ namespace arbitrator
         private:
             std::map<std::string, double> plugin_costs_;
     };
-};
+}
 
 #endif //__ARBITRATOR_INCLUDE_FIXED_PRIORITY_COST_FUNCTION_HPP__

arbitrator/include/planning_strategy.hpp

@@ -32,9 +32,9 @@ namespace arbitrator
         public:
             /**
              * \brief Generate a plausible maneuver plan
-             * 
+             *
              * \param start_state The starting state of the vehicle to plan for
-             * 
+             *
              * \return A maneuver plan from the vehicle's current state
              */
             virtual carma_planning_msgs::msg::ManeuverPlan generate_plan(const VehicleState& start_state) = 0;
@@ -44,6 +44,6 @@ namespace arbitrator
              */
             virtual ~PlanningStrategy(){};
     };
-};
+}
 
-#endif //__ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__
+#endif //__ARBITRATOR_INCLUDE_PLANNING_STRATEGY_HPP__

arbitrator/include/plugin_neighbor_generator.hpp

@@ -25,11 +25,11 @@ namespace arbitrator
 {
     /**
      * \brief Implementation of the NeighborGenerator interface using plugins
-     * 
+     *
      * Queries plugins via the CapabilitiesInterface to contribute additional
      * maneuvers as the potential child/neighbor nodes of a plan in progress.
-     * 
-     * \tparam T The type of CapabilitiesInterface to use. Templated to enable 
+     *
+     * \tparam T The type of CapabilitiesInterface to use. Templated to enable
      *      testing
      */
     template <class T>
@@ -44,7 +44,7 @@ namespace arbitrator
                 ci_(ci) {};
 
             /**
-             * Generates a list of neighbor states for the given plan using 
+             * Generates a list of neighbor states for the given plan using
              * the plugins available to the system
              * \param plan The plan that is the current search state
              * \param initial_state The initial state of the vehicle at the start of plan. This will be provided to planners for specific use when plan is empty
@@ -54,8 +54,8 @@ namespace arbitrator
         private:
             std::shared_ptr<T> ci_;
     };
-};
+}
 
 #include "plugin_neighbor_generator.tpp"
 
-#endif //__ARBITRATOR_INCLUDE_PLUGIN_NEIGHBOR_GENERATOR_HPP__
+#endif //__ARBITRATOR_INCLUDE_PLUGIN_NEIGHBOR_GENERATOR_HPP__