diff --git a/vda5050_core/CMakeLists.txt b/vda5050_core/CMakeLists.txt index e68245a..9f70b93 100644 --- a/vda5050_core/CMakeLists.txt +++ b/vda5050_core/CMakeLists.txt @@ -8,6 +8,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(PahoMqttCpp REQUIRED) find_package(fmt REQUIRED) +find_package(vda5050_types REQUIRED) include(GNUInstallDirs) @@ -33,11 +34,23 @@ target_include_directories(logger $ ) +add_library(client src/vda5050_core/client/order/order_manager.cpp) +target_link_libraries(client + PRIVATE + vda5050_types::vda5050_types +) +target_include_directories(client + PUBLIC + $ + $ +) + add_library(vda5050_core INTERFACE) target_link_libraries(vda5050_core INTERFACE mqtt_client logger + client ) target_include_directories(vda5050_core INTERFACE @@ -54,6 +67,7 @@ install( TARGETS mqtt_client logger + client vda5050_core EXPORT export_vda5050_core LIBRARY DESTINATION lib @@ -76,7 +90,12 @@ if(BUILD_TESTING) ament_cppcheck() find_package(ament_cmake_cpplint REQUIRED) - ament_cpplint() + set(cpplint_filters + "-whitespace/newline" + ) + ament_cpplint( + FILTERS "${cpplint_filters}" + ) find_package(ament_cmake_clang_format REQUIRED) ament_clang_format( @@ -94,6 +113,18 @@ if(BUILD_TESTING) test/integration/mqtt_client/test_paho_mqtt_client.cpp ) target_link_libraries(test_mqtt_client mqtt_client logger) + + find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_OrderManager + test/unit/order_manager/test_order_manager.cpp) + target_link_libraries(test_OrderManager + client + logger + vda5050_types::vda5050_types + ) + target_include_directories(test_OrderManager + PUBLIC $ + $) endif() ament_package() diff --git a/vda5050_core/include/vda5050_core/client/order/order_manager.hpp b/vda5050_core/include/vda5050_core/client/order/order_manager.hpp new file mode 100644 index 0000000..a755120 --- /dev/null +++ b/vda5050_core/include/vda5050_core/client/order/order_manager.hpp @@ -0,0 +1,204 @@ +/** + * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific + * Advanced Remanufacturing and Technology Centre + * A*STAR Research Entities (Co. Registration No. 199702110H) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef VDA5050_CORE__CLIENT__ORDER__ORDER_MANAGER_HPP_ +#define VDA5050_CORE__CLIENT__ORDER__ORDER_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include "vda5050_core/client/order/validation_result.hpp" +#include "vda5050_core/logger/logger.hpp" +#include "vda5050_types/order.hpp" +#include "vda5050_types/state.hpp" + +namespace vda5050_core { +namespace client { +namespace order { + +/// \brief Class that handles incoming orders on a vehicle. +class OrderManager +{ +public: + /// \brief OrderManager constructor + /// + /// \param sm Reference to the StateManager tracking the vehicle's current + /// state. + OrderManager(); + + /// TODO: i think it may make more sense to pass in a snapshot of the state + /// \brief Updates the current order on the vehicle. + /// + /// \param order The order update to be applied on the current order. + /// \param state A snapshot of the vehicle's current state + /// + /// \return True if order update has been accepted by the order manager, false + /// otherwise. + ValidationResult update_current_order( + vda5050_types::Order& order, const vda5050_types::State& state); + + /// \brief Puts a new order on the vehicle + /// + /// \param order The new Order object to be stored and executed by the vehicle + /// \param state A snapshot of the vehicle's current state + /// + /// \return True if the new order has been accepted by the order manager, + /// false otherwise + ValidationResult make_new_order( + vda5050_types::Order& order, const vda5050_types::State& state); + + /// \brief Returns the next graph element of the current order that is to be + /// executed. + /// + /// \return The graph element that is to be executed next. + const std::optional> + next_graph_element(); + +private: + class Order : public vda5050_types::Order + { + public: + explicit Order(vda5050_types::Order& order); + + const vda5050_types::Node& decision_point() const + { + return decision_point_; + } + + const std::vector>& + graph() const + { + return graph_; + } + + const std::vector>& + base() const + { + return base_; + } + + const std::vector>& + horizon() const + { + return horizon_; + } + + /// \brief Stitch this order with another order and update the + /// order_update_id if stitching is successful. + /// + /// \param order The incoming order to stitch to this order. + void stitch_and_set_order_update_id(Order order); + + private: + /// \brief The graph created by the nodes and edges of this order. + std::vector> graph_; + + /// \brief The base of this order. Contains the released nodes and edges + /// sorted in ascending sequenceId + std::vector> base_; + + /// \brief The horizion of this order. Contains the unreleased nodes and + /// edges sorted in ascending sequenceId + std::vector> + horizon_; + + /// \brief The last node in this order's base, or the last released node + /// according to sequenceId + vda5050_types::Node decision_point_; + + /// \brief Populate the graph_ member variable. + void populate_graph(); + + /// \brief Populate the base_ and horizon_ member variables + void populate_base_and_horizon(); + + /// \brief Set decision_point_ to the last released node in the order. + void set_decision_point(); + + /// \brief Stitch this order with another order. + void stitch_order(Order order); + + /// \brief Set a new order_update_id. + void set_order_update_id(uint32_t new_order_update_id); + }; + + /// \brief The order that is currently on the vehicle + std::optional current_order_; + + /// \brief The index of current order's graph element that is to be dispatched + /// next + size_t current_graph_element_index_; + + /// TODO Should we be doing JSON validation here? + /// \brief Reference to the JSON validator + bool json_validator_; /// TODO: (shawnkchan) I assume this needs to be + /// modular, but using this as a placeholder for now + + /// \brief Checks that vehicle is no longer executing an order + /// + /// \param state A snapshot of the vehicle's current state + /// + /// \return True if vehicle is not executing an order + bool is_vehicle_still_executing(const vda5050_types::State& state); + + /// \brief Checks that vehicle is not waiting for an update to its current + /// order (i.e: the vehicle's current order has no horizon) + /// + /// \return True if vehicle is waiting for an update to its current order + bool is_vehicle_waiting_for_update(); + + /// \brief Checks if order_update is a valid continuation from the current + /// order on the vehicle + /// + /// \param order_update The incoming order update + /// + /// \return True if order_update is a valid continuation + bool is_update_order_valid_continuation(vda5050_types::Order& order_update); + + /// \brief Accept a validated order and set it to the vehicle's current_order_ + /// + /// \param valid_order The validated order + void accept_new_order(vda5050_types::Order order); + + /// \brief Reject an order + /// TODO This is currently incomplete. To find out what logic needs to be + /// handled during order rejection + void reject_order(); + + /// \brief Helper function to return a vector of errors for an invalid + /// continuation error during an order update. + /// + /// \param order_start_node_sequence_id sequenceId of the first node in the + /// incoming order \param state_last_node_sequence_id lastNodeSequenceId of + /// the vehicle's state \param order_start_node_node_id nodeId of the first + /// node in the incoming order \param state_last_node_id nodeId of the + /// vehicle's state + /// + /// \return Vector of Error structs for each error that occurred. + std::vector invalid_continuation_errors( + const Order& received_order, const vda5050_types::State& state); +}; + +} // namespace order +} // namespace client +} // namespace vda5050_core + +#endif // VDA5050_CORE__CLIENT__ORDER__ORDER_MANAGER_HPP_ diff --git a/vda5050_core/include/vda5050_core/client/order/validation_result.hpp b/vda5050_core/include/vda5050_core/client/order/validation_result.hpp new file mode 100644 index 0000000..7e932bc --- /dev/null +++ b/vda5050_core/include/vda5050_core/client/order/validation_result.hpp @@ -0,0 +1,45 @@ +/** + * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific + * Advanced Remanufacturing and Technology Centre + * A*STAR Research Entities (Co. Registration No. 199702110H) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#ifndef VDA5050_CORE__CLIENT__ORDER__VALIDATION_RESULT_HPP_ +#define VDA5050_CORE__CLIENT__ORDER__VALIDATION_RESULT_HPP_ + +#include "vector" + +#include + +namespace vda5050_core { +namespace client { +namespace order { + +/// \brief Struct that details the validity of an order +struct ValidationResult +{ + /// \brief True if an order is valid, false otherwise + bool valid; + + /// \brief A vector of error(s) that resulted in an invalid order. Empty if + /// order is valid. + std::vector errors; +}; + +} // namespace order +} // namespace client +} // namespace vda5050_core + +#endif // VDA5050_CORE__CLIENT__ORDER__VALIDATION_RESULT_HPP_ diff --git a/vda5050_core/package.xml b/vda5050_core/package.xml index 8aacb84..003ec03 100644 --- a/vda5050_core/package.xml +++ b/vda5050_core/package.xml @@ -9,11 +9,14 @@ ament_cmake + vda5050_types + ament_cmake_cppcheck ament_cmake_cpplint ament_cmake_clang_format ament_cmake_copyright ament_cmake_gmock + vda5050_types ament_cmake diff --git a/vda5050_core/src/vda5050_core/client/order/order_manager.cpp b/vda5050_core/src/vda5050_core/client/order/order_manager.cpp new file mode 100644 index 0000000..eec88b5 --- /dev/null +++ b/vda5050_core/src/vda5050_core/client/order/order_manager.cpp @@ -0,0 +1,564 @@ +/** + * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific + * Advanced Remanufacturing and Technology Centre + * A*STAR Research Entities (Co. Registration No. 199702110H) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include +#include + +#include "vda5050_core/client/order/order_manager.hpp" +#include "vda5050_core/client/order/validation_result.hpp" +#include "vda5050_core/logger/logger.hpp" +#include "vda5050_types/action_status.hpp" +#include "vda5050_types/error.hpp" +#include "vda5050_types/error_level.hpp" +#include "vda5050_types/error_reference.hpp" +#include "vda5050_types/node.hpp" +#include "vda5050_types/order.hpp" +#include "vda5050_types/state.hpp" + +namespace vda5050_core { +namespace client { +namespace order { + +OrderManager::OrderManager() : current_graph_element_index_{0} {}; + +ValidationResult OrderManager::update_current_order( + vda5050_types::Order& order, const vda5050_types::State& state) +{ + /// TODO: Check for graph validation + + Order received_order{order}; + ValidationResult res{true, {}}; + + /// Check that this is actually an update order + if ( + current_order_.has_value() && + received_order.order_id == current_order_->order_id) + { + /// check if received order is deprecated + if (received_order.order_update_id < current_order_->order_update_id) + { + reject_order(); + + VDA5050_ERROR( + "Order Manager Error: orderUpdateError. Received order's " + "order_update_id is less than the vehicle's current order " + "order_update_id."); + + /// create error struct + vda5050_types::Error order_update_deprecated_error{}; + order_update_deprecated_error.error_type = + "Order Manager Error: orderUpdateError"; + order_update_deprecated_error.error_description = + "order_update_id of the incoming order is less than the " + "order_update_id of the order currently on the vehicle"; + order_update_deprecated_error.error_level = + vda5050_types::ErrorLevel::WARNING; + + /// create error references for this error + vda5050_types::ErrorReference order_id_error_reference{ + "orderId", received_order.order_id}; + vda5050_types::ErrorReference order_update_id_error_reference{ + "orderUpdateId", std::to_string(received_order.order_update_id)}; + + /// add error references to this error + order_update_deprecated_error.error_references = { + order_id_error_reference, order_update_id_error_reference}; + + res.valid = false; + res.errors.push_back(order_update_deprecated_error); + return res; + } + + /// check if order update is currently on the vehicle + else if (received_order.order_update_id == current_order_->order_update_id) + { + VDA5050_WARN( + "Order Manager Warning: Received a duplicate order update. Discarding " + "Order message."); + + /// create error struct + vda5050_types::Error duplicate_order_update_error{}; + duplicate_order_update_error.error_type = "Order Manager Error"; + duplicate_order_update_error.error_description = + "order_update_id of the incoming order is equal to the order_update_id " + "of the order currently on the vehicle."; + duplicate_order_update_error.error_level = + vda5050_types::ErrorLevel::WARNING; + + /// create error references for this error + vda5050_types::ErrorReference order_id_error_reference{ + "orderId", received_order.order_id}; + vda5050_types::ErrorReference order_update_id_error_reference{ + "orderUpdateId", std::to_string(received_order.order_update_id)}; + + /// add error references to this error + duplicate_order_update_error.error_references = { + order_id_error_reference, order_update_id_error_reference}; + + res.valid = false; + res.errors.push_back(duplicate_order_update_error); + return res; + } + + else if ( + is_vehicle_still_executing(state) && is_vehicle_waiting_for_update()) + { + if ( + received_order.nodes.front().node_id != + current_order_->decision_point().node_id) + { + VDA5050_ERROR( + "Order Manager Error: orderUpdateError. Order update has been " + "rejected as nodeIds of the stitching nodes do not match."); + + reject_order(); + + vda5050_types::Error mismatched_stitching_node_error{}; + mismatched_stitching_node_error.error_type = + "Order Manager Error: orderUpdateError"; + mismatched_stitching_node_error.error_description = + "node_id of the received order's first node does not match the " + "node_id of the vehicle's current decision point. Order update is " + "unable to be stitched with the current order on the vehicle."; + mismatched_stitching_node_error.error_level = + vda5050_types::ErrorLevel::WARNING; + + vda5050_types::ErrorReference order_id_error_reference{ + "orderId", received_order.order_id}; + vda5050_types::ErrorReference order_update_id_error_reference{ + "orderUpdateId", std::to_string(received_order.order_update_id)}; + + mismatched_stitching_node_error.error_references = { + order_id_error_reference, order_update_id_error_reference}; + + res.valid = false; + res.errors.push_back(mismatched_stitching_node_error); + return res; + } + + else + { + /// order update can be accepted as it is a continuation of the + /// currently running order + current_order_->stitch_and_set_order_update_id(received_order); + return res; + } + } + else + { + if ( + received_order.nodes.front().sequence_id != + state.last_node_sequence_id && + received_order.nodes.front().node_id != state.last_node_id) + { + VDA5050_ERROR( + "Order Manager Error: orderUpdateError. Order update has been " + "rejected as it is not a valid continuation of the previously " + "completed order."); + + reject_order(); + + res.valid = false; + res.errors = invalid_continuation_errors(received_order, state); + return res; + } + else + { + /// order update can be accepted as it is a continuation of the + /// previously executed order + /// TODO: Update internal state + current_order_->stitch_and_set_order_update_id(received_order); + return res; + } + } + } + else + { + VDA5050_ERROR( + "Order Manager Error: Update order has been rejected as its order_id " + "does not match the order_id of the order currently on the vehicle."); + + reject_order(); + + vda5050_types::Error not_an_update_order_error{}; + not_an_update_order_error.error_type = "Order Manager Error"; + not_an_update_order_error.error_description = + "order_id of the received order does not match the order_id of the order " + "currently on the vehicle."; + + vda5050_types::ErrorReference order_id_error_reference{ + "orderId", received_order.order_id}; + + not_an_update_order_error.error_references = {order_id_error_reference}; + + res.valid = false; + res.errors.push_back(not_an_update_order_error); + return res; + } +} + +ValidationResult OrderManager::make_new_order( + vda5050_types::Order& order, const vda5050_types::State& state) +{ + /// TODO: Check for graph validation + Order received_order{order}; + ValidationResult res{true, {}}; + + if ( + !current_order_.has_value() || + (current_order_.has_value() && + received_order.order_id != current_order_->order_id)) + { + /// if no current order exists, the vehicle can accept a new order + if (!current_order_) + { + accept_new_order(received_order); + return res; + } + /// if the vehicle is not carrying out an action and if the vehicle has no + /// horizon, it can accept a new order + bool vehicle_ready_for_new_order = + !is_vehicle_still_executing(state) && !is_vehicle_waiting_for_update(); + + /// TODO: This assumes that StateManager sets lastNodeId once the vehicle is + /// within deviation range + bool node_is_trivially_reachable = + received_order.nodes.front().node_id == state.last_node_id; + + if (vehicle_ready_for_new_order && node_is_trivially_reachable) + { + accept_new_order(received_order); + return res; + } + else + { + /// reject order and throw error + reject_order(); + + if (!vehicle_ready_for_new_order) + { + VDA5050_ERROR( + "Order Manager Error: Vehicle is not ready to accept a new order. " + "Vehicle is either still executing or waiting for an order update."); + + vda5050_types::Error not_ready_for_new_order_error{}; + not_ready_for_new_order_error.error_type = "Order Manager Error"; + not_ready_for_new_order_error.error_level = + vda5050_types::ErrorLevel::WARNING; + not_ready_for_new_order_error.error_description = + "Vehicle is not ready for a new order as it is still executing its " + "current order or still has a horizon and hence requires and order " + "update first."; + + res.errors.push_back(not_ready_for_new_order_error); + } + + if (!node_is_trivially_reachable) + { + VDA5050_ERROR( + "Order Manager Error: noRouteErrror. Received order's start node is " + "not trivially reachable."); + + vda5050_types::Error not_trivially_reachable_error{}; + not_trivially_reachable_error.error_type = + "Order Manager Error: noRouteError"; + not_trivially_reachable_error.error_level = + vda5050_types::ErrorLevel::WARNING; + not_trivially_reachable_error.error_description = + "Vehicle cannot reach the new order's first node from the vehicle's " + "last reached node"; + + vda5050_types::ErrorReference first_node_id_error_reference{ + "nodeId", received_order.nodes.front().node_id}; + + not_trivially_reachable_error.error_references = { + first_node_id_error_reference}; + + res.errors.push_back(not_trivially_reachable_error); + } + + res.valid = false; + return res; + } + } + else + { + reject_order(); + + VDA5050_ERROR( + "Order Manager Error: Expected a new order but was given an order with " + "the same order_id"); + + vda5050_types::Error duplicate_order_error{}; + duplicate_order_error.error_type = "Order Manager Error"; + duplicate_order_error.error_level = vda5050_types::ErrorLevel::WARNING; + duplicate_order_error.error_description = + "OrderManager::make_new_order() was given an order with the same orderId " + "as the order currently on the vehicle."; + + vda5050_types::ErrorReference new_order_id_error_reference{ + "orderId", received_order.order_id}; + vda5050_types::ErrorReference current_order_id_error_reference{ + "orderId", current_order_->order_id}; + + duplicate_order_error.error_references = { + new_order_id_error_reference, current_order_id_error_reference}; + + res.valid = false; + res.errors.push_back(duplicate_order_error); + return res; + } +} + +const std::optional> +OrderManager::next_graph_element() +{ + if (current_graph_element_index_ >= current_order_->base().size()) + { + return std::nullopt; + } + std::variant graph_element = + current_order_->base().at(current_graph_element_index_); + + current_graph_element_index_++; + + return graph_element; +} + +bool OrderManager::is_vehicle_still_executing(const vda5050_types::State& state) +{ + bool node_states_empty = state.node_states.empty(); + bool action_states_executing{false}; + + if (!state.action_states.empty()) + { + for (const auto& action_state : state.action_states) + { + if ( + action_state.action_status != vda5050_types::ActionStatus::FINISHED && + action_state.action_status != vda5050_types::ActionStatus::FAILED) + { + action_states_executing = true; + break; + } + } + } + return !node_states_empty || action_states_executing; +} + +bool OrderManager::is_vehicle_waiting_for_update() +{ + /// if horizon size is not 0, vehicle is waiting on an update + if (current_order_->horizon().size() != 0) + { + return true; + } + return false; +} + +void OrderManager::accept_new_order(vda5050_types::Order order) +{ + /// set the state of the current order to the newly accepted order. order_id_ + /// and order_update_id will be updated. + current_order_ = Order(order); + + /// set the index to the start of the new order + current_graph_element_index_ = 0; +} + +void OrderManager::reject_order() +{ + /// TODO: this will eventually return some sort of struct that BTree will use + return; +} + +std::vector OrderManager::invalid_continuation_errors( + const Order& received_order, const vda5050_types::State& state) +{ + uint32_t order_start_node_sequence_id{ + received_order.nodes.front().sequence_id}; + uint32_t state_last_node_sequence_id{state.last_node_sequence_id}; + std::string order_start_node_node_id{received_order.nodes.front().node_id}; + + /// vector to hold all errors generated from an invalid continuation + std::vector errors; + + /// error references for the received order + vda5050_types::ErrorReference order_id_error_reference{ + "orderId", received_order.order_id}; + vda5050_types::ErrorReference order_update_id_error_reference{ + "orderUpdateId", std::to_string(received_order.order_update_id)}; + + /// append Error struct to errors array if the sequence_ids are not equal + vda5050_types::Error mismatched_sequence_id_error{}; + if (order_start_node_sequence_id != state_last_node_sequence_id) + { + mismatched_sequence_id_error.error_type = + "Order Manager Error: orderUpdateError"; + mismatched_sequence_id_error.error_description = + "sequence_id of the incoming order's first node does not match the " + "vehicle state's last_node_sequence_id."; + mismatched_sequence_id_error.error_level = + vda5050_types::ErrorLevel::WARNING; + mismatched_sequence_id_error.error_references = { + order_id_error_reference, order_update_id_error_reference}; + + errors.push_back(mismatched_sequence_id_error); + } + + /// append Error struct to errors array if node_ids are not equal + vda5050_types::Error mismatched_node_id_error{}; + if (order_start_node_node_id != state.last_node_id) + { + mismatched_node_id_error.error_type = + "Order Manager Error: orderUpdateError"; + mismatched_node_id_error.error_description = + "node_id of the incoming order's first node does not match the vehicle " + "state's last_node_id."; + mismatched_node_id_error.error_level = vda5050_types::ErrorLevel::WARNING; + mismatched_node_id_error.error_references = { + order_id_error_reference, order_update_id_error_reference}; + + errors.push_back(mismatched_node_id_error); + } + + return errors; +} + +OrderManager::Order::Order(vda5050_types::Order& order) +: vda5050_types::Order(order) +{ + populate_graph(); + populate_base_and_horizon(); + set_decision_point(); +} + +void OrderManager::Order::stitch_and_set_order_update_id(Order order) +{ + try + { + stitch_order(order); + } + catch (const std::exception& e) + { + std::cerr << e.what() << '\n'; + } + + set_order_update_id(order.order_update_id); +} + +void OrderManager::Order::populate_graph() +{ + auto getSequenceId{ + [](auto const& graph_element) { return graph_element.sequence_id; }}; + + auto sequenceIdComparison{[&getSequenceId](auto const& a, auto const& b) { + return std::visit(getSequenceId, a) < std::visit(getSequenceId, b); + }}; + + for (vda5050_types::Node& n : nodes) + { + graph_.push_back(n); + } + + for (vda5050_types::Edge& e : edges) + { + graph_.push_back(e); + } + + std::sort(graph_.begin(), graph_.end(), sequenceIdComparison); +} + +void OrderManager::Order::populate_base_and_horizon() +{ + auto isReleased{ + [](auto&& graph_element) -> bool { return graph_element.released; }}; + + for (const auto& graph_element : graph_) + { + bool is_released = std::visit(isReleased, graph_element); + + if (is_released) + { + base_.push_back(graph_element); + } + else + { + horizon_.push_back(graph_element); + } + } +} + +void OrderManager::Order::set_decision_point() +{ + if (base_.empty()) + { + VDA5050_FATAL("Order Manager Error: Base of the given order is empty."); + return; + } + + if ( + const vda5050_types::Node* pNode = + std::get_if(&base_.back())) + { + decision_point_ = *pNode; + } + else + { + VDA5050_FATAL( + "Order Manager Error: Last graph element in the given order is not of " + "type vda50505_types::Node."); + } +} + +/// stitch this order with another order. stitching node could be horizon or +/// base node. +void OrderManager::Order::stitch_order(Order order) +{ + /// check that decision point of this order matches with the first node of the + /// incoming order and if the first node of the incoming order is released + vda5050_types::Node stitching_node{order.nodes.front()}; + if ( + decision_point_.node_id != stitching_node.node_id || + !stitching_node.released) + { + VDA5050_FATAL( + "Order Manager Error: Stitching node of the incoming order does not " + "match the current order's decision point."); + return; + } + + /// concatenate the current and incoming graphs + graph_.pop_back(); /// pop the last node from the current graph + graph_.insert(graph_.end(), order.graph().begin(), order.graph().end()); + + /// update the base and horizon of the current order + populate_base_and_horizon(); +} + +void OrderManager::Order::set_order_update_id(uint32_t new_order_update_id) +{ + order_update_id = new_order_update_id; +} + +} // namespace order +} // namespace client +} // namespace vda5050_core diff --git a/vda5050_core/test/unit/order_manager/test_order_manager.cpp b/vda5050_core/test/unit/order_manager/test_order_manager.cpp new file mode 100644 index 0000000..6022ec8 --- /dev/null +++ b/vda5050_core/test/unit/order_manager/test_order_manager.cpp @@ -0,0 +1,458 @@ +/** + * Copyright (C) 2025 ROS-Industrial Consortium Asia Pacific + * Advanced Remanufacturing and Technology Centre + * A*STAR Research Entities (Co. Registration No. 199702110H) + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ + +#include +#include +#include +#include + +#include "vda5050_core/client/order/order_manager.hpp" +#include "vda5050_types/action.hpp" +#include "vda5050_types/edge.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/node.hpp" +#include "vda5050_types/node_state.hpp" +#include "vda5050_types/order.hpp" +#include "vda5050_types/state.hpp" + +class OrderManagerTest : public testing::Test +{ +protected: + /// Basic set of nodes and edges to construct a fully released order + vda5050_types::Node n1{"node1", 1, true, {}, std::nullopt, std::nullopt}; + vda5050_types::Edge e2{"edge2", 2, + "node1", "node5", + true, {}, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt}; + vda5050_types::Node n3{"node3", 3, true, {}, std::nullopt, std::nullopt}; + vda5050_types::Edge e4{"edge4", 4, + "node1", "node5", + true, {}, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt}; + vda5050_types::Node n5{"node5", 5, true, {}, std::nullopt, std::nullopt}; + + std::vector fully_released_nodes = {n1, n3, n5}; + std::vector fully_released_edges = {e2, e4}; + vda5050_types::Order fully_released_order{}; + + /// create a valid new order that the vehicle can reach from the + /// fully_released_order + vda5050_types::Edge e6{"edge6", 6, + "node5", "node7", + true, {}, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt}; + vda5050_types::Node n7{"node7", 7, true, {}, std::nullopt, std::nullopt}; + std::vector order2Nodes{n5, n7}; + std::vector order2Edges{e6}; + vda5050_types::Order order2{}; + + /// Create a partially released order + vda5050_types::Edge unreleased_e4{"edge4", 4, + "node1", "node5", + false, {}, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt}; + vda5050_types::Node unreleased_n5{"node5", 5, false, {}, + std::nullopt, std::nullopt}; + std::vector partially_released_nodes = { + n1, n3, unreleased_n5}; + std::vector partially_released_edges = { + e2, unreleased_e4}; + vda5050_types::Order partially_released_order{}; + + /// Update the partially released order + std::vector order_update_nodes = {n3, n5}; + std::vector order_update_edges = {e4}; + vda5050_types::Order order_update{}; + + /// Instance of an OrderManager + vda5050_core::client::order::OrderManager orderManager{}; + + /// Snapshot of the AGV's state if it has no existing order + vda5050_types::State init_state{}; + + /// Snapshot of the AGV's state after accepting and completing + /// fully_released_order + vda5050_types::State fully_released_state{}; + + /// Snapshot of the AGV's state after accepting and completing + /// partially_released_order + vda5050_types::State partially_released_state{}; + + /// Snapshot of the AGV's state after accepting and completing order_update + vda5050_types::State order_update_state{}; + + /// Setup function that runs before each test + void SetUp() override + { + fully_released_order.order_id = "order1"; + fully_released_order.order_update_id = 0; + fully_released_order.nodes = fully_released_nodes; + fully_released_order.edges = fully_released_edges; + + order2.order_id = "order2"; + order2.order_update_id = 0; + order2.nodes = order2Nodes; + order2.edges = order2Edges; + + partially_released_order.order_id = "order1"; + partially_released_order.order_update_id = 0; + partially_released_order.nodes = partially_released_nodes; + partially_released_order.edges = partially_released_edges; + + order_update.order_id = "order1"; + order_update.order_update_id = 1; + order_update.nodes = order_update_nodes; + order_update.edges = order_update_edges; + + /// NOTE: fully_released_state is AFTER the vehicle has completed the order, + /// so node and edge states should be empty! + fully_released_state.order_id = "order1"; + fully_released_state.last_node_id = "node5"; + fully_released_state.last_node_sequence_id = 5; + fully_released_state.order_update_id = 0; + + partially_released_state.order_id = "order1"; + partially_released_state.last_node_id = "node3"; + partially_released_state.last_node_sequence_id = 3; + partially_released_state.order_update_id = 0; + + order_update_state.order_id = "order1"; + order_update_state.last_node_id = "node5"; + order_update_state.last_node_sequence_id = 5; + order_update_state.order_update_id = 1; + } + + /// \brief Helper function to create a vector of node states + std::vector create_node_states( + std::vector& nodes) + { + std::vector node_states_vector; + for (vda5050_types::Node n : nodes) + { + vda5050_types::NodeState node_state{}; + node_state.node_id = n.node_id; + node_state.sequence_id = n.sequence_id; + node_state.released = n.released; + + node_states_vector.push_back(node_state); + } + + return node_states_vector; + } + + /// \brief Helper function to create a vector of edge states + std::vector create_edge_states( + std::vector& edges) + { + std::vector edge_states_vector; + + for (vda5050_types::Edge e : edges) + { + vda5050_types::EdgeState edge_state{}; + edge_state.edge_id = e.edge_id; + edge_state.sequence_id = e.sequence_id; + edge_state.released = e.released; + + edge_states_vector.push_back(edge_state); + } + + return edge_states_vector; + } +}; + +/// \brief Test if OrderManager successfully parses a new, valid order while no +/// current order exists on the vehicle. Assumes that vehicle is ready to +/// receive a new order. +TEST_F(OrderManagerTest, NewOrderNoCurrentOrder) +{ + bool is_new_order_accepted{ + orderManager.make_new_order(fully_released_order, init_state).valid}; + + EXPECT_EQ(is_new_order_accepted, true); +} + +/// \brief Test if OrderManager successfully parses a new, valid order while a +/// current order exists on the vehicle. Assumes that vehicle is ready to +/// receive a new order. +TEST_F(OrderManagerTest, NewOrderWithCurrentOrder) +{ + bool is_first_order_accepted{ + orderManager.make_new_order(fully_released_order, init_state).valid}; + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_second_order_accepted{ + orderManager.make_new_order(order2, fully_released_state).valid}; + + EXPECT_EQ(is_second_order_accepted, true); +} + +/// \brief Test if OrderManager rejects an order if vehicle is still executing +/// an order +TEST_F(OrderManagerTest, NewOrderNodeStatesNotEmpty) +{ + bool is_first_order_accepted{ + orderManager.make_new_order(fully_released_order, init_state).valid}; + + EXPECT_EQ(is_first_order_accepted, true); + + /// create State with non-empty NodeStates + std::vector unexecuted_nodes{n3, n5}; + std::vector unexecuted_edges{e4}; + std::vector fully_released_node_states{ + create_node_states(unexecuted_nodes)}; + std::vector fully_released_edge_states{ + create_edge_states(unexecuted_edges)}; + fully_released_state.node_states = fully_released_node_states; + fully_released_state.edge_states = fully_released_edge_states; + + ::testing::internal::CaptureStderr(); + + bool is_second_order_accepted{ + orderManager.make_new_order(order2, fully_released_state).valid}; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_second_order_accepted, false); +} + +/// \brief Test if OrderManager rejects order and throws an error if vehicle +/// still has a horizon and is waiting on an update +TEST_F(OrderManagerTest, NewOrderVehicleWaitingForUpdate) +{ + bool is_first_order_accepted{ + orderManager.make_new_order(partially_released_order, init_state).valid}; + + EXPECT_EQ(is_first_order_accepted, true); + + ::testing::internal::CaptureStderr(); + + bool is_second_order_accepted{ + orderManager.make_new_order(order2, partially_released_state).valid}; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_second_order_accepted, false); +} + +/// \brief Test if OrderManager rejects order and throws an error if the new +/// order's first node is not trivially reachable by the vehicle +TEST_F(OrderManagerTest, NewOrderNodeNotTriviallyReachable) +{ + bool is_first_order_accepted{ + orderManager.make_new_order(partially_released_order, init_state).valid}; + + EXPECT_EQ(is_first_order_accepted, true); + + /// create an order with a non-trivially reachable node + vda5050_types::Node n7{"node7", 7, true, {}, std::nullopt, std::nullopt}; + vda5050_types::Edge e8{"edge8", 8, + "node7", "node9", + true, {}, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt, std::nullopt, + std::nullopt}; + vda5050_types::Node n9{"node9", 9, true, {}, std::nullopt, std::nullopt}; + + std::vector unreachableOrderNodes{n7, n9}; + std::vector unreachableOrderEdges{e8}; + vda5050_types::Order unreachableOrder{}; + unreachableOrder.order_id = "unreachableOrder"; + unreachableOrder.order_update_id = 0; + unreachableOrder.nodes = unreachableOrderNodes; + unreachableOrder.edges = unreachableOrderEdges; + + ::testing::internal::CaptureStderr(); + + bool is_unreachable_order_accepted{ + orderManager.make_new_order(unreachableOrder, fully_released_state).valid}; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_unreachable_order_accepted, false); +} + +/// \brief Test if OrderManager successfully parses a valid order update when +/// the vehicle is ready for one +TEST_F(OrderManagerTest, BasicOrderUpdate) +{ + bool is_first_order_accepted = + orderManager.make_new_order(partially_released_order, init_state).valid; + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = + orderManager.update_current_order(order_update, partially_released_state) + .valid; + + EXPECT_EQ(is_order_update_accepted, true); +} + +/// \brief Test if OrderManager rejects order and throws an error if the order +/// update is deprecated +TEST_F(OrderManagerTest, OrderUpdateDeprecated) +{ + bool is_first_order_accepted = + orderManager.make_new_order(partially_released_order, init_state).valid; + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = + orderManager.update_current_order(order_update, partially_released_state) + .valid; + + EXPECT_EQ(is_order_update_accepted, true); + + std::vector deprecated_update_nodes{n3, n5, n7}; + std::vector deprecated_update_edges{e4, e6}; + vda5050_types::Order deprecated_update_order{}; + deprecated_update_order.order_id = "order1"; + deprecated_update_order.order_update_id = 0; + deprecated_update_order.nodes = deprecated_update_nodes; + deprecated_update_order.edges = deprecated_update_edges; + + ::testing::internal::CaptureStderr(); + + bool is_deprecated_update_accepted = + orderManager + .update_current_order(deprecated_update_order, order_update_state) + .valid; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_deprecated_update_accepted, false); +} + +/// \brief Test if OrderManager discards the order update if it is already on +/// the vehicle +TEST_F(OrderManagerTest, OrderUpdateOnVehicle) +{ + bool is_first_order_accepted = + orderManager.make_new_order(partially_released_order, init_state).valid; + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = + orderManager.update_current_order(order_update, partially_released_state) + .valid; + + EXPECT_EQ(is_order_update_accepted, true); + + ::testing::internal::CaptureStderr(); + + bool is_duplicate_order_update_accepted = + orderManager.update_current_order(order_update, order_update_state).valid; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_duplicate_order_update_accepted, false); +} + +/// \brief Test if OrderManager rejects order and throws an error if the update +/// order is not a valid continuation of a previous order +TEST_F(OrderManagerTest, OrderUpdateInvalidContinuationOfCurrentOrder) +{ + bool is_first_order_accepted = + orderManager.make_new_order(partially_released_order, init_state).valid; + + EXPECT_EQ(is_first_order_accepted, true); + + /// Create an orderUpdate that is not a valid continuation from + /// partially_released_order + std::vector invalid_continuation_nodes{n5, n7}; + std::vector invalid_continuation_edges{e6}; + vda5050_types::Order invalid_continuation{}; + invalid_continuation.order_id = "order1"; + invalid_continuation.order_update_id = 1; + invalid_continuation.nodes = invalid_continuation_nodes; + invalid_continuation.edges = invalid_continuation_edges; + + ::testing::internal::CaptureStderr(); + + bool is_invalid_order_update_accepted = + orderManager + .update_current_order(invalid_continuation, partially_released_state) + .valid; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_EQ(is_invalid_order_update_accepted, false); +} + +/// \brief Test if OrderManager returns the graph elements from the base of an +/// order correctly +TEST_F(OrderManagerTest, GetNextGraphElement) +{ + bool is_first_order_accepted = + orderManager.make_new_order(partially_released_order, init_state).valid; + + EXPECT_EQ(is_first_order_accepted, true); + + if ( + std::optional> + graph_element_ref = orderManager.next_graph_element()) + { + std::variant graph_element = + graph_element_ref.value(); + vda5050_types::Node graph_element_1 = + std::get(graph_element); + EXPECT_EQ(graph_element_1.node_id, n1.node_id); + } + + if (auto graph_element_ref = orderManager.next_graph_element()) + { + auto graph_element = graph_element_ref.value(); + vda5050_types::Edge graph_element_2 = + std::get(graph_element); + EXPECT_EQ(graph_element_2.edge_id, e2.edge_id); + } + + if (auto graph_element_ref = orderManager.next_graph_element()) + { + auto graph_element = graph_element_ref.value(); + vda5050_types::Node graph_element_3 = + std::get(graph_element); + EXPECT_EQ(graph_element_3.node_id, n3.node_id); + } + + auto final_element = orderManager.next_graph_element(); + EXPECT_FALSE(final_element.has_value()); +}