Feat/persistent service client refactor

README.md

@@ -26,17 +26,19 @@ Here's a procedure on how to get started with the configurations and the launchi
 - [ros_queue_msgs](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs) (ROS messages and services used as interface for the ros_queuing system, included already in the ros_queuing_system repo).
 - [ros_queue_msgs](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs) (Implementation examples and dummy stochastic services to test the system, included already in the ros_queuing_system repo).
 - [rosparam_utils](https://github.com/etienn8/rosparam_utils) (Tools to fetch rosparams more easily. Not directly dependent but some dependent packages need it).
+- [ros_boosted_utilities](https://github.com/etienn8/ros_boosted_utilities) (Contains persistent ROS service clients to make the interfaces between the system faster.)
 ##### Optional
 - [Catkin tools](https://catkin-tools.readthedocs.io/en/latest/#)(Tools to help build all the packages in parallel and to test the system)
 	- Follow their [installation](https://catkin-tools.readthedocs.io/en/latest/installing.html) and [initialization](https://catkin-tools.readthedocs.io/en/latest/quick_start.html) guide before building this package.
 
 
 #### Building
 
-To build from source, clone the [rosparam_utils](https://github.com/etienn8/rosparam_utils) repo, clone the latest version from this repository into your catkin workspace and compile all the packages using
+To build from source, clone the [rosparam_utils](https://github.com/etienn8/rosparam_utils), clone the [ros_boosted_utilities](https://github.com/etienn8/ros_boosted_utilities) repo, clone the latest version from this repository into your catkin workspace and compile all the packages using
 
 	cd catkin_workspace/src
 	git clone https://github.com/etienn8/rosparam_utils.git
+	git clone https://github.com/etienn8/ros_boosted_utilities.git
 	git clone https://github.com/etienn8/ros_queuing_system.git
 	cd ../
 	rosdep install --from-paths . --ignore-src

queue_controller/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(catkin REQUIRED COMPONENTS
   actionlib
   std_srvs
   std_msgs
+  ros_boosted_utilities
 )
 
 ## System dependencies are found with CMake's conventions
@@ -109,8 +110,8 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
-   INCLUDE_DIRS include
-#  LIBRARIES queue_controller
+   INCLUDE_DIRS include ${ros_boosted_utilities_INCLUDE_DIRS}
+   LIBRARIES queue_controller_utils_lib
    CATKIN_DEPENDS queue_server ros_queue_msgs roscpp message_runtime std_srvs std_msgs actionlib_msgs actionlib
 #  DEPENDS system_lib
 )
@@ -127,9 +128,9 @@ include_directories(
 )
 
 ## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/queue_controller.cpp
-# )
+add_library(queue_controller_utils_lib
+   src/queue_controller_utils.cpp
+)
 
 ## Add cmake target dependencies of the library
 ## as an example, code may need to be generated before libraries
@@ -151,8 +152,14 @@ add_executable(transmission_vector_queue_controller_node src/transmission_vector
 ## same as for the library above
 add_dependencies(transmission_vector_queue_controller_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
+## Specify libraries to link a library or executable target against
+target_link_libraries(queue_controller_utils_lib
+  ${catkin_LIBRARIES}
+)
+
 ## Specify libraries to link a library or executable target against
 target_link_libraries(transmission_vector_queue_controller_node
+  queue_controller_utils_lib
   ${catkin_LIBRARIES}
 )
 

queue_controller/config/min_drift-plus-penalty_template.yaml

@@ -2,20 +2,38 @@
 
 # Indicate the type of controller. Supported types: min_drift_plus_penalty, renewal_min_drift_plus_penalty
 controller_type: "min_drift_plus_penalty"
-# Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller. Float value.
+
+# If set to true, the controller will start a new control loop once the last control loop is finished. If set to
+# false, the controler will wait to for a service call to start_control_loop or an empty topic on start_control_loop. 
+# Default set to true.
+is_periodic: true
+
+# Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller. 
+#  Not used if the parameter is_periodic is set to false. Float value.
 time_step: 0.1
 # If set to false, the controller will find an optimal next action to take and then update the virtual queues based 
 # on that optimal solution. If set to true, the controller will update the virtual queues based on the current of 
 # state of the system then find an optimal next action. Default: set to false
 inverse_control_and_steps: false
 
+# If set to true, the controller will trigger or send changes to the queue server base on the flag inverse_control_and_steps.
+# If set to false, the controller will not send any changes to the specified queue_server_name. Its used if the user wants 
+# to implement another mecanism to change the virtual queues in queue server or to run multiple controller in parallel 
+# for the same queue server where only one of them is sending changes and not the others. Default: set to true.
+responsible_for_virtual_queue_changes: true
+
+# When true, a topic will be published name "controller_costs" that will output the cost of each action
+# and each internal metric of the actions. Default is set to false.
+measure_cost: false
+
 # Renewal timing parameters.
 # Minimum time for the renewal process. The controller will not start before that time even if the action is reached. 
 # The action reached is triggered by a message to a goal_reached topic. Only used if the controller is of type renewal_min_drift_plus_penalty
 # Seconds in float value.
 min_renewal_time: 0.0
 # Maximum time for the renewal process. The controller will start a new control step even if the action isn't reached. 
-# The action reached is triggered by a message to a goal_reached topic. Only used if the controller is of type renewal_min_drift_plus_penalty
+# The action reached is triggered by a message to a goal_reached topic. Only used if the controller is of type renewal_min_drift_plus_penalty and
+# the parameter is_periodic is set to true. 
 # Seconds in float value.
 max_renewal_time: 2.0
 
@@ -71,4 +89,18 @@ v_parameter: 1.0
 # If set to true, the controller will optimize the ratio of the time average penalty over the expected renewal frame duration.
 # If set to false, the controller will optimize the time average penalty.
 # Only used for the renewal_min_drift_plus_penalty type of controller.
-is_penalty_renewal_dependent: false
+is_penalty_renewal_dependent: false
+
+# ====== Multi controller synchronization.======
+# Flag that indicate if this controller should wait for another controller to finish its optimization before sending
+# action and sending the virtual changes to a queue server. Also publish a topic called controller_started and another called optimization_done
+# Default is set to false.
+part_of_multicontroller_synchronization: true
+# Topic starts a control loop when called. A called to the topic start_control_loop is the same as calling this topic.
+# Mainly used to be connected on a control_loop_started topic of another controller in a multicontroller scenario.
+# It's not used if the parameter is_periodic is set to true. 
+start_control_loop_sync_topic: ""
+# Topic of the controller that this controller depends on. It can connect to a optimization_done topic of another controoler.
+# It will not be called if part_of_multicontroller_synchronization is false. Leaving it empty means the
+# controller is not dependent on another controller.
+dependent_on_controller_topic: ""

queue_controller/include/queue_controller/action_configs/transmission_vector_controller_config.hpp

@@ -5,10 +5,12 @@
 #include "ros_queue_msgs/PotentialTransmissionVectorSet.h"
 #include "ros_queue_msgs/PotentialTransmissionVectorSpaceFetch.h"
 #include "ros_queue_msgs/TransmissionVectorAction.h"
+#include "ros_queue_msgs/TransmissionVectorControllerCostsList.h"
 
 
 typedef ros_queue_msgs::MetricTransmissionVectorPredictions METRIC_CONTROL_PREDICTION_SRV;
 typedef ros_queue_msgs::PotentialTransmissionVectorSet POTENTIAL_ACTION_SET_MSG;
 typedef ros_queue_msgs::PotentialTransmissionVectorSpaceFetch POTENTIAL_ACTION_SET_SRV;
 typedef ros_queue_msgs::TransmissionVectorAction ACTION_LIB_OUTPUT_TYPE;
-typedef ros_queue_msgs::TransmissionVectorGoal ACTION_LIB_OUTPUT_GOAL_TYPE;
+typedef ros_queue_msgs::TransmissionVectorGoal ACTION_LIB_OUTPUT_GOAL_TYPE;
+typedef ros_queue_msgs::TransmissionVectorControllerCostsList CONTROLLER_COSTS_LIST_TYPE;

queue_controller/include/queue_controller/controller_queue_struct.hpp

@@ -5,12 +5,17 @@
 
 #include "ros/ros.h"
 
+#include "ros_queue_msgs/FloatRequest.h"
+
+#include "ros_boosted_utilities/persistent_service_client.hpp"
+
 using std::string;
 
 /**
  * @brief Structure that contains all the necessary parameters about a queue that the controller 
  * needs to evaluate its changes and compute the best action.
 */
+template <typename TMetricControlPredictionSrv>
 struct ControllerQueueStruct
 {
     /**
@@ -58,28 +63,28 @@ struct ControllerQueueStruct
      * of the system and a given potential action. Should not be defined if arrival_based_on_queue_size_service_
      * is used. If both are defined, expected_arrival_service_ will be used.
     */
-    ros::ServiceClient expected_arrival_service_;
+    PersistentServiceClient<TMetricControlPredictionSrv> expected_arrival_service_;
 
     /**
      * @brief ROS Service used to compute the expected departures of a queue based on a the current state
      * of the system and a given potential action. Should not be defined if departure_based_on_queue_size_service_
      * is used. If both are defined, expected_arrival_service_ will be used.
     */
-    ros::ServiceClient expected_departure_service_;
+    PersistentServiceClient<TMetricControlPredictionSrv> expected_departure_service_;
 
     /**
      * @brief ROS Service used to get a a metric thas is independant from the action. Should not be defined 
      * if expected_arrival_service_ is used. If both are defined, expected_arrival_service_ will be used.
      * @details Since it uses a different service definition compared to expected_arrival_service_ because
      * the latter uses a potential action as an input, arrival_independant_from_action_service_ need to be created.
     */
-    ros::ServiceClient arrival_independent_from_action_service_;
+    PersistentServiceClient<ros_queue_msgs::FloatRequest> arrival_independent_from_action_service_;
 
     /**
      * @brief ROS Service used to get a current queue size if it's used as the departure metric. Should not be defined 
      * if expected_departure_service_ is used. If both are defined, expected_departure_service_ will be used.
      * @details Since it uses a different service definition  compared to expected_departure_service_ because
      * the latter uses a potential action as an input, departure_independant_from_action_service_ need to be created.
     */
-    ros::ServiceClient departure_independent_from_action_service_;
+    PersistentServiceClient<ros_queue_msgs::FloatRequest> departure_independent_from_action_service_;
 };