diff --git a/README.md b/README.md index c1b0361..4fa1b05 100644 --- a/README.md +++ b/README.md @@ -26,6 +26,7 @@ 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. @@ -33,10 +34,11 @@ Here's a procedure on how to get started with the configurations and the launchi #### 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 diff --git a/queue_controller/.assets/queue_controller_synchronization_diagram_v2.png b/queue_controller/.assets/queue_controller_synchronization_diagram_v2.png new file mode 100644 index 0000000..489fda5 Binary files /dev/null and b/queue_controller/.assets/queue_controller_synchronization_diagram_v2.png differ diff --git a/queue_controller/CMakeLists.txt b/queue_controller/CMakeLists.txt index 416dfa2..4e925e1 100644 --- a/queue_controller/CMakeLists.txt +++ b/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} ) diff --git a/queue_controller/README.MD b/queue_controller/README.MD index 40f598b..1db71b4 100644 --- a/queue_controller/README.MD +++ b/queue_controller/README.MD @@ -29,22 +29,24 @@ This is research code, expect that it changes often and any fitness for a partic - [message_runtime](http://wiki.ros.org/message_runtime) (ROS message dependencies package), - [actionlib_msgs](http://wiki.ros.org/actionlib_msgs) (Message definitions of the actionlib package used for the output of the controller), - [actionlib](http://wiki.ros.org/actionlib) (Interfaces that gives a result and feedback for an action. Used by the output of the controller), +- [ros_boosted_utilities](https://github.com/etienn8/ros_boosted_utilities) (Contains persistent ROS service clients to make the interfaces between the system faster.) - [std_srvs](http://wiki.ros.org/std_srvs) (ROS standard service definitions), - [std_msgs](http://wiki.ros.org/std_msgs) (ROS standard service definitions), - std::map (Standard library for C++ dictionnaries), - std::utility (Standard library that permits move semantics), - std::memory (Standard library for smart pointers), -- std::chrono (Used for the the timing of the controller' steps), +- std::chrono (Used for the timing of the controller' steps), - std::mutex (Library of mutexes to protect variable access from race condition), - std::vector sudo rosdep install --from-paths src #### 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 @@ -69,7 +71,7 @@ Or get the last renewal time with: `rosservice call //get_last_renewal_time` ## Optimization formulation -Let's start with an example to understand why the following optimization process is relevant and why queues are used to formulate the problem. Imagine a long run mission where a robot needs to explore an unknown communication-constrained environment as quick as possible and needs to return information to a base station as frequent as possible. Let's assume that the robot tries to optimize those two metrics at the same time. Thus, a formulation could be that it will penalize leaving the communication radius of the base station and it will penalize being far from new exploration frontiers. Finding the best action that optimize both metrics at each step might prevent the robot from leaving the base station's communication radius even though it has no new information to report. Therefore, optimizing this formulation at each time step might not lead to the quickest exploration overall compared to a method where the robot could leave the base station when it has no information to report. Instead of directly optimize this formulation at each time step, minimizing the the time average of exploration and the frequent return on the overall mission might be more accurately what's desired. For example, the robot might leave the communication range for a few time frames thus optimizing more the exploration, then returns to the base station to report the information and then cycles between those two behaviors like that. Thus, the robot is optimizing on average both metrics but, in the point of view of a time step, it chose one metric over the other. Event if its seems trivial to hardcode this two-metric optimization example by hand, the problem might have more metrics to follow on average. Thus, a general framework could be helpful. If the problem is cast well, that's where queue-stabilizing controllers might become interesting. Those controllers find, at each time step, the optimal action to follow all its time average constraints while optimizing a time average metric. +Let's start with an example to understand why the following optimization process is relevant and why queues are used to formulate the problem. Imagine a long run mission where a robot needs to explore an unknown communication-constrained environment as quick as possible and needs to return information to a base station as frequent as possible. Let's assume that the robot tries to optimize those two metrics at the same time. Thus, a formulation could be that it will penalize leaving the communication radius of the base station and it will penalize being far from new exploration frontiers. Finding the best action that optimize both metrics at each step might prevent the robot from leaving the base station's communication radius even though it has no new information to report. Therefore, optimizing this formulation at each time step might not lead to the quickest exploration overall compared to a method where the robot could leave the base station when it has no information to report. Instead of directly optimize this formulation at each time step, minimizing the time average of exploration and the frequent return on the overall mission might be more accurately what's desired. For example, the robot might leave the communication range for a few time frames thus optimizing more the exploration, then returns to the base station to report the information and then cycles between those two behaviors like that. Thus, the robot is optimizing on average both metrics but, in the point of view of a time step, it chose one metric over the other. Event if its seems trivial to hardcode this two-metric optimization example by hand, the problem might have more metrics to follow on average. Thus, a general framework could be helpful. If the problem is cast well, that's where queue-stabilizing controllers might become interesting. Those controllers find, at each time step, the optimal action to follow all its time average constraints while optimizing a time average metric. The goal of queue_controller is to optimize the following formulation[1]: ```math @@ -86,7 +88,7 @@ The greedy algorithm used at each time step to optimize the general problem abov \begin{aligned}Minimize:\quad & V\hat{y}_0(\alpha(t),\omega(t)) + \sum^{K}_{k=1}w_kQ_k(t)[\hat{a}_k(\alpha(t),\omega(t))-\hat{b}_k(\alpha(t),\omega(t))] + \sum^{L}_{l=1}w_lZ_{l}(t)\hat{y}_l(\alpha(t),\omega(t)) + \sum^{H}_{j=1}w_jH_{j}(t)\hat{e}_j(\alpha(t),\omega(t)) \\Subject\ to: \quad& \alpha(t) \in A_{\omega(t)}\end{aligned} ``` -where $`\omega(t)`$ represents the state of the system (considered as a random variable), $`\alpha(t)`$ represents a potential action in the set of all action $`A_{\omega(t)}`$ at time $`t`$, $`V`$ represents a parameter that prioritize the penalty over the queue stabilization the higher it is, $`w_k\textrm{, }w_j \textrm{ and } w_j`$ represents weights that prioritize queues between each other, $`Q_k(t)`$ represents the size of the real queue $`k`$ at time $`t`$, $`Z_l(t) \textrm{ and } H_j(t)`$ represents the size of the virtual queues $`j`$ and $`l`$ respectively at time $`t`$, $`\hat{a}_k`$ represents the number of incoming data in the real queue $`k`$, $`\hat{b}_k`$ represents the number of data that could be transmitted, $`\hat{y}_l`$ represents the changes (arrival minus departure) of the virtual queue $`l`$ and $`\hat{e}_j$ represents the changes (arrival minus departure) of the queue virtual queue $j$. The function $\hat{y}_0\textrm{, }\hat{a}_k\textrm{, }\hat{b}_k\textrm{, }\hat{y}_l\textrm{ and }\hat{e}_j`$ are all function evaluated at the current state $`\omega(t)`$ and potential action $`\alpha(t)`$. In other words, the controller will evaluate this cost function for each action in the action set based on predictions on what the penalty will be and how the queues will changes knowing the current state of the system. For the interested reader, the "memory" that allows each greedy controller steps to be aware of the average is embedded in the queue size that changes based. +where $`\omega(t)`$ represents the state of the system (considered as a random variable), $`\alpha(t)`$ represents a potential action in the set of all action $`A_{\omega(t)}`$ at time $`t`$, $`V`$ represents a parameter that prioritize the penalty over the queue stabilization the higher it is, $`w_k\textrm{, }w_j \textrm{ and } w_j`$ represents weights that prioritize queues between each other, $`Q_k(t)`$ represents the size of the real queue $`k`$ at time $`t`$, $`Z_l(t) \textrm{ and } H_j(t)`$ represents the size of the virtual queues $`j`$ and $`l`$ respectively at time $`t`$, $`\hat{a}_k`$ represents the number of incoming data in the real queue $`k`$, $`\hat{b}_k`$ represents the number of data that could be transmitted, $`\hat{y}_l`$ represents the changes (arrival minus departure) of the virtual queue $`l`$ and $`\hat{e}_j$ represents the changes (arrival minus departure) of the queue virtual queue $j$. The function $\hat{y}_0\textrm{, }\hat{a}_k\textrm{, }\hat{b}_k\textrm{, }\hat{y}_l\textrm{ and }\hat{e}_j`$ are all function evaluated at the current state $`\omega(t)`$ and potential action $`\alpha(t)`$. In other words, the controller will evaluate this cost function for each action in the action set based on predictions on what the penalty will be and how the queues will changes knowing the current state of the system. For the interested reader, the "memory" that allows each greedy controller steps to be aware of the average is embedded in the queue size that changes over based on the control actions. Once the best action is found, the queues will be updated based on the arrivals and departures expected from the optimal action [1]. A configuration also allowed the system to update the queues at the beginning of the next time step instead. It allows the queues to be updated with the real change in the system, thus removing bias from a bad action execution and bad prediction models. A renewal version of the min drift-plus-penalty algorithm is also implemented [1]. That version takes into account that the time between the controller calls might not be constant. Thus the controller scales some specified penalty, arrivals and departures with the expected time it takes to perform the action to ensure a certain rate. The change in the formulation is shown in the configuration section below. ## System explanation @@ -136,20 +138,30 @@ Five messages and services definition needs to be defined at compilation for the --- # The feedback ``` +6. The message used to log the internal cost of each controller metrics for a given action. Two fields should be defined: A variable named `action` with the type of the action type and a variable name `costs` of the type [ros_queue_msgs/ControllerActionCosts](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs/msg/ControllerActionCosts.msg). Add it in the [ros_queue_msgs/msg](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs/msg) directory. Following the same example, the content of `TransmissionVectorControllerCosts.msg` defined in [ros_queue_msgs/msg](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs/msg)is : + ``` + ros_queue_msgs/TransmissionVector action + ros_queue_msgs/ControllerActionCosts costs + ``` +7. The message of the list of all the internal costs for each action for a control steps. It should have a single field named `action_costs_list` that is an array of the message defined in step 6. Add it in the [ros_queue_msgs/msg](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs/msg) directory. Following the same example, the content of `TransmissionVectorControllerCosts.msg` defined in [ros_queue_msgs/msg](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue_msgs/msg)is : + ``` + ros_queue_msgs/TransmissionVectorControllerCosts[] action_costs_list + ``` + Once all the new messages created, don't forget to include to the [CMakeList.txt](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/CMakeLists.txt) of the ros_queue_msgs package to generate them. Once done, build the catkin workspace. #### Create the new source files 1. In the directory `queue_controller/include/queue_controller/action_configs/`, copy and paste the `transmission_vector_controller_config.hpp` file in the same directory. 2. Renamed it with a relevant name linked to the new added action. 3. Edit the content of the new file: - 1. Replace all the `#include` of the old messages with the five new messages created above. - 2. Change the type of the `typedef` (not the the name of the `typedef`) in the following manner (be careful to add the "Action" and "Goal" suffix at the end of the two last message definitions respectively): + 1. Replace all the five first `#include` of the old messages with the first five new messages created above. The last include should be replaced by the message created at the step 7. + 2. Change the type of the `typedef` (not the name of the `typedef`) in the following manner (be careful to add the "Action" and "Goal" suffix at the end of the two last message definitions respectively): ``` typedef ros_queue_msgs:: METRIC_CONTROL_PREDICTION_SRV; typedef ros_queue_msgs:: POTENTIAL_ACTION_SET_MSG; typedef ros_queue_msgs:: POTENTIAL_ACTION_SET_SRV; typedef ros_queue_msgs::Action ACTION_LIB_OUTPUT_TYPE; - typedef ros_queue_msgs::Goal ACTION_LIB_OUTPUT_GOAL_TYPE; + typedef ros_queue_msgs::Goal CONTROLLER_COSTS_LIST_TYPE; ``` 4. In the directory `queue_controller/src/` copy and paste the `transmission_vector_queue_controller_node.cpp` file in the same directory. 5. Renamed it with a relevant name linked to the new added action. @@ -172,8 +184,12 @@ Since most of the configuration is required (except when it's indicated that it **min_drift-plus-penalty_template.yaml**: Its a template of all possible parameters of the queue_controller node. It's meant to be copied and adapted. It's generally included via a path in a launchfile. - `controller_type`(string): The type of controller that should be used. The two supported types are: `min_drift_plus_penalty` and `renewal_min_drift_plus_penalty`. -- `time_step` (float): Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller. +- `is_periodic`(bool): If set to true, the controller will indefinitely start a new control loop after the current loop based on the timing of the controller type. If set to false, the controller will only execute a control loop whenever it received a service call to it's service `start_control_loop` or a empty msg on its `start_control_loop` topic. +- `time_step` (float): Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller and if the parameter `is_periodic` is set to true. +- `include_execution_time_in_period` (bool, default: false): If set to true, the controller will include it's executation time in the period specified by `time_step`. It will give a fixed frequency of 1/`time_step`. If set to false, the controller will wait for the time specified by `time_step` after it's done which represent the time given to perform the action. The frequency of the controller will not be 1/`time_step` since the controller execution time will add delay before the next execution. - `inverse_control_and_steps` (bool, default: false): If set to false, the controller will 1)find an optimal action to take and then 2) update the virtual queues based on that optimal solution. If set to true, the controller will 1)update the virtual queues based on the current of state of the system and then 2)find an optimal next action. +- `responsible_for_virtual_queue_changes` (bool, default: true): 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. +- `measure_cost: false`(bool): 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 at each controller step. Default is set to false. - `min_renewal_time`(float): Lower bound time for the renewal process. The controller will not start again 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`. - `max_renewal_time`(float): Upper bound time for the renewal process. The controller will start again at that time if the action is not 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`. - `action_server_name`(string): Name of the actionlib server to which the best action will be sent as a goal. That server as the responsibility to send back a goal reached for the renewal system if it reaches the action. @@ -192,6 +208,11 @@ Since most of the configuration is required (except when it's indicated that it - `penalty_service_name`(string): Name of the service for the controller to get all the penalties for all the given action in the action set in its request. The service definition should match the service definition of the `METRIC_CONTROL_PREDICTION_SRV` type in the controller node's cpp file - `v_parameter`(float): It's the weight that multiplies the penalty in the cost function. It specifies the trade-off between optimizing the variable and the delays (linked to the expected size of the queues). A higher value will bring the penalty closer to its optimal value at the expanse of increasing the expected queue sizes. It represents mathematically the parameter $V$ from the section "Optimization formulation". - `is_penalty_renewal_dependent`(bool): 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. It's used when the value to minimize is $\frac{\bar{v}_o}{\bar{T}}$ instead of $\bar{y}_0$. Only used if the controller is of type `renewal_min_drift_plus_penalty`. +- `part_of_multicontroller_synchronization`(bool): Flag that indicates if this controller is part of a multi controller setup. It will send empty msgs on a topic `control_loop_started` at the begining of the control loop and a topic `optimization_done` after the optimization but before sending the best action and the manual changes to the queue server. +- `start_control_loop_sync_topic` (string): If defined, if the parameter `is_periodic` is set to false and `part_of_multicontroller_synchronization` is true, it will add a new subscribe with the specified name that will start a new control loop when it receive an empty msg. This is in addition to the `start_control_loop` topic. The main application for this is to put the `/control_loop_started` of a master queue controller so both controller starts at the same time in a multi-controller system that needs to be synchronized. +- `dependent_on_controller_topic` (string): Name of the topic that the controller will need to wait for before sending its command and changes .If defined and if `part_of_multicontroller_synchronization` is true, this controller will pause after its optimization and before to publish on `optimization_done`. A subscriber will be created that listens to the given topic name. When this subscriber is sent an empty msg, it will wake wake-up the controller allowing it to continue. If the message is received before the wait, the controller will simply continue. If this parameter is not defined or left empty, the controller will not wait to receive a message before continuing. This parameter is mainly used in a multi-controller setup where you specified the `/optimization` of a controller that needs to have completed its optimization before the present controller can continue. A cascade setup could be used like in the following figure where a main controller needs to wait for all other controller before sendind its control value. +![](.assets/queue_controller_synchronization_diagram_v2.png) + ## Launch files * **queue_controller.launch:** Starts a queue_controller node based on a given configuration and action type. - **`queue_controller_name`**: Name of the queue controller that will be use as the node's name and the prefixes of all it's exposed services. Default: `queue_controller`. @@ -227,17 +248,34 @@ For each queues: Service that evaluates how a queue will diminish for each given action in its request. The results should be returned in an array with a size equal to the number of actions and where the indexes of the departures match the indexes of their corresponding actions from the input's array. If the `departure_action_dependent` parameter is set to true in the config file for this queue, the request is empty and only one value is returned that will be used for all the actions during the optimization process. Usually the latter has no impact on the optimization process if it's not renewal dependent. * **`///getQueueInfo`** ([ros_queue_msgs/QueueInfoFetch](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/QueueInfoFetch.srv)) Service that gets the meta information of the queue from the [queue_server](https://github.com/etienn8/ros_queuing_system/tree/main/queue_server) . Mainly it gets if the queue is virtual. -#### Published Topics -* **`renewal_time`** (std_msgs/Float32) - Message that indicates the elapsed time between the start of the current controller call and the start of the previous controller call. -* **`//virtual_queue_manual_changes`** ([ros_queue_msgs/VirtualQueueChangesList](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/VirtualQueueChangesList.msg)) - Message containing an array of changes that need to be applied to the specified virtual queues. Those changes are based on the arrival and departure expected from the optimal action that was found. Only used if `inverse_control_and_steps` is set to false. #### Services -* **`get_last_renewal_time`** ([ros_queue_msgs/FloatRequest](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/FloatRequest.srv)) - Service that can be called to get the elapsed time between the start of the current controller call and the start of the previous one (last renewal time). For example, you can get the last renewal time from the console with: +* **`get_last_renewal_time`** ([ros_queue_msgs/GetQueueControllerTiming](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/GetQueueControllerTiming.srv)) + Service that can be called to get the elapsed time between the start of the current controller call and the start of the previous one (last renewal time) in seconds. It also gives the time it took to execute the control loop in seconds. For example, you can get the last renewal time from the console with: rosservice call //get_last_renewal_time +* **`start_control_loop`** (std_srvs/Empty) + Service that will start a control loop of the controller when called. It's only available if the parameter `is_periodic` is set to false. Note that for a renewable system, the controller will not wait for the renewal time of the action. For example, you can get start a control loop from the console with: + + rosservice call //start_control_loop +#### Published Topics +* **`control_loop_started`** (std_msgs/Empty) + Empty message that is published at the begining of the control loop or after the virtual queue change if the parameter `inverse_control_and_steps` is true. It's only published if the paramter `part_of_multicontroller_synchronization` is true. Mainly used for synchronizing muliple controllers together. +* **`optimization_done`** (std_msgs/Empty) + Empty message that is published after the optimization found the best command and before the sending command and virtual changes to the queue server. It's only published if the paramter `part_of_multicontroller_synchronization` is true. Mainly used for synchronizing muliple controllers together. +* **`measure_cost`** [ros_queue_msgs/TransmissionVectorControllerCostsList](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/TransmissionVectorControllerCostsList.msg) + Message that contains all the costs for each action and its metrics (penalty and queue). This message is sent at each end of the controller's loop. This topic isn't advertised if the `measure_cost` parameter is set to false. +* **`renewal_time`** ([ros_queue_msgs/QueueControllerTiming](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/QueueControllerTiming.msg)) + Message that indicates the elapsed time between the start of the current controller call and the start of the previous controller loop and the time it took to execute the previous control loop in seconds. +* **`//virtual_queue_manual_changes`** ([ros_queue_msgs/VirtualQueueChangesList](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/VirtualQueueChangesList.msg)) + Message containing an array of changes that need to be applied to the specified virtual queues. Those changes are based on the arrival and departure expected from the optimal action that was found. Only used if `inverse_control_and_steps` is set to false. +#### Subscribed Topics +* **`start_control_loop`** (std_msgs/Empty) + When called, it will start a control loop asynchronously. It's only available if the parameter `is_periodic` is set to false. +* **``** (std_msgs/Empty) + When called, it will start a control loop asynchronously. It's only available if the parameter `is_periodic` is set to false and the parameter `part_of_multicontroller_synchronization` is set to true. +* **``** (std_msgs/Empty) + Topic defined to indicate that the present controller should block its output until it receives an empty message on this topic. It's only used if `part_of_multicontroller_synchronization` is set to true. See the `Config files` section for more details. #### Parameters All the queue_server parameters are shown in the Config Files sections. diff --git a/queue_controller/config/min_drift-plus-penalty_template.yaml b/queue_controller/config/min_drift-plus-penalty_template.yaml index 2e515ad..73ccd23 100644 --- a/queue_controller/config/min_drift-plus-penalty_template.yaml +++ b/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 \ No newline at end of file +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: "" \ No newline at end of file diff --git a/queue_controller/include/queue_controller/action_configs/transmission_vector_controller_config.hpp b/queue_controller/include/queue_controller/action_configs/transmission_vector_controller_config.hpp index 00eb5a4..928f6a3 100644 --- a/queue_controller/include/queue_controller/action_configs/transmission_vector_controller_config.hpp +++ b/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; \ No newline at end of file +typedef ros_queue_msgs::TransmissionVectorGoal ACTION_LIB_OUTPUT_GOAL_TYPE; +typedef ros_queue_msgs::TransmissionVectorControllerCostsList CONTROLLER_COSTS_LIST_TYPE; \ No newline at end of file diff --git a/queue_controller/include/queue_controller/controller_queue_struct.hpp b/queue_controller/include/queue_controller/controller_queue_struct.hpp index a87615f..44d14ed 100644 --- a/queue_controller/include/queue_controller/controller_queue_struct.hpp +++ b/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 struct ControllerQueueStruct { /** @@ -58,14 +63,14 @@ 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 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 expected_departure_service_; /** * @brief ROS Service used to get a a metric thas is independant from the action. Should not be defined @@ -73,7 +78,7 @@ struct ControllerQueueStruct * @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 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 @@ -81,5 +86,5 @@ struct ControllerQueueStruct * @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 departure_independent_from_action_service_; }; \ No newline at end of file diff --git a/queue_controller/include/queue_controller/queue_controller.hpp b/queue_controller/include/queue_controller/queue_controller.hpp index d72f163..2ed51cc 100644 --- a/queue_controller/include/queue_controller/queue_controller.hpp +++ b/queue_controller/include/queue_controller/queue_controller.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include #include @@ -15,20 +16,28 @@ #include "controller_queue_struct.hpp" #include "queue_controller_utils.hpp" +#include "ros_queue_msgs/ControllerActionCosts.h" #include "ros_queue_msgs/FloatRequest.h" +#include "ros_queue_msgs/GetQueueControllerTiming.h" +#include "ros_queue_msgs/MetricCosts.h" +#include "ros_queue_msgs/QueueControllerTiming.h" #include "ros_queue_msgs/QueueInfoFetch.h" #include "ros_queue_msgs/QueueServerStateFetch.h" #include "ros_queue_msgs/VirtualQueueChangesList.h" +#include "ros_queue_msgs/TransmissionVectorControllerCostsList.h" #include #include "std_msgs/Float32.h" +#include "std_msgs/Empty.h" #include "std_srvs/Empty.h" #include "rosparam_utils/xmlrpc_utils.hpp" #include "rosparam_utils/parameter_package_fetch_struct.hpp" #include "rosparam_utils/data_validation.hpp" +#include "ros_boosted_utilities/persistent_service_client.hpp" + #include "queue_server/queue_server_utils.hpp" using std::string; @@ -39,11 +48,17 @@ struct ActionTrait { using ActionType = typename TActionSetType::_action_set_type::value_type; }; -template +template +struct ActionCostTrait { + using ActionCostType = typename TControllerCostListType::_action_costs_list_type::value_type; +}; + +template class QueueController { public: typedef typename ActionTrait::ActionType ActionType; + typedef typename ActionCostTrait::ActionCostType ActionCostType; enum ControllerType { @@ -51,12 +66,12 @@ class QueueController RenewalDriftPlusPenalty }; - QueueController(ros::NodeHandle& nh): nh_(nh), async_spinner_(1, &async_spinner_queue_), async_nh_("~") + QueueController(ros::NodeHandle& nh): nhp_(nh), nh_(ros::NodeHandle()), async_spinner_(1, &async_spinner_queue_), async_nhp_("~"), async_nh_(nh_) { bool can_create_controller = true; string controller_type_name; - if(nh.getParam("controller_type", controller_type_name)) + if(nhp_.getParam("controller_type", controller_type_name)) { if(controller_type_name == "min_drift_plus_penalty") { @@ -78,45 +93,65 @@ class QueueController ROS_ERROR_STREAM("Missing the controller type"); } + if (nhp_.getParam("is_periodic",is_periodic_)) + { + if(is_periodic_) + { + ROS_INFO_STREAM("The controller will run periodically."); + } + else + { + ROS_INFO_STREAM("The controller will be triggered by a service call."); + } + } + else + { + ROS_WARN_STREAM("Missing the is_periodic parameter. The controller will run periodically."); + } + if(controller_type_ == ControllerType::DriftPlusPenalty) { - if(!nh.getParam("time_step", controller_time_step_)) + if(is_periodic_ && !nhp_.getParam("time_step", controller_time_step_)) { can_create_controller = false; ROS_ERROR_STREAM("Missing the controller time step"); } } + // Configure the async spinner for the asynchronous fetch of the renewal time and synchronization callbacks + async_nhp_.setCallbackQueue(&async_spinner_queue_); + async_nh_.setCallbackQueue(&async_spinner_queue_); + if(controller_type_ == ControllerType::RenewalDriftPlusPenalty) { - if(!nh.getParam("max_renewal_time", max_renewal_time_)) + if(!nhp_.getParam("max_renewal_time", max_renewal_time_)) { can_create_controller = false; ROS_ERROR_STREAM("Missing the max_renewal_time"); } - if(!nh.getParam("min_renewal_time", min_renewal_time_)) + if(!nhp_.getParam("min_renewal_time", min_renewal_time_)) { can_create_controller = false; ROS_ERROR_STREAM("Missing the min_renewal_time"); } - if(!nh.getParam("is_penalty_renewal_dependent", is_penalty_renewal_dependent_)) + if(!nhp_.getParam("is_penalty_renewal_dependent", is_penalty_renewal_dependent_)) { ROS_WARN_STREAM("Missing the flag is_penalty_renewal_dependent that indicates if the penalty is dependent on the renewal time. Will be set to false."); } - renewal_time_pub_ = nh_.advertise("renewal_time", 10); + renewal_time_pub_ = nhp_.advertise("renewal_time", 10); string expected_renewal_time_service_name; - if(!nh.getParam("expected_renewal_time_service_name", expected_renewal_time_service_name) || expected_renewal_time_service_name.empty()) + if(!nhp_.getParam("expected_renewal_time_service_name", expected_renewal_time_service_name) || expected_renewal_time_service_name.empty()) { can_create_controller = false; ROS_ERROR_STREAM("Missing the expected_renewal_time_service_name."); } else { - renewal_service_client_ = nh_.serviceClient(expected_renewal_time_service_name, true); + renewal_service_client_ = PersistentServiceClient(expected_renewal_time_service_name); ROS_INFO_STREAM("Waiting for the expected renewal time service named :" << expected_renewal_time_service_name); renewal_service_client_.waitForExistence(); @@ -124,20 +159,23 @@ class QueueController // Start an asynchronous spinner to handle the last renewal time service in parallel to the controller. ROS_INFO_STREAM("Creating the last renewal time service server."); - async_nh_.setCallbackQueue(&async_spinner_queue_); - last_renewal_time_service_server_ = async_nh_.advertiseService("get_last_renewal_time", &QueueController::lastRenewalTimeServiceCallback, this); - async_spinner_.start(); + last_renewal_time_service_server_ = async_nhp_.advertiseService("get_last_renewal_time", &QueueController::lastRenewalTimeServiceCallback, this); } - if(!nh.getParam("inverse_control_and_steps", inversed_control_and_update_steps_)) + if(!nhp_.getParam("inverse_control_and_steps", inversed_control_and_update_steps_)) { ROS_WARN_STREAM("Missing the flag inverse_control_and_steps that indicates if the control and update steps are inverted. Will be set to false."); } + if(!nhp_.getParam("responsible_for_virtual_queue_changes", responsible_for_virtual_queue_changes_)) + { + ROS_WARN_STREAM("Missing the flag responsible_for_virtual_queue_changes that indicates if the controller should trigger the changes of the virtual queues of the queue server. Will be set to true."); + } + string solution_space_service_name; - if(nh.getParam("solution_space_service_name", solution_space_service_name)) + if(nhp_.getParam("solution_space_service_name", solution_space_service_name)) { - solution_space_client_ = nh_.serviceClient(solution_space_service_name, true); + solution_space_client_ = PersistentServiceClient(solution_space_service_name); ROS_INFO_STREAM("Waiting for the solution space client named :" << solution_space_service_name); solution_space_client_.waitForExistence(); @@ -149,9 +187,9 @@ class QueueController } string penalty_service_name; - if(nh.getParam("penalty_service_name", penalty_service_name)) + if(nhp_.getParam("penalty_service_name", penalty_service_name)) { - penalty_service_client_ = nh_.serviceClient(penalty_service_name, true); + penalty_service_client_ = PersistentServiceClient(penalty_service_name); ROS_INFO_STREAM("Waiting for the penalty space client named :" << penalty_service_name); penalty_service_client_.waitForExistence(); @@ -162,7 +200,7 @@ class QueueController ROS_ERROR_STREAM("Missing the penalty_service_name."); } - if(nh.getParam("v_parameter", v_parameter_)) + if(nhp_.getParam("v_parameter", v_parameter_)) { if(v_parameter_< 0.0f) { @@ -175,7 +213,19 @@ class QueueController ROS_ERROR_STREAM("Missing the v_parameter."); } - if(!nh.getParam("queue_server_name", queue_server_name_)) + if(nhp_.getParam("measure_cost", measure_cost_)) + { + if(measure_cost_) + { + cost_publisher_ = nhp_.advertise("controller_costs", 10); + } + } + else + { + ROS_WARN_STREAM("Missing the measure_cost parameter. Will be set to false."); + } + + if(!nhp_.getParam("queue_server_name", queue_server_name_)) { can_create_controller = false; ROS_ERROR_STREAM("Missing the queue_server_name."); @@ -191,7 +241,7 @@ class QueueController const xmlrpc_utils::ParameterPackageFetchStruct fetch_struct(options); vector parsed_queue_configs= - xmlrpc_utils::fetchMatchingParametersFromList(nh_, ros::this_node::getName(), + xmlrpc_utils::fetchMatchingParametersFromList(nhp_, ros::this_node::getName(), "queue_list", fetch_struct); // Does the queue server exist and transfer queue_configs in internal queue structs @@ -201,42 +251,117 @@ class QueueController } string action_server_name; - if(!nh.getParam("action_server_name", action_server_name)) + if(!nhp_.getParam("action_server_name", action_server_name)) { can_create_controller = false; ROS_ERROR_STREAM("Missing the action_server_name."); } + string dependent_on_controller_topic; + string start_control_loop_sync_topic; + if(nhp_.getParam("part_of_multicontroller_synchronization", part_of_multicontroller_synchronization_)) + { + if (part_of_multicontroller_synchronization_) + { + if(nhp_.getParam("dependent_on_controller_topic",dependent_on_controller_topic)) + { + if (dependent_on_controller_topic.empty()) + { + ROS_INFO_STREAM("Queue controller is not dependant on any topic to send its command. Since its dependent_on_controller_topic parameter is empty."); + } + else + { + ROS_INFO_STREAM("Queue controller is dependant on topic "<< dependent_on_controller_topic << " to send its command."); + is_dependent_on_a_another_controller_ = true; + } + } + else + { + ROS_INFO_STREAM("Queue controller is not dependant on any topic to send its command. Since the dependent_on_controller_topic parameter is not set."); + } + if(nhp_.getParam("start_control_loop_sync_topic", start_control_loop_sync_topic)) + { + if (start_control_loop_sync_topic.empty()) + { + ROS_INFO_STREAM("Queue controller doesn't have a user-defined topic to start its control loop."); + } + else + { + ROS_INFO_STREAM("Queue controller uses"<< start_control_loop_sync_topic << " to start its control loop."); + } + } + else + { + ROS_INFO_STREAM("Queue controller doesn't have a user-defined topic to start its control loop. Since the start_control_loop_sync_topic parameter is not set."); + } + } + } + else + { + ROS_WARN("The flag part_of_multicontroller_synchronization is not set. It will be set to false."); + } + // Intialize controller if (can_create_controller) { // Connect to queue_server for queue sizes - server_state_client_ = nh_.serviceClient("/" + queue_server_name_ + "/get_server_state", true); + server_state_client_ = PersistentServiceClient(queue_server_name_ + "/get_server_state"); ROS_INFO_STREAM("Waiting for the server service named :" << server_state_client_.getService()); server_state_client_.waitForExistence(); - // Connect to queue_server for queue server udpates (depends on steps order) - if (!inversed_control_and_update_steps_) - { - manual_virtual_queue_changes_ = nh_.advertise("/" + queue_server_name_ + "/virtual_queue_manual_changes", 1000); - } - else + if(responsible_for_virtual_queue_changes_) { - virtual_queues_trigger_ = nh_.serviceClient("/" + queue_server_name_ + "/trigger_service", true); - ROS_INFO_STREAM("Waiting for the server service named :" << virtual_queues_trigger_.getService()); - virtual_queues_trigger_.waitForExistence(); + // Connect to queue_server for queue server udpates (depends on steps order) + if (!inversed_control_and_update_steps_) + { + manual_virtual_queue_changes_ = nh_.advertise(queue_server_name_ + "/virtual_queue_manual_changes", 1000); + } + else + { + virtual_queues_trigger_ = PersistentServiceClient(queue_server_name_ + "/trigger_service"); + ROS_INFO_STREAM("Waiting for the server service named :" << virtual_queues_trigger_.getService()); + virtual_queues_trigger_.waitForExistence(); + } } // Create ouptut topic best_action_client_ = std::make_unique>(action_server_name, true); + if(!is_periodic_) + { + start_control_loop_service_server_ = nhp_.advertiseService("start_control_loop", &QueueController::startControlLoopServiceCallback, this); + start_control_loop_sub_ = nhp_.subscribe("start_control_loop", 1, &QueueController::startControlLoopCallback, this); + } - if(controller_type_ == ControllerType::RenewalDriftPlusPenalty) + if(part_of_multicontroller_synchronization_) + { + if(is_dependent_on_a_another_controller_) + { + dependent_controller_finished_sub_ = async_nh_.subscribe(dependent_on_controller_topic, 1, &QueueController::dependentControllerFinishedCallback, this); + + while(dependent_controller_finished_sub_.getNumPublishers() == 0) + { + ROS_WARN_STREAM_THROTTLE(2, "Waiting for the dependent controller to be online. On topic "<< dependent_controller_finished_sub_.getTopic()); + ros::Duration(0.1).sleep(); + } + } + optimization_done_pub_ = nhp_.advertise("optimization_done", 10); + control_loop_started_pub_ = nhp_.advertise("control_loop_started", 1, true); + + if(!is_periodic_ && !start_control_loop_sync_topic.empty()) + { + start_control_loop_sync_sub_ = nh_.subscribe(start_control_loop_sync_topic, 1, &QueueController::startControlLoopCallback, this); + } + } + + if(controller_type_ == ControllerType::RenewalDriftPlusPenalty || + (part_of_multicontroller_synchronization_ && is_dependent_on_a_another_controller_)) { async_spinner_.start(); - } + } + ROS_INFO("Queue Controller started"); is_initialized_ = true; } } @@ -273,79 +398,106 @@ class QueueController return; } - if (controller_type_ == ControllerType::DriftPlusPenalty) + if(is_periodic_) { - static ros::Rate loop_rate(1.0f/controller_time_step_); - while(ros::ok()) + if (controller_type_ == ControllerType::DriftPlusPenalty) { - controllerCallback(); - ros::spinOnce(); - loop_rate.sleep(); + ros::Rate loop_rate(1.0f/controller_time_step_); + while(ros::ok()) + { + controllerCallback(); + ros::spinOnce(); + loop_rate.sleep(); + } } - } - else if (controller_type_ == ControllerType::RenewalDriftPlusPenalty) - { - while(ros::ok()) + else if (controller_type_ == ControllerType::RenewalDriftPlusPenalty) { - if(!best_action_client_waited_) + while(ros::ok()) { - if(!best_action_client_->waitForServer(ros::Duration(5.0))) + if(!best_action_client_waited_) { - ROS_WARN_STREAM("The action server named wasn't found. Queue controller will be running with a perdiod of max_renewal_time ("<< max_renewal_time_ << "s) between each control step and will continue to try to connect to server."); + if(!best_action_client_->waitForServer(ros::Duration(5.0))) + { + ROS_WARN_STREAM("The action server named wasn't found. Queue controller will be running with a perdiod of max_renewal_time ("<< max_renewal_time_ << "s) between each control step and will continue to try to connect to server."); + } + + best_action_client_waited_ = true; } - best_action_client_waited_ = true; - } + // Start to measure how much time the controlle takes time to exectue + ros::Time controller_start_time_point = ros::Time::now(); - std_msgs::Float32 renewal_time_msg; - // Access protection to the last_renewal_time_ variable - { - std::lock_guard lock(last_renewal_time_mutex_); - static bool first_renewal = true; - if(first_renewal) + ros_queue_msgs::QueueControllerTiming renewal_time_msg; + // Access protection to the last_renewal_time_ variable { - first_renewal = false; - last_renewal_time_point = ros::Time::now(); - } + std::lock_guard lock(last_renewal_time_mutex_); + if(is_first_renewal_loop_) + { + is_first_renewal_loop_ = false; + last_renewal_time_point = ros::Time::now(); + last_controller_execution_time_ = 0; + } - // Compute the real elapsed time since the last renewal. - last_renewal_time_ = (ros::Time::now() - last_renewal_time_point).toSec(); + // Compute the real elapsed time since the last renewal. + last_renewal_time_ = (ros::Time::now() - last_renewal_time_point).toSec(); + + renewal_time_msg.renewal_time = last_renewal_time_; + renewal_time_msg.execution_time = last_controller_execution_time_; + ROS_DEBUG_STREAM("Time since last renewal: " << last_renewal_time_); + } + renewal_time_pub_.publish(renewal_time_msg); - renewal_time_msg.data = last_renewal_time_; - ROS_DEBUG_STREAM("Time since last renewal: " << last_renewal_time_); - } - renewal_time_pub_.publish(renewal_time_msg); - - // Compute the controller - controllerCallback(); - - last_renewal_time_point = ros::Time::now(); - - if(best_action_client_->isServerConnected()) - { - // Wait for the best action to be reached or wait for the max_renewal_time - bool finished_before_max_time = best_action_client_->waitForResult(ros::Duration(max_renewal_time_)); + // Compute the controller + controllerCallback(); - // We abandon the last goal if its still on going. - if (!finished_before_max_time) { - best_action_client_->cancelGoal(); + // Measure how much time the controlle took to execute. + std::lock_guard lock(last_renewal_time_mutex_); + last_controller_execution_time_ = (ros::Time::now() - controller_start_time_point).toSec(); } - double elapsed_time = (ros::Time::now() - last_renewal_time_point).toSec(); + last_renewal_time_point = ros::Time::now(); - // Wait for t_min is reached if the goal was reached before. - if (elapsed_time < min_renewal_time_) + if(best_action_client_->isServerConnected()) { - ros::Duration(min_renewal_time_ - elapsed_time).sleep(); + // Wait for the best action to be reached or wait for the max_renewal_time + bool finished_before_max_time = best_action_client_->waitForResult(ros::Duration(max_renewal_time_)); + + // We abandon the last goal if its still on going. + if (!finished_before_max_time) + { + best_action_client_->cancelGoal(); + } + + double elapsed_time = (ros::Time::now() - last_renewal_time_point).toSec(); + + // Wait for t_min is reached if the goal was reached before. + if (elapsed_time < min_renewal_time_) + { + ros::Duration(min_renewal_time_ - elapsed_time).sleep(); + } + } + else + { + ros::Duration(max_renewal_time_).sleep(); } - } - else - { - ros::Duration(max_renewal_time_).sleep(); } } } + else + { + // If non periodic, a service is used to start a control loop. + ros::spin(); + } + } + + bool startControlLoopServiceCallback(std_srvs::Empty::Request& req, + std_srvs::Empty::Response& res) + { + ROS_DEBUG_STREAM("Starting control loop from service call for the controller: " << ros::this_node::getName()); + + controllerCallback(); + return true; } private: @@ -354,19 +506,183 @@ class QueueController */ bool is_initialized_ = false; + /** + * @brief Node handle with the namespace prefix of the namespace of the node. Mainly used for the declaration of services + * for user-defined services. + */ ros::NodeHandle nh_; + + /** + * @brief Private node handle with namespace prefixed with the namespace of the node and it's name. Mainly used for declaration + * of topics and services provided for the node. + */ + ros::NodeHandle nhp_; /** - * @brief Nodehandle that uses the async_spinner_queue_ as its callback queue. It's use as an interface + * @brief Pivate Nodehandle that uses the async_spinner_queue_ as its callback queue. It's use as an interface + * to create services from that should be manage in parallel with the main thread. + */ + ros::NodeHandle async_nhp_; + + /** + * @brief Pivate Nodehandle that uses the async_spinner_queue_ as its callback queue. It's use as an interface * to create services from that should be manage in parallel with the main thread. */ ros::NodeHandle async_nh_; + // ===== Synchronization parameters ====== + + /** + * @brief Flag that indicates if the controller should run periodically or should be triggered by a service call. + */ + bool is_periodic_ = true; + + /** */ + /** + * @brief ROS Service Server that launches a control loop if the flag is_periodic_ is set to false; + */ + ros::ServiceServer start_control_loop_service_server_; + + /** + * @brief Flag that indicates if the queue controller is part of a multi-controller system + * that are synchronized between them. + */ + bool part_of_multicontroller_synchronization_ = false; + + /** + * @brief Flag that indicates if the controller should wait for another controller before continuing. + */ + bool is_dependent_on_a_another_controller_ = false; + + /** + * @brief ROS publisher that indicates the begining of the control loop. Mainly used by the director controller + * to send a control signal to all dependent controllers to start their control loops. + */ + ros::Publisher control_loop_started_pub_; + + /** + * @brief ROS Subscriber that when called, the controller will start a control loop. Similar to start_control_loop_service_server_. + * It will be used only if is_periodic_ is set to false. + */ + ros::Subscriber start_control_loop_sub_; + + /** + * @brief ROS Subscriber that when called, the controller will start a control loop. Its a duplication of the start_control_loop_sub_ + * bu explicitely used for the synchronization and can be defined by the user. + * It will be used only if is_periodic_ is set to false. + */ + ros::Subscriber start_control_loop_sync_sub_; + + /** + * @brief Callback for the start_control_loop_sub_ that will start a control loop based on the reception of a empty topic. + * @param msg Empty message that indicates the start of the control loop. + */ + void startControlLoopCallback(const std_msgs::Empty::ConstPtr& msg) + { + if (is_dependent_on_a_another_controller_) + { + std::lock_guard lock(is_waiting_for_other_controller_mutex_access); + is_waiting_for_other_controller_ = true; + } + + ROS_DEBUG_STREAM("Starting control loop from topic for the controller: " << ros::this_node::getName()); + controllerCallback(); + } + + /** + * @brief Flag that indicates if the responder controller is waiting for another controller to finish before sending its action. + */ + bool is_waiting_for_other_controller_ = false; + + /** + * @brief ROS Subscriber that listens for dependent queue to finish and signal this controller when it happens. + */ + ros::Subscriber dependent_controller_finished_sub_; + + /** + * @brief ROS Publisher that sends a empty message to signal that this controller has finished and it's + * dependent controller has signal it to continue. + */ + ros::Publisher optimization_done_pub_; + + /** + * @brief Mutex that protects access to the is_waiting_for_other_controller_ variable. + */ + std::mutex is_waiting_for_other_controller_mutex_access; + + /** + * @brief Mutex used to signal and notify when a dependent controller finished. + */ + std::mutex controller_synchronization_mutex_; + + /** + * @brief Condition variable that verifies if a dependent controller finished its optimization. + */ + std::condition_variable controller_synchronization_cv_; + + /** + * @brief Starting method of the synchronization mechanimim called at the begening of the controller to + * ppublish and emptry start message and set the is_waiting_for_other_controller_ flag to true if + * the is_dependent_on_a_another_controller_ flag is true. + */ + void startSynchronization() + { + std_msgs::Empty empty_msg; + control_loop_started_pub_.publish(empty_msg); + + if(is_dependent_on_a_another_controller_) + { + std::lock_guard lock(is_waiting_for_other_controller_mutex_access); + is_waiting_for_other_controller_ = true; + } + } + + /** + * @brief Wait for the dependent controller to finish its optimization by waiting + * for the is_waiting_for_other_controller_ flag to be set to false by the callback of the dependent_controller_finished_sub_. + */ + void waitForSynchronization() + { + if(is_waiting_for_other_controller_) + { + ROS_DEBUG_STREAM("Waiting for dependent controller to finish its optimization. On topic "<< dependent_controller_finished_sub_.getTopic()); + std::unique_lock lock(controller_synchronization_mutex_); + while(is_waiting_for_other_controller_ && ros::ok()) + { + // This perdiodic wait_for() is used instead of wait() to allow the thread to detect if + // ROS has received a signal to shutdown. Otherwise, the thread would be blocked until a SIGTERM is called. + controller_synchronization_cv_.wait_for(lock, std::chrono::seconds(1), [this]{return (!this->is_waiting_for_other_controller_ || !ros::ok());}); + } + ROS_DEBUG_STREAM("Finished waiting for dependent controller"); + } + } + + /** + * @brief Callback for the dependent_controller_finished_sub_ that will signal the controller that the controller + * it's depending from finish its optimation. Thus allowing this controller to continue to send its best action. + * @param msg Empty message that indicates the dependent controller finished its optimization. + */ + void dependentControllerFinishedCallback(const std_msgs::Empty::ConstPtr& msg) + { + { + std::lock_guard lock(controller_synchronization_mutex_); + is_waiting_for_other_controller_ = false; + } + controller_synchronization_cv_.notify_one(); + } + + // ===== Renewwal parameters ====== + /** * @brief Time at which the last renewal was done. Used by the renewal_min_drift_plus_penalty controller. */ ros::Time last_renewal_time_point; + /** + * @brief Duration of the last execution of the controller + */ + float last_controller_execution_time_; + /** * @brief Last elapsed time between the last renewal and the current time in seconds. Used by the renewal_min_drift_plus_penalty controller * and only used with virtual updates that are based on the current state of the system (inversed_control_and_update_steps_ is set to true). @@ -376,17 +692,22 @@ class QueueController /** * @brief ROS service client used to computed the expected time ot execute each given action. */ - ros::ServiceClient renewal_service_client_; + PersistentServiceClient renewal_service_client_; /** * @brief ROS topic publisher that periodically sends the last renewal time. */ - ros::Publisher renewal_time_pub_; + ros::Publisher renewal_time_pub_; + + /** + * @brief Flag used to know if the first control loop of the renewal controller has been done. + */ + bool is_first_renewal_loop_ = true; /** * @brief ROS service client used to get the current states of the queues in the queue server. */ - ros::ServiceClient server_state_client_; + PersistentServiceClient server_state_client_; /** * @brief ROS topic publisher use to send manual changes to the virtual queues of a queue server. @@ -399,7 +720,7 @@ class QueueController * @brief ROS Service client that whenever it's called, the virtual queues in the queue server * will update based on the current system's state. */ - ros::ServiceClient virtual_queues_trigger_; + PersistentServiceClient virtual_queues_trigger_; /** * @brief Action lib client that sends the optimal action to a server. @@ -411,12 +732,22 @@ class QueueController */ bool best_action_client_waited_ = false; + /** + * @brief Publisher that publishes the cost of each action and all its internal metrics. + */ + ros::Publisher cost_publisher_; + + /** + * @brief Flag that indicates if we should measure and publish the cost of the controller steps. + */ + bool measure_cost_ = false; + /** * @brief Map of all the queues used by the queue_controller from a queue_server. The key * is the queue's name and the value is all the usefull parameters of the queue to evaluate and * udpate the queues. */ - map> queue_list_; + map>> queue_list_; /** * @brief Populate the internal queue_list_ based on the parsed_queue_configs. It will also verify @@ -430,7 +761,7 @@ class QueueController // Create structure to parse the queue params from the queue server ROS_INFO_STREAM("Waiting for queue serve named " << queue_server_name_ << " to be online. Waiting at most 10 seconds."); - if (ros::service::waitForService("/"+queue_server_name_+"/get_server_state", ros::Duration(10))) + if (ros::service::waitForService(queue_server_name_+"/get_server_state", ros::Duration(10))) { ROS_INFO_STREAM("Queue server found. Resuming queue controller initialization."); @@ -442,7 +773,7 @@ class QueueController if(ros::service::call(queue_server_name_+"/"+queue_name_temp+"/getQueueInfo", queue_info_srv)) { - auto new_controller_struct = std::make_unique(); + auto new_controller_struct = std::make_unique>(); new_controller_struct->queue_name_ = queue_name_temp; new_controller_struct->is_virtual_ = queue_info_srv.response.info.is_virtual; @@ -484,7 +815,7 @@ class QueueController { new_controller_struct->is_arrival_action_dependent = true; new_controller_struct->expected_arrival_service_ = - nh_.serviceClient(service_name_temp, true); + PersistentServiceClient(service_name_temp); ROS_INFO_STREAM("Waiting for the server service named :" << new_controller_struct->expected_arrival_service_.getService()); new_controller_struct->expected_arrival_service_.waitForExistence(); } @@ -497,7 +828,7 @@ class QueueController } new_controller_struct->is_arrival_action_dependent = false; new_controller_struct->arrival_independent_from_action_service_ = - nh_.serviceClient(service_name_temp, true); + PersistentServiceClient(service_name_temp); ROS_INFO_STREAM("Waiting for the server service named :" << new_controller_struct->arrival_independent_from_action_service_.getService()); new_controller_struct->arrival_independent_from_action_service_.waitForExistence(); } @@ -532,7 +863,7 @@ class QueueController { new_controller_struct->is_departure_action_dependent= true; new_controller_struct->expected_departure_service_ = - nh_.serviceClient(service_name_temp, true); + PersistentServiceClient(service_name_temp); ROS_INFO_STREAM("Waiting for the server service named :" << new_controller_struct->expected_departure_service_.getService()); new_controller_struct->expected_departure_service_.waitForExistence(); } @@ -545,7 +876,7 @@ class QueueController } new_controller_struct->is_departure_action_dependent= false; new_controller_struct->departure_independent_from_action_service_ = - nh_.serviceClient(service_name_temp, true); + PersistentServiceClient(service_name_temp); ROS_INFO_STREAM("Waiting for the server service named :" << new_controller_struct->expected_departure_service_.getService()); new_controller_struct->expected_departure_service_.waitForExistence(); @@ -601,11 +932,12 @@ class QueueController /** * @brief Service callback that returns the last renewal time. Used with renewal system. */ - bool lastRenewalTimeServiceCallback(ros_queue_msgs::FloatRequest::Request& req, - ros_queue_msgs::FloatRequest::Response& res) + bool lastRenewalTimeServiceCallback(ros_queue_msgs::GetQueueControllerTiming::Request& req, + ros_queue_msgs::GetQueueControllerTiming::Response& res) { std::lock_guard lock(last_renewal_time_mutex_); - res.value = last_renewal_time_; + res.timing.renewal_time = last_renewal_time_; + res.timing.execution_time = last_controller_execution_time_; return true; } @@ -633,12 +965,17 @@ class QueueController ROS_DEBUG_STREAM("Queue controller: Starting control loop of "<< ros::this_node::getName()); - if (inversed_control_and_update_steps_) + if (inversed_control_and_update_steps_ && responsible_for_virtual_queue_changes_) { updateVirtualQueuesBasedOnCurrentState(); } time_cursor = queue_controller_utils::updateTimePointAndGetTimeDifferenceMS(time_cursor, virtual_queues_current_state_update_time_spent); + if(part_of_multicontroller_synchronization_) + { + startSynchronization(); + } + TPotentialActionSetMsg action_set = getActionSet(); time_cursor = queue_controller_utils::updateTimePointAndGetTimeDifferenceMS(time_cursor, action_set_time_spent); @@ -662,10 +999,19 @@ class QueueController time_cursor = queue_controller_utils::updateTimePointAndGetTimeDifferenceMS(time_cursor, compute_optimization_time_spent); + if(part_of_multicontroller_synchronization_) + { + if(is_dependent_on_a_another_controller_) + { + waitForSynchronization(); + } + optimization_done_pub_.publish(std_msgs::Empty()); + } + sendBestCommand(best_action_parameters.action); time_cursor = queue_controller_utils::updateTimePointAndGetTimeDifferenceMS(time_cursor, send_best_action_time_spent); - if (!inversed_control_and_update_steps_) + if (!inversed_control_and_update_steps_ && responsible_for_virtual_queue_changes_) { updateVirtualQueuesBasedOnBestAction(best_action_parameters); } @@ -723,8 +1069,6 @@ class QueueController TPotentialActionSetMsg getActionSet() { ROS_DEBUG("Calling action set service"); - - queue_controller_utils::check_persistent_service_connection(nh_, solution_space_client_); TPotentialActionSetSrv potential_set_srv; if(!solution_space_client_.call(potential_set_srv)) @@ -754,7 +1098,7 @@ class QueueController template struct AsyncMetricFutureStruct { - std::shared_ptr client_; + std::shared_ptr> client_; TSrvType current_srv_msg_; bool current_call_succeeded_; @@ -773,13 +1117,13 @@ class QueueController * if the parameter_type is arrival or departure. */ void addServiceCallToActionFutureArray(std::vector>>>& future_array, - ros::ServiceClient& client, + PersistentServiceClient& client, const TPotentialActionSetMsg& action_set_msg, ParameterType parameter_type, string queue_name = "") { auto temp_metric_control_future_struct = std::make_shared>(); - temp_metric_control_future_struct->client_ = std::make_shared(client); + temp_metric_control_future_struct->client_ = std::make_shared>(client); temp_metric_control_future_struct->current_srv_msg_.request.action_set = action_set_msg; temp_metric_control_future_struct->parameter_type_ = parameter_type; @@ -811,13 +1155,13 @@ class QueueController * if the parameter_type is arrival or departure. */ void addServiceCallToFloatFutureArray(std::vector>>>& future_array, - ros::ServiceClient& client, + PersistentServiceClient& client, const TPotentialActionSetMsg& action_set_msg, ParameterType parameter_type, string queue_name = "") { auto temp_metric_control_future_struct = std::make_shared>(); - temp_metric_control_future_struct->client_ = std::make_shared(client); + temp_metric_control_future_struct->client_ = std::make_shared>(client); temp_metric_control_future_struct->parameter_type_ = parameter_type; if (parameter_type == ParameterType::Arrival || parameter_type == ParameterType::Departure) @@ -869,13 +1213,11 @@ class QueueController std::vector>>> float_request_future_array; // Penalty service - queue_controller_utils::check_persistent_service_connection(nh_, penalty_service_client_); addServiceCallToActionFutureArray(metric_control_future_array, penalty_service_client_, action_set_msg, ParameterType::Penalty); if (controller_type_ == ControllerType::RenewalDriftPlusPenalty) { // Expected time - queue_controller_utils::check_persistent_service_connection(nh_, renewal_service_client_); addServiceCallToActionFutureArray(metric_control_future_array, renewal_service_client_, action_set_msg, ParameterType::RenewalTime); } @@ -887,24 +1229,20 @@ class QueueController // Arrivals if(queue_it->second->is_arrival_action_dependent) { - queue_controller_utils::check_persistent_service_connection(nh_, queue_it->second->expected_arrival_service_); addServiceCallToActionFutureArray(metric_control_future_array, queue_it->second->expected_arrival_service_, action_set_msg, ParameterType::Arrival, queue_name); } else { - queue_controller_utils::check_persistent_service_connection(nh_, queue_it->second->arrival_independent_from_action_service_); addServiceCallToFloatFutureArray(float_request_future_array, queue_it->second->arrival_independent_from_action_service_, action_set_msg, ParameterType::Arrival, queue_name); } // Departures if(queue_it->second->is_departure_action_dependent) { - queue_controller_utils::check_persistent_service_connection(nh_, queue_it->second->expected_departure_service_); addServiceCallToActionFutureArray(metric_control_future_array, queue_it->second->expected_departure_service_, action_set_msg, ParameterType::Departure, queue_name); } else { - queue_controller_utils::check_persistent_service_connection(nh_, queue_it->second->departure_independent_from_action_service_); addServiceCallToFloatFutureArray(float_request_future_array, queue_it->second->departure_independent_from_action_service_, action_set_msg, ParameterType::Departure, queue_name); } } @@ -1094,17 +1432,48 @@ class QueueController auto best_action = action_parameters_list.begin(); bool is_first_cost = true; + + TControllerCostListType action_costs_lists; for (auto action_parameters_it = action_parameters_list.begin(); action_parameters_it != action_parameters_list.end(); ++action_parameters_it) { - action_parameters_it->cost = v_parameter_*action_parameters_it->penalty; + ActionCostType action_named_action_costs; + ros_queue_msgs::ControllerActionCosts action_costs_msg; + + const float penalty_cost = v_parameter_*action_parameters_it->penalty; + action_parameters_it->cost = penalty_cost; + + if(measure_cost_) + { + ros_queue_msgs::MetricCosts penalty_cost_msg; + penalty_cost_msg.metric_name = "penalty"; + penalty_cost_msg.cost = penalty_cost; + action_costs_msg.metric_costs.push_back(std::move(penalty_cost_msg)); + } for(auto queue_it = action_parameters_it->queue_parameters.begin(); queue_it != action_parameters_it->queue_parameters.end(); ++queue_it) { const string& queue_name = queue_it->first; - action_parameters_it->cost += queue_list_[queue_name]->weight_ * - queue_it->second.current_size * - (queue_it->second.expected_arrivals - queue_it->second.expected_departures); + const float queue_cost = queue_list_[queue_name]->weight_ * + queue_it->second.current_size * + (queue_it->second.expected_arrivals - queue_it->second.expected_departures); + action_parameters_it->cost += queue_cost; + + if(measure_cost_) + { + ros_queue_msgs::MetricCosts queue_cost_msg; + queue_cost_msg.metric_name = queue_name; + queue_cost_msg.cost = queue_cost; + action_costs_msg.metric_costs.push_back(std::move(queue_cost_msg)); + } + } + + if(measure_cost_) + { + action_costs_msg.total_cost = action_parameters_it->cost; + action_named_action_costs.action = action_parameters_it->action; + action_named_action_costs.costs = action_costs_msg; + action_costs_lists.action_costs_list.push_back(std::move(action_named_action_costs)); } if (is_first_cost) @@ -1116,6 +1485,15 @@ class QueueController { best_action = action_parameters_it; } + else if(action_parameters_it->cost == best_action->cost) + { + ROS_WARN("Queue controller: Two actions have the same cost. The first one will be selected."); + } + } + + if(measure_cost_) + { + cost_publisher_.publish(action_costs_lists); } return *best_action; @@ -1132,11 +1510,26 @@ class QueueController bool is_first_cost = true; + TControllerCostListType action_costs_lists; + for (auto action_parameters_it = action_parameters_list.begin(); action_parameters_it != action_parameters_list.end(); ++action_parameters_it) { + ActionCostType action_named_action_costs; + ros_queue_msgs::ControllerActionCosts action_costs_msg; + const double& expected_renewal_time = action_parameters_it->expected_renewal_time; - action_parameters_it->cost = v_parameter_*action_parameters_it->penalty; + float penalty_cost = v_parameter_*action_parameters_it->penalty; + action_parameters_it->cost = penalty_cost; + + if(measure_cost_) + { + ros_queue_msgs::MetricCosts penalty_cost_msg; + penalty_cost_msg.metric_name = "penalty"; + penalty_cost_msg.cost = penalty_cost; + action_costs_msg.metric_costs.push_back(std::move(penalty_cost_msg)); + } + for(auto queue_it = action_parameters_it->queue_parameters.begin(); queue_it != action_parameters_it->queue_parameters.end(); ++queue_it) { const string& queue_name = queue_it->first; @@ -1152,9 +1545,19 @@ class QueueController queue_it->second.expected_departures = expected_renewal_time*queue_it->second.expected_departures; } - action_parameters_it->cost += queue_list_[queue_name]->weight_ * - queue_it->second.current_size * - (queue_it->second.expected_arrivals - queue_it->second.expected_departures); + float queue_cost = queue_list_[queue_name]->weight_ * + queue_it->second.current_size * + (queue_it->second.expected_arrivals - queue_it->second.expected_departures); + + action_parameters_it->cost += queue_cost; + + if(measure_cost_) + { + ros_queue_msgs::MetricCosts queue_cost_msg; + queue_cost_msg.metric_name = queue_name; + queue_cost_msg.cost = queue_cost; + action_costs_msg.metric_costs.push_back(std::move(queue_cost_msg)); + } } // TODO: Verify formulation @@ -1163,6 +1566,15 @@ class QueueController action_parameters_it->cost = action_parameters_it->cost/expected_renewal_time; } + if(measure_cost_) + { + // Note here that the total cost is the cost per renewal time and not the sum of the costs. + action_costs_msg.total_cost = action_parameters_it->cost; + action_named_action_costs.action = action_parameters_it->action; + action_named_action_costs.costs = action_costs_msg; + action_costs_lists.action_costs_list.push_back(std::move(action_named_action_costs)); + } + if (is_first_cost) { is_first_cost = false; @@ -1174,6 +1586,11 @@ class QueueController } } + if(measure_cost_) + { + cost_publisher_.publish(action_costs_lists); + } + return *best_action; } @@ -1198,8 +1615,6 @@ class QueueController */ void updateVirtualQueuesBasedOnCurrentState() { - queue_controller_utils::check_persistent_service_connection(nh_, virtual_queues_trigger_); - std_srvs::Empty trigger; if(!virtual_queues_trigger_.call(trigger)) { @@ -1243,6 +1658,16 @@ class QueueController */ bool inversed_control_and_update_steps_ = false; + + /** + * @brief 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. + */ + bool responsible_for_virtual_queue_changes_ = true; + /** * @brief Indicate the controller types. It defines the type of optimization and the used metrics. */ @@ -1270,13 +1695,13 @@ class QueueController * @brief ROS service client that returns a set of possible actions that the controller will used * to compute its objetives metrics. The best solution will be a solution in this set. */ - ros::ServiceClient solution_space_client_; + PersistentServiceClient solution_space_client_; /** * @brief Service client that evaluates the optimization variable (penalty) given the state of * the system and an evaluating solution; */ - ros::ServiceClient penalty_service_client_; + PersistentServiceClient penalty_service_client_; /** * @brief The V parameter that is multplied with the penalty (like a weight). diff --git a/queue_controller/include/queue_controller/queue_controller_utils.hpp b/queue_controller/include/queue_controller/queue_controller_utils.hpp index b37a814..810a8a7 100644 --- a/queue_controller/include/queue_controller/queue_controller_utils.hpp +++ b/queue_controller/include/queue_controller/queue_controller_utils.hpp @@ -9,30 +9,18 @@ using std::string; namespace queue_controller_utils { - template - void check_persistent_service_connection(ros::NodeHandle nh, ros::ServiceClient& client) - { - if (!client.isValid()) - { - const string service_name = client.getService(); - - ROS_WARN_STREAM("Lost connection to service :" << service_name<<". Trying to reconnect and waiting until available."); - client = nh.serviceClient(service_name, true); - client.waitForExistence(); - ROS_WARN_STREAM("Restored connection to "< updateTimePointAndGetTimeDifferenceMS(std::chrono::time_point time_0, double& time_diff) - { - std::chrono::time_point new_time_point = std::chrono::high_resolution_clock::now(); - time_diff = std::chrono::duration_cast(new_time_point - time_0).count(); - return new_time_point; - } + std::chrono::time_point updateTimePointAndGetTimeDifferenceMS(std::chrono::time_point time_0, double& time_diff); + + /** + * @brief Indicate if the controller type is a known renewal controller (it's time step is not constant). + * @param controller_type The controller type. + * @return Returns true if the controller is a renewal type. + */ + bool isRenewalController(const string& controller_type); } \ No newline at end of file diff --git a/queue_controller/package.xml b/queue_controller/package.xml index b18e056..a3e7b2f 100644 --- a/queue_controller/package.xml +++ b/queue_controller/package.xml @@ -31,6 +31,7 @@ actionlib std_srvs std_msgs + ros_boosted_utilities diff --git a/queue_controller/src/queue_controller_utils.cpp b/queue_controller/src/queue_controller_utils.cpp new file mode 100644 index 0000000..fa79eab --- /dev/null +++ b/queue_controller/src/queue_controller_utils.cpp @@ -0,0 +1,20 @@ +#include "queue_controller/queue_controller_utils.hpp" + +#include + +using std::string; + +namespace queue_controller_utils +{ + std::chrono::time_point updateTimePointAndGetTimeDifferenceMS(std::chrono::time_point time_0, double& time_diff) + { + std::chrono::time_point new_time_point = std::chrono::high_resolution_clock::now(); + time_diff = std::chrono::duration_cast(new_time_point - time_0).count(); + return new_time_point; + } + + bool isRenewalController(const string& controller_type) + { + return controller_type == "renewal_min_drift_plus_penalty"; + } +} \ No newline at end of file diff --git a/queue_controller/src/transmission_vector_queue_controller_node.cpp b/queue_controller/src/transmission_vector_queue_controller_node.cpp index 7c7b16d..3c08ea4 100644 --- a/queue_controller/src/transmission_vector_queue_controller_node.cpp +++ b/queue_controller/src/transmission_vector_queue_controller_node.cpp @@ -6,9 +6,9 @@ int main(int argc, char **argv) { ros::init(argc, argv, "queue_controller"); - ros::NodeHandle nh("~"); + ros::NodeHandle nhp("~"); - QueueController queue_controller(nh); + QueueController queue_controller(nhp); // Perform the ros::spin() to process the callbacks and runs the controller. queue_controller.spin(); diff --git a/queue_server/README.md b/queue_server/README.md index 10a02a4..6a43dc5 100644 --- a/queue_server/README.md +++ b/queue_server/README.md @@ -35,10 +35,11 @@ This is research code, expect that it changes often and any fitness for a partic sudo rosdep install --from-paths src #### 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 @@ -122,7 +123,7 @@ For all real queues configured in the config files: Publishes the states of each queue. It's mainly containing the size of the queues. * **`server_stats`** ((ros_queue_msgs/QueueServerStats)[https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/QueueServerStats.msg]) - Publishes stats about the mean average metrics of the real queues (time average arrivals and departures and mean queue size). Only active if the param `compute_statistics` is set to true. + Publishes stats about the mean average metrics of the queues (time average arrivals and departures and mean queue size). Only active if the param `compute_statistics` is set to true. For all real queues configured in the config files * **`/`** ([Type of first message received on the arrival_topic_name of a real queue]) @@ -134,10 +135,15 @@ For all real queues configured in the config files rosservice call //trigger_service -* **`get_server_state`** ([std_queue_msgs/QueueServerStateFetch](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/msg/QueueServerState.msg)) +* **`get_server_state`** ([ros_queue_msgs/QueueServerStateFetch](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/QueueServerStateFetch.msg)) Returns the current server states. It mainly includes the current queue sizes. For example, you can display the server states from the console with: - rosservice call //trigger_service + rosservice call //get_server_state + +* **`get_server_stats`** ([ros_queue_msgs/QueueServerStatsFetch](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/QueueServerStatsFetch.msg)) + Returns the current server states. It mainly includes the time average size, arrivals and departures of each queues. It's only available if the param `compute_statistics` is set to true. For example, you can display the server states from the console with: + + rosservice call //get_server_stats For each queue (virtual and real): * **`/getQueueInfo`** ([ros_queue_msgs/QueueInfoFetch](https://github.com/etienn8/ros_queuing_system/blob/main/ros_queue_msgs/srv/QueueInfoFetch.srv)) diff --git a/queue_server/include/queue_server/queue_server.hpp b/queue_server/include/queue_server/queue_server.hpp index 07a9c9d..a1d659e 100644 --- a/queue_server/include/queue_server/queue_server.hpp +++ b/queue_server/include/queue_server/queue_server.hpp @@ -13,6 +13,7 @@ #include "ros_queue_msgs/QueueState.h" #include "ros_queue_msgs/QueueServerStateFetch.h" +#include "ros_queue_msgs/QueueServerStatsFetch.h" #include "ros_queue_msgs/VirtualQueueChangesList.h" #include "std_srvs/Empty.h" @@ -33,7 +34,7 @@ class QueueServer * @param spin_rate Rate (event per second) at which the real queues will be checked for transmission * and the size of all queues will be published. */ - QueueServer(ros::NodeHandle& nh, float spin_rate); + QueueServer(ros::NodeHandle& nhp, float spin_rate); /** * @brief Add a virtual queue in the inequality constraint queue map. @@ -109,9 +110,9 @@ class QueueServer std::map> real_queues_; /** - * @brief ROS nodle handle to to get the params and to create the services, publisher and subscribers. + * @brief Private ROS nodle handle to get the params and to create the services, publisher and subscribers to that are static. */ - ros::NodeHandle nh_; + ros::NodeHandle nhp_; /** * @brief Name of the queue server to use a meta data. @@ -169,6 +170,12 @@ class QueueServer */ void publishServerStats(); + /** + * @brief Compute and get the current statistics about the time average of the queues and + * other pefromance metrics. + */ + ros_queue_msgs::QueueServerStats getCurrentServerStats(); + /** * @brief For each real queues, verify how much data could be sent and the queue will transmit up to that quanity if possible. */ @@ -189,6 +196,16 @@ class QueueServer */ bool serverStateCallback(ros_queue_msgs::QueueServerStateFetch::Request& req, ros_queue_msgs::QueueServerStateFetch::Response& res); + /** + * @brief Service server that provides on demand the stats of the queues. + */ + ros::ServiceServer queue_server_stats_service_; + + /** + * @brief Callback function that returns the queue server stats. + */ + bool serverStatsCallback(ros_queue_msgs::QueueServerStatsFetch::Request& req, ros_queue_msgs::QueueServerStatsFetch::Response& res); + /** * @brief Service server that provides on demand the meta information of all queues. */ diff --git a/queue_server/src/queue_server.cpp b/queue_server/src/queue_server.cpp index c484126..a5778ae 100644 --- a/queue_server/src/queue_server.cpp +++ b/queue_server/src/queue_server.cpp @@ -26,9 +26,9 @@ using std::string; -QueueServer::QueueServer(ros::NodeHandle& nh, float spin_rate): nh_(nh) +QueueServer::QueueServer(ros::NodeHandle& nhp, float spin_rate): nhp_(nhp) { - if (nh_.getParam("queue_server_name", queue_server_name_)) + if (nhp_.getParam("queue_server_name", queue_server_name_)) { ROS_INFO_STREAM("Initializing queue server named "<< queue_server_name_); } @@ -37,11 +37,13 @@ QueueServer::QueueServer(ros::NodeHandle& nh, float spin_rate): nh_(nh) ROS_INFO_STREAM("Queue server doesn't a name from the param queue_server_name. Queue serve will be initialized with an empty name."); } - if (nh_.getParam("compute_statistics", should_queues_compute_stats_)) + if (nhp_.getParam("compute_statistics", should_queues_compute_stats_)) { if(should_queues_compute_stats_) { - queue_server_stats_pub_ = nh_.advertise("server_stats", 10); + queue_server_stats_pub_ = nhp_.advertise("server_stats", 10); + queue_server_stats_service_ = nhp_.advertiseService("get_server_stats", + &QueueServer::serverStatsCallback, this); } } { @@ -50,21 +52,21 @@ QueueServer::QueueServer(ros::NodeHandle& nh, float spin_rate): nh_(nh) loadQueueParametersAndCreateQueues(); - queue_server_update_virtual_queues_service_ = nh_.advertiseService("trigger_service", + queue_server_update_virtual_queues_service_ = nhp_.advertiseService("trigger_service", &QueueServer::queueUpdateCallback, this); - queue_server_states_pub_ = nh_.advertise("server_state",10); + queue_server_states_pub_ = nhp_.advertise("server_state",10); - queue_server_states_service_ = nh_.advertiseService("get_server_state", + queue_server_states_service_ = nhp_.advertiseService("get_server_state", &QueueServer::serverStateCallback, this); - virtual_queue_manual_changes_ = nh_.subscribe("virtual_queue_manual_changes", + virtual_queue_manual_changes_ = nhp_.subscribe("virtual_queue_manual_changes", 1000, &QueueServer::virtualQueuesManualChangesCallback, this); // Create the periodic caller of the serverSpin - spin_timer_ = nh_.createTimer(ros::Duration(1.0/spin_rate), &QueueServer::serverSpin, this); + spin_timer_ = nhp_.createTimer(ros::Duration(1.0/spin_rate), &QueueServer::serverSpin, this); } void QueueServer::loadQueueParametersAndCreateQueues() @@ -72,7 +74,7 @@ void QueueServer::loadQueueParametersAndCreateQueues() // Initialization of queues based on a config file XmlRpc::XmlRpcValue queue_list; - if(nh_.getParam("queue_list", queue_list)) + if(nhp_.getParam("queue_list", queue_list)) { if(queue_list.getType() == XmlRpc::XmlRpcValue::TypeArray) { @@ -221,7 +223,7 @@ void QueueServer::checkAndCreateQueue(QueueParamStruct& queue_param_struct) std::unique_ptr new_queue = std::make_unique((int)queue_param_struct.max_queue_size_, std::move(info), - nh_, + nhp_, (struct ROSByteConvertedQueue::InterfacesArgs) { .arrival_topic_name = queue_param_struct.arrival_topic_name_, @@ -254,7 +256,7 @@ void QueueServer::checkAndCreateQueue(QueueParamStruct& queue_param_struct) if (!is_a_parameter_missing) { - ROS_INFO_STREAM("Creating virtual real queue: " << queue_param_struct.queue_name_); + ROS_INFO_STREAM("Creating virtual queue: " << queue_param_struct.queue_name_); ros_queue_msgs::QueueInfo info; info.is_virtual = queue_server_utils::isQueueTypeVirtual(queue_param_struct.type_of_queue_); info.queue_name = queue_param_struct.queue_name_; @@ -263,13 +265,13 @@ void QueueServer::checkAndCreateQueue(QueueParamStruct& queue_param_struct) std::unique_ptr new_queue = std::make_unique((int)queue_param_struct.max_queue_size_, std::move(info), - nh_, + nhp_, (struct ROSInConVirtualQueue::InterfacesArgs) { .arrival_evaluation_service_name = queue_param_struct.arrival_evaluation_service_name_, .departure_evaluation_service_name = queue_param_struct.departure_evaluation_service_name_ }); - + new_queue->mean_stats_.should_compute_means_ = should_queues_compute_stats_; addInequalityConstraintVirtualQueue(std::move(new_queue)); } else @@ -293,7 +295,7 @@ void QueueServer::checkAndCreateQueue(QueueParamStruct& queue_param_struct) if (!is_a_parameter_missing) { - ROS_INFO_STREAM("Creating virtual real queue: " << queue_param_struct.queue_name_); + ROS_INFO_STREAM("Creating virtual queue: " << queue_param_struct.queue_name_); ros_queue_msgs::QueueInfo info; info.is_virtual = queue_server_utils::isQueueTypeVirtual(queue_param_struct.type_of_queue_); info.queue_name = queue_param_struct.queue_name_; @@ -302,13 +304,14 @@ void QueueServer::checkAndCreateQueue(QueueParamStruct& queue_param_struct) std::unique_ptr new_queue = std::make_unique((int)queue_param_struct.max_queue_size_, std::move(info), - nh_, + nhp_, (struct ROSEqConVirtualQueue::InterfacesArgs) { .arrival_evaluation_service_name = queue_param_struct.arrival_evaluation_service_name_, .departure_evaluation_service_name = queue_param_struct.departure_evaluation_service_name_ }); - + + new_queue->mean_stats_.should_compute_means_ = should_queues_compute_stats_; addEqualityConstraintVirtualQueue(std::move(new_queue)); } else @@ -336,6 +339,12 @@ bool QueueServer::serverStateCallback(ros_queue_msgs::QueueServerStateFetch::Req return true; } +bool QueueServer::serverStatsCallback(ros_queue_msgs::QueueServerStatsFetch::Request& req, ros_queue_msgs::QueueServerStatsFetch::Response& res) +{ + res.queue_stats = getCurrentServerStats(); + return true; +} + bool QueueServer::queueUpdateCallback(std_srvs::Empty::Request& req, std_srvs::Empty::Request& res) { std::vector> future_list; @@ -379,12 +388,12 @@ void QueueServer::virtualQueuesManualChangesCallback(const ros_queue_msgs::Virtu if (in_it != inequality_constraint_virtual_queues_.end()) { // If found, update the queue based on requested manual changes. - in_it->second->update(changes_it->arrival - changes_it->departure); + in_it->second->update(changes_it->arrival, changes_it->departure); } else if (eq_it != equality_constraint_virtual_queues_.end()) { // If found, update the queue based on requested manual changes. - eq_it->second->update(changes_it->arrival - changes_it->departure); + eq_it->second->update(changes_it->arrival, changes_it->departure); } } } @@ -402,7 +411,7 @@ void QueueServer::publishServerStates() queue_server_states_pub_.publish(server_state_msg); } -void QueueServer::publishServerStats() +ros_queue_msgs::QueueServerStats QueueServer::getCurrentServerStats() { ros_queue_msgs::QueueServerStats server_stats_msg; @@ -414,16 +423,99 @@ void QueueServer::publishServerStats() { ros_queue_msgs::QueueStats queue_stats; queue_stats.queue_name = queue_it->first; + queue_stats.arrival_mean = queue_it->second->mean_stats_.getArrivalMean(); + queue_stats.arrival_time_average = queue_it->second->mean_stats_.getArrivalTimeAverage(); + queue_stats.total_arrival = queue_it->second->mean_stats_.getArrivalTotal(); + queue_stats.last_arrival = queue_it->second->mean_stats_.getLastArrival(); + queue_stats.departure_mean = queue_it->second->mean_stats_.getDepartureMean(); + queue_stats.departure_time_average = queue_it->second->mean_stats_.getDepartureTimeAverage(); + queue_stats.total_departure = queue_it->second->mean_stats_.getDepartureTotal(); + queue_stats.last_departure = queue_it->second->mean_stats_.getLastDeparture(); + + queue_stats.real_departure_mean = queue_it->second->mean_stats_.getRealDepartureMean(); + queue_stats.real_departure_time_average = queue_it->second->mean_stats_.getRealDepartureTimeAverage(); + queue_stats.total_real_departure = queue_it->second->mean_stats_.getRealDepartureTotal(); + queue_stats.last_real_departure = queue_it->second->mean_stats_.getLastRealDeparture(); + queue_stats.current_size = queue_it->second->getSize(); queue_stats.size_mean = queue_it->second->mean_stats_.getSizeMean(); queue_stats.converted_remaining_mean = queue_it->second->mean_stats_.getConvertedRemainingMean(); + queue_stats.seconds_since_start =queue_it->second->mean_stats_.getSecondsSinceStart(); server_stats_msg.queue_stats.push_back(std::move(queue_stats)); } } - queue_server_stats_pub_.publish(server_stats_msg); + + for (auto queue_it = inequality_constraint_virtual_queues_.begin(); queue_it != inequality_constraint_virtual_queues_.end(); ++queue_it) + { + if (queue_it->second->mean_stats_.should_compute_means_) + { + ros_queue_msgs::QueueStats queue_stats; + queue_stats.queue_name = queue_it->first; + + queue_stats.arrival_mean = queue_it->second->mean_stats_.getArrivalMean(); + queue_stats.arrival_time_average = queue_it->second->mean_stats_.getArrivalTimeAverage(); + queue_stats.total_arrival = queue_it->second->mean_stats_.getArrivalTotal(); + queue_stats.last_arrival = queue_it->second->mean_stats_.getLastArrival(); + + queue_stats.departure_mean = queue_it->second->mean_stats_.getDepartureMean(); + queue_stats.departure_time_average = queue_it->second->mean_stats_.getDepartureTimeAverage(); + queue_stats.total_departure = queue_it->second->mean_stats_.getDepartureTotal(); + queue_stats.last_departure = queue_it->second->mean_stats_.getLastDeparture(); + + queue_stats.real_departure_mean = queue_it->second->mean_stats_.getRealDepartureMean(); + queue_stats.real_departure_time_average = queue_it->second->mean_stats_.getRealDepartureTimeAverage(); + queue_stats.total_real_departure = queue_it->second->mean_stats_.getRealDepartureTotal(); + queue_stats.last_real_departure = queue_it->second->mean_stats_.getLastRealDeparture(); + + queue_stats.current_size = queue_it->second->getSize(); + queue_stats.size_mean = queue_it->second->mean_stats_.getSizeMean(); + queue_stats.change_mean = queue_it->second->mean_stats_.getChangeTimeAverage(); + queue_stats.seconds_since_start =queue_it->second->mean_stats_.getSecondsSinceStart(); + + server_stats_msg.queue_stats.push_back(std::move(queue_stats)); + } + } + + for (auto queue_it = equality_constraint_virtual_queues_.begin(); queue_it != equality_constraint_virtual_queues_.end(); ++queue_it) + { + if (queue_it->second->mean_stats_.should_compute_means_) + { + ros_queue_msgs::QueueStats queue_stats; + queue_stats.queue_name = queue_it->first; + + queue_stats.arrival_mean = queue_it->second->mean_stats_.getArrivalMean(); + queue_stats.arrival_time_average = queue_it->second->mean_stats_.getArrivalTimeAverage(); + queue_stats.total_arrival = queue_it->second->mean_stats_.getArrivalTotal(); + queue_stats.last_arrival = queue_it->second->mean_stats_.getLastArrival(); + + queue_stats.departure_mean = queue_it->second->mean_stats_.getDepartureMean(); + queue_stats.departure_time_average = queue_it->second->mean_stats_.getDepartureTimeAverage(); + queue_stats.total_departure = queue_it->second->mean_stats_.getDepartureTotal(); + queue_stats.last_departure = queue_it->second->mean_stats_.getLastDeparture(); + + queue_stats.real_departure_mean = queue_it->second->mean_stats_.getRealDepartureMean(); + queue_stats.real_departure_time_average = queue_it->second->mean_stats_.getRealDepartureTimeAverage(); + queue_stats.total_real_departure = queue_it->second->mean_stats_.getRealDepartureTotal(); + queue_stats.last_real_departure = queue_it->second->mean_stats_.getLastRealDeparture(); + + queue_stats.current_size = queue_it->second->getSize(); + queue_stats.size_mean = queue_it->second->mean_stats_.getSizeMean(); + queue_stats.change_mean = queue_it->second->mean_stats_.getChangeTimeAverage(); + queue_stats.seconds_since_start =queue_it->second->mean_stats_.getSecondsSinceStart(); + + server_stats_msg.queue_stats.push_back(std::move(queue_stats)); + } + } + + return server_stats_msg; +} + +void QueueServer::publishServerStats() +{ + queue_server_stats_pub_.publish(getCurrentServerStats()); } void QueueServer::transmitRealQueues() diff --git a/queue_server/src/queue_server_node.cpp b/queue_server/src/queue_server_node.cpp index 2937aa9..03777a6 100644 --- a/queue_server/src/queue_server_node.cpp +++ b/queue_server/src/queue_server_node.cpp @@ -6,18 +6,18 @@ int main(int argc, char **argv) { ros::init(argc, argv, "queue_server"); - ros::NodeHandle nh("~"); + ros::NodeHandle nhp("~"); // Initialize a queue server float server_spin_rate = 10.0f; - if(!nh.getParam("server_spin_rate", server_spin_rate)) + if(!nhp.getParam("server_spin_rate", server_spin_rate)) { ROS_WARN_STREAM("Server spin rate wasn't specified, the server will spin at " << server_spin_rate << " hz."); } - QueueServer queue_server(nh, server_spin_rate); + QueueServer queue_server(nhp, server_spin_rate); ros::spin(); } \ No newline at end of file diff --git a/ros_queue/CMakeLists.txt b/ros_queue/CMakeLists.txt index e1c6486..a704b95 100644 --- a/ros_queue/CMakeLists.txt +++ b/ros_queue/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(catkin REQUIRED COMPONENTS ros_queue_msgs message_generation topic_tools + ros_boosted_utilities ) ## System dependencies are found with CMake's conventions @@ -105,7 +106,7 @@ generate_messages( ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( - INCLUDE_DIRS include + INCLUDE_DIRS include ${ros_boosted_utilities_INCLUDE_DIRS} LIBRARIES ${QUEUE_LIB_NAME} ${ROSQUEUE_LIB_NAME} CATKIN_DEPENDS roscpp message_runtime topic_tools ros_queue_msgs #DEPENDS system_lib @@ -127,6 +128,7 @@ include_directories( src/${QUEUE_LIB_NAME}/dynamic_queue.cpp src/${QUEUE_LIB_NAME}/dynamic_virtual_queue.cpp src/${QUEUE_LIB_NAME}/virtual_queue.cpp + src/${QUEUE_LIB_NAME}/mean_stats.cpp ) add_library(${ROSQUEUE_LIB_NAME} diff --git a/ros_queue/README.md b/ros_queue/README.md index caa7a52..9f7bc66 100644 --- a/ros_queue/README.md +++ b/ros_queue/README.md @@ -42,6 +42,7 @@ This is research code, expect that it changes often and any fitness for a partic - [ros_queue_msgs](https://github.com/etienn8/ros_queuing_system/tree/feat/readme_ros_queue_and_server/ros_queue_msgs)(ROS data structures for the queues interfaces), - [topic_tools](http://wiki.ros.org/topic_tools) (Contains the Shapeshifter class that allows to store ROS message types that are only know at runtime), - [rostest](http://wiki.ros.org/rostest) (ROS test library that allow automated testing of the *lib_queue* and *ros_queue libraries*), +- [ros_boosted_utilities](https://github.com/etienn8/ros_boosted_utilities) (Contains persistent ROS service clients to make the interfaces between the system faster.) - [std::deque](https://en.cppreference.com/w/cpp/container/deque)(Standard library double ended-queues), - std::utility (Standard library that permits move semantics), - std::mutex (Standard library to manage concurrency) @@ -50,16 +51,18 @@ This is research code, expect that it changes often and any fitness for a partic sudo rosdep install --from-paths src #### 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 catkin_make If you're using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html), you could use `catkin build` command instead of `catkin_make`. + ### Unit Tests With [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html), run the unit tests of all packages with diff --git a/ros_queue/include/ros_queue/lib_queue/dynamic_converted_queue.hpp b/ros_queue/include/ros_queue/lib_queue/dynamic_converted_queue.hpp index 5243209..acf7528 100644 --- a/ros_queue/include/ros_queue/lib_queue/dynamic_converted_queue.hpp +++ b/ros_queue/include/ros_queue/lib_queue/dynamic_converted_queue.hpp @@ -139,13 +139,17 @@ class DynamicConvertedQueue: public IDynamicQueue lock(queue_manipulation_mutex_); + if (mean_stats_.should_compute_means_) + { + mean_stats_.increaseDepartureMean(nb_departing_converted_size); + } int total_departures = 0; if (!this->internal_queue_.empty()) { int front_element_size = static_cast(this->internal_queue_.front().converted_size_); - while(nb_departing_converted_size > front_element_size) + while(nb_departing_converted_size >= front_element_size) { if(this->internal_queue_.empty()) { @@ -165,7 +169,13 @@ class DynamicConvertedQueue: public IDynamicQueueinternal_queue_.empty()) + { + mean_stats_.increaseConvertedRemainingMean(nb_departing_converted_size); + } } int total_arrivals = 0; @@ -249,7 +259,8 @@ class DynamicConvertedQueue: public IDynamicQueue: public IDynamicQueue lock(queue_manipulation_mutex_); - int total_departure = 0; + if (mean_stats_.should_compute_means_) + { + mean_stats_.increaseDepartureMean(nb_departing_converted_size); + } + + int total_departures = 0; if (!this->internal_queue_.empty()) { int front_element_size = static_cast(this->internal_queue_.front().converted_size_); - while(nb_departing_converted_size > front_element_size) + while(nb_departing_converted_size >= front_element_size) { if(this->internal_queue_.empty()) { @@ -540,7 +556,7 @@ class DynamicConvertedQueue: public IDynamicQueueinternal_queue_.front().element_)); this->internal_queue_.pop_front(); @@ -551,8 +567,13 @@ class DynamicConvertedQueue: public IDynamicQueueinternal_queue_.empty()) + { + mean_stats_.increaseConvertedRemainingMean(nb_departing_converted_size); + } } int total_arrivals = 0; @@ -640,6 +661,7 @@ class DynamicConvertedQueue: public IDynamicQueue #include "float_compare.hpp" +#include "mean_stats.hpp" #include "I_dynamic_virtual_queue.hpp" #include "ros_queue/lib_queue/virtual_queue.hpp" @@ -30,6 +31,34 @@ class InConVirtualQueue: public IDynamicVirtualQueue return this->internal_queue_.size(); }; + /** + * @brief Updates the queue by adding the arrival and removing the departure. It also computes time average metrics. + * @param arrival Amount of element to add to the queue. + * @param departure Amount of element to remove from the queue. + * @return Boolean of it the queue overflowed while adding elements. + */ + bool update(const float arrival, const float departure) + { + if (mean_stats_.should_compute_means_) + { + const float current_queue_size = getSize(); + const float real_departure = (current_queue_size + arrival < departure)? + current_queue_size + arrival: departure; + mean_stats_.increaseArrivalMean(arrival); + mean_stats_.increaseDepartureMean(departure); + mean_stats_.increaseRealDepartureMean(real_departure); + } + + bool queue_overflowed = update(arrival - departure); + + if(mean_stats_.should_compute_means_) + { + mean_stats_.increaseSizeMean(getSize()); + } + + return queue_overflowed; + } + /** * @brief Updates the queue by adding the arriving_elements and by removing the specifiy number of departing elements while respecting the maximum queue size. * @param change Change in the queue that wants to be added to the queue @@ -68,6 +97,12 @@ class InConVirtualQueue: public IDynamicVirtualQueue return float_compare(this->internal_queue_.size(), this->max_queue_size_); } + /** + * @brief Object that computes the time average of arrivals and + * departures and the mean of the queue size. + */ + MeanStats mean_stats_; + protected: /** * @brief Mutex to protect access to the internal queue. @@ -98,6 +133,36 @@ class EqConVirtualQueue: public IDynamicVirtualQueue return this->internal_queue_.size(); }; + /** + * @brief Updates the queue by adding the arrival and removing the departure. It also computes time average metrics. + * @param arrival Amount of element to add to the queue. + * @param departure Amount of element to remove from the queue. + * @return Boolean of it the queue overflowed while adding elements. + */ + bool update(const float arrival, const float departure) + { + if (mean_stats_.should_compute_means_) + { + const float current_queue_size = getSize(); + mean_stats_.increaseArrivalMean(arrival); + mean_stats_.increaseDepartureMean(departure); + + const float real_departure = (current_queue_size - departure < -this->max_queue_size_)? + current_queue_size + this->max_queue_size_: departure; + + mean_stats_.increaseRealDepartureMean(departure); + } + + bool queue_overflowed = update(arrival - departure); + + if(mean_stats_.should_compute_means_) + { + mean_stats_.increaseSizeMean(getSize()); + } + + return queue_overflowed; + } + /** * @brief Updates the queue by adding the arriving_elements and by removing the specifiy number of departing elements while respecting the maximum queue size. * @param change Change in the queue that wants to be added to the queue @@ -138,6 +203,12 @@ class EqConVirtualQueue: public IDynamicVirtualQueue return float_compare(this->internal_queue_.size(), this->max_queue_size_) || float_compare(this->internal_queue_.size(), -this->max_queue_size_); } + /** + * @brief Object that computes the time average of arrivals and + * departures and the mean of the queue size. + */ + MeanStats mean_stats_; + protected: /** * @brief Mutex to protect access to the internal queue. diff --git a/ros_queue/include/ros_queue/lib_queue/mean_stats.hpp b/ros_queue/include/ros_queue/lib_queue/mean_stats.hpp index d32b37d..082b6cf 100644 --- a/ros_queue/include/ros_queue/lib_queue/mean_stats.hpp +++ b/ros_queue/include/ros_queue/lib_queue/mean_stats.hpp @@ -2,83 +2,58 @@ #include -struct MeanStats +class MeanStats { - double arrival_sum_= 0.0f; - double departure_sum_ = 0.0f; - double size_sum_ = 0.0f; - long long mean_sample_size = 0; - double converted_remaining_sum = 0.0f; - long long remaining_sample_sum = 0; - - bool should_compute_means_ = false; - - std::chrono::time_point time_0_; - - MeanStats() - { - time_0_ =std::chrono::high_resolution_clock::now(); - } - - void increaseArrivalMean(float new_arrival) - { - arrival_sum_ += new_arrival; - }; - - double getArrivalMean() - { - double current_time_point_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count(); - - if (current_time_point_diff == 0) - { - return 0.0; - } - return arrival_sum_/current_time_point_diff*1000.0; - } - - void increaseDepartureMean(float new_departure) - { - departure_sum_ += new_departure; - }; - - double getDepartureMean() - { - double current_time_point_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count(); - - if (current_time_point_diff == 0) - { - return 0.0; - } - return departure_sum_/current_time_point_diff*1000.0; - } - - void increaseSizeMean(float new_queue_size) - { - size_sum_ += new_queue_size; - mean_sample_size += 1; - }; - - double getSizeMean() - { - if(mean_sample_size == 0) - { - return 0.0; - } - return size_sum_/mean_sample_size; - } - - void increaseConvertedRemainingMean(float current_remaining) - { - converted_remaining_sum += current_remaining; - remaining_sample_sum += 1; - }; - - double getConvertedRemainingMean() - { - if(remaining_sample_sum == 0) - { - return 0.0; - } - return converted_remaining_sum/remaining_sample_sum; - } + public: + MeanStats(); + + void increaseArrivalMean(float new_arrival); + double getArrivalTimeAverage(); + double getArrivalMean(); + double getArrivalTotal(); + double getLastArrival(); + + void increaseDepartureMean(float new_departure); + double getDepartureTimeAverage(); + double getDepartureMean(); + double getDepartureTotal(); + double getLastDeparture(); + + void increaseRealDepartureMean(float new_departure); + double getRealDepartureTimeAverage(); + double getRealDepartureMean(); + double getRealDepartureTotal(); + double getLastRealDeparture(); + + double getChangeTimeAverage(); + + void increaseSizeMean(float new_queue_size); + double getSizeMean(); + + void increaseConvertedRemainingMean(float current_remaining); + double getConvertedRemainingMean(); + + double getSecondsSinceStart(); + + bool should_compute_means_ = false; + + private: + double arrival_sum_= 0.0f; + long long arrival_sample_size_ = 0; + double last_arrival_ = 0.0f; + + double departure_sum_ = 0.0f; + long long departure_sample_size_ = 0; + double last_departure_ = 0.0f; + + double real_departure_sum_ = 0.0f; + long long real_departure_sample_size_ = 0; + double last_real_departure_ = 0.0f; + + double size_sum_ = 0.0f; + long long mean_sample_size = 0; + double converted_remaining_sum = 0.0f; + long long remaining_sample_sum = 0; + + std::chrono::time_point time_0_; }; \ No newline at end of file diff --git a/ros_queue/include/ros_queue/ros_byte_converted_queue.hpp b/ros_queue/include/ros_queue/ros_byte_converted_queue.hpp index de2284a..75da321 100644 --- a/ros_queue/include/ros_queue/ros_byte_converted_queue.hpp +++ b/ros_queue/include/ros_queue/ros_byte_converted_queue.hpp @@ -19,6 +19,8 @@ #include "ros_queue_msgs/FloatRequest.h" #include "ros_queue_msgs/QueueInfo.h" +#include "ros_boosted_utilities/persistent_service_client.hpp" + using std::string; using std::invalid_argument; @@ -83,10 +85,11 @@ class ROSByteConvertedQueue: public DynamicConvertedQueue(interfaces.transmission_evaluation_service_name); + transmission_evaluation_service_client_ = PersistentServiceClient(nh_, interfaces.transmission_evaluation_service_name); + transmission_evaluation_service_client_.waitForExistence(); } - manual_transmit_subscriber_ = nh_.subscribe("nb_bytes_to_transmit", 10, &ROSByteConvertedQueue::manualTransmissionCallback, this); + manual_transmit_subscriber_ = nhp_.subscribe("nb_bytes_to_transmit", 10, &ROSByteConvertedQueue::manualTransmissionCallback, this); } /** @@ -114,7 +117,7 @@ class ROSByteConvertedQueue: public DynamicConvertedQueueupdateInConvertedSize(std::move(empty_queue), nb_of_byte_to_transmit); } } - } - else - { - ROS_WARN_STREAM_THROTTLE(4, info_.queue_name<<": Tried to be updated based on quality of service, but its transmission_evaluation_service_ is not available."); + else + { + ROS_WARN_STREAM_THROTTLE(4, info_.queue_name<<": Tried to be updated based on quality of service, but its transmission_evaluation_service_ is not available."); + } } } @@ -266,7 +269,7 @@ class ROSByteConvertedQueue: public DynamicConvertedQueue transmission_evaluation_service_client_; /** * @brief ROS subscriber that receives message that that indicate a number of diff --git a/ros_queue/include/ros_queue/ros_converted_queue.hpp b/ros_queue/include/ros_queue/ros_converted_queue.hpp index 94467f6..5e8cde5 100644 --- a/ros_queue/include/ros_queue/ros_converted_queue.hpp +++ b/ros_queue/include/ros_queue/ros_converted_queue.hpp @@ -15,6 +15,8 @@ #include "ros_queue_msgs/QueueInfo.h" +#include "ros_boosted_utilities/persistent_service_client.hpp" + using std::string; using std::invalid_argument; @@ -85,7 +87,8 @@ class ROSConvertedQueue: public DynamicConvertedQueue(interfaces.arrival_prediction_service_name); + arrival_service_client_ = PersistentServiceClient(nh, interfaces.arrival_prediction_service_name); + arrival_service_client_.waitForExistence(); } else { @@ -104,7 +107,8 @@ class ROSConvertedQueue: public DynamicConvertedQueue(interfaces.transmission_prediction_service_name); + transmission_service_client_ = PersistentServiceClient(nh, interfaces.transmission_prediction_service_name); + transmission_service_client_.waitForExistence(); } else { @@ -135,7 +139,7 @@ class ROSConvertedQueue: public DynamicConvertedQueue(interfaces.conversion_service_name); + conversion_service_client_ = nh_.serviceClient(interfaces.conversion_service_name); } else { @@ -164,12 +168,9 @@ class ROSConvertedQueue: public DynamicConvertedQueue arrival_service_client_; + /** * @brief Function pointer for the arrival prediction service calls. * @param TPredictionServiceClass& Service class used as a data structure to pass input to predictions. @@ -322,7 +321,8 @@ class ROSConvertedQueue: public DynamicConvertedQueue transmission_service_client_; + /** * @brief Function pointer for the transmission prediction service calls. * @param TPredictionServiceClass& Service class used as a data structure to pass input to predictions. diff --git a/ros_queue/include/ros_queue/ros_queue.hpp b/ros_queue/include/ros_queue/ros_queue.hpp index cab62ae..d9dc4b6 100644 --- a/ros_queue/include/ros_queue/ros_queue.hpp +++ b/ros_queue/include/ros_queue/ros_queue.hpp @@ -13,6 +13,8 @@ #include "ros_queue_msgs/QueueInfo.h" +#include "ros_boosted_utilities/persistent_service_client.hpp" + using std::string; using std::invalid_argument; using std::deque; @@ -70,7 +72,8 @@ class ROSQueue: public DynamicQueue::Ele } else if (!interfaces.arrival_prediction_service_name.empty()) { - arrival_service_client_ = nh.serviceClient(interfaces.arrival_prediction_service_name); + arrival_service_client_ = PersistentServiceClient(nh, interfaces.arrival_prediction_service_name); + arrival_service_client_.waitForExistence(); } else { @@ -89,7 +92,8 @@ class ROSQueue: public DynamicQueue::Ele } else if (!interfaces.transmission_prediction_service_name.empty()) { - transmission_service_client_ = nh.serviceClient(interfaces.transmission_prediction_service_name); + transmission_service_client_ = PersistentServiceClient(nh, interfaces.transmission_prediction_service_name); + transmission_service_client_.waitForExistence(); } else { @@ -133,12 +137,9 @@ class ROSQueue: public DynamicQueue::Ele TPredictionServiceClass local_service= service; // Service ROS call - if (arrival_service_client_.waitForExistence(WAIT_DURATION_FOR_SERVICE_EXISTENCE)) + if (arrival_service_client_.call(local_service)) { - if (arrival_service_client_.call(local_service)) - { - return local_service.response.prediction; - } + return local_service.response.prediction; } else { @@ -166,12 +167,9 @@ class ROSQueue: public DynamicQueue::Ele TPredictionServiceClass local_service = service; // Service ROS call - if (transmission_service_client_.waitForExistence(WAIT_DURATION_FOR_SERVICE_EXISTENCE)) + if (transmission_service_client_.call(local_service)) { - if (transmission_service_client_.call(local_service)) - { - return local_service.response.prediction; - } + return local_service.response.prediction; } else { @@ -222,7 +220,8 @@ class ROSQueue: public DynamicQueue::Ele /** * @brief Service client used for the arrival prediction service calls. */ - ros::ServiceClient arrival_service_client_; + PersistentServiceClient arrival_service_client_; + /** * @brief Function pointer for the arrival prediction service calls. * @param TPredictionServiceClass& Service class used as a data structure to pass input to predictions. @@ -232,7 +231,8 @@ class ROSQueue: public DynamicQueue::Ele /** * @brief Service client used for the transmission prediction service calls. */ - ros::ServiceClient transmission_service_client_; + PersistentServiceClient transmission_service_client_; + /** * @brief Function pointer for the transmission prediction service calls. * @param TPredictionServiceClass& Service class used as a data structure to pass input to predictions. diff --git a/ros_queue/include/ros_queue/ros_queue_common_interfaces.hpp b/ros_queue/include/ros_queue/ros_queue_common_interfaces.hpp index 9945942..435fa0a 100644 --- a/ros_queue/include/ros_queue/ros_queue_common_interfaces.hpp +++ b/ros_queue/include/ros_queue/ros_queue_common_interfaces.hpp @@ -19,15 +19,15 @@ class ROSQueueCommonInterfaces * @param info ros_queue_msgs::QueueInfo rvalue that contains meta data about the queue. * @param nh Its ros::NodeHandle used to create the services and make sure that a node handle exists during the life time of the ROSQueue. */ - ROSQueueCommonInterfaces(ros::NodeHandle& nh, ros_queue_msgs::QueueInfo&& info): info_(std::move(info)), nh_(nh) + ROSQueueCommonInterfaces(ros::NodeHandle& nh, ros_queue_msgs::QueueInfo&& info): info_(std::move(info)), nhp_(nh), nh_(ros::NodeHandle()) { if (!info_.queue_name.empty()) { string queue_size_service_name = info_.queue_name + "/getQueueSize"; string queue_info_service_name = info_.queue_name + "/getQueueInfo"; - queue_size_service_ = nh_.advertiseService(queue_size_service_name, &ROSQueueCommonInterfaces::getSizeServiceCallback, this); - queue_info_service_ = nh_.advertiseService(queue_info_service_name, &ROSQueueCommonInterfaces::getQueueInfoCallback, this); + queue_size_service_ = nhp_.advertiseService(queue_size_service_name, &ROSQueueCommonInterfaces::getSizeServiceCallback, this); + queue_info_service_ = nhp_.advertiseService(queue_info_service_name, &ROSQueueCommonInterfaces::getQueueInfoCallback, this); } else { @@ -46,6 +46,11 @@ class ROSQueueCommonInterfaces */ ros::NodeHandle nh_; + /** + * @brief Private ROS Node handle used to create the service calls for the predictions. + */ + ros::NodeHandle nhp_; + /** * @brief ROS service server to provide the state of the queue. */ diff --git a/ros_queue/include/ros_queue/ros_virtual_queue.hpp b/ros_queue/include/ros_queue/ros_virtual_queue.hpp index 7fa8266..627b4bd 100644 --- a/ros_queue/include/ros_queue/ros_virtual_queue.hpp +++ b/ros_queue/include/ros_queue/ros_virtual_queue.hpp @@ -13,6 +13,8 @@ #include "ros_queue_msgs/FloatRequest.h" #include "ros_queue_msgs/QueueInfo.h" +#include "ros_boosted_utilities/persistent_service_client.hpp" + using std::string; using std::invalid_argument; @@ -64,7 +66,8 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt } else if (!interfaces.arrival_evaluation_service_name.empty()) { - arrival_evaluation_service_client_ = nh_.serviceClient(interfaces.arrival_evaluation_service_name); + arrival_evaluation_service_client_ = PersistentServiceClient(nh, interfaces.arrival_evaluation_service_name); + arrival_evaluation_service_client_.waitForExistence(); } else { @@ -83,7 +86,8 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt } else if (!interfaces.departure_evaluation_service_name.empty()) { - departure_evaluation_service_client_ = nh_.serviceClient(interfaces.departure_evaluation_service_name); + departure_evaluation_service_client_ = PersistentServiceClient(nh, interfaces.departure_evaluation_service_name); + departure_evaluation_service_client_.waitForExistence(); } else { @@ -112,13 +116,10 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt ros_queue_msgs::FloatRequest local_service; // Service ROS call - if (arrival_evaluation_service_client_.waitForExistence(WAIT_DURATION_FOR_SERVICE_EXISTENCE)) + if (arrival_evaluation_service_client_.call(local_service)) { - if (arrival_evaluation_service_client_.call(local_service)) - { - arrival = local_service.response.value; - was_arrival_evaluated = true; - } + arrival = local_service.response.value; + was_arrival_evaluated = true; } else { @@ -136,13 +137,10 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt ros_queue_msgs::FloatRequest local_service; // Service ROS call - if (departure_evaluation_service_client_.waitForExistence(WAIT_DURATION_FOR_SERVICE_EXISTENCE)) + if (departure_evaluation_service_client_.call(local_service)) { - if (departure_evaluation_service_client_.call(local_service)) - { - departure = local_service.response.value; - was_departure_evaluated = true; - } + departure = local_service.response.value; + was_departure_evaluated = true; } else { @@ -152,7 +150,7 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt if (was_arrival_evaluated && was_departure_evaluated) { - update(arrival-departure); + update(arrival, departure); } else { @@ -175,7 +173,8 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt /** * @brief Service client used to compute the increase of the virtual queue. */ - ros::ServiceClient arrival_evaluation_service_client_; + PersistentServiceClient arrival_evaluation_service_client_; + /** * @brief Function pointer used to compute the increase of the virtual queue. */ @@ -184,7 +183,8 @@ class ROSVirtualQueue: public TDynamicVirtualQueueType, public ROSQueueCommonInt /** * @brief Service client used to compute the decrease of the virtual queue. */ - ros::ServiceClient departure_evaluation_service_client_; + PersistentServiceClient departure_evaluation_service_client_; + /** * @brief Function pointer used to compute the decrease of the virtual queue. */ diff --git a/ros_queue/package.xml b/ros_queue/package.xml index b292dbe..059bbee 100644 --- a/ros_queue/package.xml +++ b/ros_queue/package.xml @@ -7,7 +7,7 @@ - etienne + Etienne Villemure @@ -62,6 +62,8 @@ ros_queue_msgs topic_tools + ros_boosted_utilities + diff --git a/ros_queue/src/lib_queue/mean_stats.cpp b/ros_queue/src/lib_queue/mean_stats.cpp new file mode 100644 index 0000000..e462162 --- /dev/null +++ b/ros_queue/src/lib_queue/mean_stats.cpp @@ -0,0 +1,145 @@ +#include "ros_queue/lib_queue/mean_stats.hpp" + +MeanStats::MeanStats() +{ + time_0_ =std::chrono::high_resolution_clock::now(); +} + +void MeanStats::increaseArrivalMean(float new_arrival) +{ + arrival_sum_ += new_arrival; + ++arrival_sample_size_; + last_arrival_ = new_arrival; +}; + +double MeanStats::getArrivalTimeAverage() +{ + double current_time_point_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count(); + + if (current_time_point_diff == 0) + { + return 0.0; + } + return arrival_sum_/current_time_point_diff*1000.0; +} + +double MeanStats::getArrivalMean() +{ + return arrival_sum_/arrival_sample_size_; +} + +double MeanStats::getArrivalTotal() +{ + return arrival_sum_; +} + +double MeanStats::getLastArrival() +{ + return last_arrival_; +} + +void MeanStats::increaseDepartureMean(float new_departure) +{ + departure_sum_ += new_departure; + ++departure_sample_size_; + last_departure_ = new_departure; +}; + +double MeanStats::getDepartureTimeAverage() +{ + double current_time_point_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count(); + + if (current_time_point_diff == 0) + { + return 0.0; + } + return departure_sum_/current_time_point_diff*1000.0; +} + +double MeanStats::getDepartureMean() +{ + return departure_sum_/departure_sample_size_; +} + +double MeanStats::getDepartureTotal() +{ + return departure_sum_; +} + +double MeanStats::getLastDeparture() +{ + return last_departure_; +} + +void MeanStats::increaseRealDepartureMean(float new_departure) +{ + real_departure_sum_ += new_departure; + ++real_departure_sample_size_; + last_real_departure_ = new_departure; +} + +double MeanStats::getRealDepartureTimeAverage() +{ + double current_time_point_diff = std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count(); + + if (current_time_point_diff == 0) + { + return 0.0; + } + return real_departure_sum_/current_time_point_diff*1000.0; +} + +double MeanStats::getRealDepartureMean() +{ + return real_departure_sum_/real_departure_sample_size_; +} + +double MeanStats::getRealDepartureTotal() +{ + return real_departure_sum_; +} + +double MeanStats::getLastRealDeparture() +{ + return last_real_departure_; +} + +double MeanStats::getChangeTimeAverage() +{ + return getArrivalTimeAverage() - getDepartureTimeAverage(); +} + +void MeanStats::increaseSizeMean(float new_queue_size) +{ + size_sum_ += new_queue_size; + mean_sample_size += 1; +}; + +double MeanStats::getSizeMean() +{ + if(mean_sample_size == 0) + { + return 0.0; + } + return size_sum_/mean_sample_size; +} + +void MeanStats::increaseConvertedRemainingMean(float current_remaining) +{ + converted_remaining_sum += current_remaining; + remaining_sample_sum += 1; +}; + +double MeanStats::getConvertedRemainingMean() +{ + if(remaining_sample_sum == 0) + { + return 0.0; + } + return converted_remaining_sum/remaining_sample_sum; +} + +double MeanStats::getSecondsSinceStart() +{ + return std::chrono::duration_cast(std::chrono::high_resolution_clock::now() - time_0_).count()/1000.0; +} \ No newline at end of file diff --git a/ros_queue_experiments/.gitignore b/ros_queue_experiments/.gitignore new file mode 100644 index 0000000..bae310f --- /dev/null +++ b/ros_queue_experiments/.gitignore @@ -0,0 +1,2 @@ +experiment_bags +scripts/__pycache__ \ No newline at end of file diff --git a/ros_queue_experiments/CMakeLists.txt b/ros_queue_experiments/CMakeLists.txt new file mode 100644 index 0000000..2fb6fc0 --- /dev/null +++ b/ros_queue_experiments/CMakeLists.txt @@ -0,0 +1,268 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ros_queue_experiments) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS + queue_controller + queue_server + ros_queue_msgs + roscpp + message_generation + actionlib + actionlib_msgs + message_filters + ros_boosted_utilities +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +## To declare and build messages, services or actions from within this +## package, follow these steps: +## * Let MSG_DEP_SET be the set of packages whose message types you use in +## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). +## * In the file package.xml: +## * add a build_depend tag for "message_generation" +## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET +## * If MSG_DEP_SET isn't empty the following dependency has been pulled in +## but can be declared for certainty nonetheless: +## * add a exec_depend tag for "message_runtime" +## * In this file (CMakeLists.txt): +## * add "message_generation" and every package in MSG_DEP_SET to +## find_package(catkin REQUIRED COMPONENTS ...) +## * add "message_runtime" and every package in MSG_DEP_SET to +## catkin_package(CATKIN_DEPENDS ...) +## * uncomment the add_*_files sections below as needed +## and list every .msg/.srv/.action file to be processed +## * uncomment the generate_messages entry below +## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) + +## Generate messages in the 'msg' folder +add_message_files( + FILES + ActionPerformance.msg + AuvStates.msg + MetricPerformance.msg +) + +## Generate services in the 'srv' folder +add_service_files( + FILES + GetRealAUVStates.srv + SendNewAUVCommand.srv +) + +## Generate actions in the 'action' folder +# add_action_files( +# FILES +# Action1.action +# Action2.action +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs # Or other packages containing msgs + ros_queue_msgs +) + +################################################ +## Declare ROS dynamic reconfigure parameters ## +################################################ + +## To declare antransmissiond build dynamic reconfigure parameters within this +## package, follow these steps: +## * In the file package.xml: +## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" +## * In this file (CMakeLists.txt): +## * add "dynamic_reconfigure" to +## find_package(catkin REQUIRED COMPONENTS ...) +## * uncomment the "generate_dynamic_reconfigure_options" section below +## and list every .cfg file to be processed + +## Generate dynamic reconfigure parameters in the 'cfg' folder +# generate_dynamic_reconfigure_options( +# cfg/DynReconf1.cfg +# cfg/DynReconf2.cfg +# ) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## 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 ros_queue_experiments + CATKIN_DEPENDS queue_controller queue_server ros_queue_msgs roscpp +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + ${catkin_INCLUDE_DIRS} + include +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/ros_queue_experiments.cpp +# ) + +## Add cmake target dependencies of the library +## as an example, code may need to be generated before libraries +## either from message generation or dynamic reconfigure +# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +## With catkin_make all packages are built within a single CMake context +## The recommended prefix ensures that target names across packages don't collide + +add_executable(performance_monitor_node + src/performance_monitor/performance_monitor_node.cpp + src/performance_monitor/metric_monitor.cpp + src/performance_monitor/action_monitor.cpp + src/performance_monitor/localization_monitor.cpp + src/performance_monitor/low_temperature_monitor.cpp + src/performance_monitor/penalty_monitor.cpp + src/performance_monitor/real_queue_monitor.cpp + src/performance_monitor/temperature_monitor.cpp + src/performance_monitor/virtual_queue_monitor.cpp + src/auv_states.cpp +) + +add_executable(AUV_system_node + src/auv_system.cpp + src/auv_node.cpp + src/auv_states.cpp + src/metrics/dual_metric_services.cpp + src/metrics/low_temperature_services.cpp + src/metrics/metric_services.cpp + src/auv_state_manager.cpp + src/metrics/temperature_services.cpp + src/metrics/renewal_time_services.cpp + src/metrics/localization_services.cpp + src/metrics/task_publisher.cpp + src/metrics/penalty_services.cpp + src/proportion_time_spent.cpp +) + +add_executable(perturbation_action_node + src/disturbed_action_server_node.cpp + src/disturbed_action_server.cpp + src/auv_states.cpp +) + +add_executable(service_tester_node + src/test_services.cpp + src/auv_states.cpp +) + +## Rename C++ executable without prefix +## The above recommended prefix causes long target names, the following renames the +## target back to the shorter version for ease of user use +## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" +#set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") + +## Add cmake target dependencies of the executable +## same as for the library above +add_dependencies(AUV_system_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(service_tester_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(perturbation_action_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +add_dependencies(performance_monitor_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(AUV_system_node + ${catkin_LIBRARIES} +) + +target_link_libraries(service_tester_node + ${catkin_LIBRARIES} +) + +target_link_libraries(perturbation_action_node + ${catkin_LIBRARIES} +) + +target_link_libraries(performance_monitor_node + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# catkin_install_python(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html +# install(TARGETS ${PROJECT_NAME}_node +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark libraries for installation +## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html +# install(TARGETS ${PROJECT_NAME} +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_ros_queue_experiments.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/ros_queue_experiments/config/experiment_setups/bad_prediction_model/metric_config.yaml b/ros_queue_experiments/config/experiment_setups/bad_prediction_model/metric_config.yaml new file mode 100644 index 0000000..7647223 --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/bad_prediction_model/metric_config.yaml @@ -0,0 +1,116 @@ +temp_target: 40.0 +low_temp_target: 0.0 +temp: + - real_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + - prediction_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + +localization_target: 10.0 +localization: + - real_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + - prediction_model: + - TaskZone: + - loc_uncertainty: 10.5 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 8.4 # cm + +# The renewal time is the time of exploiting a zone. The difference between the real model and +# is due to timing errors in the system. +renewal_time: + - real_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + - prediction_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + +# The penalty represents the cost in energy (joule) to change from one state to another. +penalty: + - real_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + - prediction_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + +arrival_task_per_second: 8.0 +task_metrics: + - real_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second + - prediction_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/bad_prediction_model/perturbation_config.yaml b/ros_queue_experiments/config/experiment_setups/bad_prediction_model/perturbation_config.yaml new file mode 100644 index 0000000..d7765bc --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/bad_prediction_model/perturbation_config.yaml @@ -0,0 +1,2 @@ +pertubation_at_each_x_control_steps: 0 # If 0, no perturbation will be applied +pertubation_type: "not_moving" # "not_moving" or "offset_next_step" \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/metric_config.yaml b/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/metric_config.yaml new file mode 100644 index 0000000..1a09957 --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/metric_config.yaml @@ -0,0 +1,116 @@ +temp_target: 40.0 +low_temp_target: 0.0 +temp: + - real_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + - prediction_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + +localization_target: 10.0 +localization: + - real_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + - prediction_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + +# The renewal time is the time of exploiting a zone. The difference between the real model and +# is due to timing errors in the system. +renewal_time: + - real_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + - prediction_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + +# The penalty represents the cost in energy (joule) to change from one state to another. +penalty: + - real_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + - prediction_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + +arrival_task_per_second: 8.0 +task_metrics: + - real_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second + - prediction_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/perturbation_config.yaml b/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/perturbation_config.yaml new file mode 100644 index 0000000..d7765bc --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/perfect_model_and_setup/perturbation_config.yaml @@ -0,0 +1,2 @@ +pertubation_at_each_x_control_steps: 0 # If 0, no perturbation will be applied +pertubation_type: "not_moving" # "not_moving" or "offset_next_step" \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/perturbation_on_action/metric_config.yaml b/ros_queue_experiments/config/experiment_setups/perturbation_on_action/metric_config.yaml new file mode 100644 index 0000000..1a09957 --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/perturbation_on_action/metric_config.yaml @@ -0,0 +1,116 @@ +temp_target: 40.0 +low_temp_target: 0.0 +temp: + - real_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + - prediction_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + +localization_target: 10.0 +localization: + - real_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + - prediction_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + +# The renewal time is the time of exploiting a zone. The difference between the real model and +# is due to timing errors in the system. +renewal_time: + - real_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + - prediction_model: + - fromTaskZone: + - toTaskZone: 1.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #t + - fromHighLocZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 1.0 #ts + +# The penalty represents the cost in energy (joule) to change from one state to another. +penalty: + - real_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + - prediction_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + +arrival_task_per_second: 8.0 +task_metrics: + - real_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second + - prediction_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/perturbation_on_action/perturbation_config.yaml b/ros_queue_experiments/config/experiment_setups/perturbation_on_action/perturbation_config.yaml new file mode 100644 index 0000000..8b8f8cb --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/perturbation_on_action/perturbation_config.yaml @@ -0,0 +1,2 @@ +pertubation_at_each_x_control_steps: 3 # If 0, no perturbation will be applied +pertubation_type: "offset_next_step" # "not_moving" or "offset_next_step" \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/time_uncertainties/metric_config.yaml b/ros_queue_experiments/config/experiment_setups/time_uncertainties/metric_config.yaml new file mode 100644 index 0000000..448ac1e --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/time_uncertainties/metric_config.yaml @@ -0,0 +1,116 @@ +temp_target: 40.0 +low_temp_target: 0.0 +temp: + - real_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + - prediction_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + +localization_target: 10.0 +localization: + - real_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + - prediction_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + +# The renewal time is the time of exploiting a zone. The difference between the real model and +# is due to timing errors in the system. +renewal_time: + - real_model: + - fromTaskZone: + - toTaskZone: 2.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #t + - fromHighLocZone: + - toTaskZone: 2.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #ts + - prediction_model: + - fromTaskZone: + - toTaskZone: 1.5 #ts + - toHighLocZone: 0.5 #t + - toColdZone: 0.1 #t + - fromHighLocZone: + - toTaskZone: 1.5 #t + - toHighLocZone: 0.5 #ts + - toColdZone: 0.5 #t + - fromColdZone: + - toTaskZone: 0.5 #t + - toHighLocZone: 0.5 #t + - toColdZone: 0.1 #ts + +# The penalty represents the cost in energy (joule) to change from one state to another. +penalty: + - real_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + - prediction_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + +arrival_task_per_second: 8.0 +task_metrics: + - real_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second + - prediction_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/time_uncertainties/perturbation_config.yaml b/ros_queue_experiments/config/experiment_setups/time_uncertainties/perturbation_config.yaml new file mode 100644 index 0000000..d7765bc --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/time_uncertainties/perturbation_config.yaml @@ -0,0 +1,2 @@ +pertubation_at_each_x_control_steps: 0 # If 0, no perturbation will be applied +pertubation_type: "not_moving" # "not_moving" or "offset_next_step" \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/metric_config.yaml b/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/metric_config.yaml new file mode 100644 index 0000000..214c9fa --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/metric_config.yaml @@ -0,0 +1,116 @@ +temp_target: 40.0 +low_temp_target: 0.0 +temp: + - real_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + - prediction_model: + - TaskZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 0.0 # Celcius Degree per second + - HighLocZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 4.0 # Celcius Degree per second + - ColdZone: + - increase: 5.0 # Celcius Degree per second + - decrease: 15.0 # Celcius Degree per second + +localization_target: 10.0 +localization: + - real_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + - prediction_model: + - TaskZone: + - loc_uncertainty: 15.0 # cm + - HighLocZone: + - loc_uncertainty: 1.0 # cm + - ColdZone: + - loc_uncertainty: 12.0 # cm + +# The renewal time is the time of exploiting a zone. The difference between the real model and +# is due to timing errors in the system. +renewal_time: + - real_model: + - fromTaskZone: + - toTaskZone: 2.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #t + - fromHighLocZone: + - toTaskZone: 2.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #ts + - prediction_model: + - fromTaskZone: + - toTaskZone: 2.0 #ts + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #t + - fromHighLocZone: + - toTaskZone: 2.0 #t + - toHighLocZone: 1.0 #ts + - toColdZone: 1.0 #t + - fromColdZone: + - toTaskZone: 1.0 #t + - toHighLocZone: 1.0 #t + - toColdZone: 0.5 #ts + +# The penalty represents the cost in energy (joule) to change from one state to another. +penalty: + - real_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + - prediction_model: + - fromTaskZone: + - toTaskZone: 0.0 + - toHighLocZone: 2.0 + - toColdZone: 2.0 + - fromHighLocZone: + - toTaskZone: 2.0 + - toHighLocZone: 0.0 + - toColdZone: 1.0 + - fromColdZone: + - toTaskZone: 2.0 + - toHighLocZone: 1.0 + - toColdZone: 0.0 + +arrival_task_per_second: 8.0 +task_metrics: + - real_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second + - prediction_model: + - TaskZone: + - departing_tasks_rate: 25.0 # Tasks per second + - HighLocZone: + - departing_tasks_rate: 0.0 # Tasks per second + - ColdZone: + - departing_tasks_rate: 0.0 # Tasks per second \ No newline at end of file diff --git a/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/perturbation_config.yaml b/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/perturbation_config.yaml new file mode 100644 index 0000000..d7765bc --- /dev/null +++ b/ros_queue_experiments/config/experiment_setups/variable_time_action_setup/perturbation_config.yaml @@ -0,0 +1,2 @@ +pertubation_at_each_x_control_steps: 0 # If 0, no perturbation will be applied +pertubation_type: "not_moving" # "not_moving" or "offset_next_step" \ No newline at end of file diff --git a/ros_queue_experiments/config/min_drift-plus-penalty.yaml b/ros_queue_experiments/config/min_drift-plus-penalty.yaml new file mode 100644 index 0000000..93dd0c6 --- /dev/null +++ b/ros_queue_experiments/config/min_drift-plus-penalty.yaml @@ -0,0 +1,183 @@ +# Configuration of a min drift-plus-penalty controller + +# Indicate the type of controller. Supported types: min_drift_plus_penalty, renewal_min_drift_plus_penalty +controller_type: "$(arg controller_type)" + +# 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. Default set to true +is_periodic: true + +# Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller. 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: $(arg inverse_control_and_steps) + +# 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: true + +# 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 +# Seconds in float value. +max_renewal_time: 2.5 + +# Name of the actionlib server to send the action to. +action_server_name: "perturbation_node/disturbed_action_server" + +# Solution space service that provides a list of all available actions. +solution_space_service_name: "auv_system_node/potential_action_set" + +# Name of the service to evaluate the expected renewal time for all actions. +expected_renewal_time_service_name: "auv_system_node/renewal_time/expected_metric" + +# ====== Queue configurations. Indicate all the queues used in the controllers equation. ====== +# Most of the queues' information will be fetch automatically through the parameters of the indicated queue_server. +# Name of the queue_server linked to the controller. Should be given in absolute path. +queue_server_name: "$(arg server_name)" +# List all the queues used from the queue server. The queue controller will try to stabilize all those queues. An arbitrary number of queues can be added. +queue_list: + # The name of the queue. Should match a queue name from the queue_server + - TemperatureQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 100.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/temperature/arrival/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/temperature/departure/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: $(arg is_target_renewal_dependent) + + - LowTemperatureQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 100.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/low_temperature/arrival/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: $(arg is_target_renewal_dependent) + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/low_temperature/departure/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: false + + - LocalizationQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 100.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/localization/arrival/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/localization/departure/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: $(arg is_target_renewal_dependent) + + - TaskQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 5.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/task/arrival/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/task/departure/$(arg rate_or_change)/expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: false + +# ====== Variable (penalty) to minimize configurations.====== +# Name of the service to evaluate the penalty based on the current state and the potential action. +penalty_service_name: "auv_system_node/penalty/expected_metric" +# The V parameter that is multplied with the penalty (like a weight). When a high penalty metric also increases the queue size, +# this parameter balances the tradeoff between a lower penalty or lower queue delays. A high V will prioritize the minimization +# of the penalty while a low V will prioritize low queue sizes and delays. +v_parameter: 1000.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 + +# ====== 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 or commenting it means the +# controller is not dependent on another controller. +dependent_on_controller_topic: "optimal_controller/optimization_done" \ No newline at end of file diff --git a/ros_queue_experiments/config/min_drift-plus-penalty_perfect_model.yaml b/ros_queue_experiments/config/min_drift-plus-penalty_perfect_model.yaml new file mode 100644 index 0000000..c7000b0 --- /dev/null +++ b/ros_queue_experiments/config/min_drift-plus-penalty_perfect_model.yaml @@ -0,0 +1,183 @@ +# Configuration of a min drift-plus-penalty controller + +# Indicate the type of controller. Supported types: min_drift_plus_penalty, renewal_min_drift_plus_penalty +controller_type: "renewal_min_drift_plus_penalty" + +# 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. Default set to true +is_periodic: false + +# Period between each controller optimization process. Only used for the min_drift_plus_penalty type of controller. 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: $(arg inverse_control_and_steps) + +# 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: false + +# 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: true + +# 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 +# Seconds in float value. +max_renewal_time: 2.5 + +# Name of the actionlib server to send the action to. +action_server_name: "optimal_action_node/disturbed_action_server" + +# Solution space service that provides a list of all available actions. +solution_space_service_name: "auv_system_node/potential_action_set" + +# Name of the service to evaluate the expected renewal time for all actions. +expected_renewal_time_service_name: "auv_system_node/renewal_time/real_expected_metric" + +# ====== Queue configurations. Indicate all the queues used in the controllers equation. ====== +# Most of the queues' information will be fetch automatically through the parameters of the indicated queue_server. +# Name of the queue_server linked to the controller. Should be given in absolute path. +queue_server_name: "$(arg server_name)" +# List all the queues used from the queue server. The queue controller will try to stabilize all those queues. An arbitrary number of queues can be added. +queue_list: + # The name of the queue. Should match a queue name from the queue_server + - TemperatureQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 1000.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/temperature/arrival/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/temperature/departure/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: true + + - LowTemperatureQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 1000.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/low_temperature/arrival/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: true + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/low_temperature/departure/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: false + + - LocalizationQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 1000.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/localization/arrival/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/localization/departure/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: true + + - TaskQueue: + # Weight in the cost function multplied with the size of the queue. + # A higher value means the controller will try to stabilize it faster compared to the other queues. + # It could also be set as a normalization factor. + - weight: 1.0 + + # Name of the service to evaluate the expected arrivals of the queue based. + - expected_arrival_service_name: "auv_system_node/task/arrival/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - arrival_action_dependent: true + # If set to true, the arrival will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - arrival_renewal_dependent: false + + # Name of the service to evaluate the expected departures of the queue based on the evaluated action. + - expected_departure_service_name: "auv_system_node/task/departure/change/real_expected_metric" + # If set to false, the arrival service request will be empty and use the srv ros_queue_msgs/FloatRequest. Default value. + # If set to true, the arrival service request will use the services specified by the template of the controller in its cpp file. + - departure_action_dependent: true + # If set to true, the departures will be multiplied by the expected renewal time. In renewal systems, it's mainly used + # when the metric needs to scale with time like a static time average goal. Only used for the renewal_min_drift_plus_penalty + # type of controller. + - departure_renewal_dependent: false + +# ====== Variable (penalty) to minimize configurations.====== +# Name of the service to evaluate the penalty based on the current state and the potential action. +penalty_service_name: "auv_system_node/penalty/real_expected_metric" +# The V parameter that is multplied with the penalty (like a weight). When a high penalty metric also increases the queue size, +# this parameter balances the tradeoff between a lower penalty or lower queue delays. A high V will prioritize the minimization +# of the penalty while a low V will prioritize low queue sizes and delays. +v_parameter: 100.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 + +# ====== 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: "queue_controller/control_loop_started" + +# 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: "monitoring_node/monitoring_done" \ No newline at end of file diff --git a/ros_queue_experiments/config/queue_server_config.yaml b/ros_queue_experiments/config/queue_server_config.yaml new file mode 100644 index 0000000..0427220 --- /dev/null +++ b/ros_queue_experiments/config/queue_server_config.yaml @@ -0,0 +1,70 @@ +# Template a of configurations for the queue server. See the queue_config.yaml to copy the configuration of queues. +queue_server_name: "$(arg server_name)" + +# The frequency in loops/sec at which the queue sizes will be published and the real queue will be checked for transmission. +server_spin_rate: 20.0 + +# When true, queues will publish a topic with their time average metrics (queue size, arrivals and departures). +# Default is false. +compute_statistics: true + +queue_list: + - TemperatureQueue: # Queue name + # Existing types of queues: + # - real_queue: Queues that store ROS messages and its size is equal to sum of all its messages in bytes. + # - inequality_constraint_virtual_queue: Virtual queues that can't go below a 0 value. + # - equality_constraint_virtual_queue: Virtual queues that can go below a 0 value. + - type_of_queue: "inequality_constraint_virtual_queue" + + # A float value for virtual queues and value in bytes for real_queues. Don't forget to put a decimal, otherwise an error might occur. + - max_queue_size: 100000.0 + + # === Services calls for virtual queues to evaluate the change in their size. Only used for virtual queues. === + - arrival_evaluation_service_name: "auv_system_node/temperature/arrival/$(arg rate_or_change)/real_metric" + - departure_evaluation_service_name: "auv_system_node/temperature/departure/$(arg rate_or_change)/real_metric" + + - LowTemperatureQueue: # Queue name + # Existing types of queues: + # - real_queue: Queues that store ROS messages and its size is equal to sum of all its messages in bytes. + # - inequality_constraint_virtual_queue: Virtual queues that can't go below a 0 value. + # - equality_constraint_virtual_queue: Virtual queues that can go below a 0 value. + - type_of_queue: "inequality_constraint_virtual_queue" + + # A float value for virtual queues and value in bytes for real_queues. Don't forget to put a decimal, otherwise an error might occur. + - max_queue_size: 100000.0 + + # === Services calls for virtual queues to evaluate the change in their size. Only used for virtual queues. === + - arrival_evaluation_service_name: "auv_system_node/low_temperature/arrival/$(arg rate_or_change)/real_metric" + - departure_evaluation_service_name: "auv_system_node/low_temperature/departure/$(arg rate_or_change)/real_metric" + + - LocalizationQueue: # Queue name + # Existing types of queues: + # - real_queue: Queues that store ROS messages and its size is equal to sum of all its messages in bytes. + # - inequality_constraint_virtual_queue: Virtual queues that can't go below a 0 value. + # - equality_constraint_virtual_queue: Virtual queues that can go below a 0 value. + - type_of_queue: "inequality_constraint_virtual_queue" + + # A float value for virtual queues and value in bytes for real_queues. Don't forget to put a decimal, otherwise an error might occur. + - max_queue_size: 100000.0 + + # === Services calls for virtual queues to evaluate the change in their size. Only used for virtual queues. === + - arrival_evaluation_service_name: "auv_system_node/localization/arrival/$(arg rate_or_change)/real_metric" + - departure_evaluation_service_name: "auv_system_node/localization/departure/$(arg rate_or_change)/real_metric" + + - TaskQueue: # Queue name + # Existing types of queues: + # - real_queue: Queues that store ROS messages and its size is equal to sum of all its messages in bytes. + # - inequality_constraint_virtual_queue: Virtual queues that can't go below a 0 value. + # - equality_constraint_virtual_queue: Virtual queues that can go below a 0 value. + - type_of_queue: "real_queue" + + # A float value for virtual queues and value in bytes for real_queues. Don't forget to put a decimal, otherwise an error might occur. + - max_queue_size: 6250000.0 # 50 mb + + # === Topic name from which data will be stored and on which to publish data. Only used for real queues. === + - arrival_topic_name: "auv_system_node/incoming_tasks" + - tranmission_topic_name: "departing_tasks" + + # === Service name that allows a real queue how much data in bytes it could transmit. Only used for real queues. === + - transmission_evaluation_service_name: "auv_system_node/qos_transmission" + diff --git a/ros_queue_experiments/include/ros_queue_experiments/auv_forward_declarations.hpp b/ros_queue_experiments/include/ros_queue_experiments/auv_forward_declarations.hpp new file mode 100644 index 0000000..5a39e43 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/auv_forward_declarations.hpp @@ -0,0 +1,4 @@ +#pragma once + +class AUVStateManager; +class AUVSystem; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/auv_state_manager.hpp b/ros_queue_experiments/include/ros_queue_experiments/auv_state_manager.hpp new file mode 100644 index 0000000..e9b8474 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/auv_state_manager.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include + +#include "auv_forward_declarations.hpp" + +#include "proportion_time_spent.hpp" + +#include "auv_states.hpp" +#include "ros_queue_experiments/AuvStates.h" + +class AUVStateManager +{ + public: + + /** + * @brief Initialization of the states. + */ + AUVStateManager(AUVSystem* auv_system); + + /** + * @brief Get the current states of the AUV. + * It will the state at the start of the last transition et extrapolate the current states. + * + * @return AUVStates::Zones + */ + ros_queue_experiments::AuvStates getCurrentStates(); + + /** + * @brief Set the auv to a new zone and take a snapshot of the current states. + * + * @param new_zone Zone to transition to from the real action applied. + * @param command_of_controller Command that the controller would have applied without perturbation. + */ + void commandToNextZone(AUVStates::Zones new_zone, ros_queue_msgs::TransmissionVector command_of_controller); + + private: + + /** + * @brief Time of when the last action was performed. + */ + ros::Time last_transition_time_; + + /** + * @brief Current zone that the AUV is in currently and states at the moment of transitions. + */ + ros_queue_experiments::AuvStates states_at_last_transition_; + + /** + * @brief Mutex to protect the current zone. + */ + std::mutex state_access_mutex_; + + /** + * @brief Pointer to the AUVSystem class to have access to the metrics. + */ + AUVSystem* auv_system_; + + /** + * @brief Object that keeps track of the time spent in each zone. + */ + ProportionTimeSpent time_spent_in_zones_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/auv_states.hpp b/ros_queue_experiments/include/ros_queue_experiments/auv_states.hpp new file mode 100644 index 0000000..1b7401e --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/auv_states.hpp @@ -0,0 +1,31 @@ +#pragma once +#include + +namespace AUVStates +{ + /** + * @brief Possible zones (states) that the AUV can be in. The count enumerator is not a zone + * and is only used to know the number of states there is. + */ + enum Zones + { + TaskZone =0, + HighLocZone, + ColdZone, + count + }; + + /** + * @brief Decode the corresponding zone (state) from the transmission vector. + * @param transmission_vector Transmission vector to decode the zone from. + * @return Corresponding zone (state) from the transmission vector. + */ + AUVStates::Zones getZoneFromTransmissionVector(const ros_queue_msgs::TransmissionVector& transmission_vector); + + /** + * @brief Get the equivalent transmission vector for a given zone. + * @param zone Zone (state) to get the equivalent transmission vector for. + * @return Equivalent transmission vector for the given zone. + */ + ros_queue_msgs::TransmissionVector getTransmissionVectorFromZone(AUVStates::Zones zone); +} \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/auv_system.hpp b/ros_queue_experiments/include/ros_queue_experiments/auv_system.hpp new file mode 100644 index 0000000..0c4e903 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/auv_system.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include "auv_forward_declarations.hpp" + +#include "ros/ros.h" + +#include + +#include "ros_queue_experiments/metrics/low_temperature_services.hpp" +#include "ros_queue_experiments/metrics/metric_services.hpp" +#include "ros_queue_experiments/metrics/temperature_services.hpp" +#include "ros_queue_experiments/metrics/renewal_time_services.hpp" +#include "ros_queue_experiments/metrics/localization_services.hpp" +#include "ros_queue_experiments/metrics/task_publisher.hpp" +#include "ros_queue_experiments/metrics/penalty_services.hpp" + +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_experiments/GetRealAUVStates.h" +#include "ros_queue_experiments/SendNewAUVCommand.h" + +#include "ros_queue_msgs/PotentialTransmissionVectorSpaceFetch.h" + +#include "std_msgs/Empty.h" + +class AUVSystem +{ + public: + AUVSystem(ros::NodeHandle& nh); + + // Metric service servers + std::shared_ptr penalty_metric_services_; + std::shared_ptr expected_time_services_; + std::shared_ptr temperature_services_; + std::shared_ptr low_temperature_services_; + std::shared_ptr localization_services_; + std::shared_ptr task_services_; + + private: + /** + * @brief Current states of the auv and its metrics. + */ + std::shared_ptr auv_state_manager_; + + /** + * @brief ROS private node handle to manage the servers. + */ + ros::NodeHandle nh_; + + + /** + * @brief ROS node handle with namesapce name resolution. + */ + ros::NodeHandle ns_nh_; + + /** + * @brief ROS publisher for the AUV state. + */ + ros::Publisher state_pub_; + + /** + * @brief Service to get all the possible actions + */ + ros::ServiceServer action_set_service_; + + /** + * @brief Callback the service to return all the possible actions + */ + bool potentialActionSetCallback(ros_queue_msgs::PotentialTransmissionVectorSpaceFetch::Request& req, + ros_queue_msgs::PotentialTransmissionVectorSpaceFetch::Response& res); + + /** + * @brief Service to get the current internal states of the UAV. + */ + ros::ServiceServer auv_state_service_; + + /** + * @brief Callback the service to return the current internal states of the UAV. + * @param req Request to get the current states of the UAV. Is empty. + * @param res Response to get the current states of the UAV. + */ + bool auvStateCallback(ros_queue_experiments::GetRealAUVStates::Request& req, + ros_queue_experiments::GetRealAUVStates::Response& res); + + /** + * @brief Subscriber that listens to the starting step of the controller to publish the auv states for the + * monitoring system. + */ + ros::Subscriber control_started_sub_; + + /** + * @brief Callback called when the controller has started (right after sending its updates to the queue server + * of step inverted controller) and that publishes auv state msg for the monitoring. This moment is the best for + * sampling the auv state for monitoring. + * @param msg Empyt message which is the signal used by queue controller to signal the beginning of its loop. + * @return True if the callback was successful. Which is always true in this case. + */ + void stateMonitoringCallback(const std_msgs::Empty::ConstPtr& msg); + + /** + * @brief Service to set the new command to the system. + */ + ros::ServiceServer command_service_; + + /** + * @brief Callback the service to set the new command to the system and publish the auv states. + * @param req Request to set the new command to the system. + * @param res Return the time to complete the command. + */ + bool commandCallback(ros_queue_experiments::SendNewAUVCommand::Request& req, + ros_queue_experiments::SendNewAUVCommand::Response& res); +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/disturbed_action_server.hpp b/ros_queue_experiments/include/ros_queue_experiments/disturbed_action_server.hpp new file mode 100644 index 0000000..a509e93 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/disturbed_action_server.hpp @@ -0,0 +1,111 @@ +#pragma once + +#include +#include + +#include "ros/ros.h" +#include "ros_boosted_utilities/persistent_service_client.hpp" +#include "actionlib/server/simple_action_server.h" + +#include "ros_queue_msgs/TransmissionVectorAction.h" +#include "ros_queue_msgs/QueueServerStatsFetch.h" +#include "ros_queue_experiments/GetRealAUVStates.h" +#include "ros_queue_experiments/SendNewAUVCommand.h" + +class DisturbedActionServer +{ + public: + DisturbedActionServer(ros::NodeHandle& nh); + + private: + enum class PerturbationType + { + NotMoving =0, + OffsetPlusOne + }; + + /** + * @brief ROS node handle to manage the servers. + */ + ros::NodeHandle nh_; + + /** + * @brief Private ROS node handle to manage the servers. + */ + ros::NodeHandle nhp_; + + /** + * @brief Action server to receive the action from the queue controller. + */ + actionlib::SimpleActionServer action_server_; + + /** + * @brief Callaback when a action is received. It adds a perturbation if needed and send it to the AUV system. + */ + void executeActionCallback(const ros_queue_msgs::TransmissionVectorGoalConstPtr& goal); + + /** + * @brief Callback when a command is received from the queue controller. + * It sets the states of the AUV system and waits for the renewal. If a new goal is received + * while the old one is still active, the old one is aborted. + */ + void commandReceivedCallback(); + + /** + * @brief Callback when a preempt is received from the queue controller. + */ + void preemptActionCallback(); + + /** + * @brief Control steps between each pertubations. + */ + int perturbation_at_each_x_control_steps_ = 0; + + /** + * @brief Timer that sends a sucess for the current action based on the renewal the last renewal received. + * It's reset a each new received action. + */ + ros::Timer send_sucess_timer_; + + /** + * @brief Callback called when the timer expires. It sends a success to the queue controller for the current action. + * @param event Timer event. + */ + void sendSuccessCallback(const ros::TimerEvent& event); + + /** + * @brief Number of steps since the last perturbation. + */ + int steps_since_last_perturbation_ = 0; + + /** + * @brief Type of perturbation. + */ + PerturbationType perturbation_type_ = PerturbationType::NotMoving; + + /** + * @brief Service client to get the current AUV states + */ + PersistentServiceClient auv_state_client_; + + /** + * @brief Service client to send the disturbed action to the AUV system. + */ + PersistentServiceClient auv_action_client_; + + /** + * @brief Service client to get the queue server current stats. + */ + PersistentServiceClient queue_stats_client_; + + /** + * @brief ROS publisher to indicated the desired and the real taken actions. + */ + ros::Publisher action_performance_publisher_; + + /** + * @brief Flag that indicates if the action server is dummy. + * If it's dummy, no services will be defined to send over a auv system + */ + bool is_dummy_ = false; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/dual_metric_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/dual_metric_services.hpp new file mode 100644 index 0000000..ae4d2de --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/dual_metric_services.hpp @@ -0,0 +1,116 @@ +#pragma once + +#include "ros/ros.h" + +#include +#include + +#include "ros_queue_experiments/auv_state_manager.hpp" +#include "ros_queue_experiments/metrics/renewal_time_services.hpp" + +#include +#include +#include +#include + + +class DualMetricServices +{ + public: + DualMetricServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services); + + protected: + std::string metric_name_; + + ros::NodeHandle nh_; + + ros::NodeHandle ns_nh_; + + std::shared_ptr renewal_time_services_; + + ros::ServiceClient real_renewal_service_; + + // Change services + ros::ServiceServer real_arrival_metric_service_; + ros::ServiceServer real_arrival_prediction_service_; + ros::ServiceServer expected_arrival_metric_service_; + + ros::ServiceServer real_departure_metric_service_; + ros::ServiceServer real_departure_prediction_service_; + ros::ServiceServer expected_departure_metric_service_; + + // Rates services + ros::ServiceServer real_arrival_rate_metric_service_; + ros::ServiceServer expected_arrival_rate_metric_service_; + + ros::ServiceServer real_departure_rate_metric_service_; + ros::ServiceServer expected_departure_rate_metric_service_; + + std::shared_ptr auv_state_manager_; + + // Change callbacks + virtual bool realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) = 0; + + virtual bool realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + virtual bool expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + virtual bool realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) = 0; + + virtual bool realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + virtual bool expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + // Rate callbacks + virtual bool realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) = 0; + + virtual bool expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + virtual bool realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) = 0; + + virtual bool expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + ros_queue_experiments::AuvStates getCurrentStates(); + private: + // Change callbacks + bool realArrivalServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + bool realArrivalPredictionServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + bool expectedArrivalServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + bool realDepartureServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + bool realDeparturePredictionServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + bool expectedDepartureServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + // Rate callbacks + bool realArrivalRateServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + bool expectedArrivalRateServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + bool realDepartureRateServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + bool expectedDepartureRateServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/localization_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/localization_services.hpp new file mode 100644 index 0000000..c7860b0 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/localization_services.hpp @@ -0,0 +1,58 @@ +#pragma once + +#include +#include + +#include "ros_queue_experiments/auv_states.hpp" + +#include "dual_metric_services.hpp" + +using std::string; + +class LocalizationServices: public DualMetricServices +{ + public: + LocalizationServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services); + + float getRealLocalizationUncertainty(AUVStates::Zones zone); + + protected: + // Change callbacks + virtual bool realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + // Rate callbacks + virtual bool realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + + private: + std::map predicted_localization_uncertainties_; + std::map real_localization_uncertainties_; + + float localization_target_ = 0.0f; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/low_temperature_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/low_temperature_services.hpp new file mode 100644 index 0000000..e02787f --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/low_temperature_services.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include "ros/ros.h" +#include "temperature_services.hpp" + +class LowTemperatureServices: public TemperatureServices +{ + public: + LowTemperatureServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services); + + protected: + virtual bool realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + // Rate callbacks + virtual bool realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/metric_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/metric_services.hpp new file mode 100644 index 0000000..878a260 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/metric_services.hpp @@ -0,0 +1,52 @@ +#pragma once + +#include "ros/ros.h" + +#include +#include + +#include "ros_queue_experiments/auv_state_manager.hpp" + +#include +#include +#include + + +class MetricServices +{ + public: + MetricServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager); + + protected: + std::string metric_name_; + + ros::NodeHandle nh_; + + ros::NodeHandle ns_nh_; + + ros::ServiceServer real_metric_service_; + ros::ServiceServer real_expected_metric_service_; + ros::ServiceServer expected_metric_service_; + + std::shared_ptr auv_state_manager_; + + virtual bool realMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) = 0; + + virtual bool realExpectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + virtual bool expectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) = 0; + + ros_queue_experiments::AuvStates getCurrentStates(); + private: + bool realServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + bool realExpectedServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + bool expectedServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/penalty_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/penalty_services.hpp new file mode 100644 index 0000000..3e8a347 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/penalty_services.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include + +#include "ros/ros.h" + +#include "metric_services.hpp" + +#include "ros_queue_experiments/auv_states.hpp" +#include "ros_queue_experiments/auv_state_manager.hpp" + +class PenaltyServices: public MetricServices +{ + public: + PenaltyServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager); + + float getRealPenaltyTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone); + + float getPredictedPenaltyTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone); + protected: + + virtual bool realMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realExpectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + + private: + float real_penalty_transitions_[AUVStates::Zones::count][AUVStates::Zones::count]; + float predicted_penalty_transitions_[AUVStates::Zones::count][AUVStates::Zones::count]; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/renewal_time_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/renewal_time_services.hpp new file mode 100644 index 0000000..0edf6ea --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/renewal_time_services.hpp @@ -0,0 +1,36 @@ +#pragma once + +#include + +#include "ros_queue_experiments/auv_states.hpp" + +#include "metric_services.hpp" + +using std::string; + +class RenewalTimeServices: public MetricServices +{ + public: + + RenewalTimeServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager); + + float getRealRenewalTimeWithStateTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone); + float getPredictedRenewalTimeWithStateTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone); + + float getRealRenewalTimeWithTransitionFromCurrentState(AUVStates::Zones to_zone); + float getPredictedRenewalTimeWithTransitionFromCurrentState(AUVStates::Zones to_zone); + + protected: + virtual bool realMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realExpectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + private: + float real_renewal_time_transitions_[AUVStates::Zones::count][AUVStates::Zones::count]; + float predicted_renewal_time_transitions_[AUVStates::Zones::count][AUVStates::Zones::count]; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/task_publisher.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/task_publisher.hpp new file mode 100644 index 0000000..d47d62e --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/task_publisher.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include +#include + +#include "ros/ros.h" + +#include "dual_metric_services.hpp" +#include "renewal_time_services.hpp" +#include "ros_queue_msgs/ByteSizeRequest.h" + +class TaskPublisher: public DualMetricServices +{ + public: + TaskPublisher(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services); + + protected: +// Change callbacks + virtual bool realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + // Rate callbacks + virtual bool realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + private: + ros::Publisher publisher_; + ros::Timer timer_; + + std::map predicted_task_departure_rates_; + std::map real_task_departure_rates_; + + void publishTask(const ros::TimerEvent& event); + + float arrival_task_per_second_ = 0.1f; + int last_task_id_ = 0; + + ros::ServiceServer qos_transmission_service_; + ros::Time last_transmission_time_; + double accumulated_untransmitted_time_ = 0.0; + bool qosTransmissionCallback(ros_queue_msgs::ByteSizeRequest::Request& req, + ros_queue_msgs::ByteSizeRequest::Response& res); +}; diff --git a/ros_queue_experiments/include/ros_queue_experiments/metrics/temperature_services.hpp b/ros_queue_experiments/include/ros_queue_experiments/metrics/temperature_services.hpp new file mode 100644 index 0000000..42bf22e --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/metrics/temperature_services.hpp @@ -0,0 +1,95 @@ +#pragma once + +#include +#include + +#include "ros_queue_experiments/auv_states.hpp" +#include "ros_queue_experiments/metrics/renewal_time_services.hpp" + +#include "dual_metric_services.hpp" + +using std::string; + +class TemperatureServices: public DualMetricServices +{ + public: + TemperatureServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services); + + float getRealArrival(AUVStates::Zones zone); + float getRealDeparture(AUVStates::Zones zone); + + protected: + // Temperature specific that could be swapped for inversed limits + // Change callbacks + virtual bool TempRealArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + virtual bool TempRealArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + virtual bool TempExpectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + virtual bool TempRealDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + virtual bool TempRealDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + virtual bool TempExpectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) ; + + // Rate callbacks + virtual bool TempRealArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + virtual bool TempExpectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + virtual bool TempRealDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res); + + virtual bool TempExpectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res); + + // Methods to override + // Change callbacks + virtual bool realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + // Rate callbacks + virtual bool realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + virtual bool realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) override; + + virtual bool expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) override; + + float temp_target_ = 0.0f; + private: + std::map expected_arrivals_; + std::map expected_departures_; + + std::map real_expected_arrivals_; + std::map real_expected_departures_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/action_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/action_monitor.hpp new file mode 100644 index 0000000..1378971 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/action_monitor.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include + +#include + +#include +#include +#include +#include "ros_queue_experiments/ActionPerformance.h" + +/** + * @brief Class that synchronizes the perturbated and optimal action performance messages and publishes the synchronized message. +*/ +class ActionMonitor +{ + public: + ActionMonitor(ros::NodeHandle& nh, const std::string& perturbated_action_topic, const std::string& optimal_action_topic); + + + private: + ros::NodeHandle nh_; + + ros::NodeHandle ns_nh_; + + ros::Publisher synchronized_action_performance_publisher_; + + message_filters::Subscriber perturbated_action_peformance_sub; + message_filters::Subscriber optimal_action_performance_sub; + + typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; + message_filters::Synchronizer sync; + + void action_messages_callbacks(const ros_queue_experiments::ActionPerformance::ConstPtr& perturbated_action_performance, + const ros_queue_experiments::ActionPerformance::ConstPtr& optimal_action_performance); + +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/localization_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/localization_monitor.hpp new file mode 100644 index 0000000..80c8ee6 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/localization_monitor.hpp @@ -0,0 +1,20 @@ +#pragma once +#include "metric_monitor.hpp" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +class LocalizationMonitor: public MetricMonitor +{ + public: + LocalizationMonitor(ros::NodeHandle& nh); + + protected: + virtual double getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg)override; + + virtual double getMetricTarget() override; + + virtual double getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) override; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/low_temperature_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/low_temperature_monitor.hpp new file mode 100644 index 0000000..e5a7073 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/low_temperature_monitor.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "metric_monitor.hpp" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +class LowTemperatureMonitor: public MetricMonitor +{ + public: + LowTemperatureMonitor(ros::NodeHandle& nh); + + protected: + virtual double getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg)override; + + virtual double getMetricTarget() override; + + virtual double getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) override; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/metric_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/metric_monitor.hpp new file mode 100644 index 0000000..4b5358c --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/metric_monitor.hpp @@ -0,0 +1,128 @@ +#pragma once + +#include +#include + +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +/** + * @brief Virtual class that monitors a specifif metric from a system and from its estimation given by the queue server. + * The class publishes the synchronized measurement from the queue server and the systems alongside their difference. + * The sampling frequency is based on the publication of the real state metric pulbishing rate. +*/ +class MetricMonitor +{ + public: + MetricMonitor(ros::NodeHandle& nh); + + /** + * @brief Method called to compute the monitored values of a metric and published the results on a + * ROS topic. + * @param msg Message containing the real state metric. + * @param queue_stats_metric_srv Message containing the estimated metrics from the queue server. + */ + void realStateMetricCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg, + const ros_queue_msgs::QueueServerStatsFetch& queue_stats_metric_srv); + + protected: + /** + * @brief Name of the metric. Used for logging. + */ + std::string metric_name_; + + /** + * @brief Private node handle for the metric monitor. + */ + ros::NodeHandle nh_; + + /** + * @brief Node handle for namespace resolution at the namespace of the node (non-private node handle). + */ + ros::NodeHandle ns_nh_; + + /** + * @brief Metric publisher. Should be defined by child class. + */ + ros::Publisher performance_metric_pub_; + + /** + * @brief Service client that gets the metric target. + */ + ros::ServiceClient metric_target_client_; + + /** + * @brief Get metric from auv state message. + * @param msg Message containing the auv state. + * @return Metric value. + */ + virtual double getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) = 0; + + /** + * @brief Get the theoritical integral of the continuous function of a given metric from the auv state message. + * @param msg Message containing the auv state. + * @return Integral of the metric. + */ + virtual double getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg)=0; + + /** + * @brief Get the time average the estimated metric from the queue server message + * @param msg Message containing the queue server stats. + * @return Time average of the metric estimated by the queue server. + */ + virtual double getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) = 0; + + /** + * @brief Get the arrival mean of the estimated metric from the queue server message. + * @param msg Message containing the queue server stats. + * @return Arrival mean of the metric estimated by the queue server. + */ + virtual double getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) = 0; + + /** + * @brief Get the metric target from the auv system. + */ + virtual double getMetricTarget() = 0; + + protected: + /** + * @brief Time at initialization. + */ + ros::Time init_time_; + + private: + /** + * @brief Compute the mean of the real state metric + * @return Mean of the real state metric. + */ + double computeRealStateMetricMean(); + + /** + * @brief Compute the time average of the real state metric. + * @return Time average of the real state metric. + */ + double computeRealStateMetricTimeAverage(); + + /** + * @brief Compute the mean from the integral of the theoritical continuous + * function of the metric by dividing it by the elapsed time since the start. + * @param time_integral_of_metric Integral of the metric. + * @return Mean of the real state metric based on its integral. + */ + double computeRealStateMetricContinuousMean(double time_integral_of_metric); + + /** + * @brief Add new sample of the real metric. + */ + void addNewRealStateMetricSample(double new_metric_value_sample); + + /** + * @brief Sum of real state metrics. + */ + double real_state_metric_sum_; + + /** + * @brief Number of real state samples. + */ + long long nb_real_state_samples_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/penalty_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/penalty_monitor.hpp new file mode 100644 index 0000000..1181454 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/penalty_monitor.hpp @@ -0,0 +1,53 @@ +#pragma once + +#include "ros/ros.h" +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +class PenaltyMonitor +{ + public: + /** + * @brief Initialized the penalty monitor by instantiating the publisher for the performance metric + * and initializing the time reference. + */ + PenaltyMonitor(ros::NodeHandle& nh); + + /** + * @brief Method called to compute real penalties and the penalties experienced by the controller. + * @param msg Message containing the real state metric. + * @param queue_stats_metric_srv Message containing the estimated metrics from the queue server. + */ + void realStateMetricCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg, + const ros_queue_msgs::QueueServerStatsFetch& queue_stats_metric_srv); + private: + /** + * @brief private node handle + */ + ros::NodeHandle pnh_; + + /** + * @brief The number of samples to compute the mean of the penalty + */ + long long nb_samples_penalty_ = 0; + + /** + * @brief The cumulative sum of all the incoming real penalties. + */ + double real_penalty_sum_ = 0.0; + + /** + * @brief The cumulative sum of all the incoming penalties that thought by the controller. + */ + double controller_penalty_sum_ = 0.0; + + /** + * @brief Time reference to compute the time average of the penalty. + */ + ros::Time init_time_; + + /** + * @brief Publisher to expose all the stats behind the penalty metric. + */ + ros::Publisher performance_metric_pub_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/real_queue_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/real_queue_monitor.hpp new file mode 100644 index 0000000..fde97f3 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/real_queue_monitor.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "ros/ros.h" +#include + +#include "ros_queue_msgs/QueueServerStats.h" + +class RealQueueMonitor +{ + public: + RealQueueMonitor(ros::NodeHandle& nh); + + protected: + /** + * @brief Private node handle for the metric monitor. + */ + ros::NodeHandle nh_; + + /** + * @brief Node handle for the metric monitor with name resolution at the namespace of the node. + */ + ros::NodeHandle ns_nh_; + + /** + * @brief Name of the queue real queue. Used for logging and to fetch the queue in the queue server + */ + std::string queue_name_; + + /** + * @brief Susbcriber the stats of the queue server. + */ + ros::Subscriber queue_stats_sub_; + + /** + * @brief Metric publisher. Should be defined by child class. + */ + ros::Publisher performance_metric_pub_; + + /** + * @brief Callback for the queue stats. + */ + void queueStatsCallback(const ros_queue_msgs::QueueServerStats::ConstPtr& msg); +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/temperature_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/temperature_monitor.hpp new file mode 100644 index 0000000..af16888 --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/temperature_monitor.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "metric_monitor.hpp" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +class TemperatureMonitor: public MetricMonitor +{ + public: + TemperatureMonitor(ros::NodeHandle& nh); + + protected: + virtual double getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg)override; + + virtual double getMetricTarget() override; + + virtual double getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) override; + + virtual double getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) override; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/virtual_queue_monitor.hpp b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/virtual_queue_monitor.hpp new file mode 100644 index 0000000..e74e10f --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/performance_monitor/virtual_queue_monitor.hpp @@ -0,0 +1,67 @@ +#pragma once + +#include "ros/ros.h" + +#include "ros_queue_experiments/AuvStates.h" + +#include "localization_monitor.hpp" +#include "temperature_monitor.hpp" +#include "low_temperature_monitor.hpp" +#include "penalty_monitor.hpp" + +class VirtualQueueMonitor +{ + public: + VirtualQueueMonitor(ros::NodeHandle& nh); + + private: + /** + * @brief Node handle with namespace name resolution. + */ + ros::NodeHandle nh_; + + /** + * @brief Private node handle with name resolution from node's name. + */ + ros::NodeHandle nhp_; + + /** + * @brief Service client that fetches the estimation from the queue server. + */ + ros::ServiceClient queue_server_stats_client_; + /** + * @brief ROS subscriber for the real state message that's used as an monitoring event. + */ + ros::Subscriber real_state_sub_; + + /** + * @brief Callback for the monitoring event (state message from the AUV system) + */ + void realStateCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg); + + /** + * @brief Signal publisher to the optimal controller indicating that the monitoring sample is done. + * When signal, this dependent controller is allowed to send its actions. + */ + ros::Publisher monitoring_sample_done_pub_; + + /** + * @brief Localization monitor that handles the computation and the publishing of the metrics of the localization. + */ + LocalizationMonitor localization_monitor_; + + /** + * @brief Temperature monitor that handles the computation and the publishing of the metrics of the temperature. + */ + TemperatureMonitor temperature_monitor_; + + /** + * @brief Temperature monitor for the lower bound of the temperature that handles the computation and the publishing of the metrics of the temperature. + */ + LowTemperatureMonitor low_temperature_monitor_; + + /** + * @brief Penalty monitor that handles the computation and the publishing of the metrics of the penalty. + */ + PenaltyMonitor penalty_monitor_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/include/ros_queue_experiments/proportion_time_spent.hpp b/ros_queue_experiments/include/ros_queue_experiments/proportion_time_spent.hpp new file mode 100644 index 0000000..ddeb07d --- /dev/null +++ b/ros_queue_experiments/include/ros_queue_experiments/proportion_time_spent.hpp @@ -0,0 +1,29 @@ +#pragma once + +#include + +class ProportionTimeSpent +{ + public: + /** + * @brief Add a new state to the proportion time spent. + * @return The index of the new state. + */ + int addState(); + + /** + * @brief Add time spent in the given state. + * @param time_spent Time to add in the time spent for the given index. + * @param index Represents the index of the state. + */ + void addTimeSpentInState(float time_spent, int index); + + /** + * @brief Get the proportion of time spent in each state. + * @return The proportion of time spent in each state between 0.0 to 1.0 + */ + std::vector getProportionTimeSpent() const; + + private: + std::vector time_spent_in_each_states_; +}; \ No newline at end of file diff --git a/ros_queue_experiments/launch/experiment_launcher.launch b/ros_queue_experiments/launch/experiment_launcher.launch new file mode 100644 index 0000000..6f1224e --- /dev/null +++ b/ros_queue_experiments/launch/experiment_launcher.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_queue_experiments/launch/experiment_manager.launch b/ros_queue_experiments/launch/experiment_manager.launch new file mode 100644 index 0000000..f5b926c --- /dev/null +++ b/ros_queue_experiments/launch/experiment_manager.launch @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/ros_queue_experiments/launch/queue_system_setup_template.launch b/ros_queue_experiments/launch/queue_system_setup_template.launch new file mode 100644 index 0000000..21f1b1a --- /dev/null +++ b/ros_queue_experiments/launch/queue_system_setup_template.launch @@ -0,0 +1,51 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_queue_experiments/launch/single_controller_experiment_launcher.launch b/ros_queue_experiments/launch/single_controller_experiment_launcher.launch new file mode 100644 index 0000000..d114020 --- /dev/null +++ b/ros_queue_experiments/launch/single_controller_experiment_launcher.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_queue_experiments/msg/ActionPerformance.msg b/ros_queue_experiments/msg/ActionPerformance.msg new file mode 100644 index 0000000..7cbdd55 --- /dev/null +++ b/ros_queue_experiments/msg/ActionPerformance.msg @@ -0,0 +1,11 @@ +std_msgs/Header header +ros_queue_msgs/TransmissionVector target_action +ros_queue_msgs/TransmissionVector applied_action +ros_queue_msgs/TransmissionVector optimal_action + +ros_queue_msgs/QueueServerStats queue_server_stats + +int8 controller_action_index +int8 applied_action_index +int8 action_index_difference +int8 optimal_action_index_difference \ No newline at end of file diff --git a/ros_queue_experiments/msg/AuvStates.msg b/ros_queue_experiments/msg/AuvStates.msg new file mode 100644 index 0000000..86fbc82 --- /dev/null +++ b/ros_queue_experiments/msg/AuvStates.msg @@ -0,0 +1,17 @@ +std_msgs/Header header +ros_queue_msgs/TransmissionVector current_zone +ros_queue_msgs/TransmissionVector last_zone +ros_queue_msgs/TransmissionVector zone_selected_by_controller + +float32 temperature +float32 temperature_integral + +float32 localization +float32 localization_integral + +float32 real_penalty +float32 controller_penalty + +float32 transition_completion + +float32[] proportion_time_spent_in_zones \ No newline at end of file diff --git a/ros_queue_experiments/msg/MetricPerformance.msg b/ros_queue_experiments/msg/MetricPerformance.msg new file mode 100644 index 0000000..8fcf740 --- /dev/null +++ b/ros_queue_experiments/msg/MetricPerformance.msg @@ -0,0 +1,15 @@ +std_msgs/Header header +string metric_name + +float32 current_real_value +float32 real_average_value +float32 real_continous_average_value +float32 real_time_average_value +float32 queue_server_time_average_value +float32 queue_server_arrival_mean +float32 target_value +float32 real_current_diff_with_target +float32 real_continuous_average_diff_with_target +float32 real_continuous_average_diff_with_server_mean +float32 real_continuous_average_diff_with_server_time_average +float32 seconds_since_start \ No newline at end of file diff --git a/ros_queue_experiments/package.xml b/ros_queue_experiments/package.xml new file mode 100644 index 0000000..54de6d3 --- /dev/null +++ b/ros_queue_experiments/package.xml @@ -0,0 +1,53 @@ + + + ros_queue_experiments + 0.0.0 + Contains a simple implementation of a queue stabilizing system with some packages to measure its performances + + + + + Etienne Villemure + + + + + + MIT + + + + + + + catkin + queue_controller + queue_server + ros_queue_msgs + roscpp + queue_controller + queue_server + ros_queue_msgs + roscpp + queue_controller + queue_server + ros_queue_msgs + roscpp + message_runtime + message_generation + actionlib + actionlib_msgs + message_filters + rosbag + rospy + roslaunch + ros_boosted_utilities + + + + + + + + + diff --git a/ros_queue_experiments/requirements.txt b/ros_queue_experiments/requirements.txt new file mode 100644 index 0000000..3171873 --- /dev/null +++ b/ros_queue_experiments/requirements.txt @@ -0,0 +1,2 @@ +matplotlib +SciencePlots \ No newline at end of file diff --git a/ros_queue_experiments/scripts/common_experiment_utils.py b/ros_queue_experiments/scripts/common_experiment_utils.py new file mode 100755 index 0000000..e7d6aa4 --- /dev/null +++ b/ros_queue_experiments/scripts/common_experiment_utils.py @@ -0,0 +1,238 @@ +#!/usr/bin/env python +import os +import rosbag +import rospy +import csv +from enum import Enum + +from ros_queue_experiments.msg import MetricPerformance +from ros_queue_experiments.msg import ActionPerformance + +script_path_list = os.path.normpath(os.path.abspath(__file__)).split(os.sep) +package_path = script_path_list[:-2] +LAUNCH_DIRECTORY_PATH = os.sep.join(package_path) + "/launch/" +BAG_DIRECTORY_PATH = os.sep.join(package_path) + "/experiment_bags/" +RESULT_DIRECTORY_PATH = BAG_DIRECTORY_PATH + "results/" + +class Series: + def __init__(self): + self.variable_name = "" + self.values = [] + +class QueueServerMetricStatsStruct: + def __init__(self): + self.queue_size = Series() + self.time_average_arrival = Series() + self.time_average_departure = Series() + +class QueueServerStatsStruct: + def __init__(self): + self.time_stamps = Series() + self.time_stamps.variable_name = "queue_server_time" + + self.localization_stats = QueueServerMetricStatsStruct() + self.localization_stats.queue_size.variable_name = "localization_queue_size" + self.localization_stats.time_average_arrival.variable_name = "localization_time_average_arrival" + self.localization_stats.time_average_departure.variable_name = "localization_time_average_departure" + + self.temperature_stats = QueueServerMetricStatsStruct() + self.temperature_stats.queue_size.variable_name = "temperature_queue_size" + self.temperature_stats.time_average_arrival.variable_name = "temperature_time_average_arrival" + self.temperature_stats.time_average_departure.variable_name = "temperature_time_average_departure" + + self.low_temperature_stats = QueueServerMetricStatsStruct() + self.low_temperature_stats.queue_size.variable_name = "low_temperature_queue_size" + self.low_temperature_stats.time_average_arrival.variable_name = "low_temperature_time_average_arrival" + self.low_temperature_stats.time_average_departure.variable_name = "low_temperature_time_average_departure" + + self.real_queue_stats = QueueServerMetricStatsStruct() + self.real_queue_stats.queue_size.variable_name = "real_queue_size" + self.real_queue_stats.time_average_arrival.variable_name = "real_queue_time_average_arrival" + self.real_queue_stats.time_average_departure.variable_name = "real_queue_time_average_departure" + +class MetricPerformanceStruct: + def __init__(self, metric_name: str = ""): + self.time_stamps = Series() + self.time_stamps.variable_name = metric_name + "_time" + + self.current_real_value = Series() + self.current_real_value.variable_name = metric_name + "_current_real_value" + + self.real_average_value = Series() + self.real_average_value.variable_name = metric_name + "_real_average_value" + + self.real_continous_average_value = Series() + self.real_continous_average_value.variable_name = metric_name + "_real_continous_average_value" + + self.real_time_average_value = Series() + self.real_time_average_value.variable_name = metric_name + "_real_time_average_value" + + self.queue_server_time_average_value = Series() + self.queue_server_time_average_value.variable_name = metric_name + "_queue_server_time_average_value" + + self.queue_server_arrival_mean = Series() + self.queue_server_arrival_mean.variable_name = metric_name + "_queue_server_arrival_mean" + + self.target_value = Series() + self.target_value.variable_name = metric_name + "_target_value" + + self.real_current_diff_with_target = Series() + self.real_current_diff_with_target.variable_name = metric_name + "_real_current_diff_with_target" + + self.real_continuous_average_diff_with_target = Series() + self.real_continuous_average_diff_with_target.variable_name = metric_name + "_real_continuous_average_diff_with_target" + + self.real_continuous_average_diff_with_server_mean = Series() + self.real_continuous_average_diff_with_server_mean.variable_name = metric_name + "_real_continuous_average_diff_with_server_mean" + + self.absolute_real_continuous_average_diff_with_server_mean = Series() + self.absolute_real_continuous_average_diff_with_server_mean.variable_name = metric_name + "_real_continuous_average_diff_with_server_mean_abs" + + self.real_continuous_average_diff_with_server_time_average = Series() + self.real_continuous_average_diff_with_server_time_average.variable_name = metric_name + "_real_continuous_average_diff_with_server_time_average" + +class AllMetricPerformanceStruct: + def __init__(self): + self.localization = MetricPerformanceStruct("localization") + self.temperature = MetricPerformanceStruct("temperature") + self.low_temperature = MetricPerformanceStruct("low_temperature") + self.real_queue = MetricPerformanceStruct("real_queue") + self.penalty = MetricPerformanceStruct("penalty") + + def populateWithBag(self, bag: rosbag.Bag, monitoring_prefix: str, time_init: rospy.Time): + self.__populateMetric(bag, monitoring_prefix + "monitoring_node/localization", self.localization, time_init) + self.__populateMetric(bag, monitoring_prefix + "monitoring_node/temperature", self.temperature, time_init) + self.__populateMetric(bag, monitoring_prefix + "monitoring_node/low_temperature", self.low_temperature, time_init) + self.__populateMetric(bag, monitoring_prefix + "monitoring_node/real_queue", self.real_queue, time_init) + self.__populateMetric(bag, monitoring_prefix + "monitoring_node/penalty", self.penalty, time_init) + + def __populateMetric(self, bag: rosbag.Bag, topic_name: str, metric: MetricPerformanceStruct, time_init: float): + for topic, msg, t in bag.read_messages(topics=[topic_name]): + metric.time_stamps.values.append(t.to_sec() - time_init) + metric.current_real_value.values.append(msg.current_real_value) + metric.real_average_value.values.append(msg.real_average_value) + metric.real_continous_average_value.values.append(msg.real_continous_average_value) + metric.real_time_average_value.values.append(msg.real_time_average_value) + metric.queue_server_time_average_value.values.append(msg.queue_server_time_average_value) + metric.queue_server_arrival_mean.values.append(msg.queue_server_arrival_mean) + metric.target_value.values.append(msg.target_value) + metric.real_current_diff_with_target.values.append(msg.real_current_diff_with_target) + metric.real_continuous_average_diff_with_target.values.append(msg.real_continuous_average_diff_with_target) + metric.real_continuous_average_diff_with_server_mean.values.append(msg.real_continuous_average_diff_with_server_mean) + metric.real_continuous_average_diff_with_server_time_average.values.append(msg.real_continuous_average_diff_with_server_time_average) + metric.absolute_real_continuous_average_diff_with_server_mean.values = [abs(value) for value in metric.real_continuous_average_diff_with_server_mean.values] + +def createCSV(list_of_series, csv_filename): + max_nb_rows = max([len(series.values) for series in list_of_series]) + + with open(csv_filename, 'w') as csvfile: + csvwriter = csv.writer(csvfile, delimiter=';') + + row = [] + for series in list_of_series: + row.append(series.variable_name) + + csvwriter.writerow(row) + for row_index in range(max_nb_rows): + row = [] + for series in list_of_series: + if row_index < len(series.values): + row.append(series.values[row_index]) + else: + row.append("") + csvwriter.writerow(row) + +class ActionType(Enum): + TASK_ZONE = 0, + HIGH_LOCALIZATION_ZONE = 1, + LOW_TEMPERATURE_ZONE = 2 + +def actionTypeToString(action_type: ActionType): + if action_type == ActionType.TASK_ZONE: + return "TASK_ZONE" + elif action_type == ActionType.HIGH_LOCALIZATION_ZONE: + return "HIGH_LOCALIZATION_ZONE" + elif action_type == ActionType.LOW_TEMPERATURE_ZONE: + return "LOW_TEMPERATURE_ZONE" + +def actionSeriesToStringSeries(action_series: Series): + action_string_series = Series() + action_string_series.variable_name = action_series.variable_name + action_string_series.values = [actionTypeToString(action_type) for action_type in action_series.values] + return action_string_series + +class ActionPerformanceSeries: + def __init__(self): + self.time_stamps = Series() + self.time_stamps.variable_name = "action_time" + + self.selected_action = Series() + self.selected_action.variable_name = "selected_action" + + self.applied_action = Series() + self.applied_action.variable_name = "applied_action" + + self.action_index_difference = Series() + self.action_index_difference.variable_name = "action_index_difference" + + self.synchronized_queue_stats = QueueServerStatsStruct() + + def populateWithBag(self, bag: rosbag.Bag, monitoring_prefix: str, time_init: rospy.Time): + for topic, msg, t in bag.read_messages(topics=[monitoring_prefix + "perturbation_node/action_performance"]): + self.time_stamps.values.append(t.to_sec() - time_init) + + for internal_struct,msg_action in zip([self.selected_action, self.applied_action, self.action_index_difference], [msg.controller_action_index, msg.applied_action_index, msg.action_index_difference]): + if msg_action == 0: + internal_struct.values.append(ActionType.TASK_ZONE) + elif msg_action == 1: + internal_struct.values.append(ActionType.HIGH_LOCALIZATION_ZONE) + elif msg_action == 2: + internal_struct.values.append(ActionType.LOW_TEMPERATURE_ZONE) + + for queue_stats in msg.queue_server_stats.queue_stats: + if queue_stats.queue_name == "LocalizationQueue": + self.synchronized_queue_stats.localization_stats.queue_size.values.append(queue_stats.current_size) + self.synchronized_queue_stats.localization_stats.time_average_arrival.values.append(queue_stats.arrival_time_average) + self.synchronized_queue_stats.localization_stats.time_average_departure.values.append(queue_stats.departure_time_average) + elif queue_stats.queue_name == "TemperatureQueue": + self.synchronized_queue_stats.temperature_stats.queue_size.values.append(queue_stats.current_size) + self.synchronized_queue_stats.temperature_stats.time_average_arrival.values.append(queue_stats.arrival_time_average) + self.synchronized_queue_stats.temperature_stats.time_average_departure.values.append(queue_stats.departure_time_average) + elif queue_stats.queue_name == "LowTemperatureQueue": + self.synchronized_queue_stats.low_temperature_stats.queue_size.values.append(queue_stats.current_size) + self.synchronized_queue_stats.low_temperature_stats.time_average_arrival.values.append(queue_stats.arrival_time_average) + self.synchronized_queue_stats.low_temperature_stats.time_average_departure.values.append(queue_stats.departure_time_average) + elif queue_stats.queue_name == "TaskQueue": + self.synchronized_queue_stats.real_queue_stats.queue_size.values.append(queue_stats.current_size) + self.synchronized_queue_stats.real_queue_stats.time_average_arrival.values.append(queue_stats.arrival_time_average) + self.synchronized_queue_stats.real_queue_stats.time_average_departure.values.append(queue_stats.departure_time_average) + + +class QueueEndValues: + def __init__(self): + self.localization_arrival = Series() + self.localization_arrival.variable_name = "end_localization_arrival" + + self.localization_departure = Series() + self.localization_departure.variable_name = "end_localization_departure" + + self.temperature_arrival = Series() + self.temperature_arrival.variable_name = "end_temperature_arrival" + + self.temperature_departure = Series() + self.temperature_departure.variable_name = "end_temperature_departure" + + self.low_temperature_arrival = Series() + self.low_temperature_arrival.variable_name = "end_low_temperature_arrival" + + self.low_temperature_departure = Series() + self.low_temperature_departure.variable_name = "end_low_temperature_departure" + + self.real_queue_arrival = Series() + self.real_queue_arrival.variable_name = "end_real_queue_arrival" + + self.real_queue_departure = Series() + self.real_queue_departure.variable_name = "end_real_queue_departure" + + self.penalty = Series() + self.penalty.variable_name = "end_penalty" diff --git a/ros_queue_experiments/scripts/experiment1_definition.py b/ros_queue_experiments/scripts/experiment1_definition.py new file mode 100755 index 0000000..65c292d --- /dev/null +++ b/ros_queue_experiments/scripts/experiment1_definition.py @@ -0,0 +1,192 @@ +#!/usr/bin/env python +import rospy + +import rosbag +from datetime import datetime +import csv +import matplotlib.pyplot as plt +import matplotlib.patches as patches + +from ros_queue_msgs.msg import QueueServerStats +from ros_queue_experiments.msg import MetricPerformance + +import common_experiment_utils + +class Experiment1Analyser: + def __init__(self): + time_now = datetime.now() + string_time_now = time_now.strftime("%Y-%m-%d_%H-%M-%S") + self.bag_name = "experiment1_" + string_time_now + + self.topics_to_record = [ + "/NoRew_NoInv/queue_server/server_stats", + "/NoRew_NoInv/auv_system_node/auv_state", + "/NoRew_NoInv/monitoring_node/localization", + "/NoRew_NoInv/monitoring_node/real_queue", + "/NoRew_NoInv/monitoring_node/temperature", + "/NoRew_NoInv/monitoring_node/low_temperature", + "/NoRew_NoInv/monitoring_node/penalty", + "/NoRew_NoInv/queue_controller/optimization_done", + "/NoRew_NoInv/queue_controller/control_loop_started", + "/NoRew_NoInv/perturbation_node/action_performance"] + + def generateOutput(self, time_init, bag_name, base_init_time_on_first_value=False): + bag = rosbag.Bag(common_experiment_utils.BAG_DIRECTORY_PATH + bag_name + ".bag") + + # Get init time if not set + init_time_set = False + + for topic, msg, t in bag.read_messages(topics=["/NoRew_NoInv/queue_server/server_stats"]): + if (not init_time_set) and base_init_time_on_first_value: + time_init = t.to_sec() + init_time_set = True + break + + # Get action performances + action_performances = common_experiment_utils.ActionPerformanceSeries() + action_performances.populateWithBag(bag, "/NoRew_NoInv/", time_init) + + # Get queue server end values + queue_server_arrival_departures_end_values = common_experiment_utils.QueueEndValues() + + for topic, msg, t in bag.read_messages(topics=["/NoRew_NoInv/monitoring_node/penalty"]): + queue_server_arrival_departures_end_values.penalty.values.append(msg.queue_server_time_average_value) + + queue_server_arrival_departures_end_values.localization_arrival.values.append(action_performances.synchronized_queue_stats.localization_stats.time_average_arrival.values[-1]) + queue_server_arrival_departures_end_values.localization_departure.values.append(action_performances.synchronized_queue_stats.localization_stats.time_average_departure.values[-1]) + queue_server_arrival_departures_end_values.temperature_arrival.values.append(action_performances.synchronized_queue_stats.temperature_stats.time_average_arrival.values[-1]) + queue_server_arrival_departures_end_values.temperature_departure.values.append(action_performances.synchronized_queue_stats.temperature_stats.time_average_departure.values[-1]) + queue_server_arrival_departures_end_values.low_temperature_arrival.values.append(action_performances.synchronized_queue_stats.low_temperature_stats.time_average_arrival.values[-1]) + queue_server_arrival_departures_end_values.low_temperature_departure.values.append(action_performances.synchronized_queue_stats.low_temperature_stats.time_average_departure.values[-1]) + queue_server_arrival_departures_end_values.real_queue_arrival.values.append(action_performances.synchronized_queue_stats.real_queue_stats.time_average_arrival.values[-1]) + queue_server_arrival_departures_end_values.real_queue_departure.values.append(action_performances.synchronized_queue_stats.real_queue_stats.time_average_departure.values[-1]) + queue_server_arrival_departures_end_values.penalty.values = [queue_server_arrival_departures_end_values.penalty.values[-1]] + + # Get performance metrics + all_metric_performance_structs = common_experiment_utils.AllMetricPerformanceStruct() + all_metric_performance_structs.populateWithBag(bag, "/NoRew_NoInv/", time_init) + + # Create output CSV + separator_second_graph = common_experiment_utils.Series() + separator_second_graph.variable_name = "controller_sacrifices_one_queue" + separator_end_values_table = common_experiment_utils.Series() + separator_end_values_table.variable_name = "end_values_table" + + csv_filename = common_experiment_utils.RESULT_DIRECTORY_PATH + self.bag_name + ".csv" + series_to_record = [all_metric_performance_structs.localization.time_stamps, + all_metric_performance_structs.localization.real_continous_average_value, + all_metric_performance_structs.localization.target_value, + all_metric_performance_structs.temperature.time_stamps, + all_metric_performance_structs.temperature.real_continous_average_value, + all_metric_performance_structs.temperature.target_value, + all_metric_performance_structs.low_temperature.target_value, + all_metric_performance_structs.real_queue.time_stamps, + all_metric_performance_structs.real_queue.queue_server_arrival_mean, + all_metric_performance_structs.real_queue.target_value, + all_metric_performance_structs.penalty.time_stamps, + all_metric_performance_structs.penalty.real_time_average_value, + separator_second_graph, + action_performances.time_stamps, + action_performances.synchronized_queue_stats.localization_stats.queue_size, + action_performances.synchronized_queue_stats.temperature_stats.queue_size, + action_performances.synchronized_queue_stats.low_temperature_stats.queue_size, + action_performances.synchronized_queue_stats.real_queue_stats.queue_size, + separator_end_values_table, + queue_server_arrival_departures_end_values.localization_arrival, + queue_server_arrival_departures_end_values.localization_departure, + queue_server_arrival_departures_end_values.temperature_arrival, + queue_server_arrival_departures_end_values.temperature_departure, + queue_server_arrival_departures_end_values.low_temperature_arrival, + queue_server_arrival_departures_end_values.low_temperature_departure, + queue_server_arrival_departures_end_values.real_queue_arrival, + queue_server_arrival_departures_end_values.real_queue_departure, + queue_server_arrival_departures_end_values.penalty] + + common_experiment_utils.createCSV(series_to_record, csv_filename) + + # ==== Create plots ==== + fig1, (ax1, ax2, ax3, ax4) = plt.subplots(4, 1, sharex=True) + ax1.set_title("Average of the metrics of the simulated system over time under a min drift-plus-penalty queue controller in ROS") + ax1.plot(all_metric_performance_structs.localization.time_stamps.values, all_metric_performance_structs.localization.real_continous_average_value.values, label="Metric") + ax1.plot(all_metric_performance_structs.localization.time_stamps.values, all_metric_performance_structs.localization.target_value.values,label="Target", ls="--") + ax1.set_ylabel("Localization uncertainty (m)") + ax1.grid(False) + ax1.legend() + + ax2.plot(all_metric_performance_structs.temperature.time_stamps.values, all_metric_performance_structs.temperature.real_continous_average_value.values, label="Metric") + ax2.plot(all_metric_performance_structs.temperature.time_stamps.values, all_metric_performance_structs.temperature.target_value.values, label="Upper target", ls="--") + ax2.plot(all_metric_performance_structs.low_temperature.time_stamps.values, all_metric_performance_structs.low_temperature.target_value.values, label="Lower Target", ls="--") + ax2.grid(False) + ax2.set_ylabel("Temperature (°C)") + ax2.legend() + + ax3.plot(all_metric_performance_structs.real_queue.time_stamps.values, all_metric_performance_structs.real_queue.queue_server_arrival_mean.values, label="Arrival task rate mean") + ax3.plot(all_metric_performance_structs.real_queue.time_stamps.values, all_metric_performance_structs.real_queue.target_value.values, label="Departure task rate mean") + ax3.grid(False) + ax3.set_ylabel("Task rate (Task/s)") + ax3.legend() + + ax4.plot(all_metric_performance_structs.penalty.time_stamps.values, all_metric_performance_structs.penalty.real_time_average_value.values, label="Energy usage (Penalty) (J)") + ax4.set_ylabel("Penalty (J)") + ax4.grid(False) + ax4.set_xlabel("Time (s)") + + ## Queue size graph and action sacrifices + time_frame = [57.4, 58.1] + #time_frame = [0, 60] + fig2, (ax5) = plt.subplots(1, 1, sharex=True) + ax5.set_title("Queue sizes balancing over time under a min drift-plus-penalty queue controller in ROS") + ax5.plot(action_performances.time_stamps.values, action_performances.synchronized_queue_stats.localization_stats.queue_size.values, label="Localization queue", drawstyle='steps-post') + ax5.plot(action_performances.time_stamps.values, action_performances.synchronized_queue_stats.temperature_stats.queue_size.values, label="Temperature queue", drawstyle='steps-post') + ax5.plot(action_performances.time_stamps.values, action_performances.synchronized_queue_stats.low_temperature_stats.queue_size.values, label="Low temperature queue", drawstyle='steps-post') + ax5.plot(action_performances.time_stamps.values, action_performances.synchronized_queue_stats.real_queue_stats.queue_size.values, label="Task queue", drawstyle='steps-post') + ax5.set_xlim(time_frame[0], time_frame[1]) + ax5.set_ylabel("Queue sizes") + ax5.legend() + + # Add actions + max_queue_size = max(action_performances.synchronized_queue_stats.localization_stats.queue_size.values + action_performances.synchronized_queue_stats.temperature_stats.queue_size.values + action_performances.synchronized_queue_stats.low_temperature_stats.queue_size.values + action_performances.synchronized_queue_stats.real_queue_stats.queue_size.values) + time_frame_start_index = 0 + time_frame_end_index = 0 + start_found = False + for action_index in range(len(action_performances.time_stamps.values)): + if action_performances.time_stamps.values[action_index] >= time_frame[0] and not start_found: + time_frame_start_index = action_index + start_found = True + if action_performances.time_stamps.values[action_index] >= time_frame[1]: + time_frame_end_index = action_index + break + + action_color_dict = {common_experiment_utils.ActionType.TASK_ZONE: "#00CAFF", + common_experiment_utils.ActionType.HIGH_LOCALIZATION_ZONE: "#FF00CA", + common_experiment_utils.ActionType.LOW_TEMPERATURE_ZONE: "#CAFF00"} + action_label_dict = {common_experiment_utils.ActionType.TASK_ZONE: "TaskZ", + common_experiment_utils.ActionType.HIGH_LOCALIZATION_ZONE: "LocZ", + common_experiment_utils.ActionType.LOW_TEMPERATURE_ZONE: "ColdZ"} + + last_action_index = time_frame_start_index + for action_index in range(time_frame_start_index, time_frame_end_index): + if action_performances.applied_action.values[last_action_index] != action_performances.applied_action.values[action_index]: + last_action = action_performances.applied_action.values[last_action_index] + start = action_performances.time_stamps.values[last_action_index] + elapsed_time = action_performances.time_stamps.values[action_index] - start + rect = patches.Rectangle((start, 0), elapsed_time, max_queue_size, color=action_color_dict[last_action], alpha=0.25) + ax5.add_patch(rect) + ax5.text(start + elapsed_time/2, max_queue_size-1.5, action_label_dict[last_action], ha='center', va='center') + last_action_index = action_index + + plt.show() + + def prefixToBagName(self, prefix): + self.bag_name = prefix + self.bag_name + + def getBagName(self): + return self.bag_name + + def getROSBagArguments(self): + topic_args = "" + for topic in self.topics_to_record: + topic_args += " " + topic + + return "-O "+ common_experiment_utils.BAG_DIRECTORY_PATH + self.bag_name + ".bag" + topic_args + diff --git a/ros_queue_experiments/scripts/experiment2_definition.py b/ros_queue_experiments/scripts/experiment2_definition.py new file mode 100644 index 0000000..4004011 --- /dev/null +++ b/ros_queue_experiments/scripts/experiment2_definition.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python +import rospy + +import rosbag +from datetime import datetime +import csv +import matplotlib.pyplot as plt + +from ros_queue_msgs.msg import QueueServerStats +from ros_queue_experiments.msg import MetricPerformance + +import common_experiment_utils + +class Experiment2Analyser: + def __init__(self): + time_now = datetime.now() + self.string_time_now = time_now.strftime("%Y-%m-%d_%H-%M-%S") + self.normal_bag_name = "normal_experiment2_" + self.string_time_now + self.perturbation_bag_name = "perturbation_experiment2_" + self.string_time_now + + self.topics_to_record = [ + "/NoRew_NoInv/monitoring_node/localization", + "/NoRew_NoInv/monitoring_node/real_queue", + "/NoRew_NoInv/monitoring_node/temperature", + "/NoRew_NoInv/monitoring_node/low_temperature", + "/NoRew_NoInv/perturbation_node/action_performance"] + + def generateOutput(self, normal_time_init, perturbation_time_init, normal_bag_name, pertubation_bag_name, base_init_time_on_first_value=False): + # ====== Get data from unperturbed experiment ====== + bag = rosbag.Bag(common_experiment_utils.BAG_DIRECTORY_PATH + normal_bag_name + ".bag") + + # Get init time if not set + if base_init_time_on_first_value: + for topic, msg, t in bag.read_messages(topics=["/NoRew_NoInv/perturbation_node/action_performance"]): + # Use the first message time as the init time + normal_time_init = t.to_sec() + break + + # Get action performances + normal_action_performances = common_experiment_utils.ActionPerformanceSeries() + normal_action_performances.populateWithBag(bag, "/NoRew_NoInv/", normal_time_init) + + # Get performance metrics + normal_all_metric_performance_structs = common_experiment_utils.AllMetricPerformanceStruct() + normal_all_metric_performance_structs.populateWithBag(bag, "/NoRew_NoInv/", normal_time_init) + + # ====== Get data from perturbed experiment ====== + bag = rosbag.Bag(common_experiment_utils.BAG_DIRECTORY_PATH + pertubation_bag_name + ".bag") + + # Get init time if not set + if base_init_time_on_first_value: + for topic, msg, t in bag.read_messages(topics=["/NoRew_NoInv/perturbation_node/action_performance"]): + # Use the first message time as the init time + perturbation_time_init = t.to_sec() + break + + # Get action performances + perturbation_action_performances = common_experiment_utils.ActionPerformanceSeries() + perturbation_action_performances.populateWithBag(bag, "/NoRew_NoInv/", perturbation_time_init) + + # Get performance metrics + perturbation_all_metric_performance_structs = common_experiment_utils.AllMetricPerformanceStruct() + perturbation_all_metric_performance_structs.populateWithBag(bag, "/NoRew_NoInv/", perturbation_time_init) + + normal_task_queue_departure_arrival_mean_diff = common_experiment_utils.Series() + normal_task_queue_departure_arrival_mean_diff.variable_name = "Departure and arrival diff NoRew_NoInv" + normal_task_queue_departure_arrival_mean_diff.values = [departure - arrival for arrival, departure in zip(normal_all_metric_performance_structs.real_queue.queue_server_arrival_mean.values, normal_all_metric_performance_structs.real_queue.target_value.values)] + + perturbation_task_queue_departure_arrival_mean_diff = common_experiment_utils.Series() + perturbation_task_queue_departure_arrival_mean_diff.variable_name = "Departure and arrival diff NoRew_NoInv with perturbation" + perturbation_task_queue_departure_arrival_mean_diff.values = [departure - arrival for arrival, departure in zip(perturbation_all_metric_performance_structs.real_queue.queue_server_arrival_mean.values, perturbation_all_metric_performance_structs.real_queue.target_value.values)] + + # ====== Create output CSV ====== + normal_data_separator = common_experiment_utils.Series() + normal_data_separator.variable_name = "Normal NoRew_NoInv" + perturbation_data_separator = common_experiment_utils.Series() + perturbation_data_separator.variable_name = "NoRew_NoInv with perturbation" + + csv_filename = common_experiment_utils.RESULT_DIRECTORY_PATH + "normal_experiment2_" + self.string_time_now + ".csv" + series_to_record = [normal_data_separator, + normal_all_metric_performance_structs.localization.time_stamps, + normal_all_metric_performance_structs.localization.real_continuous_average_diff_with_target, + normal_all_metric_performance_structs.localization.absolute_real_continuous_average_diff_with_server_mean, + normal_all_metric_performance_structs.temperature.time_stamps, + normal_all_metric_performance_structs.temperature.real_continuous_average_diff_with_target, + normal_all_metric_performance_structs.temperature.absolute_real_continuous_average_diff_with_server_mean, + normal_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_target, + normal_all_metric_performance_structs.real_queue.time_stamps, + normal_all_metric_performance_structs.real_queue.queue_server_arrival_mean, + normal_all_metric_performance_structs.real_queue.target_value, + normal_task_queue_departure_arrival_mean_diff, + normal_action_performances.time_stamps, + common_experiment_utils.actionSeriesToStringSeries(normal_action_performances.applied_action), + common_experiment_utils.actionSeriesToStringSeries(normal_action_performances.selected_action), + normal_action_performances.action_index_difference, + perturbation_data_separator, + perturbation_all_metric_performance_structs.localization.time_stamps, + perturbation_all_metric_performance_structs.localization.real_continuous_average_diff_with_target, + perturbation_all_metric_performance_structs.localization.absolute_real_continuous_average_diff_with_server_mean, + perturbation_all_metric_performance_structs.temperature.time_stamps, + perturbation_all_metric_performance_structs.temperature.real_continuous_average_diff_with_target, + perturbation_all_metric_performance_structs.temperature.absolute_real_continuous_average_diff_with_server_mean, + perturbation_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_target, + perturbation_all_metric_performance_structs.real_queue.time_stamps, + perturbation_all_metric_performance_structs.real_queue.queue_server_arrival_mean, + perturbation_all_metric_performance_structs.real_queue.target_value, + perturbation_task_queue_departure_arrival_mean_diff, + perturbation_action_performances.time_stamps, + common_experiment_utils.actionSeriesToStringSeries(perturbation_action_performances.applied_action), + common_experiment_utils.actionSeriesToStringSeries(perturbation_action_performances.selected_action), + perturbation_action_performances.action_index_difference] + + common_experiment_utils.createCSV(series_to_record, csv_filename) + + # ==== Create plots ==== + fig1, (ax1, ax2) = plt.subplots(2, 1, sharex=True) + fig1.suptitle("Estimation error of the queue server with the real metric mean for the localization metric of a min drift plus penalty with and without action uncertainties") + + ax1.set_title("Error between the real metric mean and the estimated metric mean from the queue server metric over time") + ax1.plot(normal_all_metric_performance_structs.localization.time_stamps.values, + normal_all_metric_performance_structs.localization.absolute_real_continuous_average_diff_with_server_mean.values, + label="Without perturbation") + ax1.plot(perturbation_all_metric_performance_structs.localization.time_stamps.values, + perturbation_all_metric_performance_structs.localization.absolute_real_continuous_average_diff_with_server_mean.values, + label="With perturbations") + ax1.set_ylabel("Estimation error (m)") + ax1.grid(False) + ax1.legend() + + ax2.set_title("Error between the real metric mean and the mean target for the localization") + ax2.plot(normal_all_metric_performance_structs.localization.time_stamps.values, + normal_all_metric_performance_structs.localization.real_continuous_average_diff_with_target.values, + label="Without perturbation") + ax2.plot(perturbation_all_metric_performance_structs.localization.time_stamps.values, + perturbation_all_metric_performance_structs.localization.real_continuous_average_diff_with_target.values, + label="With perturbations") + ax2.set_ylabel("Error (m)") + ax2.grid(False) + ax2.legend() + ax2.set_xlabel("Time (s)") + + fig2, (ax3, ax4) = plt.subplots(2, 1, sharex=True) + fig2.suptitle("Estimation error of the queue server with the real metric mean for the temperature metric of a min drift plus penalty with and without action uncertainties") + + ax3.set_title("Absolute error between the real metric mean and the estimated metric mean from the queue server metric over time") + ax3.plot(normal_all_metric_performance_structs.temperature.time_stamps.values, + normal_all_metric_performance_structs.temperature.absolute_real_continuous_average_diff_with_server_mean.values, + label="Without perturbation") + ax3.plot(perturbation_all_metric_performance_structs.temperature.time_stamps.values, + perturbation_all_metric_performance_structs.temperature.absolute_real_continuous_average_diff_with_server_mean.values, + label="With perturbations") + ax3.set_ylabel("Estimation error (C)") + ax3.grid(False) + ax3.legend() + + ax4.set_title("Absolute error between the real metric mean and the mean target for the temperature") + ax4.plot(normal_all_metric_performance_structs.temperature.time_stamps.values, + normal_all_metric_performance_structs.temperature.real_continuous_average_diff_with_target.values, + label="Without perturbation") + ax4.plot(perturbation_all_metric_performance_structs.temperature.time_stamps.values, + perturbation_all_metric_performance_structs.temperature.real_continuous_average_diff_with_target.values, + label="With perturbations") + ax4.set_ylabel("Error (C)") + ax4.grid(False) + ax4.legend() + ax4.set_xlabel("Time (s)") + + fig3, (ax5, ax6) = plt.subplots(2, 1, sharex=True) + fig3.suptitle("Estimation error of the queue server with the real metric mean for the low temperature metric of a min drift plus penalty with and without action uncertainties") + + ax5.set_title("Absolute error between the real metric mean and the estimated metric mean from the queue server metric over time") + ax5.plot(normal_all_metric_performance_structs.low_temperature.time_stamps.values, + normal_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_server_mean.values, + label="Without perturbation") + ax5.plot(perturbation_all_metric_performance_structs.low_temperature.time_stamps.values, + perturbation_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_server_mean.values, + label="With perturbations") + ax5.set_ylabel("Estimation error (C)") + ax5.grid(False) + ax5.legend() + + ax6.set_title("Absolute error between the real metric mean and the mean target for the low temperature") + ax6.plot(normal_all_metric_performance_structs.low_temperature.time_stamps.values, + normal_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_target.values, + label="Without perturbation") + ax6.plot(perturbation_all_metric_performance_structs.low_temperature.time_stamps.values, + perturbation_all_metric_performance_structs.low_temperature.real_continuous_average_diff_with_target.values, + label="With perturbations") + ax6.set_ylabel("Error (C)") + ax6.grid(False) + ax6.legend() + ax6.set_xlabel("Time (s)") + + fig4, ax7 = plt.subplots(1, 1, sharex=True) + fig4.suptitle("Task queue departure mean difference with its arrivals mean for a min drift plus penalty with and without action uncertainties") + ax7.set_title("Difference between the queue server arrival mean and the target value over time") + + ax7.plot(normal_all_metric_performance_structs.real_queue.time_stamps.values, + normal_task_queue_departure_arrival_mean_diff.values, + label="Without perturbation") + ax7.plot(perturbation_all_metric_performance_structs.real_queue.time_stamps.values, + perturbation_task_queue_departure_arrival_mean_diff.values, + label="With perturbations") + ax7.set_ylabel("Difference (Task/s)") + ax7.grid(False) + ax7.legend() + ax7.set_xlabel("Time (s)") + plt.show() + + def getNormalBagName(self): + return self.normal_bag_name + + def getPerturbationBagName(self): + return self.perturbation_bag_name + + def getNormalROSBagArguments(self): + topic_args = "" + for topic in self.topics_to_record: + topic_args += " " + topic + + return "-O "+ common_experiment_utils.BAG_DIRECTORY_PATH + self.normal_bag_name + ".bag" + topic_args + + def getPerturbationROSBagArguments(self): + topic_args = "" + for topic in self.topics_to_record: + topic_args += " " + topic + + return "-O "+ common_experiment_utils.BAG_DIRECTORY_PATH + self.perturbation_bag_name + ".bag" + topic_args + diff --git a/ros_queue_experiments/scripts/experiment_instance.py b/ros_queue_experiments/scripts/experiment_instance.py new file mode 100755 index 0000000..9eee705 --- /dev/null +++ b/ros_queue_experiments/scripts/experiment_instance.py @@ -0,0 +1,71 @@ +#!/usr/bin/env python +import rospy +import roslaunch + +import common_experiment_utils + +class ExperimentInstance: + def __init__(self, uuid, experiment_launcher_args, duration_sec, analyser): + + self.analyser = analyser + cli_args = [common_experiment_utils.LAUNCH_DIRECTORY_PATH + "experiment_launcher.launch"] + experiment_launcher_args + ["rosbag_args:=" + self.analyser.getROSBagArguments()] + roslaunch_args = cli_args[1:] + roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(cli_args)[0], roslaunch_args)] + + self.duration_sec = duration_sec + + self.parent = roslaunch.parent.ROSLaunchParent(uuid, roslaunch_file) + + def execute(self, generate_output=True): + self.time_init = rospy.Time.now() + self.parent.start() + rospy.sleep(self.duration_sec) + self.parent.shutdown() + # Analyse results + if generate_output: + self.analyser.generateOutput(self.time_init.to_sec(), self.analyser.getBagName()) + + + +class Experiment2Instance: + def __init__(self, uuid, duration_sec, analyser): + + self.duration_sec = duration_sec + + self.analyser = analyser + + normal_cli_args = [common_experiment_utils.LAUNCH_DIRECTORY_PATH + "experiment_launcher.launch"] + ['experiment_setup:=perfect_model_and_setup', "rosbag_args:=" + self.analyser.getNormalROSBagArguments()] + normal_roslaunch_args = normal_cli_args[1:] + normal_roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(normal_cli_args)[0], + normal_roslaunch_args)] + + self.normal_parent = roslaunch.parent.ROSLaunchParent(uuid, normal_roslaunch_file) + + perturbation_cli_args = [common_experiment_utils.LAUNCH_DIRECTORY_PATH + "experiment_launcher.launch"] + ['experiment_setup:=perturbation_on_action', "rosbag_args:=" + self.analyser.getPerturbationROSBagArguments()] + perturbation_roslaunch_args = perturbation_cli_args[1:] + perturbation_roslaunch_file = [(roslaunch.rlutil.resolve_launch_arguments(perturbation_cli_args)[0], + perturbation_roslaunch_args)] + + self.perturbation_parent = roslaunch.parent.ROSLaunchParent(uuid, perturbation_roslaunch_file) + + def execute(self, generate_output=True): + self.normal_time_init = rospy.Time.now() + self.normal_parent.start() + rospy.sleep(self.duration_sec) + self.normal_parent.shutdown() + + self.perturbation_time_init = rospy.Time.now() + self.perturbation_parent.start() + rospy.sleep(self.duration_sec) + self.perturbation_parent.shutdown() + + # Analyse results + if generate_output: + self.analyser.generateOutput(self.normal_time_init.to_sec(), + self.perturbation_time_init.to_sec(), + self.analyser.getNormalBagName(), + self.analyser.getPerturbationBagName(), + base_init_time_on_first_value=False) + + + diff --git a/ros_queue_experiments/scripts/experiment_manager.py b/ros_queue_experiments/scripts/experiment_manager.py new file mode 100755 index 0000000..1c76956 --- /dev/null +++ b/ros_queue_experiments/scripts/experiment_manager.py @@ -0,0 +1,49 @@ +#!/usr/bin/env python + +import os +import rospy +import roslaunch +import rosbag +from datetime import datetime +import csv +from ros_queue_msgs.msg import QueueServerStats + +import common_experiment_utils +import experiment_instance +import experiment1_definition +import experiment2_definition + +import matplotlib.pyplot as plt +import scienceplots + +# Using the formating from https://github.com/garrettj403/SciencePlots to follow IEEE requirements +plt.style.use('science') +# plt.style.use(['science', 'ieee']) + +if __name__ == "__main__": + # Start the launch file + rospy.init_node('experiment_manager', anonymous=False) + + uuid = roslaunch.rlutil.get_or_generate_uuid(None, False) + roslaunch.configure_logging(uuid) + + # Experiment 1 + + # cli_args = ['experiment_setup:=perfect_model_and_setup'] + + # exp1_analyser = experiment1_definition.Experiment1Analyser() + # exp1 = experiment_instance.ExperimentInstance(uuid, cli_args, 60, exp1_analyser) + # exp1.execute(generate_output=False) + # #exp1.analyser.generateOutput(0, "experiment1_2024-06-10_14-55-02", base_init_time_on_first_value=True) + # exp1.analyser.generateOutput(exp1.time_init.to_sec(), exp1.analyser.getBagName()) + # rospy.loginfo("Experiment 1 completed") + + # Experiment 2 + exp2_analyser = experiment2_definition.Experiment2Analyser() + exp2 = experiment_instance.Experiment2Instance(uuid, 90, exp2_analyser) + #exp2.execute(generate_output=False) + exp2.analyser.generateOutput(0,0, "normal_experiment2_2024-06-19_15-03-02", "perturbation_experiment2_2024-06-19_15-03-02",base_init_time_on_first_value=True) + + + + diff --git a/ros_queue_experiments/src/auv_node.cpp b/ros_queue_experiments/src/auv_node.cpp new file mode 100644 index 0000000..4d4a713 --- /dev/null +++ b/ros_queue_experiments/src/auv_node.cpp @@ -0,0 +1,17 @@ +#include "ros/ros.h" + +#include "ros_queue_experiments/auv_system.hpp" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "auv_node"); + ros::NodeHandle nh("~"); + + ros::Rate loop_rate(10); + + AUVSystem auv_system(nh); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/auv_state_manager.cpp b/ros_queue_experiments/src/auv_state_manager.cpp new file mode 100644 index 0000000..c36fc42 --- /dev/null +++ b/ros_queue_experiments/src/auv_state_manager.cpp @@ -0,0 +1,94 @@ +#include "ros_queue_experiments/auv_state_manager.hpp" +#include "ros_queue_experiments/auv_system.hpp" +#include "ros_queue_experiments/auv_states.hpp" + +AUVStateManager::AUVStateManager(AUVSystem* auv_system): auv_system_(auv_system) +{ + // Initialize to cold zone + states_at_last_transition_.current_zone = AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::TaskZone); + states_at_last_transition_.last_zone = AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::TaskZone); + states_at_last_transition_.zone_selected_by_controller = AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::TaskZone); + states_at_last_transition_.localization = 1.0; + states_at_last_transition_.temperature = 20.0; + + states_at_last_transition_.real_penalty = 0.0; + states_at_last_transition_.controller_penalty = 0.0; + states_at_last_transition_.transition_completion = 0.0; + + time_spent_in_zones_.addState(); + time_spent_in_zones_.addState(); + time_spent_in_zones_.addState(); + + last_transition_time_ = ros::Time::now(); +} + +ros_queue_experiments::AuvStates AUVStateManager::getCurrentStates() +{ + std::lock_guard lock(state_access_mutex_); + + // Get the rates of change + ros_queue_experiments::AuvStates current_states; + current_states = states_at_last_transition_; + + if (auv_system_) + { + current_states.header.stamp = ros::Time::now(); + + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + const AUVStates::Zones last_zone = AUVStates::getZoneFromTransmissionVector(current_states.last_zone); + const AUVStates::Zones zone_selected_by_controller = AUVStates::getZoneFromTransmissionVector(current_states.zone_selected_by_controller); + + const double elapsed_time_since_last_transition = (ros::Time::now() - last_transition_time_).toSec(); + + float temperature_rate = auv_system_->temperature_services_->getRealArrival(current_zone) - auv_system_->temperature_services_->getRealDeparture(current_zone); + current_states.temperature_integral += current_states.temperature*elapsed_time_since_last_transition + + 0.5*temperature_rate*elapsed_time_since_last_transition*elapsed_time_since_last_transition; + + current_states.temperature += temperature_rate * elapsed_time_since_last_transition; + + double renewal_time = auv_system_->expected_time_services_->getRealRenewalTimeWithStateTransition(last_zone, current_zone); + current_states.transition_completion = (elapsed_time_since_last_transition / renewal_time); + if(current_states.transition_completion > 1.0) + { + current_states.transition_completion = 1.0; + } + + current_states.real_penalty = auv_system_->penalty_metric_services_->getRealPenaltyTransition(last_zone, current_zone); + current_states.controller_penalty = auv_system_->penalty_metric_services_->getPredictedPenaltyTransition(last_zone, zone_selected_by_controller); + + + float current_localization = auv_system_->localization_services_->getRealLocalizationUncertainty(current_zone); + current_states.localization_integral += current_localization*elapsed_time_since_last_transition; + current_states.localization = current_localization; + + // Get the proportion of time spent in each zone + current_states.proportion_time_spent_in_zones = time_spent_in_zones_.getProportionTimeSpent(); + + return current_states; + } + + return current_states; +} + +void AUVStateManager::commandToNextZone(AUVStates::Zones new_zone, ros_queue_msgs::TransmissionVector command_of_controller) +{ + ros_queue_experiments::AuvStates current_state = getCurrentStates(); + + std::lock_guard lock(state_access_mutex_); + + // Save the time spent in the last zone + float elapsed_time = (ros::Time::now() - last_transition_time_).toSec(); + time_spent_in_zones_.addTimeSpentInState(elapsed_time, AUVStates::getZoneFromTransmissionVector(current_state.current_zone)); + + // Save the current states to serve as the new point to evaluate the future states from. + states_at_last_transition_ = current_state; + + // Set the new zone and the timing of the transition. + states_at_last_transition_.last_zone = states_at_last_transition_.current_zone; + states_at_last_transition_.current_zone = AUVStates::getTransmissionVectorFromZone(new_zone); + states_at_last_transition_.zone_selected_by_controller = command_of_controller; + states_at_last_transition_.transition_completion = 0.0; + + last_transition_time_ = ros::Time::now(); +} + diff --git a/ros_queue_experiments/src/auv_states.cpp b/ros_queue_experiments/src/auv_states.cpp new file mode 100644 index 0000000..ec913e2 --- /dev/null +++ b/ros_queue_experiments/src/auv_states.cpp @@ -0,0 +1,69 @@ +#include "ros_queue_experiments/auv_states.hpp" + +#include "ros/ros.h" + +AUVStates::Zones AUVStates::getZoneFromTransmissionVector(const ros_queue_msgs::TransmissionVector& transmission_vector) +{ + bool transition_found = false; + AUVStates::Zones zone = AUVStates::Zones::TaskZone; + + if (transmission_vector.transmission_vector[0]) + { + zone = AUVStates::Zones::TaskZone; + transition_found = true; + } + if (transmission_vector.transmission_vector[1]) + { + if(!transition_found) + { + zone = AUVStates::Zones::HighLocZone; + transition_found = true; + } + else + { + ROS_ERROR("More than one transition found in the transmission vector. This is not allowed."); + } + } + if (transmission_vector.transmission_vector[2]) + { + if(!transition_found) + { + zone = AUVStates::Zones::ColdZone; + transition_found = true; + } + else + { + ROS_ERROR("More than one transition found in the transmission vector. This is not allowed."); + } + } + + if (!transition_found) + { + ROS_ERROR("No transition found in the transmission vector. This is not allowed."); + } + return zone; +} + +ros_queue_msgs::TransmissionVector AUVStates::getTransmissionVectorFromZone(AUVStates::Zones zone) +{ + ros_queue_msgs::TransmissionVector transmission_vector; + transmission_vector.transmission_vector = std::vector{0, 0, 0}; + + switch (zone) + { + case AUVStates::Zones::TaskZone: + transmission_vector.transmission_vector[0] = 1; + break; + case AUVStates::Zones::HighLocZone: + transmission_vector.transmission_vector[1] = 1; + break; + case AUVStates::Zones::ColdZone: + transmission_vector.transmission_vector[2] = 1; + break; + default: + ROS_ERROR("Zone not recognized. Transmission vector will be set to all false."); + break; + } + + return transmission_vector; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/auv_system.cpp b/ros_queue_experiments/src/auv_system.cpp new file mode 100644 index 0000000..5c51824 --- /dev/null +++ b/ros_queue_experiments/src/auv_system.cpp @@ -0,0 +1,104 @@ +#include "ros_queue_experiments/auv_system.hpp" +#include "ros_queue_experiments/auv_state_manager.hpp" + +#include "ros_queue_experiments/metrics/low_temperature_services.hpp" +#include "ros_queue_experiments/metrics/metric_services.hpp" +#include "ros_queue_experiments/metrics/temperature_services.hpp" +#include "ros_queue_experiments/metrics/renewal_time_services.hpp" +#include "ros_queue_experiments/metrics/localization_services.hpp" +#include "ros_queue_experiments/metrics/task_publisher.hpp" +#include "ros_queue_experiments/metrics/penalty_services.hpp" + +#include "ros_queue_experiments/AuvStates.h" + +AUVSystem::AUVSystem(ros::NodeHandle& nh): nh_(nh), ns_nh_(ros::NodeHandle()) +{ + // Initialize the state manager + auv_state_manager_ = std::make_shared(this); + + expected_time_services_ = std::make_shared(nh_, "renewal_time", auv_state_manager_); + temperature_services_ = std::make_shared(nh_, "temperature", auv_state_manager_, expected_time_services_); + low_temperature_services_ = std::make_shared(nh_, "low_temperature", auv_state_manager_, expected_time_services_); + task_services_ = std::make_shared(nh_, "task", auv_state_manager_, expected_time_services_); + localization_services_ = std::make_shared(nh_, "localization", auv_state_manager_, expected_time_services_); + penalty_metric_services_ = std::make_shared(nh_, "penalty", auv_state_manager_); + + auv_state_service_ = nh_.advertiseService("get_auv_state", &AUVSystem::auvStateCallback, this); + + command_service_ = nh_.advertiseService("new_command", &AUVSystem::commandCallback, this); + + // Initialize the action set service + action_set_service_ = nh_.advertiseService("potential_action_set", &AUVSystem::potentialActionSetCallback, this); + + // Initialize the publishers + state_pub_ = nh_.advertise("auv_state", 1); + + control_started_sub_ = ns_nh_.subscribe("optimal_controller/control_loop_started", 1, &AUVSystem::stateMonitoringCallback, this); +} + +bool AUVSystem::potentialActionSetCallback(ros_queue_msgs::PotentialTransmissionVectorSpaceFetch::Request& req, + ros_queue_msgs::PotentialTransmissionVectorSpaceFetch::Response& res) +{ + if (auv_state_manager_) + { + res.action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::TaskZone)); + res.action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::HighLocZone)); + res.action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::ColdZone)); + } + else + { + ROS_ERROR("AUV state manager not initialized"); + return false; + } + return true; +} + +bool AUVSystem::auvStateCallback(ros_queue_experiments::GetRealAUVStates::Request& req, + ros_queue_experiments::GetRealAUVStates::Response& res) +{ + if(auv_state_manager_) + { + res.states = auv_state_manager_->getCurrentStates(); + } + else + { + ROS_ERROR("AUV state manager not initialized"); + return false; + } + return true; +} + + +bool AUVSystem::commandCallback(ros_queue_experiments::SendNewAUVCommand::Request& req, + ros_queue_experiments::SendNewAUVCommand::Response& res) +{ + if(auv_state_manager_) + { + // Send current state before changing the zone to the new one so the monitoring can see changes + // that the queue server should have been updated with. + //state_pub_.publish(auv_state_manager_->getCurrentStates()); + + AUVStates::Zones new_zone = AUVStates::getZoneFromTransmissionVector(req.command); + res.time_to_execute = expected_time_services_->getRealRenewalTimeWithTransitionFromCurrentState(new_zone); + auv_state_manager_->commandToNextZone(new_zone, req.command_of_controller); + ROS_INFO("New command received. Going to zone %d", new_zone); + } + else + { + ROS_ERROR("AUV state manager not initialized"); + return false; + } + return true; +} + +void AUVSystem::stateMonitoringCallback(const std_msgs::Empty::ConstPtr& msg) +{ + if(auv_state_manager_) + { + state_pub_.publish(auv_state_manager_->getCurrentStates()); + } + else + { + ROS_ERROR("AUV state manager not initialized"); + } +} \ No newline at end of file diff --git a/ros_queue_experiments/src/disturbed_action_server.cpp b/ros_queue_experiments/src/disturbed_action_server.cpp new file mode 100644 index 0000000..a71b200 --- /dev/null +++ b/ros_queue_experiments/src/disturbed_action_server.cpp @@ -0,0 +1,197 @@ +#include "ros_queue_experiments/disturbed_action_server.hpp" + +#include "ros_queue_experiments/ActionPerformance.h" +#include "ros_queue_experiments/auv_states.hpp" + +#include "std_msgs/UInt8.h" + +#include + +DisturbedActionServer::DisturbedActionServer(ros::NodeHandle& nh): nhp_(nh), nh_(ros::NodeHandle()), action_server_(nhp_, "disturbed_action_server", false) +{ + if(nhp_.getParam("pertubation_at_each_x_control_steps", perturbation_at_each_x_control_steps_)) + { + if (perturbation_at_each_x_control_steps_ > 0) + { + ROS_INFO_STREAM("Perturbation set at each " << perturbation_at_each_x_control_steps_ << " control steps."); + } + else if(perturbation_at_each_x_control_steps_ == 0) + { + ROS_INFO_STREAM("perturbation_at_each_x_control_steps_ set to 0 so no perturbations will be applied."); + } + else + { + ROS_ERROR("Pertubation at each x control steps cannot be negative. No perturbations will be applied."); + } + } + else + { + ROS_ERROR("Pertubation at each x control steps not set. Not perturbations will be applied."); + } + + std::string perturbation_type_param; + if(nhp_.getParam("pertubation_type", perturbation_type_param)) + { + if (perturbation_type_param == "not_moving") + { + perturbation_type_ = PerturbationType::NotMoving; + } + else if (perturbation_type_param == "offset_next_step") + { + perturbation_type_ = PerturbationType::OffsetPlusOne; + } + else + { + ROS_ERROR("Pertubation type not recognized. No perturbations will be applied. Supported types: not_moving, offset_next_step."); + } + } + + nhp_.getParam("is_dummy", is_dummy_); + + if(is_dummy_) + { + ROS_INFO_STREAM("The disturbed action server "<< ros::this_node::getName() << " is dummy and will not any action to the system. No perturbations are applied."); + } + else + { + auv_state_client_ = PersistentServiceClient(nh_, "auv_system_node/get_auv_state"); + ROS_INFO("Waiting for the AUV state service to be ready..."); + auv_state_client_.waitForExistence(); + ROS_INFO("AUV state service is ready."); + + auv_action_client_ = PersistentServiceClient(nh_, "auv_system_node/new_command"); + ROS_INFO("Waiting for the AUV action service to be ready..."); + auv_action_client_.waitForExistence(); + ROS_INFO("AUV action service is ready."); + + send_sucess_timer_ = nhp_.createTimer(ros::Duration(1.0), &DisturbedActionServer::sendSuccessCallback, this, true); + send_sucess_timer_.stop(); + } + + action_server_.registerGoalCallback(boost::bind(&DisturbedActionServer::commandReceivedCallback, this)); + action_server_.registerPreemptCallback(boost::bind(&DisturbedActionServer::preemptActionCallback, this)); + action_server_.start(); + + queue_stats_client_ = PersistentServiceClient(nh_, "queue_server/get_server_stats"); + queue_stats_client_.waitForExistence(); + action_performance_publisher_ = nhp_.advertise("action_performance", 10); +} + +void DisturbedActionServer::commandReceivedCallback() +{ + ros::Time start_time = ros::Time::now(); + + // Cancel old action if its still active and stop it's success callback from being called + if (action_server_.isActive()) + { + action_server_.setAborted(); + send_sucess_timer_.stop(); + } + + auto goal = action_server_.acceptNewGoal(); + ros_queue_msgs::TransmissionVector target_action = goal->action_goal; + ros_queue_msgs::TransmissionVector disturbed_action = goal->action_goal; + + // Add perturbations to the action + if (!is_dummy_) + { + ros_queue_experiments::GetRealAUVStates states_srv; + if(auv_state_client_.call(states_srv)) + { + if ((perturbation_at_each_x_control_steps_ != 0) && + (steps_since_last_perturbation_ == perturbation_at_each_x_control_steps_-1)) + { + if (perturbation_type_ == PerturbationType::NotMoving) + { + disturbed_action = states_srv.response.states.current_zone; + } + else if (perturbation_type_ == PerturbationType::OffsetPlusOne) + { + for(int zone_index =0 ; zone_index < disturbed_action.transmission_vector.size(); zone_index++) + { + if (disturbed_action.transmission_vector[zone_index] == 1) + { + // If last zone, set it to the first zone + if ((zone_index + 1) == disturbed_action.transmission_vector.size()) + { + disturbed_action.transmission_vector[0] = 1; + } + else + { + disturbed_action.transmission_vector[zone_index + 1] = 1; + } + + disturbed_action.transmission_vector[zone_index] = 0; + break; + } + } + } + } + ++steps_since_last_perturbation_; + if (steps_since_last_perturbation_ == perturbation_at_each_x_control_steps_) + { + steps_since_last_perturbation_ = 0; + } + } + else + { + ROS_ERROR("Failed to call service to get the current AUV states. No perturbations will be applied."); + } + + // Send the disturbed action to the AUV system + ros_queue_experiments::SendNewAUVCommand action_srv; + action_srv.request.command = disturbed_action; + action_srv.request.command_of_controller = target_action; + + float time_to_execute = 0.0; + if(auv_action_client_.call(action_srv)) + { + time_to_execute = action_srv.response.time_to_execute; + } + else + { + ROS_ERROR("Failed to call service to send the disturbed action to the AUV system."); + } + + // Reset the time of the success callback and start it. + float elapsed_time = (ros::Time::now() - start_time).toSec(); + send_sucess_timer_.setPeriod(ros::Duration(time_to_execute-elapsed_time), true); + send_sucess_timer_.start(); + } + + // Send action performance monitoring + ros_queue_experiments::ActionPerformance action_performance_msg; + ros_queue_msgs::QueueServerStatsFetch queue_server_stats_fetch; + if(queue_stats_client_.call(queue_server_stats_fetch)) + { + action_performance_msg.header.stamp = ros::Time::now(); + + action_performance_msg.target_action = target_action; + action_performance_msg.applied_action = disturbed_action; + + action_performance_msg.controller_action_index = AUVStates::getZoneFromTransmissionVector(action_performance_msg.target_action); + action_performance_msg.applied_action_index = AUVStates::getZoneFromTransmissionVector(action_performance_msg.applied_action); + action_performance_msg.queue_server_stats = queue_server_stats_fetch.response.queue_stats; + + action_performance_publisher_.publish(action_performance_msg); + } + else + { + ROS_ERROR("Perturbation node: Failed to call service to get the queue server stats."); + } + +} + +void DisturbedActionServer::preemptActionCallback() +{ + ROS_INFO("Preempting the action."); + action_server_.setPreempted(); + send_sucess_timer_.stop(); +} + +void DisturbedActionServer::sendSuccessCallback(const ros::TimerEvent& event) +{ + ros_queue_msgs::TransmissionVectorResult result; + result.success = true; + action_server_.setSucceeded(result); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/disturbed_action_server_node.cpp b/ros_queue_experiments/src/disturbed_action_server_node.cpp new file mode 100644 index 0000000..6c02950 --- /dev/null +++ b/ros_queue_experiments/src/disturbed_action_server_node.cpp @@ -0,0 +1,15 @@ +#include "ros_queue_experiments/disturbed_action_server.hpp" + +#include "ros/ros.h" + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "disturbed_action_server"); + ros::NodeHandle nh("~"); + + DisturbedActionServer disturbed_action_server(nh); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/metrics/dual_metric_services.cpp b/ros_queue_experiments/src/metrics/dual_metric_services.cpp new file mode 100644 index 0000000..4771943 --- /dev/null +++ b/ros_queue_experiments/src/metrics/dual_metric_services.cpp @@ -0,0 +1,92 @@ +#include "ros_queue_experiments/metrics/dual_metric_services.hpp" + +#include "ros_queue_msgs/GetQueueControllerTiming.h" + +DualMetricServices::DualMetricServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services): nh_(nh), ns_nh_(ros::NodeHandle()), metric_name_(metric_name), auv_state_manager_(auv_state_manager), renewal_time_services_(renewal_time_services) +{ + real_renewal_service_ = ns_nh_.serviceClient("queue_controller/get_last_renewal_time"); + + real_arrival_metric_service_ = nh_.advertiseService(metric_name_ + "/arrival/change/real_metric", &DualMetricServices::realArrivalServiceMetricCallback, this); + real_arrival_prediction_service_ = nh_.advertiseService(metric_name_ + "/arrival/change/real_expected_metric", &DualMetricServices::realArrivalPredictionServiceMetricCallback, this); + expected_arrival_metric_service_ = nh_.advertiseService(metric_name_ + "/arrival/change/expected_metric", &DualMetricServices::expectedArrivalServiceMetricCallback, this); + real_departure_metric_service_ = nh_.advertiseService(metric_name_ + "/departure/change/real_metric", &DualMetricServices::realDepartureServiceMetricCallback, this); + real_departure_prediction_service_ = nh_.advertiseService(metric_name_ + "/departure/change/real_expected_metric", &DualMetricServices::realDeparturePredictionServiceMetricCallback, this); + expected_departure_metric_service_ = nh_.advertiseService(metric_name_ + "/departure/change/expected_metric", &DualMetricServices::expectedDepartureServiceMetricCallback, this); + + real_arrival_rate_metric_service_ = nh_.advertiseService(metric_name_ + "/arrival/rate/real_metric", &DualMetricServices::realArrivalRateServiceMetricCallback, this); + expected_arrival_rate_metric_service_ = nh_.advertiseService(metric_name_ + "/arrival/rate/expected_metric", &DualMetricServices::expectedArrivalRateServiceMetricCallback, this); + real_departure_rate_metric_service_ = nh_.advertiseService(metric_name_ + "/departure/rate/real_metric", &DualMetricServices::realDepartureRateServiceMetricCallback, this); + expected_departure_rate_metric_service_ = nh_.advertiseService(metric_name_ + "/departure/rate/expected_metric", &DualMetricServices::expectedDepartureRateServiceMetricCallback, this); +} + +// Change services +bool DualMetricServices::realArrivalServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return realArrivalMetricCallback(req, res); +} + +bool DualMetricServices::realArrivalPredictionServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return realArrivalPredictionMetricCallback(req, res); +} + +bool DualMetricServices::expectedArrivalServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return expectedArrivalMetricCallback(req, res); +} + +bool DualMetricServices::realDepartureServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return realDepartureMetricCallback(req, res); +} + +bool DualMetricServices::realDeparturePredictionServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return realDeparturePredictionMetricCallback(req, res); +} + +bool DualMetricServices::expectedDepartureServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return expectedDepartureMetricCallback(req, res); +} + +// Rate services +bool DualMetricServices::realArrivalRateServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return realArrivalRateMetricCallback(req, res); +} + +bool DualMetricServices::expectedArrivalRateServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return expectedArrivalRateMetricCallback(req, res); +} + +bool DualMetricServices::realDepartureRateServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return realDepartureRateMetricCallback(req, res); +} + +bool DualMetricServices::expectedDepartureRateServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return expectedDepartureRateMetricCallback(req, res); +} + +ros_queue_experiments::AuvStates DualMetricServices::getCurrentStates() +{ + if(auv_state_manager_ == nullptr) + { + ROS_ERROR("AUVStateManager is not set"); + return ros_queue_experiments::AuvStates(); + } + return auv_state_manager_->getCurrentStates(); +} diff --git a/ros_queue_experiments/src/metrics/localization_services.cpp b/ros_queue_experiments/src/metrics/localization_services.cpp new file mode 100644 index 0000000..281f549 --- /dev/null +++ b/ros_queue_experiments/src/metrics/localization_services.cpp @@ -0,0 +1,251 @@ +#include "ros_queue_experiments/metrics/localization_services.hpp" + +#include + +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_experiments/auv_states.hpp" + +using std::string; + +LocalizationServices::LocalizationServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services): DualMetricServices(nh, metric_name, auv_state_manager, renewal_time_services) +{ + XmlRpc::XmlRpcValue localization_config; + if(!nh_.getParam("localization_target", localization_target_)) + { + ROS_ERROR("Localization target is not set"); + } + + if(nh_.getParam("localization", localization_config)) + { + for(int model_index =0; model_index < localization_config.size(); ++model_index) + { + auto model_it = localization_config[model_index].begin(); + + XmlRpc::XmlRpcValue loc_prediction_config = model_it->second; + + for(int zone_config_index = 0; zone_config_index < loc_prediction_config.size(); ++zone_config_index) + { + auto zone_config_it = loc_prediction_config[zone_config_index].begin(); + + const string& zone_name = zone_config_it->first; + XmlRpc::XmlRpcValue zone_values = zone_config_it->second; + + AUVStates::Zones zone_from_config = AUVStates::Zones::TaskZone; + + if (zone_name == "TaskZone") + { + zone_from_config = AUVStates::Zones::TaskZone; + } + else if(zone_name == "ColdZone") + { + zone_from_config = AUVStates::Zones::ColdZone; + } + else if(zone_name == "HighLocZone") + { + zone_from_config = AUVStates::Zones::HighLocZone; + } + else + { + break; + } + + for(int value_index = 0; value_index < zone_values.size(); ++value_index) + { + auto value_param = zone_values[value_index].begin(); + const string& value_name = value_param->first; + + if (value_name == "loc_uncertainty") + { + if(model_it->first == "prediction_model") + { + predicted_localization_uncertainties_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + else if(model_it->first == "real_model") + { + real_localization_uncertainties_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + } + } + } + } + } +} + + +// Change services +bool LocalizationServices::realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_msgs::GetQueueControllerTiming last_renewal_msg; + + if(real_renewal_service_.call(last_renewal_msg)) + { + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + float localization_rate = getRealLocalizationUncertainty(current_zone); + + float last_renewal_time = last_renewal_msg.response.timing.renewal_time; + + // Return the change as the integral of the rate of the localization + res.value = last_renewal_time*localization_rate; + + // Add the change that happened during the controller execution + AUVStates::Zones last_zone = AUVStates::getZoneFromTransmissionVector(current_states.last_zone); + float last_zone_localization_rate = getRealLocalizationUncertainty(last_zone); + + float last_controller_execution_time = last_renewal_msg.response.timing.execution_time; + res.value += last_zone_localization_rate*last_controller_execution_time; + } + else + { + ROS_ERROR_STREAM("Localization service couldn't call the last renewal service: "<getRealRenewalTimeWithTransitionFromCurrentState(zone); + + res.predictions.push_back(real_localization_uncertainties_[zone]*expected_time); + } + } + + return true; +} + +bool LocalizationServices::expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + if(renewal_time_services_) + { + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector &action = req.action_set.action_set[action_index]; + AUVStates::Zones zone = AUVStates::getZoneFromTransmissionVector(action); + + float expected_time = renewal_time_services_->getPredictedRenewalTimeWithTransitionFromCurrentState(zone); + + res.predictions.push_back(predicted_localization_uncertainties_[zone]*expected_time); + } + } + + return true; +} + + bool LocalizationServices::realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_msgs::GetQueueControllerTiming last_renewal_msg; + + if(real_renewal_service_.call(last_renewal_msg)) + { + float localization_target_rate_last_state = this->localization_target_; + + float last_renewal_time = last_renewal_msg.response.timing.renewal_time; + float last_controller_execution_time = last_renewal_msg.response.timing.execution_time; + + // Return the change as the integral of the rate of the localization + res.value = localization_target_rate_last_state*(last_renewal_time + last_controller_execution_time); + } + else + { + ROS_ERROR_STREAM("Localization service couldn't call the last renewal service: "< auv_state_manager, std::shared_ptr renewal_time_services): + TemperatureServices(nh, metric_name, auv_state_manager, renewal_time_services) +{ + if(!nh_.getParam("low_temp_target", temp_target_)) + { + ROS_ERROR("Missing low_temp_target parameter."); + } +} + +// Overriden methods +bool LowTemperatureServices::realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealDepartureMetricCallback(req, res); +} + +bool LowTemperatureServices::realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempRealDeparturePredictionMetricCallback(req, res); +} + +bool LowTemperatureServices::expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedDepartureMetricCallback(req, res); +} + +bool LowTemperatureServices::realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealArrivalMetricCallback(req, res); +} + +bool LowTemperatureServices::realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempRealArrivalPredictionMetricCallback(req, res); +} + +bool LowTemperatureServices::expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedArrivalMetricCallback(req, res); +} + +// Rate callbacks +bool LowTemperatureServices::realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealDepartureRateMetricCallback(req, res); +} + +bool LowTemperatureServices::expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedDepartureRateMetricCallback(req, res); +} + +bool LowTemperatureServices::realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealArrivalRateMetricCallback(req, res); +} + +bool LowTemperatureServices::expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedArrivalRateMetricCallback(req, res); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/metrics/metric_services.cpp b/ros_queue_experiments/src/metrics/metric_services.cpp new file mode 100644 index 0000000..0425697 --- /dev/null +++ b/ros_queue_experiments/src/metrics/metric_services.cpp @@ -0,0 +1,38 @@ +#include "ros_queue_experiments/metrics/metric_services.hpp" + +MetricServices::MetricServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager): nh_(nh), ns_nh_(ros::NodeHandle()), metric_name_(metric_name), auv_state_manager_(auv_state_manager) +{ + real_metric_service_ = nh_.advertiseService(metric_name_ + "/real_metric", &MetricServices::realServiceMetricCallback, this); + expected_metric_service_ = nh_.advertiseService(metric_name_ + "/expected_metric", &MetricServices::expectedServiceMetricCallback, this); + real_expected_metric_service_ = nh_.advertiseService(metric_name_ + "/real_expected_metric", &MetricServices::realExpectedServiceMetricCallback, this); +} + + +bool MetricServices::realServiceMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return realMetricCallback(req, res); +} + +bool MetricServices::expectedServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return expectedMetricCallback(req, res); +} + + +bool MetricServices::realExpectedServiceMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return realExpectedMetricCallback(req, res); +} + +ros_queue_experiments::AuvStates MetricServices::getCurrentStates() +{ + if(auv_state_manager_ == nullptr) + { + ROS_ERROR("AUVStateManager is not set"); + return ros_queue_experiments::AuvStates(); + } + return auv_state_manager_->getCurrentStates(); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/metrics/penalty_services.cpp b/ros_queue_experiments/src/metrics/penalty_services.cpp new file mode 100644 index 0000000..1aa05d7 --- /dev/null +++ b/ros_queue_experiments/src/metrics/penalty_services.cpp @@ -0,0 +1,141 @@ +#include "ros_queue_experiments/metrics/penalty_services.hpp" + +#include + +using std::string; + +PenaltyServices::PenaltyServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager): MetricServices(nh, metric_name, auv_state_manager) +{ + XmlRpc::XmlRpcValue penalty_time_config; + + if(nh_.getParam("penalty", penalty_time_config)) + { + for(int model_index =0; model_index < penalty_time_config.size(); ++model_index) + { + auto model_it = penalty_time_config[model_index].begin(); + + XmlRpc::XmlRpcValue model_configs = model_it->second; + + bool is_real_model = false; + + if(model_it->first == "real_model") + { + is_real_model = true; + } + for (int from_zone_index = 0 ; from_zone_index < model_configs.size(); ++from_zone_index) + { + auto from_zone_xmlrp_it = model_configs[from_zone_index].begin(); + + AUVStates::Zones from_zone; + if(from_zone_xmlrp_it->first == "fromTaskZone") + { + from_zone = AUVStates::Zones::TaskZone; + } + else if (from_zone_xmlrp_it->first == "fromHighLocZone") + { + from_zone = AUVStates::Zones::HighLocZone; + } + else if (from_zone_xmlrp_it->first == "fromColdZone") + { + from_zone = AUVStates::Zones::ColdZone; + } + else + { + break; + } + + XmlRpc::XmlRpcValue from_zone_configs = from_zone_xmlrp_it->second; + + for(int to_zone_index = 0; to_zone_index < from_zone_configs.size(); ++to_zone_index) + { + auto to_zone_config_it = from_zone_configs[to_zone_index].begin(); + + const string& zone_name = to_zone_config_it->first; + XmlRpc::XmlRpcValue transition_time = to_zone_config_it->second; + + AUVStates::Zones to_zone = AUVStates::Zones::TaskZone; + + if (zone_name == "toTaskZone") + { + to_zone = AUVStates::Zones::TaskZone; + } + else if(zone_name == "toHighLocZone") + { + to_zone = AUVStates::Zones::HighLocZone; + } + else if(zone_name == "toColdZone") + { + to_zone = AUVStates::Zones::ColdZone; + } + else + { + break; + } + + if (is_real_model) + { + real_penalty_transitions_[from_zone][to_zone] = static_cast(static_cast(transition_time)); + } + else + { + predicted_penalty_transitions_[from_zone][to_zone] = static_cast(static_cast(transition_time)); + } + } + } + } + } +} + +float PenaltyServices::getRealPenaltyTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone) +{ + return real_penalty_transitions_[from_zone][to_zone]; +} + +float PenaltyServices::getPredictedPenaltyTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone) +{ + return this->predicted_penalty_transitions_[from_zone][to_zone]; +} + +bool PenaltyServices::realMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + AUVStates::Zones from_zone = AUVStates::getZoneFromTransmissionVector(current_states.last_zone); + + res.value = real_penalty_transitions_[from_zone][from_zone]; + return true; +} + +bool PenaltyServices::realExpectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + const ros_queue_experiments::AuvStates current_states = getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + for (int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector& action = req.action_set.action_set[action_index]; + AUVStates::Zones action_zone = AUVStates::getZoneFromTransmissionVector(action); + res.predictions.push_back(real_penalty_transitions_[current_zone][action_zone]); + } + + return true; +} + +bool PenaltyServices::expectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + const ros_queue_experiments::AuvStates current_states = getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + for (int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector& action = req.action_set.action_set[action_index]; + AUVStates::Zones action_zone = AUVStates::getZoneFromTransmissionVector(action); + res.predictions.push_back(predicted_penalty_transitions_[current_zone][action_zone]); + } + + return true; +} + diff --git a/ros_queue_experiments/src/metrics/renewal_time_services.cpp b/ros_queue_experiments/src/metrics/renewal_time_services.cpp new file mode 100644 index 0000000..ee47c2a --- /dev/null +++ b/ros_queue_experiments/src/metrics/renewal_time_services.cpp @@ -0,0 +1,173 @@ +#include "ros_queue_experiments/metrics/renewal_time_services.hpp" + +RenewalTimeServices::RenewalTimeServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager): MetricServices(nh, metric_name, auv_state_manager) +{ + XmlRpc::XmlRpcValue renewal_time_config; + + if(nh_.getParam("renewal_time", renewal_time_config)) + { + for(int model_index =0; model_index < renewal_time_config.size(); ++model_index) + { + auto model_it = renewal_time_config[model_index].begin(); + + XmlRpc::XmlRpcValue model_configs = model_it->second; + + bool is_real_model = false; + + if(model_it->first == "real_model") + { + is_real_model = true; + } + for (int from_zone_index = 0 ; from_zone_index < model_configs.size(); ++from_zone_index) + { + auto from_zone_xmlrp_it = model_configs[from_zone_index].begin(); + + AUVStates::Zones from_zone; + if(from_zone_xmlrp_it->first == "fromTaskZone") + { + from_zone = AUVStates::Zones::TaskZone; + } + else if (from_zone_xmlrp_it->first == "fromHighLocZone") + { + from_zone = AUVStates::Zones::HighLocZone; + } + else if (from_zone_xmlrp_it->first == "fromColdZone") + { + from_zone = AUVStates::Zones::ColdZone; + } + else + { + break; + } + + XmlRpc::XmlRpcValue from_zone_configs = from_zone_xmlrp_it->second; + + for(int to_zone_index = 0; to_zone_index < from_zone_configs.size(); ++to_zone_index) + { + auto to_zone_config_it = from_zone_configs[to_zone_index].begin(); + + const string& zone_name = to_zone_config_it->first; + XmlRpc::XmlRpcValue transition_time = to_zone_config_it->second; + + AUVStates::Zones to_zone = AUVStates::Zones::TaskZone; + + if (zone_name == "toTaskZone") + { + to_zone = AUVStates::Zones::TaskZone; + } + else if(zone_name == "toHighLocZone") + { + to_zone = AUVStates::Zones::HighLocZone; + } + else if(zone_name == "toColdZone") + { + to_zone = AUVStates::Zones::ColdZone; + } + else + { + break; + } + + if (is_real_model) + { + real_renewal_time_transitions_[from_zone][to_zone] = static_cast(static_cast(transition_time)); + } + else + { + predicted_renewal_time_transitions_[from_zone][to_zone] = static_cast(static_cast(transition_time)); + } + } + } + } + } +} + +float RenewalTimeServices::getRealRenewalTimeWithStateTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone) +{ + return real_renewal_time_transitions_[from_zone][to_zone]; +} + +float RenewalTimeServices::getPredictedRenewalTimeWithStateTransition(AUVStates::Zones from_zone, AUVStates::Zones to_zone) +{ + return predicted_renewal_time_transitions_[from_zone][to_zone]; +} + +float RenewalTimeServices::getRealRenewalTimeWithTransitionFromCurrentState(AUVStates::Zones to_zone) +{ + if(auv_state_manager_) + { + const ros_queue_experiments::AuvStates current_states = auv_state_manager_->getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + return getRealRenewalTimeWithStateTransition(current_zone, to_zone); + } + + return 0.0; +} + +float RenewalTimeServices::getPredictedRenewalTimeWithTransitionFromCurrentState(AUVStates::Zones to_zone) +{ + if(auv_state_manager_) + { + const ros_queue_experiments::AuvStates current_states = auv_state_manager_->getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + return getPredictedRenewalTimeWithStateTransition(current_zone, to_zone); + } + + return 0.0; +} + +bool RenewalTimeServices::realMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + if(auv_state_manager_) + { + const ros_queue_experiments::AuvStates current_states = auv_state_manager_->getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + const AUVStates::Zones to_zone = AUVStates::getZoneFromTransmissionVector(current_states.last_zone); + return getRealRenewalTimeWithStateTransition(current_zone, to_zone); + } + return true; +} + +bool RenewalTimeServices::realExpectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(auv_state_manager_) + { + const ros_queue_experiments::AuvStates current_states = auv_state_manager_->getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + for (int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector& action = req.action_set.action_set[action_index]; + AUVStates::Zones action_zone = AUVStates::getZoneFromTransmissionVector(action); + res.predictions.push_back(getRealRenewalTimeWithStateTransition(current_zone, action_zone)); + } + + return true; + } + return false; +} + +bool RenewalTimeServices::expectedMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(auv_state_manager_) + { + const ros_queue_experiments::AuvStates current_states = auv_state_manager_->getCurrentStates(); + const AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + for (int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector& action = req.action_set.action_set[action_index]; + AUVStates::Zones action_zone = AUVStates::getZoneFromTransmissionVector(action); + res.predictions.push_back(getPredictedRenewalTimeWithStateTransition(current_zone, action_zone)); + } + + return true; + } + return false; +} + diff --git a/ros_queue_experiments/src/metrics/task_publisher.cpp b/ros_queue_experiments/src/metrics/task_publisher.cpp new file mode 100644 index 0000000..f63a67b --- /dev/null +++ b/ros_queue_experiments/src/metrics/task_publisher.cpp @@ -0,0 +1,283 @@ +#include "ros_queue_experiments/metrics/task_publisher.hpp" + +#include + +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_experiments/auv_states.hpp" + +#include "ros_queue_msgs/ByteSizeRequest.h" + +#include "std_msgs/Int32.h" + +using std::string; + +TaskPublisher::TaskPublisher(ros::NodeHandle& nh, std::string metric_name, + std::shared_ptr auv_state_manager, + std::shared_ptr renewal_time_services): + DualMetricServices(nh, metric_name, auv_state_manager, renewal_time_services) + { + XmlRpc::XmlRpcValue tasks_config; + if(nh_.getParam("task_metrics", tasks_config)) + { + for(int model_index =0; model_index < tasks_config.size(); ++model_index) + { + auto model_it = tasks_config[model_index].begin(); + + XmlRpc::XmlRpcValue task_prediction_config = model_it->second; + + for(int zone_config_index = 0; zone_config_index < task_prediction_config.size(); ++zone_config_index) + { + auto zone_config_it = task_prediction_config[zone_config_index].begin(); + + const string& zone_name = zone_config_it->first; + XmlRpc::XmlRpcValue zone_values = zone_config_it->second; + + AUVStates::Zones zone_from_config = AUVStates::Zones::TaskZone; + + if (zone_name == "TaskZone") + { + zone_from_config = AUVStates::Zones::TaskZone; + } + else if(zone_name == "ColdZone") + { + zone_from_config = AUVStates::Zones::ColdZone; + } + else if(zone_name == "HighLocZone") + { + zone_from_config = AUVStates::Zones::HighLocZone; + } + else + { + break; + } + + for(int value_index = 0; value_index < zone_values.size(); ++value_index) + { + auto value_param = zone_values[value_index].begin(); + const string& value_name = value_param->first; + + if (value_name == "departing_tasks_rate") + { + if(model_it->first == "prediction_model") + { + predicted_task_departure_rates_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + else if(model_it->first == "real_model") + { + real_task_departure_rates_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + } + } + } + } + } + + if(nh_.getParam("arrival_task_per_second", arrival_task_per_second_)) + { + if(arrival_task_per_second_ > 0.0) + { + publisher_ = nh_.advertise("incoming_tasks", 10); + timer_ = nh_.createTimer(ros::Duration(1.0/arrival_task_per_second_), &TaskPublisher::publishTask, this); + } + else + { + ROS_ERROR("Arrival tasks per second must be greater than 0"); + } + } + else + { + ROS_ERROR("Arrival tasks per second is not set"); + } + + + last_transmission_time_ = ros::Time::now(); + qos_transmission_service_ = nh_.advertiseService("qos_transmission",&TaskPublisher::qosTransmissionCallback,this); +} + +bool TaskPublisher::qosTransmissionCallback(ros_queue_msgs::ByteSizeRequest::Request& req, + ros_queue_msgs::ByteSizeRequest::Response& res) +{ + ros::Time current_time = ros::Time::now(); + double time_diff = (current_time - last_transmission_time_).toSec() + accumulated_untransmitted_time_; + + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + + if (abs(real_task_departure_rates_[current_zone]) > 1e-6) + { + const float second_between_messages = 1.0/real_task_departure_rates_[current_zone]; + while(time_diff >= second_between_messages) + { + res.nb_of_bytes += sizeof(std_msgs::Int32); + time_diff -= second_between_messages; + } + + accumulated_untransmitted_time_ = time_diff; + } + else + { + accumulated_untransmitted_time_ = 0.0; + res.nb_of_bytes = 0; + } + + last_transmission_time_ = current_time; + return true; +} + +void TaskPublisher::publishTask(const ros::TimerEvent& event) +{ + std_msgs::Int32 task; + task.data = last_task_id_; + publisher_.publish(task); + ++last_task_id_; +} + + +// Change services +bool TaskPublisher::realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + //TODO add real arrival + return false; +} + +bool TaskPublisher::expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(renewal_time_services_) + { + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + AUVStates::Zones target_zone = AUVStates::getZoneFromTransmissionVector(req.action_set.action_set[action_index]); + + float renewal_time = renewal_time_services_->getPredictedRenewalTimeWithTransitionFromCurrentState(target_zone); + int nb_tasks = static_cast(arrival_task_per_second_*renewal_time); + //ROS_WARN_STREAM("Expected arrival float: "<< arrival_task_per_second_*renewal_time<< std::endl<<"Expected arrival int: "<< nb_tasks); + res.predictions.push_back(nb_tasks*sizeof(std_msgs::Int32)); + } + } + else + { + ROS_ERROR("Renewal time services not set"); + } + + return true; +} + +bool TaskPublisher::realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(renewal_time_services_) + { + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + AUVStates::Zones target_zone = AUVStates::getZoneFromTransmissionVector(req.action_set.action_set[action_index]); + + float renewal_time = renewal_time_services_->getRealRenewalTimeWithTransitionFromCurrentState(target_zone); + int nb_tasks = static_cast(arrival_task_per_second_*renewal_time); + //ROS_WARN_STREAM("Expected arrival float: "<< arrival_task_per_second_*renewal_time<< std::endl<<"Expected arrival int: "<< nb_tasks); + res.predictions.push_back(nb_tasks*sizeof(std_msgs::Int32)); + } + } + else + { + ROS_ERROR("Renewal time services not set"); + } + + return true; +} + +bool TaskPublisher::realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + //TODO add real departure + return false; +} + +bool TaskPublisher::expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(renewal_time_services_) + { + for(int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + AUVStates::Zones target_zone = AUVStates::getZoneFromTransmissionVector(req.action_set.action_set[action_index]); + + float renewal_time = renewal_time_services_->getPredictedRenewalTimeWithTransitionFromCurrentState(target_zone); + + int nb_tasks = static_cast(predicted_task_departure_rates_[target_zone]*renewal_time); + res.predictions.push_back(nb_tasks*sizeof(std_msgs::Int32)); + } + } + else + { + ROS_ERROR("Renewal time services not set"); + } + + return true; +} + +bool TaskPublisher::realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(renewal_time_services_) + { + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + AUVStates::Zones target_zone = AUVStates::getZoneFromTransmissionVector(req.action_set.action_set[action_index]); + + float renewal_time = renewal_time_services_->getRealRenewalTimeWithTransitionFromCurrentState(target_zone); + int nb_tasks = static_cast(real_task_departure_rates_[target_zone]*renewal_time); + + res.predictions.push_back(nb_tasks*sizeof(std_msgs::Int32)); + } + } + else + { + ROS_ERROR("Renewal time services not set"); + } + + return true; +} + +// Rate services +bool TaskPublisher::realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + res.value = arrival_task_per_second_*sizeof(std_msgs::Int32); + return true; +} + +bool TaskPublisher::expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + res.predictions.push_back(arrival_task_per_second_*sizeof(std_msgs::Int32)); + } + + return true; +} + +bool TaskPublisher::realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + res.value = real_task_departure_rates_[current_zone]*sizeof(std_msgs::Int32); + + return true; +} + +bool TaskPublisher::expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + for(int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + AUVStates::Zones target_zone = AUVStates::getZoneFromTransmissionVector(req.action_set.action_set[action_index]); + + res.predictions.push_back(predicted_task_departure_rates_[target_zone]*sizeof(std_msgs::Int32)); + } + + return true; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/metrics/temperature_services.cpp b/ros_queue_experiments/src/metrics/temperature_services.cpp new file mode 100644 index 0000000..045abd4 --- /dev/null +++ b/ros_queue_experiments/src/metrics/temperature_services.cpp @@ -0,0 +1,357 @@ +#include "ros_queue_experiments/metrics/temperature_services.hpp" +#include + +#include "ros_queue_experiments/AuvStates.h" +#include "ros_queue_experiments/auv_states.hpp" + +using std::string; + +TemperatureServices::TemperatureServices(ros::NodeHandle& nh, std::string metric_name, std::shared_ptr auv_state_manager, std::shared_ptr renewal_time_services): DualMetricServices(nh, metric_name, auv_state_manager, renewal_time_services) +{ + XmlRpc::XmlRpcValue temperature_config; + + if(!nh_.getParam("temp_target", temp_target_)) + { + ROS_ERROR("Missing temp_target parameter."); + } + + if(nh_.getParam("temp", temperature_config)) + { + for(int model_index =0; model_index < temperature_config.size(); ++model_index) + { + auto model_it = temperature_config[model_index].begin(); + + XmlRpc::XmlRpcValue temp_prediction_config = model_it->second; + + for(int zone_config_index = 0; zone_config_index < temp_prediction_config.size(); ++zone_config_index) + { + auto zone_config_it = temp_prediction_config[zone_config_index].begin(); + + const string& zone_name = zone_config_it->first; + XmlRpc::XmlRpcValue zone_values = zone_config_it->second; + + AUVStates::Zones zone_from_config = AUVStates::Zones::TaskZone; + + if (zone_name == "TaskZone") + { + zone_from_config = AUVStates::Zones::TaskZone; + } + else if(zone_name == "ColdZone") + { + zone_from_config = AUVStates::Zones::ColdZone; + } + else if(zone_name == "HighLocZone") + { + zone_from_config = AUVStates::Zones::HighLocZone; + } + else + { + break; + } + + for(int value_index = 0; value_index < zone_values.size(); ++value_index) + { + auto value_param = zone_values[value_index].begin(); + const string& value_name = value_param->first; + if(model_it->first == "prediction_model") + { + if (value_name == "increase") + { + expected_arrivals_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + else if (value_name == "decrease") + { + expected_departures_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + } + if(model_it->first == "real_model") + { + if (value_name == "increase") + { + real_expected_arrivals_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + else if (value_name == "decrease") + { + real_expected_departures_[zone_from_config] = static_cast(static_cast(value_param->second)); + } + } + } + } + } + } +} + +float TemperatureServices::getRealArrival(AUVStates::Zones zone) +{ + return real_expected_arrivals_[zone]; +} + +float TemperatureServices::getRealDeparture(AUVStates::Zones zone) +{ + return real_expected_departures_[zone]; +} + +// Change service +bool TemperatureServices::TempRealArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_msgs::GetQueueControllerTiming last_renewal_msg; + + if(real_renewal_service_.call(last_renewal_msg)) + { + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + /* We truly want the temperature at the end of the last state but the current temperature + should be a good approximation since the error will be the temp_rate*(~2*service_call_time) */ + float current_temperature = current_states.temperature; + + AUVStates::Zones current_zone = AUVStates::getZoneFromTransmissionVector(current_states.current_zone); + float current_zone_temp_rate = this->real_expected_arrivals_[current_zone] - this->real_expected_departures_[current_zone]; + + float last_renewal_time = last_renewal_msg.response.timing.renewal_time; + + /* Return the equivalent queue change which is the integral of the temperature over time. + Since, only the end temperature is avaiblable, the integral is done from that point.*/ + float temperature_at_start_of_frame = current_temperature - current_zone_temp_rate*last_renewal_time; + res.value = temperature_at_start_of_frame*last_renewal_time + 0.5*current_zone_temp_rate*last_renewal_time*last_renewal_time; + + /* To capture all the changes that happened since the last change to the virtual queues, it is necessary to + add the change that happened during the last controller execution. */ + AUVStates::Zones last_zone = AUVStates::getZoneFromTransmissionVector(current_states.last_zone); + float last_zone_temp_rate = this->real_expected_arrivals_[last_zone] - this->real_expected_departures_[last_zone]; + + float controller_execution_time = last_renewal_msg.response.timing.execution_time; + res.value += temperature_at_start_of_frame*controller_execution_time - 0.5*last_zone_temp_rate*controller_execution_time*controller_execution_time; + } + else + { + ROS_ERROR_STREAM("Temperature service couldn't call the last renewal service: "<getRealRenewalTimeWithTransitionFromCurrentState(zone); + + /** + * Assume that the temperature of the trajectory is the same as the end zone. Thus we + * integrate the temperature over time. integrated_temperature = temp_init*time + 0.5*(a-b)*time^2 + */ + float temperature_diff = real_expected_arrivals_[zone] - real_expected_departures_[zone]; + float integrated_temperature = current_states.temperature*predicted_renewal_time + 0.5*temperature_diff*predicted_renewal_time*predicted_renewal_time; + + res.predictions.push_back(integrated_temperature); + } + + return true; + } + return false; +} + +bool TemperatureServices::TempExpectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + if(renewal_time_services_) + { + ros_queue_experiments::AuvStates current_states = getCurrentStates(); + + for(int action_index =0; action_index < req.action_set.action_set.size(); ++action_index) + { + ros_queue_msgs::TransmissionVector &action = req.action_set.action_set[action_index]; + AUVStates::Zones zone = AUVStates::getZoneFromTransmissionVector(action); + + float predicted_renewal_time = renewal_time_services_->getPredictedRenewalTimeWithTransitionFromCurrentState(zone); + + /** + * Assume that the temperature of the trajectory is the same as the end zone. Thus we + * integrate the temperature over time. integrated_temperature = temp_init*time + 0.5*(a-b)*time^2 + */ + float temperature_diff = expected_arrivals_[zone] - expected_departures_[zone]; + float integrated_temperature = current_states.temperature*predicted_renewal_time + 0.5*temperature_diff*predicted_renewal_time*predicted_renewal_time; + + res.predictions.push_back(integrated_temperature); + } + + return true; + } + return false; +} + +bool TemperatureServices::TempRealDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + ros_queue_msgs::GetQueueControllerTiming last_renewal_msg; + + if(real_renewal_service_.call(last_renewal_msg)) + { + float last_renewal_time = last_renewal_msg.response.timing.renewal_time; + float last_controller_execution_time = last_renewal_msg.response.timing.execution_time; + + // Integral of the target over the last renewal time and controller execution time + res.value = this->temp_target_*(last_renewal_time + last_controller_execution_time); + } + else + { + ROS_ERROR_STREAM("Temperature service couldn't call the last renewal service: "<getPredictedRenewalTimeWithTransitionFromCurrentState(zone); + + /** + * Assume that the temperature of the trajectory is the same as the end zone. Thus we + * integrate the change over time as if the change was static over the action. + * It thus gives the temperature at the end of the action. + */ + + float temperature_change = predicted_renewal_time*(expected_arrivals_[zone] - expected_departures_[zone]); + float predicted_temperature = current_states.temperature + temperature_change; + + res.predictions.push_back(predicted_temperature); + } + + return true; + } + return false; +} + +bool TemperatureServices::TempRealDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + res.value = temp_target_; + return true; +} + +bool TemperatureServices::TempExpectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + for(int action_index = 0; action_index < req.action_set.action_set.size(); ++action_index) + { + res.predictions.push_back(temp_target_); + } + return true; +} + +// Overriden methods +bool TemperatureServices::realArrivalMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealArrivalMetricCallback(req, res); +} + +bool TemperatureServices::realArrivalPredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempRealArrivalPredictionMetricCallback(req, res); +} + +bool TemperatureServices::expectedArrivalMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedArrivalMetricCallback(req, res); +} + +bool TemperatureServices::realDepartureMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealDepartureMetricCallback(req, res); +} + +bool TemperatureServices::realDeparturePredictionMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempRealDeparturePredictionMetricCallback(req, res); +} + +bool TemperatureServices::expectedDepartureMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedDepartureMetricCallback(req, res); +} + +// Rate callbacks +bool TemperatureServices::realArrivalRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealArrivalRateMetricCallback(req, res); +} + +bool TemperatureServices::expectedArrivalRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedArrivalRateMetricCallback(req, res); +} + +bool TemperatureServices::realDepartureRateMetricCallback(ros_queue_msgs::FloatRequest::Request& req, + ros_queue_msgs::FloatRequest::Response& res) +{ + return TempRealDepartureRateMetricCallback(req, res); +} + +bool TemperatureServices::expectedDepartureRateMetricCallback(ros_queue_msgs::MetricTransmissionVectorPredictions::Request& req, + ros_queue_msgs::MetricTransmissionVectorPredictions::Response& res) +{ + return TempExpectedDepartureRateMetricCallback(req, res); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/action_monitor.cpp b/ros_queue_experiments/src/performance_monitor/action_monitor.cpp new file mode 100644 index 0000000..dd0ebe5 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/action_monitor.cpp @@ -0,0 +1,32 @@ +#include "ros_queue_experiments/performance_monitor/action_monitor.hpp" +#include "ros_queue_experiments/auv_states.hpp" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +ActionMonitor::ActionMonitor(ros::NodeHandle& nh, const std::string& perturbated_action_topic, const std::string& optimal_action_topic): + nh_(nh), + ns_nh_(ros::NodeHandle()), + perturbated_action_peformance_sub(ns_nh_, perturbated_action_topic, 1), + optimal_action_performance_sub(ns_nh_, optimal_action_topic, 1), + sync(MySyncPolicy(10), perturbated_action_peformance_sub, optimal_action_performance_sub) +{ + synchronized_action_performance_publisher_ = nh_.advertise("synced_action_performances", 10); + sync.registerCallback(boost::bind(&ActionMonitor::action_messages_callbacks, this, _1, _2)); +} + +void ActionMonitor::action_messages_callbacks(const ros_queue_experiments::ActionPerformance::ConstPtr& perturbated_action_performance, + const ros_queue_experiments::ActionPerformance::ConstPtr& optimal_action_performance) +{ + ros_queue_experiments::ActionPerformance synchronized_action_performance; + synchronized_action_performance.header.stamp = ros::Time::now(); + + synchronized_action_performance.target_action = perturbated_action_performance->target_action; + synchronized_action_performance.applied_action = perturbated_action_performance->applied_action; + synchronized_action_performance.optimal_action = optimal_action_performance->target_action; + + synchronized_action_performance.controller_action_index = AUVStates::getZoneFromTransmissionVector(perturbated_action_performance->target_action); + synchronized_action_performance.applied_action_index = AUVStates::getZoneFromTransmissionVector(synchronized_action_performance.applied_action); + synchronized_action_performance.action_index_difference = AUVStates::getZoneFromTransmissionVector(synchronized_action_performance.applied_action) - AUVStates::getZoneFromTransmissionVector(synchronized_action_performance.target_action); + synchronized_action_performance.optimal_action_index_difference = AUVStates::getZoneFromTransmissionVector(synchronized_action_performance.optimal_action) - AUVStates::getZoneFromTransmissionVector(synchronized_action_performance.target_action); + + synchronized_action_performance_publisher_.publish(synchronized_action_performance); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/localization_monitor.cpp b/ros_queue_experiments/src/performance_monitor/localization_monitor.cpp new file mode 100644 index 0000000..16d14b4 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/localization_monitor.cpp @@ -0,0 +1,67 @@ +#include "ros_queue_experiments/performance_monitor/localization_monitor.hpp" + +#include "ros_queue_msgs/FloatRequest.h" + +#include "ros_queue_experiments/MetricPerformance.h" + +LocalizationMonitor::LocalizationMonitor(ros::NodeHandle& nh): MetricMonitor(nh) +{ + metric_name_ = "Localization"; + // There could be a race condition since this publisher might not be ready when the first callback of the auv state publisher is called. + performance_metric_pub_ = nh_.advertise("localization", 1); + + metric_target_client_ = ns_nh_.serviceClient("auv_system_node/localization/departure/rate/real_metric"); + ROS_INFO_STREAM("Waiting for existence of the "<< metric_target_client_.getService() << " service"); + metric_target_client_.waitForExistence(); + ROS_INFO_STREAM("Service "<< metric_target_client_.getService() << " exists"); +} + +double LocalizationMonitor::getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->localization; +} + +double LocalizationMonitor::getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "LocalizationQueue") + { + return queue_stats.total_arrival/(ros::Time::now() - init_time_).toSec(); + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "LocalizationQueue"); + return 0.0; +} + +double LocalizationMonitor::getMetricTarget() +{ + ros_queue_msgs::FloatRequest metric_target_srv; + if(metric_target_client_.call(metric_target_srv)) + { + return metric_target_srv.response.value; + } + else + { + ROS_ERROR("Failed to call service auv_system_node/localization/departure/real_metric"); + return 0.0; + } +} + +double LocalizationMonitor::getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->localization_integral; +} + +double LocalizationMonitor::getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "LocalizationQueue") + { + return queue_stats.arrival_mean; + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "LocalizationQueue"); + return 0.0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/low_temperature_monitor.cpp b/ros_queue_experiments/src/performance_monitor/low_temperature_monitor.cpp new file mode 100644 index 0000000..efb89cd --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/low_temperature_monitor.cpp @@ -0,0 +1,67 @@ +#include "ros_queue_experiments/performance_monitor/low_temperature_monitor.hpp" + +#include "ros_queue_msgs/FloatRequest.h" + +#include "ros_queue_experiments/MetricPerformance.h" + +LowTemperatureMonitor::LowTemperatureMonitor(ros::NodeHandle& nh): MetricMonitor(nh) +{ + metric_name_ = "LowTemperature"; + // There could be a race condition since this publisher might not be ready when the first callback of the auv state publisher is called. + performance_metric_pub_ = nh_.advertise("low_temperature", 1); + + metric_target_client_ = ns_nh_.serviceClient("auv_system_node/low_temperature/arrival/rate/real_metric"); + ROS_INFO_STREAM("Waiting for existence of the "<< metric_target_client_.getService() << " service"); + metric_target_client_.waitForExistence(); + ROS_INFO_STREAM("Service "<< metric_target_client_.getService() << " exists"); +} + +double LowTemperatureMonitor::getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->temperature; +} + +double LowTemperatureMonitor::getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "LowTemperatureQueue") + { + return queue_stats.departure_time_average; + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "LowTemperatureQueue"); + return 0.0; +} + +double LowTemperatureMonitor::getMetricTarget() +{ + ros_queue_msgs::FloatRequest metric_target_srv; + if(metric_target_client_.call(metric_target_srv)) + { + return metric_target_srv.response.value; + } + else + { + ROS_ERROR("Failed to call service auv_system_node/temperature/departure/real_metric"); + return 0.0; + } +} + +double LowTemperatureMonitor::getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->temperature_integral; +} + +double LowTemperatureMonitor::getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "LowTemperatureQueue") + { + return queue_stats.departure_mean; + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "LowTemperatureQueue"); + return 0.0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/metric_monitor.cpp b/ros_queue_experiments/src/performance_monitor/metric_monitor.cpp new file mode 100644 index 0000000..d9c24b2 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/metric_monitor.cpp @@ -0,0 +1,78 @@ +#include "ros_queue_experiments/performance_monitor/metric_monitor.hpp" +#include "ros_queue_msgs/QueueServerStatsFetch.h" +#include "ros_queue_experiments/MetricPerformance.h" + +MetricMonitor::MetricMonitor(ros::NodeHandle& nh): nh_(nh), ns_nh_(ros::NodeHandle()) +{ + real_state_metric_sum_ = 0; + nb_real_state_samples_ = 0; + init_time_ = ros::Time::now(); +} + +void MetricMonitor::realStateMetricCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg, + const ros_queue_msgs::QueueServerStatsFetch& queue_stats_metric_srv) +{ + ros_queue_experiments::MetricPerformance metric_performance_msgs; + + metric_performance_msgs.header.stamp = ros::Time::now(); + + metric_performance_msgs.metric_name = metric_name_; + + double real_state_metric_value = getMetricFromAuvState(msg); + metric_performance_msgs.current_real_value = real_state_metric_value; + + addNewRealStateMetricSample(real_state_metric_value); + metric_performance_msgs.real_average_value = computeRealStateMetricMean(); + metric_performance_msgs.real_time_average_value = computeRealStateMetricTimeAverage(); + metric_performance_msgs.real_continous_average_value = computeRealStateMetricContinuousMean(getContinuousIntegralOfMetricFromAuvState(msg)); + + metric_performance_msgs.queue_server_time_average_value = getQueueServerTimeAverageMetric(queue_stats_metric_srv.response); + metric_performance_msgs.queue_server_arrival_mean = getQueueServerArrivalMeanMetric(queue_stats_metric_srv.response); + + double target = getMetricTarget(); + metric_performance_msgs.target_value = target; + metric_performance_msgs.real_current_diff_with_target = real_state_metric_value - target; + metric_performance_msgs.real_continuous_average_diff_with_target = metric_performance_msgs.real_continous_average_value - target; + metric_performance_msgs.real_continuous_average_diff_with_server_mean = metric_performance_msgs.real_continous_average_value - metric_performance_msgs.queue_server_arrival_mean; + metric_performance_msgs.real_continuous_average_diff_with_server_time_average = metric_performance_msgs.real_continous_average_value - metric_performance_msgs.queue_server_time_average_value; + + metric_performance_msgs.seconds_since_start = (ros::Time::now() - init_time_).toSec(); + performance_metric_pub_.publish(metric_performance_msgs); +} + +void MetricMonitor::addNewRealStateMetricSample(double new_metric_value_sample) +{ + real_state_metric_sum_ += new_metric_value_sample; + nb_real_state_samples_++; +} + +double MetricMonitor::computeRealStateMetricMean() +{ + if(nb_real_state_samples_ == 0) + { + return 0; + } + + return real_state_metric_sum_ / nb_real_state_samples_; +} + +double MetricMonitor::computeRealStateMetricTimeAverage() +{ + if(nb_real_state_samples_ == 0) + { + return 0; + } + + return real_state_metric_sum_ / (ros::Time::now() - init_time_).toSec(); +} + + +double MetricMonitor::computeRealStateMetricContinuousMean(double time_integral_of_metric) +{ + if(nb_real_state_samples_ == 0) + { + return 0; + } + + return time_integral_of_metric / (ros::Time::now() - init_time_).toSec(); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/penalty_monitor.cpp b/ros_queue_experiments/src/performance_monitor/penalty_monitor.cpp new file mode 100644 index 0000000..3218952 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/penalty_monitor.cpp @@ -0,0 +1,36 @@ +#include "ros_queue_experiments/performance_monitor/penalty_monitor.hpp" + +#include "ros_queue_experiments/MetricPerformance.h" + +PenaltyMonitor::PenaltyMonitor(ros::NodeHandle& nh): pnh_(nh) +{ + performance_metric_pub_ = pnh_.advertise("penalty", 1); + init_time_ = ros::Time::now(); +} + +void PenaltyMonitor::realStateMetricCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg, + const ros_queue_msgs::QueueServerStatsFetch& queue_stats_metric_srv) +{ + const double real_penalty = msg->real_penalty; + const double controller_penalty = msg->controller_penalty; + const float current_elapsed_time = (ros::Time::now() - init_time_).toSec(); + + real_penalty_sum_ += real_penalty; + controller_penalty_sum_ += controller_penalty; + nb_samples_penalty_++; + + ros_queue_experiments::MetricPerformance metric_performance_msg; + metric_performance_msg.header.stamp = ros::Time::now(); + metric_performance_msg.metric_name = "penalty"; + + metric_performance_msg.current_real_value = real_penalty; + metric_performance_msg.real_average_value = real_penalty_sum_ / nb_samples_penalty_; + metric_performance_msg.real_time_average_value = real_penalty_sum_/current_elapsed_time; + + metric_performance_msg.target_value = msg->controller_penalty; // This one is misleading but it's for the sake of usage of the same message + metric_performance_msg.queue_server_arrival_mean = controller_penalty_sum_ / nb_samples_penalty_; + metric_performance_msg.queue_server_time_average_value = controller_penalty_sum_/current_elapsed_time; + + metric_performance_msg.real_continuous_average_diff_with_server_mean = metric_performance_msg.real_average_value - metric_performance_msg.queue_server_arrival_mean; + performance_metric_pub_.publish(metric_performance_msg); +} diff --git a/ros_queue_experiments/src/performance_monitor/performance_monitor_node.cpp b/ros_queue_experiments/src/performance_monitor/performance_monitor_node.cpp new file mode 100644 index 0000000..6544286 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/performance_monitor_node.cpp @@ -0,0 +1,19 @@ +#include "ros/ros.h" + +#include "ros_queue_experiments/performance_monitor/action_monitor.hpp" +#include "ros_queue_experiments/performance_monitor/real_queue_monitor.hpp" +#include "ros_queue_experiments/performance_monitor/virtual_queue_monitor.hpp" + +int main(int argc, char* argv[]) +{ + ros::init(argc, argv, "performance_monitor_node"); + ros::NodeHandle nh("~"); + + VirtualQueueMonitor virtual_queue_monitor(nh); + RealQueueMonitor real_queue_monitor(nh); + ActionMonitor action_monitor(nh, "perturbation_node/action_performance", "optimal_action_node/action_performance"); + + ros::spin(); + + return 0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/real_queue_monitor.cpp b/ros_queue_experiments/src/performance_monitor/real_queue_monitor.cpp new file mode 100644 index 0000000..ef1c1e8 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/real_queue_monitor.cpp @@ -0,0 +1,33 @@ +#include "ros_queue_experiments/performance_monitor/real_queue_monitor.hpp" + +#include "ros_queue_experiments/MetricPerformance.h" + +RealQueueMonitor::RealQueueMonitor(ros::NodeHandle& nh): nh_(nh), ns_nh_(ros::NodeHandle()) +{ + queue_name_ = "TaskQueue"; + queue_stats_sub_ = ns_nh_.subscribe("queue_server/server_stats", 1, &RealQueueMonitor::queueStatsCallback, this); + performance_metric_pub_ = nh_.advertise("real_queue", 1); +} + +void RealQueueMonitor::queueStatsCallback(const ros_queue_msgs::QueueServerStats::ConstPtr& msg) +{ + ros_queue_experiments::MetricPerformance metric_performance_msg; + + for (auto queue_stats : msg->queue_stats) + { + if(queue_stats.queue_name == queue_name_) + { + metric_performance_msg.header.stamp = ros::Time::now(); + metric_performance_msg.metric_name = queue_name_; + + metric_performance_msg.queue_server_time_average_value = queue_stats.arrival_time_average; + metric_performance_msg.queue_server_arrival_mean = queue_stats.arrival_mean; + metric_performance_msg.target_value = queue_stats.departure_time_average; + + metric_performance_msg.real_continuous_average_diff_with_target = metric_performance_msg.queue_server_time_average_value - metric_performance_msg.target_value; + performance_metric_pub_.publish(metric_performance_msg); + return; + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", queue_name_.c_str()); +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/temperature_monitor.cpp b/ros_queue_experiments/src/performance_monitor/temperature_monitor.cpp new file mode 100644 index 0000000..774f168 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/temperature_monitor.cpp @@ -0,0 +1,67 @@ +#include "ros_queue_experiments/performance_monitor/temperature_monitor.hpp" + +#include "ros_queue_msgs/FloatRequest.h" + +#include "ros_queue_experiments/MetricPerformance.h" + +TemperatureMonitor::TemperatureMonitor(ros::NodeHandle& nh): MetricMonitor(nh) +{ + metric_name_ = "Temperature"; + // There could be a race condition since this publisher might not be ready when the first callback of the auv state publisher is called. + performance_metric_pub_ = nh_.advertise("temperature", 1); + + metric_target_client_ = ns_nh_.serviceClient("auv_system_node/temperature/departure/rate/real_metric"); + ROS_INFO_STREAM("Waiting for existence of the "<< metric_target_client_.getService() << " service"); + metric_target_client_.waitForExistence(); + ROS_INFO_STREAM("Service "<< metric_target_client_.getService() << " exists"); +} + +double TemperatureMonitor::getMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->temperature; +} + +double TemperatureMonitor::getQueueServerTimeAverageMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "TemperatureQueue") + { + return queue_stats.total_arrival/(ros::Time::now() - init_time_).toSec(); + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "TemperatureQueue"); + return 0.0; +} + +double TemperatureMonitor::getMetricTarget() +{ + ros_queue_msgs::FloatRequest metric_target_srv; + if(metric_target_client_.call(metric_target_srv)) + { + return metric_target_srv.response.value; + } + else + { + ROS_ERROR("Failed to call service auv_system_node/temperature/departure/real_metric"); + return 0.0; + } +} + +double TemperatureMonitor::getContinuousIntegralOfMetricFromAuvState(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + return msg->temperature_integral; +} + +double TemperatureMonitor::getQueueServerArrivalMeanMetric(const ros_queue_msgs::QueueServerStatsFetch::Response& msg) +{ + for (auto queue_stats : msg.queue_stats.queue_stats) + { + if(queue_stats.queue_name == "TemperatureQueue") + { + return queue_stats.arrival_mean; + } + } + ROS_WARN_THROTTLE(1, "Queue server does not have a queue named %s", "TemperatureQueue"); + return 0.0; +} \ No newline at end of file diff --git a/ros_queue_experiments/src/performance_monitor/virtual_queue_monitor.cpp b/ros_queue_experiments/src/performance_monitor/virtual_queue_monitor.cpp new file mode 100644 index 0000000..64c3c41 --- /dev/null +++ b/ros_queue_experiments/src/performance_monitor/virtual_queue_monitor.cpp @@ -0,0 +1,34 @@ +#include "ros_queue_experiments/performance_monitor/virtual_queue_monitor.hpp" + +#include "std_msgs/Empty.h" +#include "ros_queue_msgs/QueueServerStatsFetch.h" + +VirtualQueueMonitor::VirtualQueueMonitor(ros::NodeHandle& nh): nhp_(nh), nh_(ros::NodeHandle()), localization_monitor_(nhp_), temperature_monitor_(nhp_), low_temperature_monitor_(nhp_), penalty_monitor_(nhp_) +{ + queue_server_stats_client_ = nh_.serviceClient("queue_server/get_server_stats"); + ROS_INFO_STREAM("Waiting for existence of the "<< queue_server_stats_client_.getService() << " service"); + queue_server_stats_client_.waitForExistence(); + ROS_INFO_STREAM("Service "<< queue_server_stats_client_.getService() << " exists"); + + monitoring_sample_done_pub_ = nhp_.advertise("monitoring_done",1); + real_state_sub_ = nh_.subscribe("auv_system_node/auv_state", 1, &VirtualQueueMonitor::realStateCallback, this); +} + +void VirtualQueueMonitor::realStateCallback(const ros_queue_experiments::AuvStates::ConstPtr& msg) +{ + ros_queue_msgs::QueueServerStatsFetch queue_stats_metric_srv; + + if(queue_server_stats_client_.call(queue_stats_metric_srv)) + { + localization_monitor_.realStateMetricCallback(msg, queue_stats_metric_srv); + temperature_monitor_.realStateMetricCallback(msg, queue_stats_metric_srv); + low_temperature_monitor_.realStateMetricCallback(msg, queue_stats_metric_srv); + penalty_monitor_.realStateMetricCallback(msg, queue_stats_metric_srv); + + monitoring_sample_done_pub_.publish(std_msgs::Empty()); + } + else + { + ROS_ERROR_STREAM("Virtual queue monitoring node failed to call service "<< queue_server_stats_client_.getService()); + } +} \ No newline at end of file diff --git a/ros_queue_experiments/src/proportion_time_spent.cpp b/ros_queue_experiments/src/proportion_time_spent.cpp new file mode 100644 index 0000000..5aef522 --- /dev/null +++ b/ros_queue_experiments/src/proportion_time_spent.cpp @@ -0,0 +1,29 @@ +#include "ros_queue_experiments/proportion_time_spent.hpp" + +int ProportionTimeSpent::addState() +{ + time_spent_in_each_states_.push_back(0.0); + return time_spent_in_each_states_.size() - 1; +} + +void ProportionTimeSpent::addTimeSpentInState(float time_spent, int index) +{ + time_spent_in_each_states_[index] += time_spent; +} + +std::vector ProportionTimeSpent::getProportionTimeSpent() const +{ + std::vector proportion_time_spent; + float total_time_spent = 0.0; + for (float time_spent : time_spent_in_each_states_) + { + total_time_spent += time_spent; + } + for (float time_spent : time_spent_in_each_states_) + { + proportion_time_spent.push_back(time_spent / total_time_spent); + } + return proportion_time_spent; +} + +//std::vector time_spent_in_each_states_; diff --git a/ros_queue_experiments/src/test_services.cpp b/ros_queue_experiments/src/test_services.cpp new file mode 100644 index 0000000..bce0265 --- /dev/null +++ b/ros_queue_experiments/src/test_services.cpp @@ -0,0 +1,112 @@ +#include "ros/ros.h" + +#include + +#include "ros_queue_msgs/MetricTransmissionVectorPredictions.h" +#include "ros_queue_experiments/auv_states.hpp" + +int main(int argc, char *argv[]) +{ + ros::init(argc, argv, "service_callers"); + + ros::NodeHandle nh; + + ros::ServiceClient predicted_temperature_service = nh.serviceClient("auv_system_node/temperature/arrival/expected_metric"); + predicted_temperature_service.waitForExistence(); + + ros::ServiceClient predicted_renewal_service = nh.serviceClient("auv_system_node/renewal_time/expected_metric"); + predicted_renewal_service.waitForExistence(); + + ros::ServiceClient predicted_penalty_service = nh.serviceClient("auv_system_node/penalty/expected_metric"); + predicted_penalty_service.waitForExistence(); + + ros::ServiceClient localization_temperature_service = nh.serviceClient("auv_system_node/localization/arrival/expected_metric"); + localization_temperature_service.waitForExistence(); + + ros::ServiceClient task_departure_service = nh.serviceClient("auv_system_node/task/departure/expected_metric"); + task_departure_service.waitForExistence(); + + ros_queue_msgs::PotentialTransmissionVectorSet action_set; + action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::TaskZone)); + action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::HighLocZone)); + action_set.action_set.push_back(AUVStates::getTransmissionVectorFromZone(AUVStates::Zones::ColdZone)); + + ROS_INFO_STREAM("Renewal time service response:"); + ros_queue_msgs::MetricTransmissionVectorPredictions renewal_time_predictions; + renewal_time_predictions.request.action_set = action_set; + if(!predicted_renewal_service.call(renewal_time_predictions)) + { + ROS_ERROR("Failed to call renewal time service."); + } + else + { + for(int action_index = 0; action_index < action_set.action_set.size(); ++action_index) + { + ROS_INFO_STREAM(renewal_time_predictions.response.predictions[action_index]); + } + } + + ROS_INFO_STREAM("Penalty service response:"); + ros_queue_msgs::MetricTransmissionVectorPredictions penalty_predictions; + penalty_predictions.request.action_set = action_set; + if(!predicted_penalty_service.call(penalty_predictions)) + { + ROS_ERROR("Failed to call penalty service."); + } + else + { + for(int action_index = 0; action_index < action_set.action_set.size(); ++action_index) + { + ROS_INFO_STREAM(penalty_predictions.response.predictions[action_index]); + } + } + + ROS_INFO_STREAM("Temperature service response:"); + ros_queue_msgs::MetricTransmissionVectorPredictions temperature_time_predictions; + temperature_time_predictions.request.action_set = action_set; + if(!predicted_temperature_service.call(temperature_time_predictions)) + { + ROS_ERROR("Failed to call temperature service."); + } + else + { + for(int action_index = 0; action_index < action_set.action_set.size(); ++action_index) + { + ROS_INFO_STREAM(temperature_time_predictions.response.predictions[action_index]); + } + } + + ROS_INFO_STREAM("Localization uncertainty service response:"); + ros_queue_msgs::MetricTransmissionVectorPredictions localization_predictions; + localization_predictions.request.action_set = action_set; + if(!localization_temperature_service.call(localization_predictions)) + { + ROS_ERROR("Failed to call localization service."); + } + else + { + for(int action_index = 0; action_index < action_set.action_set.size(); ++action_index) + { + ROS_INFO_STREAM(localization_predictions.response.predictions[action_index]); + } + } + + ROS_INFO_STREAM("Task departure service response:"); + ros_queue_msgs::MetricTransmissionVectorPredictions task_departure_predictions; + task_departure_predictions.request.action_set = action_set; + if(!task_departure_service.call(task_departure_predictions)) + { + ROS_ERROR("Failed to call task departure service."); + } + else + { + for(int action_index = 0; action_index < action_set.action_set.size(); ++action_index) + { + ROS_INFO_STREAM(task_departure_predictions.response.predictions[action_index]); + } + } + + ros::spin(); +} + + diff --git a/ros_queue_experiments/srv/GetRealAUVStates.srv b/ros_queue_experiments/srv/GetRealAUVStates.srv new file mode 100644 index 0000000..389c2d9 --- /dev/null +++ b/ros_queue_experiments/srv/GetRealAUVStates.srv @@ -0,0 +1,2 @@ +--- +ros_queue_experiments/AuvStates states \ No newline at end of file diff --git a/ros_queue_experiments/srv/SendNewAUVCommand.srv b/ros_queue_experiments/srv/SendNewAUVCommand.srv new file mode 100644 index 0000000..646642d --- /dev/null +++ b/ros_queue_experiments/srv/SendNewAUVCommand.srv @@ -0,0 +1,4 @@ +ros_queue_msgs/TransmissionVector command +ros_queue_msgs/TransmissionVector command_of_controller +--- +float32 time_to_execute \ No newline at end of file diff --git a/ros_queue_msgs/CMakeLists.txt b/ros_queue_msgs/CMakeLists.txt index d665c44..a45468f 100644 --- a/ros_queue_msgs/CMakeLists.txt +++ b/ros_queue_msgs/CMakeLists.txt @@ -52,8 +52,11 @@ find_package(catkin REQUIRED COMPONENTS ## Generate messages in the 'msg' folder add_message_files( FILES + ControllerActionCosts.msg ManualByteTransmitSize.msg + MetricCosts.msg PotentialTransmissionVectorSet.msg + QueueControllerTiming.msg QueueIntElement.msg QueueState.msg QueueTransmitTemplate.msg @@ -63,6 +66,8 @@ add_message_files( QueueServerStats.msg States.msg TransmissionVector.msg + TransmissionVectorControllerCosts.msg + TransmissionVectorControllerCostsList.msg VirtualQueueChanges.msg VirtualQueueChangesList.msg ) @@ -73,10 +78,12 @@ add_service_files( ConversionTemplateService.srv ByteSizeRequest.srv FloatRequest.srv + GetQueueControllerTiming.srv QueueInfoFetch.srv MetricPredictions.srv MetricTransmissionVectorPredictions.srv QueueServerStateFetch.srv + QueueServerStatsFetch.srv PotentialTransmissionVectorSpaceFetch.srv ) diff --git a/ros_queue_msgs/README.md b/ros_queue_msgs/README.md index 70be40c..4c3c428 100644 --- a/ros_queue_msgs/README.md +++ b/ros_queue_msgs/README.md @@ -42,8 +42,12 @@ To build from source, clone the latest version from this repository into your ca If you're using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/installing.html), you could use `catkin build` command instead of `catkin_make`. ## Messages definitions +* **`ControllerActionCosts.msg`** + List of all the metrics name and values in a control action and its total cost. * **`ManualByteTransmitSize.msg`** Structure that contains an integer indicating how many bytes a real queue should manually transfer. +* **`MetricCosts.msg`** + Name of the metric and its cost from a given control step. * **`PotentialTransmissionVectorSet.msg`** Structure that contains an array of TransmissionVector.msg messages that represents a set of potential actions. * **`QueueInfo.msg`** @@ -53,7 +57,7 @@ If you're using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/ins * **`QueueServerState .msg`** Metainformation about the [queue_server](https://github.com/etienn8/ros_queuing_system/tree/main/queue_server) and all the state of its queues given by an array of `ros_queue_msgs/QueueState`. * **`QueueStats.msg`** - Contains some statistics of the time average metrics of a queue. It includes the mean size of the queue over all frames, the time average or the arrivals and departures, and the converted_remaining_mean that corresponds to the amount of data that could be sent because the remaining data that could be sent is smaller larger than the number of bytes in the front element in the queue. + Contains some statistics of the time average metrics of a queue. It includes the mean size of the queue over all frames and the time average or the arrivals and departures. The real departure represents the real data that could be transmitted if the queue was smaller than the transmission $`b(t)`$: $`\hat{b(t)}=min(b(t),Q(t))`$ for real queues and $`\hat{b}=min(b(t),q(t)+(a))`$ for virtual queues. Also, for real queues, it also sends the converted_remaining_mean that corresponds to the amount of data that couldn't be sent because the remaining data that could be sent is smaller than the number of bytes in the front element in the queue. For virtual queues, it also sends the time average of the changes. * **`QueueServerStats.msg`** Contains an array of QueueStats of all real queues from a queue server. * **`QueueState.msg`** @@ -62,6 +66,10 @@ If you're using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/ins Its an array of element that could be stored in queues. Its message template that should be copied where its array type should be changed to reflect the stored elements in a real queue. In this implementation, the type of the array is `ros_queue_msgs/QueueIntElement`. Changing this value would required to change some include files in [ros_queue](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue) and to recompile the code. * **`TransmissionVector.msg`** Contains an array with a size that represents the number of queues in a system and where their boloean values indicate if they could transmit or not. +* **`TransmissionVectorControllerCosts.msg`** + Implementation of a TransmissionVector action linked to a **`ControllerActionCosts`** for a given action. +* **`TransmissionVectorControllerCostsList.msg`** + List of **`TransmissionVectorControllerCosts`** that gives all the costs of all the metrics of each action evaluated in one controller step. * **`VirtualQueueChanges.msg`** Manual changes on the arrival and the departures that should be manually applied to a specified(based on name of the queue) virtual queue. * **`VirtualQueueChangesList.msg`** @@ -80,7 +88,9 @@ If you're using [catkin tools](https://catkin-tools.readthedocs.io/en/latest/ins * **`QueueinfoFetch.srv`** Empty request service call that returns the `ros_queue_msgs/QueueInfo`. It's used to fetch the meta information of a queue. * **`QueueServerStateFetch.srv`** - Empty request service call that returns a `ros_queue_msgs /QueueServerState`. It's mainly used to fetch the current state of a [queue_server](https://github.com/etienn8/ros_queuing_system/tree/main/ros_queue) + Empty request service call that returns a `ros_queue_msgs /QueueServerState`. It's mainly used to fetch the current state of a [queue_server](https://github.com/etienn8/ros_queuing_system/tree/main/queue_server) +* **`QueueServerStatsFetch.srv`** + Empty request service call that returns a `ros_queue_msgs /QueueServerStats`. It's mainly used to fetch the current stats of a [queue_server](https://github.com/etienn8/ros_queuing_system/tree/main/queue_server) * **`QueueStatesPredictions.srv`** Service call that takes arbitrary `ros_queue_msgs/States` and it returns a prediction in the form of an int. This is used for predicting the value of a metric based on the current state of the system. This definition is mainly designed to be copied to change the definition of the `ros_queue_msgs/States` for any `States`. However, doing so will required to recompile the code and change the included files in the queue_controller. *Subject to change since it should return a float*. * **`MetricTransmissionVectorPredictions.srv`** diff --git a/ros_queue_msgs/msg/ControllerActionCosts.msg b/ros_queue_msgs/msg/ControllerActionCosts.msg new file mode 100644 index 0000000..136d836 --- /dev/null +++ b/ros_queue_msgs/msg/ControllerActionCosts.msg @@ -0,0 +1,2 @@ +ros_queue_msgs/MetricCosts[] metric_costs +float32 total_cost \ No newline at end of file diff --git a/ros_queue_msgs/msg/MetricCosts.msg b/ros_queue_msgs/msg/MetricCosts.msg new file mode 100644 index 0000000..81a039e --- /dev/null +++ b/ros_queue_msgs/msg/MetricCosts.msg @@ -0,0 +1,2 @@ +string metric_name +float32 cost \ No newline at end of file diff --git a/ros_queue_msgs/msg/QueueControllerTiming.msg b/ros_queue_msgs/msg/QueueControllerTiming.msg new file mode 100644 index 0000000..a806b4d --- /dev/null +++ b/ros_queue_msgs/msg/QueueControllerTiming.msg @@ -0,0 +1,2 @@ +float32 execution_time +float32 renewal_time \ No newline at end of file diff --git a/ros_queue_msgs/msg/QueueStats.msg b/ros_queue_msgs/msg/QueueStats.msg index 6b5a102..18ff12a 100644 --- a/ros_queue_msgs/msg/QueueStats.msg +++ b/ros_queue_msgs/msg/QueueStats.msg @@ -1,6 +1,23 @@ string queue_name float32 current_size float32 size_mean + +float32 total_arrival float32 arrival_mean +float32 arrival_time_average +float32 last_arrival + +float32 total_departure float32 departure_mean -float32 converted_remaining_mean \ No newline at end of file +float32 departure_time_average +float32 last_departure + +float32 real_departure_mean +float32 real_departure_time_average +float32 total_real_departure +float32 last_real_departure + +float32 converted_remaining_mean + +float32 change_mean +float32 seconds_since_start \ No newline at end of file diff --git a/ros_queue_msgs/msg/TransmissionVectorControllerCosts.msg b/ros_queue_msgs/msg/TransmissionVectorControllerCosts.msg new file mode 100644 index 0000000..8663213 --- /dev/null +++ b/ros_queue_msgs/msg/TransmissionVectorControllerCosts.msg @@ -0,0 +1,2 @@ +ros_queue_msgs/TransmissionVector action +ros_queue_msgs/ControllerActionCosts costs \ No newline at end of file diff --git a/ros_queue_msgs/msg/TransmissionVectorControllerCostsList.msg b/ros_queue_msgs/msg/TransmissionVectorControllerCostsList.msg new file mode 100644 index 0000000..7592b0b --- /dev/null +++ b/ros_queue_msgs/msg/TransmissionVectorControllerCostsList.msg @@ -0,0 +1 @@ +ros_queue_msgs/TransmissionVectorControllerCosts[] action_costs_list \ No newline at end of file diff --git a/ros_queue_msgs/srv/GetQueueControllerTiming.srv b/ros_queue_msgs/srv/GetQueueControllerTiming.srv new file mode 100644 index 0000000..6860243 --- /dev/null +++ b/ros_queue_msgs/srv/GetQueueControllerTiming.srv @@ -0,0 +1,2 @@ +--- +ros_queue_msgs/QueueControllerTiming timing \ No newline at end of file diff --git a/ros_queue_msgs/srv/QueueServerStatsFetch.srv b/ros_queue_msgs/srv/QueueServerStatsFetch.srv new file mode 100644 index 0000000..4b720c0 --- /dev/null +++ b/ros_queue_msgs/srv/QueueServerStatsFetch.srv @@ -0,0 +1,2 @@ +--- +ros_queue_msgs/QueueServerStats queue_stats \ No newline at end of file diff --git a/ros_queue_tests/config/min_drift-plus-penalty_poisson.yaml b/ros_queue_tests/config/min_drift-plus-penalty_poisson.yaml index ea837ed..66a1f77 100644 --- a/ros_queue_tests/config/min_drift-plus-penalty_poisson.yaml +++ b/ros_queue_tests/config/min_drift-plus-penalty_poisson.yaml @@ -8,6 +8,9 @@ time_step: 0.1 # 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 +# 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 # Name of the actionlib server to send the action to. action_server_name: "transmission_action_receiver_node/transmission_vector_switch" diff --git a/ros_queue_tests/config/renewal_min_drift-plus-penalty_poisson.yaml b/ros_queue_tests/config/renewal_min_drift-plus-penalty_poisson.yaml index 0384560..9b080cb 100644 --- a/ros_queue_tests/config/renewal_min_drift-plus-penalty_poisson.yaml +++ b/ros_queue_tests/config/renewal_min_drift-plus-penalty_poisson.yaml @@ -8,6 +8,9 @@ time_step: 0.1 # 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 +# 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. diff --git a/ros_queuing_system/README.md b/ros_queuing_system/README.md index 9ba49d8..9eb900d 100644 --- a/ros_queuing_system/README.md +++ b/ros_queuing_system/README.md @@ -28,13 +28,15 @@ This is research code, expect that it changes often and any fitness for a partic - [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 # ros_queuing_system - [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.) #### 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