diff --git a/vda5050_core/CMakeLists.txt b/vda5050_core/CMakeLists.txt index 4e83f28..f7e1e2c 100644 --- a/vda5050_core/CMakeLists.txt +++ b/vda5050_core/CMakeLists.txt @@ -8,81 +8,91 @@ endif() find_package(ament_cmake REQUIRED) find_package(PahoMqttCpp REQUIRED) find_package(fmt REQUIRED) +find_package(vda5050_types REQUIRED) include(GNUInstallDirs) add_library(mqtt_client src/vda5050_core/mqtt_client/paho_mqtt_client.cpp) -target_link_libraries(mqtt_client - PRIVATE - PahoMqttCpp::paho-mqttpp3 -) +target_link_libraries(mqtt_client PRIVATE PahoMqttCpp::paho-mqttpp3) target_include_directories(mqtt_client - PUBLIC - $ - $ -) + PUBLIC $ + $) add_library(logger src/vda5050_core/logger/logger.cpp) -target_link_libraries(logger - PRIVATE - fmt::fmt -) +target_link_libraries(logger PRIVATE fmt::fmt) target_include_directories(logger - PUBLIC - $ - $ -) + PUBLIC $ + $) add_library(vda5050_core INTERFACE) -target_link_libraries(vda5050_core - INTERFACE - mqtt_client - logger -) +target_link_libraries(vda5050_core INTERFACE mqtt_client logger) target_include_directories(vda5050_core - INTERFACE - $ - $ -) - -install( - DIRECTORY include/vda5050_core - DESTINATION include -) - -install( - TARGETS - mqtt_client - logger - vda5050_core + INTERFACE $ + $) + +add_library(order_graph_element src/vda5050_core/order_execution/order_graph_element.cpp) +target_include_directories(order_graph_element + PUBLIC $ + $) + +add_library(edge src/vda5050_core/order_execution/edge.cpp) +target_include_directories(edge + PUBLIC $ + $) +target_link_libraries(edge order_graph_element) + +add_library(node src/vda5050_core/order_execution/node.cpp) +target_include_directories(node + PUBLIC $ + $) +target_link_libraries(node order_graph_element) + +add_library(order src/vda5050_core/order_execution/order.cpp) +target_include_directories(order + PUBLIC $ + $) +target_link_libraries(order edge node) + +add_library(order_manager src/vda5050_core/order_execution/order_manager.cpp) +target_include_directories(order_manager + PUBLIC $ + $) +target_link_libraries(order_manager order vda5050_types::vda5050_types) + +target_link_libraries(vda5050_core + INTERFACE order_graph_element edge node order order_manager) + +install(DIRECTORY include/vda5050_core DESTINATION include) + +install(TARGETS + mqtt_client + logger + order_graph_element + edge + node + order + order_manager + vda5050_core EXPORT export_vda5050_core LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} -) + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) -install( - EXPORT export_vda5050_core +install(EXPORT export_vda5050_core DESTINATION share/${PROJECT_NAME}/cmake - NAMESPACE vda5050_core:: -) + NAMESPACE vda5050_core::) ament_export_targets(export_vda5050_core HAS_LIBRARY_TARGET) -ament_export_dependencies(PahoMqttCpp) +ament_export_dependencies(PahoMqttCpp fmt) if(BUILD_TESTING) find_package(ament_cmake_cppcheck REQUIRED) ament_cppcheck() - find_package(ament_cmake_cpplint REQUIRED) ament_cpplint() - find_package(ament_cmake_clang_format REQUIRED) - ament_clang_format( - CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../.clang-format" - ) - + ament_clang_format(CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../.clang-format") find_package(ament_cmake_copyright REQUIRED) ament_copyright() @@ -91,9 +101,18 @@ if(BUILD_TESTING) test/unit/mqtt_client/test_mqtt_client_interface.cpp test/unit/logger/test_logger.cpp test/unit/logger/test_default_logger.cpp - test/integration/mqtt_client/test_paho_mqtt_client.cpp - ) + 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_gmock(${PROJECT_NAME}_test_OrderManager + test/unit/order_manager/test_order_manager.cpp) + target_include_directories(${PROJECT_NAME}_test_OrderManager + PUBLIC $ + $) + target_link_libraries(${PROJECT_NAME}_test_OrderManager + edge node order_graph_element order_manager) endif() ament_package() diff --git a/vda5050_core/include/vda5050_core/order_execution/edge.hpp b/vda5050_core/include/vda5050_core/order_execution/edge.hpp new file mode 100644 index 0000000..bd3d59f --- /dev/null +++ b/vda5050_core/include/vda5050_core/order_execution/edge.hpp @@ -0,0 +1,78 @@ +/** + * 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__ORDER_EXECUTION__EDGE_HPP_ +#define VDA5050_CORE__ORDER_EXECUTION__EDGE_HPP_ + +#include +#include +#include + +#include "vda5050_core/order_execution/order_graph_element.hpp" + +namespace vda5050_core { + +namespace edge { + +/// \brief Class representing a VDA5050 Edge +class Edge : public order_graph_element::OrderGraphElement +{ +public: + /// \brief Edge constructor + /// + /// \param sequence_id uint32 indicating the sequence number of this edge in an order. + /// \param released Boolean indicating true if this edge is part of the base, false if it is part of the horizon. + /// \param edge_id String that uniquely identifies this edge. + /// \param start_node_id String indicating the first node of the order that this edge belongs to. + /// \param end_node_id String indicating the last node of the order that this edge belongs to. + Edge( + uint32_t sequence_id, bool released, std::string edge_id, + std::string start_node_id, std::string end_node_id); + + using Ptr = std::shared_ptr; + + std::string edge_id() const + { + return edge_id_; + } + + std::string start_node_id() const + { + return start_node_id_; + } + + std::string end_node_id() const + { + return end_node_id_; + } + +private: + /// \brief String that uniquely identifies this edge. + std::string edge_id_; + + /// \brief String indicating the first node of the order that this edge belongs to. + std::string start_node_id_; + + /// \brief String indicating the last node of the order that this edge belongs to. + std::string end_node_id_; +}; + +} // namespace edge +} // namespace vda5050_core + +#endif // VDA5050_CORE__ORDER_EXECUTION__EDGE_HPP_ diff --git a/vda5050_core/include/vda5050_core/order_execution/node.hpp b/vda5050_core/include/vda5050_core/order_execution/node.hpp new file mode 100644 index 0000000..cb9f54f --- /dev/null +++ b/vda5050_core/include/vda5050_core/order_execution/node.hpp @@ -0,0 +1,69 @@ +/** + * 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__ORDER_EXECUTION__NODE_HPP_ +#define VDA5050_CORE__ORDER_EXECUTION__NODE_HPP_ + +#include +#include +#include + +#include "vda5050_core/order_execution/order_graph_element.hpp" + +namespace vda5050_core { + +namespace node { + +/// \brief Class that represents a VDA5050 Node +class Node : public order_graph_element::OrderGraphElement +{ +public: + /// TODO (shawnkchan) addd shared_ptr to Node class + // using Ptr = std::shared_ptr; + /// \brief Node constructor + /// + /// \param sequence_id uint32 indicating the sequence number of this node in an order + /// \param released Boolean indicating true if this node is part of the base, false if it is part of the horizon + /// \param node_id String that uniquely identifies this node. + Node(uint32_t sequence_id, bool released, std::string node_id); + + using Ptr = std::shared_ptr; + + std::string node_id() const + { + return node_id_; + } + + bool released() const + { + return released_; + } + + uint32_t sequence_id() const + { + return sequence_id_; + } + +private: + /// \brief String that uniquely identifies this node. + std::string node_id_; +}; + +} // namespace node +} // namespace vda5050_core +#endif // VDA5050_CORE__ORDER_EXECUTION__NODE_HPP_ diff --git a/vda5050_core/include/vda5050_core/order_execution/order.hpp b/vda5050_core/include/vda5050_core/order_execution/order.hpp new file mode 100644 index 0000000..f0078c1 --- /dev/null +++ b/vda5050_core/include/vda5050_core/order_execution/order.hpp @@ -0,0 +1,156 @@ +/* + * 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__ORDER_EXECUTION__ORDER_HPP_ +#define VDA5050_CORE__ORDER_EXECUTION__ORDER_HPP_ + +#include +#include +#include +#include + +#include "vda5050_core/order_execution/edge.hpp" +#include "vda5050_core/order_execution/node.hpp" + +namespace vda5050_core { + +namespace order { + +/// \brief Class that represents a VDA5050 Order +class Order +{ +public: + /// \brief Order constructor + /// + /// \param order_id String that uniquely identifies this order + /// \param order_update_id unit32 designating the orderUpdateId of this order + /// \param nodes A vector of Node objects belonging to this order + /// \param edges A vector of Edge objects belonging to this order + Order( + std::string order_id, uint32_t order_update_id, + std::vector nodes, std::vector edges); + + std::string order_id() const + { + return order_id_; + } + + uint32_t order_update_id() const + { + return order_update_id_; + } + + const std::vector& nodes() const + { + return nodes_; + } + + const std::vector& edges() const + { + return edges_; + } + + std::vector>& graph() + { + return graph_; + } + + const std::vector>& graph() const + { + return graph_; + } + + std::vector>& base() + { + return base_; + } + + const std::vector>& base() const + { + return base_; + } + + std::vector>& horizon() + { + return horizon_; + } + + const std::vector>& horizon() const + { + return horizon_; + } + + const node::Node& decision_point() const + { + return decision_point_; + } + + /// \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 order); + +private: + /// \brief The orderId of this order. + std::string order_id_; + + /// \brief The orderUpdateId of this order. + uint32_t order_update_id_; + + /// \brief The nodes contained within this order. + std::vector nodes_; + + /// \brief The edges contained within this order. + std::vector edges_; + + /// \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 + node::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 Stitch this order with another order. + void stitch_order(order::Order order); + + /// \brief Set a new order_update_id. + /// + /// \param order_update_id the new order_update_id. + void set_order_update_id(uint32_t order_update_id); + + /// \brief Set decision_point_ to the last released node in the order. + /// + /// \param nodes The nodes of this order. + node::Node set_decision_point(std::vector nodes); +}; + +} // namespace order +} // namespace vda5050_core + +#endif // VDA5050_CORE__ORDER_EXECUTION__ORDER_HPP_ diff --git a/vda5050_core/include/vda5050_core/order_execution/order_graph_element.hpp b/vda5050_core/include/vda5050_core/order_execution/order_graph_element.hpp new file mode 100644 index 0000000..4a2d610 --- /dev/null +++ b/vda5050_core/include/vda5050_core/order_execution/order_graph_element.hpp @@ -0,0 +1,76 @@ +/** + * 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__ORDER_EXECUTION__ORDER_GRAPH_ELEMENT_HPP_ +#define VDA5050_CORE__ORDER_EXECUTION__ORDER_GRAPH_ELEMENT_HPP_ + +#include +#include +#include + +namespace vda5050_core { + +namespace order_graph_element { + +/// \brief Parent class representing a graph element (Node or Edge) of a VDA5050 order +class OrderGraphElement +{ +public: + /// \brief OrderGraphElement constructor + /// + /// \param sequence_id uint32 indicating the sequence number of this graph element in an order. + /// \param released Boolean indicating true if this graph element is part of the base, false if it is part of the horizon. + OrderGraphElement(uint32_t sequence_id, bool released); + + using Ptr = std::shared_ptr; + + virtual ~OrderGraphElement() = default; + + uint32_t sequence_id() const + { + return sequence_id_; + } + + bool released() const + { + return released_; + } + + /// \brief Custom < operator to enable sorting of OrderGraphElement objects by sequence_id + /// + /// \param order_graph_element The other OrderGraphElement to be compared to + /// + /// \return True if sequenceId of this OrderGraphElement is smaller than the one it is compared to. + bool operator<(const OrderGraphElement& order_graph_element) const + { + return sequence_id_ < order_graph_element.sequence_id(); + } + +protected: + /// \brief uint32 indicating the sequence number of this graph element in an order. + uint32_t sequence_id_; + + /// \brief Boolean indicating true if this graph element is part of the base, false if it is part of the horizon. + bool released_; + + /// TODO (shawnkchan) add array of actions as another common attribute. +}; + +} // namespace order_graph_element +} // namespace vda5050_core +#endif // VDA5050_CORE__ORDER_EXECUTION__ORDER_GRAPH_ELEMENT_HPP_ diff --git a/vda5050_core/include/vda5050_core/order_execution/order_manager.hpp b/vda5050_core/include/vda5050_core/order_execution/order_manager.hpp new file mode 100644 index 0000000..4f40196 --- /dev/null +++ b/vda5050_core/include/vda5050_core/order_execution/order_manager.hpp @@ -0,0 +1,169 @@ +/** + * 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__ORDER_EXECUTION__ORDER_MANAGER_HPP_ +#define VDA5050_CORE__ORDER_EXECUTION__ORDER_MANAGER_HPP_ + +#include +#include +#include +#include +#include + +#include "vda5050_core/order_execution/order.hpp" +#include "vda5050_types/state.hpp" +#include "vda5050_types/order.hpp" + +namespace vda5050_core { + +namespace order_manager { + +/// \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. + bool 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 + bool 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: + 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(); +}; + +} // namespace order_manager +} // namespace vda5050_core +#endif // VDA5050_CORE__ORDER_EXECUTION__ORDER_MANAGER_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/order_execution/edge.cpp b/vda5050_core/src/vda5050_core/order_execution/edge.cpp new file mode 100644 index 0000000..40d68d4 --- /dev/null +++ b/vda5050_core/src/vda5050_core/order_execution/edge.cpp @@ -0,0 +1,39 @@ +/** + * 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 "vda5050_core/order_execution/edge.hpp" +#include "vda5050_core/order_execution/order_graph_element.hpp" + +namespace vda5050_core { +namespace edge { + +Edge::Edge( + uint32_t sequence_id, bool released, std::string edge_id, + std::string start_node_id, std::string end_node_id) +: order_graph_element::OrderGraphElement(sequence_id, released), + edge_id_{edge_id}, + start_node_id_{start_node_id}, + end_node_id_{end_node_id} +{ +} + +} // namespace edge +} // namespace vda5050_core diff --git a/vda5050_core/src/vda5050_core/order_execution/node.cpp b/vda5050_core/src/vda5050_core/order_execution/node.cpp new file mode 100644 index 0000000..bae240b --- /dev/null +++ b/vda5050_core/src/vda5050_core/order_execution/node.cpp @@ -0,0 +1,35 @@ +/** + * 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 "vda5050_core/order_execution/node.hpp" +#include "vda5050_core/order_execution/order_graph_element.hpp" + +namespace vda5050_core { + +namespace node { + +Node::Node(uint32_t sequence_id, bool released, std::string node_id) +: order_graph_element::OrderGraphElement(sequence_id, released), + node_id_{node_id} +{ +} + +} // namespace node +} // namespace vda5050_core diff --git a/vda5050_core/src/vda5050_core/order_execution/order.cpp b/vda5050_core/src/vda5050_core/order_execution/order.cpp new file mode 100644 index 0000000..6d4ec57 --- /dev/null +++ b/vda5050_core/src/vda5050_core/order_execution/order.cpp @@ -0,0 +1,151 @@ +/** +* 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 +#include + +#include "vda5050_core/order_execution/edge.hpp" +#include "vda5050_core/order_execution/node.hpp" +#include "vda5050_core/order_execution/order.hpp" + +namespace vda5050_core { +namespace order { + +/// TODO: Look into using move semantics here +Order::Order( + std::string order_id, uint32_t order_update_id, std::vector nodes, + std::vector edges) +: order_id_{order_id}, + order_update_id_{order_update_id}, + nodes_{nodes}, + edges_{edges}, + decision_point_{set_decision_point(nodes)} +{ + populate_graph(); + populate_base_and_horizon(); + set_decision_point(nodes); +}; + +void Order::stitch_and_set_order_update_id(order::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 Order::stitch_order(order::Order order) +{ + /// stitch this order with another order. stitching node could be horizon or base node. + /// TODO: (shawnkchan) should we still check that the stitching node matches? This is likely only ever called by OrderManager, which will check that the stiching node id matches. + + /// 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 + node::Node stitching_node{order.nodes().front()}; + if ( + decision_point().node_id() != stitching_node.node_id() || + !stitching_node.released()) + { + throw std::runtime_error( + "Order error: Stitching node of the incoming order does not match the " + "current order's decision point."); + } + + /// 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(); + // populate_horizon(); + populate_base_and_horizon(); +} + +void Order::set_order_update_id(uint32_t new_order_update_id) +{ + order_update_id_ = new_order_update_id; +} + +void Order::populate_graph() +{ + for (node::Node& n : nodes_) + { + graph_.push_back(std::make_shared(n)); + } + + for (edge::Edge& e : edges_) + { + graph_.push_back(std::make_shared(e)); + } + + /// custom comparator to compare based on sequence_id + std::sort(graph_.begin(), graph_.end(), [](const std::shared_ptr& a, const std::shared_ptr& b) {return a->sequence_id() < b->sequence_id();}); +} + +void Order::populate_base_and_horizon() +{ + // std::cout << "populating base and horizon" << "\n"; + for (std::shared_ptr& graph_element : graph_) + { + // std::cout << graph_element->sequence_id() << "\n"; + + if (graph_element->released()) + { + base_.push_back(graph_element); + } + else + { + horizon_.push_back(graph_element); + } + } +} + +node::Node Order::set_decision_point(std::vector nodes) +{ + if (nodes.size() == 0) + { + throw std::runtime_error("Order constructor error: Node vector is empty"); + } + node::Node res = nodes.front(); + + for (node::Node n : nodes) + { + if (n.released()) + { + res = n; + } + else + { + return res; + } + } + return res; +} + +} // namespace order +} // namespace vda5050_core diff --git a/vda5050_core/src/vda5050_core/order_execution/order_graph_element.cpp b/vda5050_core/src/vda5050_core/order_execution/order_graph_element.cpp new file mode 100644 index 0000000..d115498 --- /dev/null +++ b/vda5050_core/src/vda5050_core/order_execution/order_graph_element.cpp @@ -0,0 +1,33 @@ +/** + * 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 "vda5050_core/order_execution/order_graph_element.hpp" + +namespace vda5050_core { +namespace order_graph_element { + +OrderGraphElement::OrderGraphElement(uint32_t sequence_id, bool released) +: sequence_id_{sequence_id}, released_{released} +{ +} + +} // namespace order_graph_element +} // namespace vda5050_core diff --git a/vda5050_core/src/vda5050_core/order_execution/order_manager.cpp b/vda5050_core/src/vda5050_core/order_execution/order_manager.cpp new file mode 100644 index 0000000..c963066 --- /dev/null +++ b/vda5050_core/src/vda5050_core/order_execution/order_manager.cpp @@ -0,0 +1,355 @@ +/** +* 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/order_execution/order_manager.hpp" +#include "vda5050_types/state.hpp" +#include "vda5050_types/action_status.hpp" +#include "vda5050_types/order.hpp" +#include "vda5050_types/node.hpp" + +namespace vda5050_core { +namespace order_manager { + +OrderManager::OrderManager() +: current_graph_element_index_{0} {}; + +bool OrderManager::update_current_order(vda5050_types::Order& order, const vda5050_types::State& state) +{ + Order received_order{ order }; + + /// 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(); + + std::cerr << "OrderManager error: Order update is deprecated." << "\n"; + + return false; + } + + /// check if order update is currently on the vehicle + else if ( + received_order.order_update_id == current_order_->order_update_id) + { + /// discard message as vehicle already has this update + std::cerr << "OrderManager warning: Received duplicate order update (orderUpdateId=" + << received_order.order_update_id << ") for order " + << received_order.order_id << ". Discarding message." << '\n'; + + /// TODO: KIV to change this once the return struct has been decided + return false; + } + + 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) + { + std::cerr << "OrderManager error: Order update rejected as nodeIds of the stitching nodes do not match." << "\n"; + reject_order(); + return false; + } + + else + { + /// order update can be accepted as it is a continuation of the currently running order + /// TODO: Update internal state + current_order_->stitch_and_set_order_update_id(received_order); + return true; + } + } + else + { + if (received_order.nodes.front().sequence_id != state.last_node_sequence_id && received_order.nodes.front().node_id != state.last_node_id) + { + std::cerr << "OrderManager error: Order update rejected as it is not a valid continuation of the previously completed order." << "\n"; + reject_order(); + return false; + } + 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 true; + } + } + } + else + { + /// TODO Check if it is okay to be throwing an error and rejecting the order, as this will only occur due to how we are implementing this in a BTree + /// update order is rejected as a new order was given instead of an update order + reject_order(); + return false; + } +} + +bool OrderManager::make_new_order(vda5050_types::Order& order, const vda5050_types::State& state) +{ + Order received_order { order }; + + 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 true; + } + /// 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 true; + } + else + { + /// reject order and throw error + reject_order(); + + if (!vehicle_ready_for_new_order && !node_is_trivially_reachable) + { + std::cerr << "OrderManager error: Vehicle is not ready to accept a new order and received order's start node is not trivially reachable." << "\n"; + } + else if (!vehicle_ready_for_new_order) + { + std::cerr << "OrderManager error: Vehicle is not ready to accept a new order. Vehicle is either still executing or waiting for an order update." << "\n"; + } + else if (!node_is_trivially_reachable) + { + std::cerr << "OrderManager error: Received order's start node is not trivially reachable." << "\n"; + } + + return false; + } + } + else + { + reject_order(); + throw std::runtime_error( + "OrderManager error: Expected a new order but was given an order the " + "same orderId."); + + return false; + } +} + +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; + + /// 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; +} + +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()) + { + throw std::runtime_error("OrderManager error: Base of the given order is empty."); + } + + if (const vda5050_types::Node* pNode = std::get_if(&base_.back())) + { + decision_point_ = *pNode; + } + else + { + throw std::runtime_error("OrderManager 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) + { + throw std::runtime_error( + "Order error: Stitching node of the incoming order does not match the " + "current order's decision point."); + } + + /// 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_manager +} // namespace vda5050_core diff --git a/vda5050_core/test/integration/mqtt_client/test_paho_mqtt_client.cpp b/vda5050_core/test/integration/mqtt_client/test_paho_mqtt_client.cpp index d36f8e9..5d20a16 100644 --- a/vda5050_core/test/integration/mqtt_client/test_paho_mqtt_client.cpp +++ b/vda5050_core/test/integration/mqtt_client/test_paho_mqtt_client.cpp @@ -18,6 +18,7 @@ #include +#include #include #include 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..01f90ef --- /dev/null +++ b/vda5050_core/test/unit/order_manager/test_order_manager.cpp @@ -0,0 +1,388 @@ +/** + * 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/order_execution/edge.hpp" +#include "vda5050_core/order_execution/node.hpp" +#include "vda5050_core/order_execution/order.hpp" +#include "vda5050_core/order_execution/order_manager.hpp" +#include "vda5050_types/action.hpp" +#include "vda5050_types/node.hpp" +#include "vda5050_types/edge.hpp" +#include "vda5050_types/order.hpp" +#include "vda5050_types/state.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/node_state.hpp" + +using ::testing::_; +using ::testing::AtLeast; +using ::testing::HasSubstr; +using ::testing::InSequence; +using ::testing::Return; + +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::order_manager::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)}; + + 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) }; + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_second_order_accepted { orderManager.make_new_order(order2, fully_released_state) }; + + 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) }; + + 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) }; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT(err, HasSubstr("OrderManager error: Vehicle is not ready to accept a new order. Vehicle is either still executing or waiting for an order update.")); + + 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) }; + + EXPECT_EQ(is_first_order_accepted, true); + + ::testing::internal::CaptureStderr(); + + bool is_second_order_accepted { orderManager.make_new_order(order2, partially_released_state)}; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT(err, HasSubstr("OrderManager error: Vehicle is not ready to accept a new order and received order's start node is not trivially reachable.")); + + 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) }; + + 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) }; + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT(err, HasSubstr("OrderManager error: Vehicle is not ready to accept a new order and received order's start node is not trivially reachable.")); + + 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); + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = orderManager.update_current_order(order_update, partially_released_state); + + 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); + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = orderManager.update_current_order(order_update, partially_released_state); + + 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); + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT(err, HasSubstr("OrderManager error: Order update is deprecated.")); + + 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); + + EXPECT_EQ(is_first_order_accepted, true); + + bool is_order_update_accepted = orderManager.update_current_order(order_update, partially_released_state); + + 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); + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT( + err, HasSubstr("OrderManager warning: Received duplicate order update")); + + 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); + + 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); + + std::string err = ::testing::internal::GetCapturedStderr(); + + EXPECT_THAT(err, HasSubstr("OrderManager error: Order update rejected as it is not a valid continuation of the previously completed order.")); + + 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); + + 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()); +} diff --git a/vda5050_types/CMakeLists.txt b/vda5050_types/CMakeLists.txt new file mode 100644 index 0000000..c1f1f01 --- /dev/null +++ b/vda5050_types/CMakeLists.txt @@ -0,0 +1,49 @@ +cmake_minimum_required(VERSION 3.20) +project(vda5050_types) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +include(GNUInstallDirs) + +add_library(vda5050_types INTERFACE) + +target_include_directories(vda5050_types + INTERFACE + $ + $ +) + +install( + DIRECTORY include/vda5050_types + DESTINATION include +) + +install( + TARGETS vda5050_types + EXPORT export_vda5050_types +) + +ament_export_include_directories(include) +ament_export_targets(export_vda5050_types HAS_LIBRARY_TARGET) + +if(BUILD_TESTING) + find_package(ament_cmake_cppcheck REQUIRED) + ament_cppcheck() + + find_package(ament_cmake_cpplint REQUIRED) + ament_cpplint() + + find_package(ament_cmake_clang_format REQUIRED) + ament_clang_format( + CONFIG_FILE "${CMAKE_CURRENT_SOURCE_DIR}/../.clang-format" + ) + + find_package(ament_cmake_copyright REQUIRED) + ament_copyright() +endif() + +ament_package() diff --git a/vda5050_types/include/vda5050_types/action.hpp b/vda5050_types/include/vda5050_types/action.hpp new file mode 100644 index 0000000..07ee526 --- /dev/null +++ b/vda5050_types/include/vda5050_types/action.hpp @@ -0,0 +1,75 @@ +/** + * 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_TYPES__ACTION_HPP_ +#define VDA5050_TYPES__ACTION_HPP_ + +#include +#include +#include + +#include "vda5050_types/action_parameter.hpp" +#include "vda5050_types/blocking_type.hpp" + +namespace vda5050_types { + +struct Action +{ + /// \brief Name of action as described in the first column of Actions and Parameters. Identifies the function of the action + std::string action_type; + + /// \brief Unique ID to identify the action and map it to the actionState in the State message + std::string action_id; + + /// \brief Regulates if the action is allowed to be executed during movement and/or parallel to other actions. + BlockingType blocking_type; + + /// \brief Additional information on the action + std::optional action_description; + + /// \brief Array of actionParameter objects for the indicated action. If not defined, the action has no parameters + /// eg: deviceId, loadId, external Triggers + std::optional> action_parameters; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Action& other) const + { + if (this->action_type != other.action_type) return false; + if (this->action_id != other.action_id) return false; + + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator!=(const Action& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_HPP_ diff --git a/vda5050_types/include/vda5050_types/action_parameter.hpp b/vda5050_types/include/vda5050_types/action_parameter.hpp new file mode 100644 index 0000000..fe10e00 --- /dev/null +++ b/vda5050_types/include/vda5050_types/action_parameter.hpp @@ -0,0 +1,61 @@ +/** + * 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_TYPES__ACTION_PARAMETER_HPP_ +#define VDA5050_TYPES__ACTION_PARAMETER_HPP_ + +#include + +namespace vda5050_types { + +/// \brief An object for the indicated action +/// eg: deviceId, loadId, external triggers +struct ActionParameter +{ + /// \brief The key of the parameter + std::string key; + + /// \brief The value of the parameter that belongs to the key + std::string value; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const ActionParameter& other) const + { + if (this->key != other.key) return false; + if (this->value != other.value) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator!=(const ActionParameter& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_PARAMETER_HPP_ diff --git a/vda5050_types/include/vda5050_types/action_parameter_factsheet.hpp b/vda5050_types/include/vda5050_types/action_parameter_factsheet.hpp new file mode 100644 index 0000000..568c623 --- /dev/null +++ b/vda5050_types/include/vda5050_types/action_parameter_factsheet.hpp @@ -0,0 +1,74 @@ +/* + * 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_TYPES__ACTION_PARAMETER_FACTSHEET_HPP_ +#define VDA5050_TYPES__ACTION_PARAMETER_FACTHSEET_HPP_ + +#include +#include + +#include "vda5050_types/value_data_type.hpp" + +namespace vda5050_types { + +/// \brief NOTE: This message is different from ActionParameter.msg used in Order.msg. +/// For more details, refer to page 54 of VDA5050 Recommendation v2.0.0. +/// ActionParameter message specifically used in Factsheet.msg. +/// Describes the array of parameters that an action has. +struct ActionParameterFactsheet +{ + /// \brief Key string for the parameter + std::string key; + + /// \brief Data type of the value field in Order.ActionParameter + ValueDataType value_data_type; + + /// \brief Free-form text: description of the parameter + std::optional description; + + /// \brief If set to "true", then this is an optional parameter. + std::optional is_optional; + + /// \brief Equality operator + /// + /// \param other The oher object to compare to + /// + /// \return is equal? + inline bool operator==(const ActionParameterFactsheet& other) const + { + if (this->key != other.key) return false; + if (this->value_data_type != other.value_data_type) return false; + if (this->description != other.description) return false; + if (this->is_optional != other.is_optional) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other objet to compare to + /// + /// \return is not equal? + inline bool operator!=(const ActionParameterFactsheet& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_PARAMETER_FACTSHEET_HPP_ diff --git a/vda5050_types/include/vda5050_types/action_scope.hpp b/vda5050_types/include/vda5050_types/action_scope.hpp new file mode 100644 index 0000000..e1f5c04 --- /dev/null +++ b/vda5050_types/include/vda5050_types/action_scope.hpp @@ -0,0 +1,39 @@ +/** + * 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_TYPES__ACTION_SCOPE_HPP_ +#define VDA5050_TYPES__ACTION_SCOPE_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for allowed scopes in AgvAction. +enum class ActionScope +{ + /// \brief Useable as an instant_action. + INSTANT, + + /// \brief Useable on nodes. + NODE, + + /// \brief Useable on edges. + EDGE +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_SCOPE_HPP_ diff --git a/vda5050_types/include/vda5050_types/action_state.hpp b/vda5050_types/include/vda5050_types/action_state.hpp new file mode 100644 index 0000000..bfb914f --- /dev/null +++ b/vda5050_types/include/vda5050_types/action_state.hpp @@ -0,0 +1,78 @@ +/* + * 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_TYPES__ACTION_STATE_HPP_ +#define VDA5050_TYPES__ACTION_STATE_HPP_ + +#include +#include + +#include "vda5050_types/action_status.hpp" + +namespace vda5050_types { + +/// \brief Describes the state of the action carried out by the AGV +/// Part of the state message +struct ActionState +{ + /// \brief Unique identifier of the action + std::string action_id; + + /// \brief The type of this action. Only for visualization + /// purposes as the order already knows the type of the action + std::optional action_type; + + /// \brief Additional information on the current action. + std::optional action_description; + + /// \brief The current progress of the action + ActionStatus action_status = ActionStatus::WAITING; + + /// \brief Description of the result. Errors will be transmitted + /// in the errors field + std::optional result_description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const ActionState& other) const + { + if (this->action_id != other.action_id) return false; + if (this->action_type != other.action_type) return false; + if (this->action_description != other.action_description) return false; + if (this->action_status != other.action_status) return false; + if (this->result_description != other.result_description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const ActionState& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/action_status.hpp b/vda5050_types/include/vda5050_types/action_status.hpp new file mode 100644 index 0000000..52f0344 --- /dev/null +++ b/vda5050_types/include/vda5050_types/action_status.hpp @@ -0,0 +1,52 @@ +/* + * 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_TYPES__ACTION_STATUS_HPP_ +#define VDA5050_TYPES__ACTION_STATUS_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for actionStatus +enum class ActionStatus +{ + /// \brief Action was received by AGV but the node where it triggers + /// was not yet reached or the edge where it is active was not yet + /// entered + WAITING, + + /// \brief Action was triggered and preparatory measures are initiated + INITIALIZING, + + /// \brief The action is running + RUNNING, + + /// \brief The action is paused because of a pause instantAction or + /// some external trigger such as a pause button on AGV + PAUSED, + + /// \brief The action is finished and a result is reported through + /// the resultDescription + FINISHED, + + /// \brief Action could not be finished for whatever reason + FAILED +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ACTION_STATUS_HPP_ diff --git a/vda5050_types/include/vda5050_types/agv_action.hpp b/vda5050_types/include/vda5050_types/agv_action.hpp new file mode 100644 index 0000000..e7dd1f5 --- /dev/null +++ b/vda5050_types/include/vda5050_types/agv_action.hpp @@ -0,0 +1,84 @@ +/** + * 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_TYPES__AGV_ACTION_HPP_ +#define VDA5050_TYPES__AGV_ACTION_HPP_ + +#include +#include +#include + +#include "vda5050_types/action_parameter_factsheet.hpp" +#include "vda5050_types/action_scope.hpp" +#include "vda5050_types/blocking_type.hpp" + +namespace vda5050_types { + +/// \brief Actions with parameters supported by the AGV. +/// This includes standard actions specified in VDA5050 and manufacturer-specific +/// actions. +struct AGVAction +{ + /// \brief Unique type of action corresponding to action.actionType + std::string action_type; + + /// \brief Array of allowed scopes for using this action type. + std::vector action_scopes; + + /// \brief Array of parameters an action has. If not defined, the action has no parameters. + std::optional> action_parameters; + + /// \brief Free-form text: description of the result. + std::optional result_description; + + /// \brief Free-form text: description of the action. + std::optional action_description; + + /// \brief Array of possible blocking types for defined action. + std::optional> blocking_types; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const AGVAction& other) const + { + if (this->action_type != other.action_type) return false; + if (this->action_scopes != other.action_scopes) return false; + if (this->action_parameters != other.action_parameters) return false; + if (this->result_description != other.result_description) return false; + if (this->action_description != other.action_description) return false; + if (this->blocking_types != other.blocking_types) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const AGVAction& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__AGV_ACTION_HPP_ diff --git a/vda5050_types/include/vda5050_types/agv_class.hpp b/vda5050_types/include/vda5050_types/agv_class.hpp new file mode 100644 index 0000000..b4f7219 --- /dev/null +++ b/vda5050_types/include/vda5050_types/agv_class.hpp @@ -0,0 +1,42 @@ +/** + * 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_TYPES__AGV_CLASS_HPP_ +#define VDA5050_TYPES__AGV_CLASS_HPP_ + +namespace vda5050_types { + +/// \brief Simplified description of AGV class for TypeSpecification. +enum class AGVClass +{ + /// \brief Forklift. + FORKLIFT, + + /// \brief AGV with conveyors on it. + CONVEYOR, + + /// \brief Tugger. + TUGGER, + + /// \brief Load carrier with or without lifting unit. + CARRIER +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__AGV_CLASS_HPP_ diff --git a/vda5050_types/include/vda5050_types/agv_geometry.hpp b/vda5050_types/include/vda5050_types/agv_geometry.hpp new file mode 100644 index 0000000..7d4869f --- /dev/null +++ b/vda5050_types/include/vda5050_types/agv_geometry.hpp @@ -0,0 +1,69 @@ +/** + * 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_TYPES__AGV_GEOMETRY_HPP_ +#define VDA5050_TYPES__AGV_GEOMETRY_HPP_ + +#include +#include + +#include "vda5050_types/envelope2d.hpp" +#include "vda5050_types/envelope3d.hpp" +#include "vda5050_types/wheel_definition.hpp" + +namespace vda5050_types { + +/// @brief Message defining the geometry properties of the AGV. eg: outlines and wheel positions. +struct AGVGeometry +{ + /// @brief Array of wheels, containing wheel arrangement and geometry. + std::optional> wheel_definitions; + + /// @brief Array of AGV envelope curves in 2D. eg: the mechanical envelopes for unloaded and loaded state, the safety fields for different speed cases. + std::optional> envelopes_2d; + + /// @brief Array of AGV envelope curves in 3D. + std::optional> envelopes_3d; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const AGVGeometry& other) const + { + if (this->wheel_definitions != other.wheel_definitions) return false; + if (this->envelopes_2d != other.envelopes_2d) return false; + if (this->envelopes_3d != other.envelopes_3d) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const AGVGeometry& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__AGV_GEOMETRY_HPP_ diff --git a/vda5050_types/include/vda5050_types/agv_kinematic.hpp b/vda5050_types/include/vda5050_types/agv_kinematic.hpp new file mode 100644 index 0000000..6914613 --- /dev/null +++ b/vda5050_types/include/vda5050_types/agv_kinematic.hpp @@ -0,0 +1,39 @@ +/** + * 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_TYPES__AGV_KINEMATIC_HPP_ +#define VDA5050_TYPES__AGV_KINEMATIC_HPP_ + +namespace vda5050_types { + +/// \brief Simplified description of AGV kinematics type for TypeSpecification. +enum class AGVKinematic +{ + /// \brief Differential drive. + DIFF, + + /// \brief Omni-directional vehicle. + OMNI, + + /// \brief Three-wheel-driven vehicle or vehicle with similar kinematics. + THREEWHEEL +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__AGV_KINEMATIC_HPP_ diff --git a/vda5050_types/include/vda5050_types/agv_position.hpp b/vda5050_types/include/vda5050_types/agv_position.hpp new file mode 100644 index 0000000..3469848 --- /dev/null +++ b/vda5050_types/include/vda5050_types/agv_position.hpp @@ -0,0 +1,95 @@ +/* + * 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_TYPES__AGV_POSITION_HPP_ +#define VDA5050_TYPES__AGV_POSITION_HPP_ + +#include +#include + +namespace vda5050_types { + +/// \brief Defines the position on the map in world coordinate system +/// Part of the state message +struct AGVPosition +{ + /// \brief Indicates if the AGV's position is initialized + /// - True if position is initialized + /// - False if position is not initialized + bool position_initialized = false; + + /// \brief Desribes the quality of localization and can be + /// used to describe the accuracy of the current position information + /// Primarily for logging and visualization purposes + /// - 0.0 denotes position unknown + /// - 1.0 denotes position known + std::optional localization_score; + + /// \brief Deviation from position in meters. Optional for vehicles + /// that cannot estimate their deviation + std::optional deviation_range; + + /// \brief X-position on the map in reference to + /// the map coordinate system. + double x = 0.0; + + /// \brief Y-position on the map in reference to + /// the map coordinate system. + double y = 0.0; + + /// \brief Orientation of the AGV in radians + /// Valid range: [-PI, PI] + double theta = 0.0; + + /// \brief Unique identification of the map in + /// which the position is referenced + std::string map_id; + + /// \brief Additional information on the map. + std::optional map_description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const AGVPosition& other) const + { + if (position_initialized != other.position_initialized) return false; + if (localization_score != other.localization_score) return false; + if (deviation_range != other.deviation_range) return false; + if (x != other.x) return false; + if (y != other.y) return false; + if (theta != other.theta) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const AGVPosition& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__AGV_POSITION_HPP_ diff --git a/vda5050_types/include/vda5050_types/battery_state.hpp b/vda5050_types/include/vda5050_types/battery_state.hpp new file mode 100644 index 0000000..eff26f0 --- /dev/null +++ b/vda5050_types/include/vda5050_types/battery_state.hpp @@ -0,0 +1,76 @@ +/* + * 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_TYPES__BATTERY_STATE_HPP_ +#define VDA5050_TYPES__BATTERY_STATE_HPP_ + +#include +#include + +namespace vda5050_types { + +/// \brief Provides information related to the battery +/// Part of the state message +struct BatteryState +{ + /// \brief State of charge in percent (%). If the AGV only provides values + /// for good or bad battery levels, these will be indicated as + /// 20% (bad) and 80% (good) + double battery_charge = 0.0; + + /// \brief Value of battery voltage + std::optional battery_voltage; + + /// \brief State of health of the battery in percent + std::optional battery_health; + + /// \brief Charging state of the AGV. True if charging is in progress + bool charging = false; + + /// \brief Estimated reach with current state of charge in meters + std::optional reach; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const BatteryState& other) const + { + if (battery_charge != other.battery_charge) return false; + if (battery_voltage != other.battery_voltage) return false; + if (battery_health != other.battery_health) return false; + if (charging != other.charging) return false; + if (reach != other.reach) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const BatteryState& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__BATTERY_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/blocking_type.hpp b/vda5050_types/include/vda5050_types/blocking_type.hpp new file mode 100644 index 0000000..749d813 --- /dev/null +++ b/vda5050_types/include/vda5050_types/blocking_type.hpp @@ -0,0 +1,38 @@ +/** + * 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_TYPES__BLOCKING_TYPE_HPP_ +#define VDA5050_TYPES__BLOCKING_TYPE_HPP_ + +namespace vda5050_types { + +enum class BlockingType +{ + /// \brief action can happen in parallel with others, including movement + NONE, + + /// \brief action can happen simultaneously with other actions, but not while moving + SOFT, + + /// \brief no other action can be performed while this action is running + HARD +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__BLOCKING_TYPE_HPP_ diff --git a/vda5050_types/include/vda5050_types/bounding_box_reference.hpp b/vda5050_types/include/vda5050_types/bounding_box_reference.hpp new file mode 100644 index 0000000..31455ed --- /dev/null +++ b/vda5050_types/include/vda5050_types/bounding_box_reference.hpp @@ -0,0 +1,72 @@ +/* + * 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_TYPES__BOUNDING_BOX_REFERENCE_HPP_ +#define VDA5050_TYPES__BOUNDING_BOX_REFERENCE_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Point of reference for the location of the bounding box. The point of +/// reference is always the center of the bounding box bottom surface +/// (at height = 0) and is described in coordinates of the AGV’s coordinate +/// system +/// Part of the state message +struct BoundingBoxReference +{ + /// \brief X-coordinate of the point of reference + double x = 0.0; + + /// \brief Y-coordinate of the point of reference + double y = 0.0; + + /// \brief Z-coordinate of the point of reference + double z = 0.0; + + /// \brief Orientation of the bounding box + std::optional theta; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const BoundingBoxReference& other) const + { + if (this->x != other.x) return false; + if (this->y != other.y) return false; + if (this->z != other.z) return false; + if (this->theta != other.theta) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const BoundingBoxReference& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__BOUNDING_BOX_REFERENCE_HPP_ diff --git a/vda5050_types/include/vda5050_types/connection.hpp b/vda5050_types/include/vda5050_types/connection.hpp new file mode 100644 index 0000000..edd4b87 --- /dev/null +++ b/vda5050_types/include/vda5050_types/connection.hpp @@ -0,0 +1,60 @@ +/* + * 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_TYPES__CONNECTION_HPP_ +#define VDA5050_TYPES__CONNECTION_HPP_ + +#include "vda5050_types/connection_state.hpp" +#include "vda5050_types/header.hpp" + +namespace vda5050_types { + +/// \brief A message containing the connection information +/// published on the /connection topic +struct Connection +{ + /// \brief The header of the message + Header header; + + /// \brief State of the connection of the AGV + ConnectionState connection_state = ConnectionState::OFFLINE; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Connection& other) const + { + return header == other.header && connection_state == other.connection_state; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Connection& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__CONNECTION_HPP_ diff --git a/vda5050_types/include/vda5050_types/connection_state.hpp b/vda5050_types/include/vda5050_types/connection_state.hpp new file mode 100644 index 0000000..ae2e7f3 --- /dev/null +++ b/vda5050_types/include/vda5050_types/connection_state.hpp @@ -0,0 +1,41 @@ +/* + * 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_TYPES__CONNECTION_STATE_HPP_ +#define VDA5050_TYPES__CONNECTION_STATE_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for connectionState +enum class ConnectionState +{ + /// \brief The connection between the broker and AGV is active + ONLINE, + + /// \brief The connection between AGV and broker has gone offline in a + /// coordinated way + OFFLINE, + + /// \brief The connection between AGV and broker has ended unexpectedly + /// (e.g. used in MQTT last-will-message) + CONNECTIONBROKEN +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__CONNECTION_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/control_point.hpp b/vda5050_types/include/vda5050_types/control_point.hpp new file mode 100644 index 0000000..41802f8 --- /dev/null +++ b/vda5050_types/include/vda5050_types/control_point.hpp @@ -0,0 +1,65 @@ +/* + * 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_TYPES__CONTROL_POINT_HPP_ +#define VDA5050_TYPES__CONTROL_POINT_HPP_ + +namespace vda5050_types { + +/// \brief ControlPoint describing a trajectory (NURBS) +/// Part of the state message +struct ControlPoint +{ + /// \brief X-coordinate described in the world coordinate system + double x = 0.0; + + /// \brief Y-coordinate described in the world coordinate system + double y = 0.0; + + /// \brief Degree of the NURBS curve defining the trajectory. If not + /// defined, the default value is 1. + /// Valid range: [1, 10] + double weight = 1.0; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const ControlPoint& other) const + { + if (this->x != other.x) return false; + if (this->y != other.y) return false; + if (this->weight != other.weight) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const ControlPoint& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__CONTROL_POINT_HPP_ diff --git a/vda5050_types/include/vda5050_types/e_stop.hpp b/vda5050_types/include/vda5050_types/e_stop.hpp new file mode 100644 index 0000000..ec24d72 --- /dev/null +++ b/vda5050_types/include/vda5050_types/e_stop.hpp @@ -0,0 +1,42 @@ +/* + * 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_TYPES__E_STOP_HPP_ +#define VDA5050_TYPES__E_STOP_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for eStop +enum class EStop +{ + /// \brief Auto-acknowledgeable e-stop is activated + AUTOACK, + + /// \brief E-stop has to be acknowledged manually at the vehicle + MANUAL, + + /// \brief Facility e-stop has to be acknowledged remotely + REMOTE, + + /// \brief No e-stop activated + NONE +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__E_STOP_HPP_ diff --git a/vda5050_types/include/vda5050_types/edge.hpp b/vda5050_types/include/vda5050_types/edge.hpp new file mode 100644 index 0000000..122e401 --- /dev/null +++ b/vda5050_types/include/vda5050_types/edge.hpp @@ -0,0 +1,151 @@ +/** + * 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_TYPES__EDGE_HPP_ +#define VDA5050_TYPES__EDGE_HPP_ + +#include +#include +#include +#include + +#include "vda5050_types/action.hpp" +#include "vda5050_types/orientation_type.hpp" +#include "vda5050_types/trajectory.hpp" + +namespace vda5050_types { + +/// \brief Defines a directional connection between two nodes. +struct Edge +{ + /// \brief Unique edge identification. + std::string edge_id; + + /// \brief Number to track the sequence of nodes and edges in an order, simplifying updates. + /// Distinguishes between nodes/edges that may be revisited in the same order. + /// Resets when a new orderId is issued. + uint32_t sequence_id; + + /// \brief The nodeId of the start node for this edge. + std::string start_node_id; + + /// \brief The nodeId of the end node for this edge. + std::string end_node_id; + + /// \brief Indicates if the edge is part of the base or horizon. + /// "true" indicates that edge belongs to the base. + /// "false" indicates that edge belongs to the horizon. + bool released; + + /// \brief Array of actionIds to be executed on the edge. + /// Array is empty if no actions are required. + /// An action triggered by an edge will only be active for the time that the AGV is + /// traversing it. When the AGV leaves the edge, the action will stop and the state + /// before entering the edge will be restored. + std::vector actions; + + /// \brief Additional information about the edge. + std::optional edge_description; + + /// \brief Permitted maximum speed [m/s] on the edge. + /// Speed is defined by the fastest measurement of the vehicle. + std::optional max_speed; + + /// \brief Permitted maximum height [m] of the vehicle including load on the edge. + std::optional max_height; + + /// \brief Permitted minimal height [m] of the load handling device on the edge. + std::optional min_height; + + /// \brief Orientation of the AGV on the edge [rad]. + /// If the AGV starts in a different orientation, rotate the vehicle on the edge to + /// the desired orientation if rotationAllowed is true. + /// If rotationAllowed is false, rotate before entering the edge. + std::optional orientation; + + /// \brief Defines if the orientation field is interpreted relative to the global + /// project specific map coordinate system or tangential to the edge. + /// If tangential to the edge, 0.0 = forwards, Pi = backwards. + /// Default value: TANGENTIAL + std::optional orientation_type; + + /// \brief Sets direction at junctions for line-guided or wire-guided vehicles. + /// To be defined initially (vehicle-individual). + /// Eg: straight, left, right. + std::optional direction; + + /// \brief "true" indicates that rotation is allowed on the edge. + /// "false" indicates that rotation is not allowed on the edge. + /// No limit, if not set. + std::optional rotation_allowed; + + /// \brief The maximum rotation speed [rad/s]. + /// No limit, if not set. + std::optional max_rotation_speed; + + /// \brief The Trajectory object for this edge as a Non-Uniform Rational B-Spline (NURBS). + /// Defines the path that the AGV should move between the start node and end node + /// of the edge. This field can be omitted if the AGV cannot process trajectories + /// or if the AGV plans its own trajectory. + std::optional trajectory; + + /// \brief Length of the path from the start node to the end node [m]. + /// Used by line-guided AGVs to decrease their speed before reaching a stop position. + std::optional length; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Edge& other) const + { + if (this->edge_id != other.edge_id) return false; + if (this->sequence_id != other.sequence_id) return false; + if (this->start_node_id != other.start_node_id) return false; + if (this->end_node_id != other.end_node_id) return false; + if (this->released != other.released) return false; + if (this->actions != other.actions) return false; + if (this->edge_description != other.edge_description) return false; + if (this->max_speed != other.max_speed) return false; + if (this->max_height != other.max_height) return false; + if (this->min_height != other.min_height) return false; + if (this->orientation != other.orientation) return false; + if (this->orientation_type != other.orientation_type) return false; + if (this->direction != other.direction) return false; + if (this->rotation_allowed != other.rotation_allowed) return false; + if (this->max_rotation_speed != other.max_rotation_speed) return false; + if (this->trajectory != other.trajectory) return false; + if (this->length != other.length) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Edge& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__EDGE_HPP_ diff --git a/vda5050_types/include/vda5050_types/edge_state.hpp b/vda5050_types/include/vda5050_types/edge_state.hpp new file mode 100644 index 0000000..9653203 --- /dev/null +++ b/vda5050_types/include/vda5050_types/edge_state.hpp @@ -0,0 +1,81 @@ +/* + * 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_TYPES__EDGE_STATE_HPP_ +#define VDA5050_TYPES__EDGE_STATE_HPP_ + +#include +#include +#include + +#include "vda5050_types/trajectory.hpp" + +namespace vda5050_types { + +/// \brief Provides information about the edge +/// Part of the state message +struct EdgeState +{ + /// \brief Unique edge identification + std::string edge_id; + + /// \brief A sequence ID to differentiate multiple edges with the same ID + uint32_t sequence_id = 0; + + /// \brief Additional information on the edge + std::optional edge_description; + + /// \brief Defines if the edge is part of the base or horizon + /// - True indicates edge is part of the base + /// - False indicates edge is part of the horizon + bool released = false; + + /// \brief The trajectory in NURBS, defined as starting from the point + /// the AGV starts to enter the edge until the point where it reports the + /// next node as traversed + std::optional trajectory; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const EdgeState& other) const + { + if (this->edge_id != other.edge_id) return false; + if (this->sequence_id != other.sequence_id) return false; + if (this->edge_description != other.edge_description) return false; + if (this->released != other.released) return false; + if (this->trajectory != other.trajectory) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const EdgeState& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__EDGE_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/envelope2d.hpp b/vda5050_types/include/vda5050_types/envelope2d.hpp new file mode 100644 index 0000000..8551c35 --- /dev/null +++ b/vda5050_types/include/vda5050_types/envelope2d.hpp @@ -0,0 +1,68 @@ +/** + * 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_TYPES__ENVELOPE2D_HPP_ +#define VDA5050_TYPES__ENVELOPE2D_HPP_ + +#include +#include +#include + +#include "vda5050_types/polygon_point.hpp" + +namespace vda5050_types { + +/// \brief AGV envelope curves in 2D. +struct Envelope2d +{ + /// \brief Name of the envelope curve set. + std::string set; + + /// \brief Envelope curve as an x/y-polygon. Polygon is assumed as closed and shall be non-self-intersecting. + std::vector polygon_points; + + /// \brief Free-form text: description of envelope curve set. + std::optional description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Envelope2d& other) const + { + if (this->set != other.set) return false; + if (this->polygon_points != other.polygon_points) return false; + if (this->description != other.description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Envelope2d& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ENVELOPE2D_HPP_ diff --git a/vda5050_types/include/vda5050_types/envelope3d.hpp b/vda5050_types/include/vda5050_types/envelope3d.hpp new file mode 100644 index 0000000..0dade18 --- /dev/null +++ b/vda5050_types/include/vda5050_types/envelope3d.hpp @@ -0,0 +1,73 @@ +/** + * 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_TYPES__ENVELOPE3D_HPP_ +#define VDA5050_TYPES__ENVELOPE3D_HPP_ + +#include +#include + +namespace vda5050_types { + +/// \brief Struct for an envelope curve in 3D. +struct Envelope3d +{ + /// \brief Name of the envelope curve set. + std::string set; + + /// \brief Format of the data, eg: DXF. + std::string format; + + /// \brief 3D-envelope curve data as a string. Format of this data is specified in the 'format' field, and should be handled accordingly. + std::optional data; + + /// \brief Protocol and URL definition for downloading the 3D-envelope curve data. eg: ftp://xxx.yyy.com/ac4dgvhoif5tghji + std::optional url; + + /// \brief Free-form text: description of envelope curve set. + std::optional description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Envelope3d& other) const + { + if (this->set != other.set) return false; + if (this->format != other.format) return false; + if (this->data != other.data) return false; + if (this->url != other.url) return false; + if (this->description != other.description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Envelope3d& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ENVELOPE3D_HPP_ diff --git a/vda5050_types/include/vda5050_types/error.hpp b/vda5050_types/include/vda5050_types/error.hpp new file mode 100644 index 0000000..0107fd3 --- /dev/null +++ b/vda5050_types/include/vda5050_types/error.hpp @@ -0,0 +1,75 @@ +/* + * 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_TYPES__ERROR_HPP_ +#define VDA5050_TYPES__ERROR_HPP_ + +#include +#include +#include + +#include "vda5050_types/error_level.hpp" +#include "vda5050_types/error_reference.hpp" + +namespace vda5050_types { + +/// \brief Error information conveyed by the AGV +/// Part of the state message +struct Error +{ + /// \brief Type/name of error + std::string error_type; + + /// \brief Array of references to identify the source of the error + std::optional> error_references; + + /// \brief Verbose description providing details + /// and possible causes of the error + std::optional error_description; + + /// \brief Severity of error + ErrorLevel error_level = ErrorLevel::WARNING; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + bool operator==(const Error& other) const + { + if (error_type != other.error_type) return false; + if (error_references != other.error_references) return false; + if (error_description != other.error_description) return false; + if (error_level != other.error_level) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Error& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ERROR_HPP_ diff --git a/vda5050_types/include/vda5050_types/error_level.hpp b/vda5050_types/include/vda5050_types/error_level.hpp new file mode 100644 index 0000000..c1f2749 --- /dev/null +++ b/vda5050_types/include/vda5050_types/error_level.hpp @@ -0,0 +1,36 @@ +/* + * 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_TYPES__ERROR_LEVEL_HPP_ +#define VDA5050_TYPES__ERROR_LEVEL_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for errorLevel +enum class ErrorLevel +{ + /// \brief AGV is ready to drive without human intervention + WARNING, + + /// \brief AGV is not in running condition + FATAL +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ERROR_LEVEL_HPP_ diff --git a/vda5050_types/include/vda5050_types/error_reference.hpp b/vda5050_types/include/vda5050_types/error_reference.hpp new file mode 100644 index 0000000..6b62f64 --- /dev/null +++ b/vda5050_types/include/vda5050_types/error_reference.hpp @@ -0,0 +1,63 @@ +/* + * 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_TYPES__ERROR_REFERENCE_HPP_ +#define VDA5050_TYPES__ERROR_REFERENCE_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Describes a variable and its value related to the error +/// Part of the state message +struct ErrorReference +{ + /// \brief Specifies the type of reference used + /// (e.g., nodeId, edgeId, orderId, actionId, etc.) + std::string reference_key; + + /// \brief The value that belongs to the reference key. For + /// example, the ID of the node where the error occurred + std::string reference_value; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + bool operator==(const ErrorReference& other) const + { + if (reference_key != other.reference_key) return false; + if (reference_value != other.reference_value) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const ErrorReference& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ERROR_REFERENCE_HPP_ diff --git a/vda5050_types/include/vda5050_types/factsheet.hpp b/vda5050_types/include/vda5050_types/factsheet.hpp new file mode 100644 index 0000000..5bc7c02 --- /dev/null +++ b/vda5050_types/include/vda5050_types/factsheet.hpp @@ -0,0 +1,93 @@ +/** + * 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_TYPES__FACTSHEET_HPP_ +#define VDA5050_TYPES__FACTSHEET_HPP_ + +#include "vda5050_types/agv_geometry.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/load_specification.hpp" +#include "vda5050_types/physical_parameters.hpp" +#include "vda5050_types/protocol_features.hpp" +#include "vda5050_types/protocol_limits.hpp" +#include "vda5050_types/type_specification.hpp" +#include "vda5050_types/vehicle_config.hpp" + +namespace vda5050_types { + +/// \brief Contains the AGV's capabilities, limits, features, and configuration +/// as required by the VDA5050 specification. +struct Factsheet +{ + /// \brief Message header that contains the common fields required across all messages + Header header; + + /// \brief Parameters that generally specify the class and the capabilities of the AGV + TypeSpecification type_specification; + + /// \brief Parameters that specify the basic physical properties of the AGV + PhysicalParameters physical_parameters; + + /// \brief Limits for length of identifiers, arrays, strings, and similar in MQTT communication + ProtocolLimits protocol_limits; + + /// \brief Supported features of VDA5050 protocol + ProtocolFeatures protocol_features; + + /// \brief Detailed definition of AGV geometry + AGVGeometry agv_geometry; + + /// \brief Abstract specification of load capabilities + LoadSpecification load_specification; + + /// \brief Summary of current software and hardware versions on the vehicle + /// and optional network information + VehicleConfig vehicle_config; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Factsheet& other) const + { + if (this->header != other.header) return false; + if (this->type_specification != other.type_specification) return false; + if (this->physical_parameters != other.physical_parameters) return false; + if (this->protocol_limits != other.protocol_limits) return false; + if (this->protocol_features != other.protocol_features) return false; + if (this->agv_geometry != other.agv_geometry) return false; + if (this->load_specification != other.load_specification) return false; + if (this->vehicle_config != other.vehicle_config) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Factsheet& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__FACTSHEET_HPP_ diff --git a/vda5050_types/include/vda5050_types/header.hpp b/vda5050_types/include/vda5050_types/header.hpp new file mode 100644 index 0000000..2784717 --- /dev/null +++ b/vda5050_types/include/vda5050_types/header.hpp @@ -0,0 +1,85 @@ +/* + * 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_TYPES__HEADER_HPP_ +#define VDA5050_TYPES__HEADER_HPP_ + +#include +#include +#include +#include + +namespace vda5050_types { + +constexpr const char* ISO8601_FORMAT = "%Y-%m-%dT%H:%M:%S"; + +/// \brief The VDA 5050 header sent with each message. These +/// are common fields across all message schemas +struct Header +{ + /// \brief Header ID of the message. It is defined per topic and incremented + /// by 1 with each sent (but not necessarily received) message. + uint32_t header_id = 0; + + /// \brief Timestamp (ISO8601, UTC) + /// YYYY-MM-DDTHH:mm:ss.ffZ (e.g., "2017-04-15T11:40:03.12Z") + std::chrono::time_point timestamp; + + /// \brief Version of the protocol [Major].[Minor].[Patch] (e.g., 1.3.2). + std::string version; + + /// \brief Manufacturer of the AGV + std::string manufacturer; + + /// \brief Serial number of the AGV. + std::string serial_number; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Header& other) const + { + if (header_id != other.header_id) return false; + if ( + std::chrono::duration_cast( + timestamp.time_since_epoch()) != + std::chrono::duration_cast( + other.timestamp.time_since_epoch())) + return false; + if (version != other.version) return false; + if (manufacturer != other.manufacturer) return false; + if (serial_number != other.serial_number) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Header& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__HEADER_HPP_ diff --git a/vda5050_types/include/vda5050_types/info.hpp b/vda5050_types/include/vda5050_types/info.hpp new file mode 100644 index 0000000..a42c443 --- /dev/null +++ b/vda5050_types/include/vda5050_types/info.hpp @@ -0,0 +1,74 @@ +/* + * 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_TYPES__INFO_HPP_ +#define VDA5050_TYPES__INFO_HPP_ + +#include +#include +#include + +#include "vda5050_types/info_level.hpp" +#include "vda5050_types/info_reference.hpp" + +namespace vda5050_types { + +/// \brief Messages carrying information for visualization or debugging +/// Part of the state message +struct Info +{ + /// \brief Type/name of information + std::string info_type; + + /// \brief Array of references to identify the source of the information + std::optional> info_references; + + /// \brief Verbose description of the information + std::optional info_description; + + /// \brief Type of the information + InfoLevel info_level = InfoLevel::DEBUG; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + bool operator==(const Info& other) const + { + if (info_type != other.info_type) return false; + if (info_references != other.info_references) return false; + if (info_description != other.info_description) return false; + if (info_level != other.info_level) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Info& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__INFO_HPP_ diff --git a/vda5050_types/include/vda5050_types/info_level.hpp b/vda5050_types/include/vda5050_types/info_level.hpp new file mode 100644 index 0000000..aefd07d --- /dev/null +++ b/vda5050_types/include/vda5050_types/info_level.hpp @@ -0,0 +1,36 @@ +/* + * 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_TYPES__INFO_LEVEL_HPP_ +#define VDA5050_TYPES__INFO_LEVEL_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for infoLevel +enum class InfoLevel +{ + /// \brief Used for debugging + DEBUG, + + /// \brief Used for visualization + INFO +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__INFO_LEVEL_HPP_ diff --git a/vda5050_types/include/vda5050_types/info_reference.hpp b/vda5050_types/include/vda5050_types/info_reference.hpp new file mode 100644 index 0000000..d023bf3 --- /dev/null +++ b/vda5050_types/include/vda5050_types/info_reference.hpp @@ -0,0 +1,62 @@ +/* + * 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_TYPES__INFO_REFERENCE_HPP_ +#define VDA5050_TYPES__INFO_REFERENCE_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Describes a variable and its value related to the information +/// Part of the state message +struct InfoReference +{ + /// \brief Specifies the type of reference used + /// (e.g., nodeId, edgeId, orderId, actionId, etc.) + std::string reference_key; + + /// \brief The value that belongs to the reference key + std::string reference_value; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + bool operator==(const InfoReference& other) const + { + if (reference_key != other.reference_key) return false; + if (reference_value != other.reference_value) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const InfoReference& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__INFO_REFERENCE_HPP_ diff --git a/vda5050_types/include/vda5050_types/instant_actions.hpp b/vda5050_types/include/vda5050_types/instant_actions.hpp new file mode 100644 index 0000000..080671e --- /dev/null +++ b/vda5050_types/include/vda5050_types/instant_actions.hpp @@ -0,0 +1,65 @@ +/** + * 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_TYPES__INSTANT_ACTIONS_HPP_ +#define VDA5050_TYPES__INSTANT_ACTIONS_HPP_ + +#include +#include "vda5050_types/action.hpp" +#include "vda5050_types/header.hpp" + +namespace vda5050_types { + +/// \brief A message containing an instantActions information +/// Published by Master Control on the topic /instantActions to inform an AGV +/// of a set of actions to be executed as soon as they arrive +struct InstantActions +{ + /// \brief Message header + Header header; + + /// \brief Array of actions that need to be performed immediately and + /// are not part of the regular order + std::vector actions; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const InstantActions& other) const + { + if (this->header != other.header) return false; + if (this->actions != other.actions) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const InstantActions& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__INSTANT_ACTIONS_HPP_ diff --git a/vda5050_types/include/vda5050_types/load.hpp b/vda5050_types/include/vda5050_types/load.hpp new file mode 100644 index 0000000..7c45d6e --- /dev/null +++ b/vda5050_types/include/vda5050_types/load.hpp @@ -0,0 +1,89 @@ +/* + * 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_TYPES__LOAD_HPP_ +#define VDA5050_TYPES__LOAD_HPP_ + +#include +#include +#include + +#include "vda5050_types/bounding_box_reference.hpp" +#include "vda5050_types/load_dimensions.hpp" + +namespace vda5050_types { + +/// \brief Information about the load the AGV is carrying +/// Part of the state message +struct Load +{ + /// \brief Unique ID of the load + /// - Empty field if the AGV can identify the load but + /// hasn't identified it yet + /// - Optional if the AGV cannot identify the load + std::optional load_id; + + /// \brief Type of load. + std::optional load_type; + + /// \brief Indicates which load handling/carrying unit of the AGV is used, + /// e.g., in case the AGV has multiple spots/positions to carry loads + std::optional load_position; + + /// \brief Point of reference for the location of the bounding box. It is + /// always the center of the bounding box bottom surface (at height = 0) + /// and is described in coordinates of the AGV coordinate system + std::optional bounding_box_reference; + + /// \brief Dimensions of the loads bounding box in meters + std::optional load_dimensions; + + /// \brief Absolute weight of the load measured in kg + std::optional weight; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Load& other) const + { + if (this->load_id != other.load_id) return false; + if (this->load_type != other.load_type) return false; + if (this->weight != other.weight) return false; + if (this->load_dimensions != other.load_dimensions) return false; + if (this->bounding_box_reference != other.bounding_box_reference) + return false; + if (this->load_position != other.load_position) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Load& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__LOAD_HPP_ diff --git a/vda5050_types/include/vda5050_types/load_dimensions.hpp b/vda5050_types/include/vda5050_types/load_dimensions.hpp new file mode 100644 index 0000000..8f10ac2 --- /dev/null +++ b/vda5050_types/include/vda5050_types/load_dimensions.hpp @@ -0,0 +1,65 @@ +/* + * 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_TYPES__LOAD_DIMENSIONS_HPP_ +#define VDA5050_TYPES__LOAD_DIMENSIONS_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Dimensions of the load's bounding box in meters +/// Part of the state message +struct LoadDimensions +{ + /// \brief Absolute length of the loads bounding box in meter + double length = 0.0; + + /// \brief Absolute width of the loads bounding box in meter + double width = 0.0; + + /// \brief Absolute height of the loads bounding box in meter + std::optional height; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const LoadDimensions& other) const + { + if (this->length != other.length) return false; + if (this->width != other.width) return false; + if (this->height != other.height) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const LoadDimensions& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__LOAD_DIMENSIONS_HPP_ diff --git a/vda5050_types/include/vda5050_types/load_set.hpp b/vda5050_types/include/vda5050_types/load_set.hpp new file mode 100644 index 0000000..ee368e8 --- /dev/null +++ b/vda5050_types/include/vda5050_types/load_set.hpp @@ -0,0 +1,143 @@ +/** + * 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_TYPES__LOAD_SET_HPP_ +#define VDA5050_TYPES__LOAD_SET_HPP_ + +#include +#include +#include +#include "vda5050_types/bounding_box_reference.hpp" +#include "vda5050_types/load_dimensions.hpp" + +namespace vda5050_types { + +/// \brief Represents one load set supported by the AGV. +struct LoadSet +{ + /// \brief Unique name of the load set (e.g., DEFAULT, SET1, etc.) + std::string set_name; + + /// \brief Type of load (e.g., EPAL, XLT1200, etc.) + std::string load_type; + + /// \brief Array of load positions between load handling devices that this load set is valid for. + /// If this parameter does not exist or is empty, this load set is valid for all load + /// handling devices on this AGV. + std::optional> load_positions; + + /// \brief Bounding box reference as defined in parameter loads[] in the state message. + std::optional bounding_box_reference; + + /// \brief Load dimensions as defined in parameter loads[] in the state message. + std::optional load_dimensions; + + /// \brief Maximum weight of this load type [kg]. + std::optional max_weight; + + /// \brief Minimum allowed height for handling this load type [m]. + /// References bounding_box_reference. + std::optional min_load_handling_height; + + /// \brief Maximum allowed height for handling this load type [m]. + /// References bounding_box_reference. + std::optional max_load_handling_height; + + /// \brief Minimum allowed depth for handling this load type [m]. + /// References bounding_box_reference. + std::optional min_load_handling_depth; + + /// \brief Maximum allowed depth for handling this load type [m]. + /// References bounding_box_reference. + std::optional max_load_handling_depth; + + /// \brief Minimum allowed tilt for handling this load type [rad]. + std::optional min_load_handling_tilt; + + /// \brief Maximum allowed tilt for handling this load type [rad]. + std::optional max_load_handling_tilt; + + /// \brief Maximum allowed speed for handling this load type [m/s]. + std::optional agv_speed_limit; + + /// \brief Maximum allowed acceleration for handling this load type [m/s²]. + std::optional agv_acceleration_limit; + + /// \brief Maximum allowed deceleration for handling this load type [m/s²]. + std::optional agv_deceleration_limit; + + /// \brief Approximate time for picking up the load [s]. + std::optional pick_time; + + /// \brief Approximate time for dropping the load [s]. + std::optional drop_time; + + /// \brief Free-form description of this load handling set. + std::optional description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const LoadSet& other) const + { + if (this->set_name != other.set_name) return false; + if (this->load_type != other.load_type) return false; + if (this->load_positions != other.load_positions) return false; + if (this->bounding_box_reference != other.bounding_box_reference) + return false; + if (this->load_dimensions != other.load_dimensions) return false; + if (this->max_weight != other.max_weight) return false; + if (this->min_load_handling_height != other.min_load_handling_height) + return false; + if (this->max_load_handling_height != other.max_load_handling_height) + return false; + if (this->min_load_handling_depth != other.min_load_handling_depth) + return false; + if (this->max_load_handling_depth != other.max_load_handling_depth) + return false; + if (this->min_load_handling_tilt != other.min_load_handling_tilt) + return false; + if (this->max_load_handling_tilt != other.max_load_handling_tilt) + return false; + if (this->agv_speed_limit != other.agv_speed_limit) return false; + if (this->agv_acceleration_limit != other.agv_acceleration_limit) + return false; + if (this->agv_deceleration_limit != other.agv_deceleration_limit) + return false; + if (this->pick_time != other.pick_time) return false; + if (this->drop_time != other.drop_time) return false; + if (this->description != other.description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const LoadSet& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__LOAD_SET_HPP_ diff --git a/vda5050_types/include/vda5050_types/load_specification.hpp b/vda5050_types/include/vda5050_types/load_specification.hpp new file mode 100644 index 0000000..afc8cc8 --- /dev/null +++ b/vda5050_types/include/vda5050_types/load_specification.hpp @@ -0,0 +1,66 @@ +/** + * 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_TYPES__LOAD_SPECIFICATION_HPP_ +#define VDA5050_TYPES__LOAD_SPECIFICATION_HPP_ + +#include +#include +#include +#include "vda5050_types/load_set.hpp" + +namespace vda5050_types { + +/// \brief Message specifying the load handling and supported load types of the AGV +struct LoadSpecification +{ + /// \brief Array of load positions / load handling devices. + /// This array contains the valid values for "state.loads[].loadPosition" and for the + /// action parameter "lhd" of pick-and-drop actions. + /// If this array doesn't exist or is empty, the AGV has no load handling device. + std::optional> load_positions; + + /// \brief Array of load sets that can be handled by the AGV. + std::optional> load_sets; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const LoadSpecification& other) const + { + if (this->load_positions != other.load_positions) return false; + if (this->load_sets != other.load_sets) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const LoadSpecification& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__LOAD_SPECIFICATION_HPP_ diff --git a/vda5050_types/include/vda5050_types/max_array_lens.hpp b/vda5050_types/include/vda5050_types/max_array_lens.hpp new file mode 100644 index 0000000..30bfaa3 --- /dev/null +++ b/vda5050_types/include/vda5050_types/max_array_lens.hpp @@ -0,0 +1,124 @@ +/** + * 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_TYPES__MAX_ARRAY_LENS_HPP_ +#define VDA5050_TYPES__MAX_ARRAY_LENS_HPP_ + +#include + +namespace vda5050_types { + +/// \brief This message defines the maximum lengths of arrays processable by the AGV +/// according to the VDA5050 specification. +struct MaxArrayLens +{ + /// \brief Maximum number of nodes per order processable by the AGV. + uint32_t order_nodes; + + /// \brief Maximum number of edges per order processable by the AGV. + uint32_t order_edges; + + /// \brief Maximum number of actions per node processable by the AGV. + uint32_t node_actions; + + /// \brief Maximum number of actions per edge processable by the AGV. + uint32_t edge_actions; + + /// \brief Maximum number of parameters per action processable by the AGV. + uint32_t actions_actions_parameters; + + /// \brief Maximum number of instant actions per message processable by the AGV. + uint32_t instant_actions; + + /// \brief Maximum number of knots per trajectory processable by the AGV. + uint32_t trajectory_knot_vector; + + /// \brief Maximum number of control points per trajectory processable by the AGV. + uint32_t trajectory_control_points; + + /// \brief Maximum number of nodeStates sent by the AGV, + /// maximum number of nodes in base of AGV. + uint32_t state_node_states; + + /// \brief Maximum number of edgeStates sent by the AGV, + /// maximum number of edges in base of AGV. + uint32_t state_edge_states; + + /// \brief Maximum number of load objects sent by the AGV. + uint32_t state_loads; + + /// \brief Maximum number of actionStates sent by the AGV. + uint32_t state_action_states; + + /// \brief Maximum number of errors sent by the AGV in one state message. + uint32_t state_errors; + + /// \brief Maximum number of information objects sent by the AGV in one state message. + uint32_t state_information; + + /// \brief Maximum number of error references sent by the AGV for each error. + uint32_t error_error_references; + + /// \brief Maximum number of info references sent by the AGV for each information. + uint32_t information_info_references; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const MaxArrayLens& other) const + { + if (this->order_nodes != other.order_nodes) return false; + if (this->order_edges != other.order_edges) return false; + if (this->node_actions != other.node_actions) return false; + if (this->edge_actions != other.edge_actions) return false; + if (this->actions_actions_parameters != other.actions_actions_parameters) + return false; + if (this->instant_actions != other.instant_actions) return false; + if (this->trajectory_knot_vector != other.trajectory_knot_vector) + return false; + if (this->trajectory_control_points != other.trajectory_control_points) + return false; + if (this->state_node_states != other.state_node_states) return false; + if (this->state_edge_states != other.state_edge_states) return false; + if (this->state_loads != other.state_loads) return false; + if (this->state_action_states != other.state_action_states) return false; + if (this->state_errors != other.state_errors) return false; + if (this->state_information != other.state_information) return false; + if (this->error_error_references != other.error_error_references) + return false; + if (this->information_info_references != other.information_info_references) + return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const MaxArrayLens& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__MAX_ARRAY_LENS_HPP_ diff --git a/vda5050_types/include/vda5050_types/max_string_lens.hpp b/vda5050_types/include/vda5050_types/max_string_lens.hpp new file mode 100644 index 0000000..4fd0726 --- /dev/null +++ b/vda5050_types/include/vda5050_types/max_string_lens.hpp @@ -0,0 +1,129 @@ +/** + * 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_TYPES__MAX_STRING_LENS_HPP_ +#define VDA5050_TYPES__MAX_STRING_LENS_HPP_ + +#include +#include + +namespace vda5050_types { + +/// \brief This message defines the maximum lengths of strings used in MQTT communication +/// and other identifiers within the VDA5050 specification. +struct MaxStringLens +{ + /// \brief Maximum MQTT message length. + std::optional msg_len; + + /// \brief Maximum length of serial number part in MQTT topics. + /// Affected parameters: + /// - order.serialNumber + /// - instantActions.serialNumber + /// - state.serialNumber + /// - visualization.serialNumber + /// - connection.serialNumber + std::optional topic_serial_len; + + /// \brief Maximum length of all other parts in MQTT topics. + /// Affected parameters: + /// - order.timestamp + /// - order.version + /// - order.manufacturer + /// - instantActions.timestamp + /// - instantActions.version + /// - instantActions.manufacturer + /// - state.timestamp + /// - state.version + /// - state.manufacturer + /// - visualization.timestamp + /// - visualization.version + /// - visualization.manufacturer + /// - connection.timestamp + /// - connection.version + /// - connection.manufacturer + std::optional topic_elem_len; + + /// \brief Maximum length of ID strings. + /// Affected parameters: + /// - order.orderId + /// - order.zoneSetId + /// - node.nodeId + /// - nodePosition.mapId + /// - action.actionId + /// - edge.edgeId + /// - edge.startNodeId + /// - edge.endNodeId + std::optional id_len; + + /// \brief Maximum length of enum and key strings. + /// Affected parameters: + /// - action.actionType + /// - action.blockingType + /// - edge.direction + /// - actionParameter.key + /// - state.operatingMode + /// - load.loadPosition + /// - load.loadType + /// - actionState.actionStatus + /// - error.errorType + /// - error.errorLevel + /// - errorReference.referenceKey + /// - info.infoType + /// - info.infoLevel + /// - safetyState.eStop + /// - connection.connectionState + std::optional enum_len; + + /// \brief Maximum length of loadId strings. + std::optional load_id_len; + + /// \brief If true, ID strings must contain numerical values only. + std::optional id_numerical_only; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const MaxStringLens& other) const + { + if (this->msg_len != other.msg_len) return false; + if (this->topic_serial_len != other.topic_serial_len) return false; + if (this->topic_elem_len != other.topic_elem_len) return false; + if (this->id_len != other.id_len) return false; + if (this->enum_len != other.enum_len) return false; + if (this->load_id_len != other.load_id_len) return false; + if (this->id_numerical_only != other.id_numerical_only) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const MaxStringLens& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__MAX_STRING_LENS_HPP_ diff --git a/vda5050_types/include/vda5050_types/network.hpp b/vda5050_types/include/vda5050_types/network.hpp new file mode 100644 index 0000000..0351ed8 --- /dev/null +++ b/vda5050_types/include/vda5050_types/network.hpp @@ -0,0 +1,75 @@ +/** + * 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_TYPES__NETWORK_HPP_ +#define VDA5050_TYPES__NETWORK_HPP_ + +#include +#include +#include + +namespace vda5050_types { + +/// \brief Information about the vehicle's network configuration. +struct Network +{ + /// \brief Array of Domain Name Servers (DNS) used by the vehicle. + std::optional> dns_servers; + + /// \brief Array of Network Time Protocol (NTP) servers used by the vehicle. + std::optional> ntp_servers; + + /// \brief A priori assigned IP address used to communicate with the MQTT broker. + /// Note: This IP address should not be modified/changed during operations. + std::optional local_ip_address; + + /// \brief The subnet mask used in the network configuration corresponding to the local IP address. + std::optional netmask; + + /// \brief The default gateway used by the vehicle, corresponding to the local IP address. + std::optional default_gateway; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Network& other) const + { + if (this->dns_servers != other.dns_servers) return false; + if (this->ntp_servers != other.ntp_servers) return false; + if (this->local_ip_address != other.local_ip_address) return false; + if (this->netmask != other.netmask) return false; + if (this->default_gateway != other.default_gateway) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Network& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__NETWORK_HPP_ diff --git a/vda5050_types/include/vda5050_types/node.hpp b/vda5050_types/include/vda5050_types/node.hpp new file mode 100644 index 0000000..40e2adb --- /dev/null +++ b/vda5050_types/include/vda5050_types/node.hpp @@ -0,0 +1,87 @@ +/** + * 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_TYPES__NODE_HPP_ +#define VDA5050_TYPES__NODE_HPP_ + +#include +#include +#include +#include + +#include "vda5050_types/action.hpp" +#include "vda5050_types/node_position.hpp" + +namespace vda5050_types { + +/// \brief Defines a single node in an Order message. +struct Node +{ + /// \brief Unique node identification + std::string node_id; + + /// \brief Number to track the sequence of nodes and edges in an order and to simplify order updates. + /// Distinguishes between nodes that may be passed more than once in the same order. + /// Runs across all nodes/edges of the same order and resets when a new orderId is issued. + uint32_t sequence_id; + + /// \brief "true" indicates that the node is part of the base + /// "false" indicates that the node is part of the horizon + bool released; + + /// \brief Array of actions to be executed at this node. + /// Empty array if no actions are required. + std::vector actions; + + /// \brief Node position, required only for vehicle-types that need node positions. + /// (e.g., not required for line-guided vehicles) + std::optional node_position; + + /// \brief Additional information about the node + std::optional node_description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Node& other) const + { + if (this->node_id != other.node_id) return false; + if (this->sequence_id != other.sequence_id) return false; + if (this->released != other.released) return false; + if (this->actions != other.actions) return false; + if (this->node_position != other.node_position) return false; + if (this->node_description != other.node_description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Node& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__NODE_HPP_ diff --git a/vda5050_types/include/vda5050_types/node_position.hpp b/vda5050_types/include/vda5050_types/node_position.hpp new file mode 100644 index 0000000..fcc895a --- /dev/null +++ b/vda5050_types/include/vda5050_types/node_position.hpp @@ -0,0 +1,101 @@ +/* + * 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_TYPES__NODE_POSITION_HPP_ +#define VDA5050_TYPES__NODE_POSITION_HPP_ + +#include +#include + +namespace vda5050_types { + +/// \brief Defines a position on a map in world coordinate system +/// Part of the state message +struct NodePosition +{ + /// \brief X-position of the AGV on the map in reference to the + /// world coordinate system in meters. + double x = 0.0; + + /// \brief Y-position of the AGV on the map in reference to the + /// world coordinate system in meters. + double y = 0.0; + + /// \brief Orientation of the AGV in radians. If provided, + /// the AGV must assume the orientation on this node + /// - If the previous edge, disallows rotation then the AGV + /// must rotate on the node + /// - If the following edge has a different orientation but + /// disallows rotation then the AGV must rotate on the node + /// before entering the edge + /// Valid range: [-PI, PI] + std::optional theta; + + /// \brief Indicates how accurate the AGV must be to drive over a node + /// for it to count as traversed + /// - If deviation = 0, then the AGV must traverese the node within the + /// normal tolerance of the AGV manufacturer + /// - If deviation > 0, then the AGV can traverse the node within the + /// deviation radius + std::optional allowed_deviation_x_y; + + /// \brief Indicates how precise the orientation defined in theta has to be met + /// on the node by the AGV. + /// Valid range: [0, PI] + std::optional allowed_deviation_theta; + + /// \brief Unique identification of the map on which the AGV is currently + /// operating + std::string map_id; + + /// \brief Additional information on the map + std::optional map_description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const NodePosition& other) const + { + if (this->allowed_deviation_theta != other.allowed_deviation_theta) + return false; + if (this->allowed_deviation_x_y != other.allowed_deviation_x_y) + return false; + if (this->map_description != other.map_description) return false; + if (this->map_id != other.map_id) return false; + if (this->theta != other.theta) return false; + if (this->x != other.x) return false; + if (this->y != other.y) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const NodePosition& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__NODE_POSITION_HPP_ diff --git a/vda5050_types/include/vda5050_types/node_state.hpp b/vda5050_types/include/vda5050_types/node_state.hpp new file mode 100644 index 0000000..47407d2 --- /dev/null +++ b/vda5050_types/include/vda5050_types/node_state.hpp @@ -0,0 +1,83 @@ +/* + * 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_TYPES__NODE_STATE_HPP_ +#define VDA5050_TYPES__NODE_STATE_HPP_ + +#include +#include +#include + +#include "vda5050_types/node_position.hpp" + +namespace vda5050_types { + +/// \brief Provides information about the node to be traversed +/// for fulfilling the order +/// Part of the state message +struct NodeState +{ + /// \brief Unique node identification + std::string node_id; + + /// \brief A sequence identification to differentiate + /// multiple nodes with the same ID + uint32_t sequence_id = 0; + + /// \brief Additional information on the node + std::optional node_description; + + /// \brief Position of the node on the active AGV map in + /// world coordinate system + std::optional node_position; + + /// \brief Defines if the node is part of the base or horizon + /// - True indicates node is part of the base + /// - False indicates node is part of the horizon + bool released = false; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const NodeState& other) const + { + if (this->node_description != other.node_description) return false; + if (this->node_id != other.node_id) return false; + if (this->node_position != other.node_position) return false; + if (this->released != other.released) return false; + if (this->sequence_id != other.sequence_id) return false; + + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const NodeState& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__NODE_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/operating_mode.hpp b/vda5050_types/include/vda5050_types/operating_mode.hpp new file mode 100644 index 0000000..bd9ac64 --- /dev/null +++ b/vda5050_types/include/vda5050_types/operating_mode.hpp @@ -0,0 +1,55 @@ +/* + * 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_TYPES__OPERATING_MODE_HPP_ +#define VDA5050_TYPES__OPERATING_MODE_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Enum values for operatingMode +enum class OperatingMode +{ + /// \brief AGV is under full control of the master control. It drives + /// and executes actions based on orders from the master control + AUTOMATIC, + + /// \brief AGV is under control of the master control in areas like + /// driving and executing actions. The driving speeds is however + /// controlled by the HMI + SEMIAUTOMATIC, + + /// \brief AGV is not under control of the master control. HMI can + /// be used to control the AGV. Only location of the AGV is sent to + /// the master control + MANUAL, + + /// \brief AGV is not under the control of the master + /// control and it doesn't send order or actions to it. Authorized + /// personnel can reconfigure the AGV + SERVICE, + + /// \brief AGV is not under the control of the master control. It is + /// being taught + TEACHIN +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__OPERATING_MODE_HPP_ diff --git a/vda5050_types/include/vda5050_types/optional_parameters.hpp b/vda5050_types/include/vda5050_types/optional_parameters.hpp new file mode 100644 index 0000000..fe9b7c1 --- /dev/null +++ b/vda5050_types/include/vda5050_types/optional_parameters.hpp @@ -0,0 +1,71 @@ +/** + * 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_TYPES__OPTIONAL_PARAMETERS_HPP_ +#define VDA5050_TYPES__OPTIONAL_PARAMETERS_HPP_ + +#include +#include +#include "vda5050_types/support_type.hpp" + +namespace vda5050_types { + +/// \brief Supported or required optional parameters. +struct OptionalParameters +{ + /// \brief Full name of optional parameter. + /// eg: "order.nodes.nodePosition.allowedDeviationTheta". + std::string parameter; + + /// \brief Type of support for the optional parameter. + SupportType support; + + /// \brief Free-form text: description of optional parameter. + /// eg: + /// - Reason why the optional parameter direction is necessary for this AGV type. + /// - The parameter nodeMarker shall contain unsigned integers only. + /// - NURBS support is limited to straight lines and circular segments. + std::optional description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const OptionalParameters& other) const + { + if (this->parameter != other.parameter) return false; + if (this->support != other.support) return false; + if (this->description != other.description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const OptionalParameters& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__OPTIONAL_PARAMETERS_HPP_ diff --git a/vda5050_types/include/vda5050_types/order.hpp b/vda5050_types/include/vda5050_types/order.hpp new file mode 100644 index 0000000..bb746aa --- /dev/null +++ b/vda5050_types/include/vda5050_types/order.hpp @@ -0,0 +1,89 @@ +/** + * 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_TYPES__ORDER_HPP_ +#define VDA5050_TYPES__ORDER_HPP_ + +#include +#include +#include +#include + +#include "vda5050_types/edge.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/node.hpp" + +namespace vda5050_types { + +/// \brief Defines the message used to communicate from master control to the AGV +struct Order +{ + /// \brief Message header that contains the common fields required across all messages + Header header; + + /// \brief Order identification used to identify multiple order messages that belongs + /// to the same order. + std::string order_id; + + /// \brief Order update identification used to identify multiple order messages that + /// belong to the same order. + uint32_t order_update_id; + + /// \brief An array of Node messages to be traversed for fulfilling the order. + /// A valid order consists of at least one Node. + /// For the case of only one Node, leave the Edge array empty. + std::vector nodes; + + /// \brief An array of Edge objects to be traversed for fulfilling the order. + std::vector edges; + + /// \brief A unique identifier of the zone set that the AGV has to use for navigation + /// or that was used by master control for planning. + /// If no zones are used, leave empty. + std::optional zone_set_id; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Order& other) const + { + if (this->header != other.header) return false; + if (this->order_id != other.order_id) return false; + if (this->order_update_id != other.order_update_id) return false; + if (this->nodes != other.nodes) return false; + if (this->edges != other.edges) return false; + if (this->zone_set_id != other.zone_set_id) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Order& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ORDER_HPP_ diff --git a/vda5050_types/include/vda5050_types/orientation_type.hpp b/vda5050_types/include/vda5050_types/orientation_type.hpp new file mode 100644 index 0000000..8c084f8 --- /dev/null +++ b/vda5050_types/include/vda5050_types/orientation_type.hpp @@ -0,0 +1,36 @@ +/** + * 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_TYPES__ORIENTATION_TYPE_HPP_ +#define VDA5050_TYPES__ORIENTATION_TYPE_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for orientationType +enum class OrientationType +{ + /// \brief Relative to the global project specific map coordinate system. + GLOBAL, + + /// \brief Tangential to the edge. + TANGENTIAL, +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__ORIENTATION_TYPE_HPP_ diff --git a/vda5050_types/include/vda5050_types/physical_parameters.hpp b/vda5050_types/include/vda5050_types/physical_parameters.hpp new file mode 100644 index 0000000..99264ae --- /dev/null +++ b/vda5050_types/include/vda5050_types/physical_parameters.hpp @@ -0,0 +1,92 @@ +/** + * 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_TYPES__PHYSICAL_PARAMETERS_HPP_ +#define VDA5050_TYPES__PHYSICAL_PARAMETERS_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Describes physical properties of the AGV. +struct PhysicalParameters +{ + /// \brief Minimal controlled continuous speed of the AGV [m/s] + double speed_min; + + /// \brief Maximum speed of the AGV [m/s] + double speed_max; + + /// \brief Maximum acceleration with maximum load [m/s²] + double acceleration_max; + + /// \brief Maximum deceleration with maximum load [m/s²] + double deceleration_max; + + /// \brief Minimum height of AGV [m] + double height_min; + + /// \brief Maximum height of AGV [m] + double height_max; + + /// \brief Width of AGV [m] + double width; + + /// \brief Length of AGV [m] + double length; + + /// \brief Minimal controlled continuous rotation speed of the AGV [rad/s] + std::optional angular_speed_min; + + /// \brief Maximum rotation speed of the AGV [rad/s] + std::optional angular_speed_max; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const PhysicalParameters& other) const + { + if (this->speed_min != other.speed_min) return false; + if (this->speed_max != other.speed_max) return false; + if (this->acceleration_max != other.acceleration_max) return false; + if (this->deceleration_max != other.deceleration_max) return false; + if (this->height_min != other.height_min) return false; + if (this->height_max != other.height_max) return false; + if (this->width != other.width) return false; + if (this->length != other.length) return false; + if (this->angular_speed_min != other.angular_speed_min) return false; + if (this->angular_speed_max != other.angular_speed_max) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const PhysicalParameters& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__PHYSICAL_PARAMETERS_HPP_ diff --git a/vda5050_types/include/vda5050_types/polygon_point.hpp b/vda5050_types/include/vda5050_types/polygon_point.hpp new file mode 100644 index 0000000..3ed5e97 --- /dev/null +++ b/vda5050_types/include/vda5050_types/polygon_point.hpp @@ -0,0 +1,58 @@ +/** + * 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_TYPES__POLYGON_POINT_HPP_ +#define VDA5050_TYPES__POLYGON_POINT_HPP_ + +namespace vda5050_types { + +/// \brief A point in a polygon. +struct PolygonPoint +{ + /// \brief X-position of the polygon point [m]. + double x; + + /// \brief Y-position of the polygon point [m]. + double y; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const PolygonPoint& other) const + { + if (this->x != other.x) return false; + if (this->y != other.y) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const PolygonPoint& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__POLYGON_POINT_HPP_ diff --git a/vda5050_types/include/vda5050_types/position.hpp b/vda5050_types/include/vda5050_types/position.hpp new file mode 100644 index 0000000..3bf80e2 --- /dev/null +++ b/vda5050_types/include/vda5050_types/position.hpp @@ -0,0 +1,64 @@ +/** + * 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_TYPES__POSITION_HPP_ +#define VDA5050_TYPES__POSITION_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Struct describing the pose of the AGV +struct Position +{ + /// \brief X-position in the AGV's coordinate system [m]. + double x; + + /// \brief Y-position in the AGV's coordinate system [m]. + double y; + + /// \brief Orientation of the wheel in the AGV coordinate system [rad]. Mandatory for fixed wheels + std::optional theta; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Position& other) const + { + if (this->x != other.x) return false; + if (this->y != other.y) return false; + if (this->theta != other.theta) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Position& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__POSITION_HPP_ diff --git a/vda5050_types/include/vda5050_types/protocol_features.hpp b/vda5050_types/include/vda5050_types/protocol_features.hpp new file mode 100644 index 0000000..b5e0841 --- /dev/null +++ b/vda5050_types/include/vda5050_types/protocol_features.hpp @@ -0,0 +1,65 @@ +/** + * 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_TYPES__PROTOCOL_FEATURES_HPP_ +#define VDA5050_TYPES__PROTOCOL_FEATURES_HPP_ + +#include +#include "vda5050_types/agv_action.hpp" +#include "vda5050_types/optional_parameters.hpp" + +namespace vda5050_types { + +/// \brief Message defining the actions and parameters supported by the AGV. +struct ProtocolFeatures +{ + /// \brief Array of supported and/or required optional parameters. + /// Optional parameters that are not listed here are assumed to be not supported + /// by the AGV + std::vector optional_parameters; + + /// \brief Array of actions with parameters supported by this AGV. This includes + /// standard actions specified in VDA5050 and manufacturer-specific actions. + std::vector agv_actions; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const ProtocolFeatures& other) const + { + if (this->optional_parameters != other.optional_parameters) return false; + if (this->agv_actions != other.agv_actions) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const ProtocolFeatures& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__PROTOCOL_FEATURES_HPP_ diff --git a/vda5050_types/include/vda5050_types/protocol_limits.hpp b/vda5050_types/include/vda5050_types/protocol_limits.hpp new file mode 100644 index 0000000..4534f73 --- /dev/null +++ b/vda5050_types/include/vda5050_types/protocol_limits.hpp @@ -0,0 +1,68 @@ +/** + * 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_TYPES__PROTOCOL_LIMITS_HPP_ +#define VDA5050_TYPES__PROTOCOL_LIMITS_HPP_ + +#include "vda5050_types/max_array_lens.hpp" +#include "vda5050_types/max_string_lens.hpp" +#include "vda5050_types/timing.hpp" + +namespace vda5050_types { + +/// \brief Message describing the protocol limitations of the AGV. +/// If a parameter is not defined or set to zero then there is no explicit limit +/// for that parameter +struct ProtocolLimits +{ + /// \brief Maximum lengths of strings + MaxStringLens max_string_lens; + + /// \brief Maximum lengths of arrays + MaxArrayLens max_array_lens; + + /// \brief Timing information + Timing timing; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const ProtocolLimits& other) const + { + if (this->max_string_lens != other.max_string_lens) return false; + if (this->max_array_lens != other.max_array_lens) return false; + if (this->timing != other.timing) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const ProtocolLimits& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__PROTOCOL_LIMITS_HPP_ diff --git a/vda5050_types/include/vda5050_types/safety_state.hpp b/vda5050_types/include/vda5050_types/safety_state.hpp new file mode 100644 index 0000000..6abee18 --- /dev/null +++ b/vda5050_types/include/vda5050_types/safety_state.hpp @@ -0,0 +1,64 @@ +/* + * 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_TYPES__SAFETY_STATE_HPP_ +#define VDA5050_TYPES__SAFETY_STATE_HPP_ + +#include "vda5050_types/e_stop.hpp" + +namespace vda5050_types { + +/// \brief Provides information about the safety state of the AGV +/// Part of the state message +struct SafetyState +{ + /// \brief Acknowledgement type of the emergency stop + EStop e_stop = EStop::NONE; + + /// \brief Protective field violation + /// - True if field is violated + /// - False if field is not violated + bool field_violation = false; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const SafetyState& other) const + { + if (this->e_stop != other.e_stop) return false; + if (this->field_violation != other.field_violation) return false; + + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const SafetyState& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__SAFETY_STATE_HPP_ diff --git a/vda5050_types/include/vda5050_types/state.hpp b/vda5050_types/include/vda5050_types/state.hpp new file mode 100644 index 0000000..3e0647d --- /dev/null +++ b/vda5050_types/include/vda5050_types/state.hpp @@ -0,0 +1,182 @@ +/* + * 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_TYPES__STATE_HPP_ +#define VDA5050_TYPES__STATE_HPP_ + +#include +#include +#include +#include + +#include "vda5050_types/action_state.hpp" +#include "vda5050_types/agv_position.hpp" +#include "vda5050_types/battery_state.hpp" +#include "vda5050_types/edge_state.hpp" +#include "vda5050_types/error.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/info.hpp" +#include "vda5050_types/load.hpp" +#include "vda5050_types/node_state.hpp" +#include "vda5050_types/operating_mode.hpp" +#include "vda5050_types/safety_state.hpp" +#include "vda5050_types/velocity.hpp" + +namespace vda5050_types { + +/// \brief A message containing state information of the AGV published +/// by the AGV on topic /state to inform the master control of +/// the current AGV state +struct State +{ + /// \brief Message header that contains the common fields required across + /// all messages + Header header; + + /// \brief Unique order identification of the current order or the + /// previous finished order. The ID is kept until a new order is + /// received. Empty string if no previous ID is available + std::string order_id; + + /// \brief Order update identification to identify that an order update + /// has been accepted by the AGV. The default value is 0 if no previous + /// order update ID is available + uint32_t order_update_id = 0; + + /// \brief Unique ID of the zone set that the AGV currently uses + /// for path planning. Must be the same as the one used in the order, + /// otherwise the AGV is to reject the order + std::optional zone_set_id; + + /// \brief Node ID of last reached node or, if AGV is currently on a + /// node, current node. Empty string if no ID is available + std::string last_node_id; + + /// \brief Sequence ID of the last reached node or, if the AGV is currently + /// on a node, sequence of current node. 0 if no ID is available + uint32_t last_node_sequence_id = 0; + + /// \brief Array of nodes that need to be traversed for fulfilling + /// the order. Empty list if the AGV is idle + std::vector node_states; + + /// \brief Array of edges that need to be traversed for fulfilling + /// the order. Empty list if the AGV is idle + std::vector edge_states; + + /// \brief Defines the position on a map in world coordinates. Each + /// floor has its own map. + std::optional agv_position; + + /// \brief The AGVs velocity in vehicle coordinates + std::optional velocity; + + /// \brief Array for information about the loads the AGV is currently + /// carrying. If an empty list is received, then the master control + /// should assume the AGV can reason about its load state and that it + /// currently does not carry any load + std::optional> loads; + + /// \brief Indicates the driving and/or rotation movements of the AGV. Does + /// not include lift movements + bool driving = false; + + /// \brief Indicates the paused state of the AGV, either because of a + /// physical button push or an instantAction, and that it can resume + /// the order + std::optional paused; + + /// \brief Request a new base from the master control when the AGV is almost + /// at the end of its base and about to reduce speed + std::optional new_base_request; + + /// \brief Used by line guided vehicles to indicate the distance it has been + /// driving past the last node.\nDistance is in meters + std::optional distance_since_last_node; + + /// \brief Array with a list of current actions and actions to be + /// finished. This may include actions from previous nodes that are + /// still in progress. When an action is completed, an updated state + /// message must be published with the action status set to finished. The + /// action states are kept until a new order is received + std::vector action_states; + + /// \brief Contains all battery-related information + BatteryState battery_state; + + /// \brief Current operating mode of the AGV + OperatingMode operating_mode = OperatingMode::AUTOMATIC; + + /// \brief Array of error names/types. Errors are kept + /// until resolution + std::vector errors; + + /// \brief Array of information messages for visualization/debugging. There + /// is no specification when these messages are deleted + std::optional> information; + + /// \brief Contains all safety-related information + SafetyState safety_state; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const State& other) const + { + if (this->header != other.header) return false; + if (this->order_id != other.order_id) return false; + if (this->order_update_id != other.order_update_id) return false; + if (this->zone_set_id != other.zone_set_id) return false; + if (this->last_node_id != other.last_node_id) return false; + if (this->last_node_sequence_id != other.last_node_sequence_id) + return false; + if (this->node_states != other.node_states) return false; + if (this->edge_states != other.edge_states) return false; + if (this->agv_position != other.agv_position) return false; + if (this->velocity != other.velocity) return false; + if (this->loads != other.loads) return false; + if (this->driving != other.driving) return false; + if (this->paused != other.paused) return false; + if (this->new_base_request != other.new_base_request) return false; + if (this->distance_since_last_node != other.distance_since_last_node) + return false; + if (this->action_states != other.action_states) return false; + if (this->battery_state != other.battery_state) return false; + if (this->operating_mode != other.operating_mode) return false; + if (this->errors != other.errors) return false; + if (this->information != other.information) return false; + if (this->safety_state != other.safety_state) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const State& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__STATE_HPP diff --git a/vda5050_types/include/vda5050_types/support_type.hpp b/vda5050_types/include/vda5050_types/support_type.hpp new file mode 100644 index 0000000..f7a84c5 --- /dev/null +++ b/vda5050_types/include/vda5050_types/support_type.hpp @@ -0,0 +1,18 @@ +#ifndef VDA5050_TYPES__SUPPORT_TYPE_HPP_ +#define VDA5050_TYPES__SUPPORT_TYPE_HPP_ + +namespace vda5050_types { + +/// \brief Type of support for OptionalParameters. +enum class SupportType +{ + /// \brief Optional parameter is supported as specified. + SUPPORTED, + + /// \brief Optional parameter is required for proper AGV operation. + REQUIRED +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__SUPPORT_TYPE_HPP_ diff --git a/vda5050_types/include/vda5050_types/timing.hpp b/vda5050_types/include/vda5050_types/timing.hpp new file mode 100644 index 0000000..956e0a7 --- /dev/null +++ b/vda5050_types/include/vda5050_types/timing.hpp @@ -0,0 +1,71 @@ +/** + * 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_TYPES__TIMING_HPP_ +#define VDA5050_TYPES__TIMING_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Timing information. +struct Timing +{ + /// \brief Minimum interval sending order messages to the AGV [s] + float min_order_interval; + + /// \brief Minimum interval for sending state messages [s] + float min_state_interval; + + /// \brief Default interval for sending state messages. If not defined, the default value + /// from the main document is used [s] + std::optional default_state_interval; + + /// \brief Default interval for sending messages on visualization topic [s] + std::optional visualization_interval; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Timing& other) const + { + if (this->min_order_interval != other.min_order_interval) return false; + if (this->min_state_interval != other.min_state_interval) return false; + if (this->default_state_interval != other.default_state_interval) + return false; + if (this->visualization_interval != other.visualization_interval) + return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Timing& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__TIMING_HPP_ diff --git a/vda5050_types/include/vda5050_types/trajectory.hpp b/vda5050_types/include/vda5050_types/trajectory.hpp new file mode 100644 index 0000000..53d83b1 --- /dev/null +++ b/vda5050_types/include/vda5050_types/trajectory.hpp @@ -0,0 +1,73 @@ +/* + * 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_TYPES__TRAJECTORY_HPP_ +#define VDA5050_TYPES__TRAJECTORY_HPP_ + +#include + +#include "vda5050_types/control_point.hpp" + +namespace vda5050_types { + +/// \brief Trajectory described as a NURBS (points defining a spline) +/// Part of the state message +struct Trajectory +{ + /// \brief The number of control points that influence a point on the + /// curve. Increasing this value increases the continuity of the curve + /// Valid range: [1, infinity) + double degree = 1.0; + + /// \brief Sequence of parameter values determining how the control point + /// affects the NURBS curve + /// Valid range: [0.0, 1.0] + std::vector knot_vector; + + /// \brief Array of control points from beginning to end, defining the + /// NURBS, explicitly including the start and end point. + std::vector control_points; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Trajectory& other) const + { + if (this->degree != other.degree) return false; + if (this->knot_vector != other.knot_vector) return false; + if (this->control_points != other.control_points) return false; + + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Trajectory& other) const + { + return !this->operator==(other); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__TRAJECTORY_HPP_ diff --git a/vda5050_types/include/vda5050_types/type_specification.hpp b/vda5050_types/include/vda5050_types/type_specification.hpp new file mode 100644 index 0000000..dc1775a --- /dev/null +++ b/vda5050_types/include/vda5050_types/type_specification.hpp @@ -0,0 +1,89 @@ +/** + * 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_TYPES__TYPE_SPECIFICATION_HPP_ +#define VDA5050_TYPES__TYPE_SPECIFICATION_HPP_ + +#include +#include +#include + +#include "vda5050_types/agv_class.hpp" +#include "vda5050_types/agv_kinematic.hpp" + +namespace vda5050_types { + +/// \brief This message describes general properties of the AGV type. +struct TypeSpecification +{ + /// \brief Free text generalized series name as specified by manufacturer + std::string series_name; + + /// \brief Simplified description of AGV kinematics type. + /// Allowed values: [DIFF, OMNI, THREEWHEEL] + AGVKinematic agv_kinematic; + + /// \brief Simplified description of AGV class. + /// Allowed values: [FORKLIFT, CONVEYOR, TUGGER, CARRIER] + AGVClass agv_class; + + /// \brief Maximum loadable mass [kg] + double max_load_mass; + + /// \brief Simplified description of localization types. + /// Example values: NATURAL, REFLECTOR, RFID, DMC, SPOT, GRID + std::vector localization_types; + + /// \brief Path planning types supported by the AGV, sorted by priority. + /// Example values: PHYSICAL_LINE_GUIDED, VIRTUAL_LINE_GUIDED, AUTONOMOUS + std::vector navigation_types; + + /// \brief Free text human-readable description of the AGV type series + std::optional series_description; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const TypeSpecification& other) const + { + if (this->series_name != other.series_name) return false; + if (this->agv_kinematic != other.agv_kinematic) return false; + if (this->agv_class != other.agv_class) return false; + if (this->max_load_mass != other.max_load_mass) return false; + if (this->localization_types != other.localization_types) return false; + if (this->navigation_types != other.navigation_types) return false; + if (this->series_description != other.series_description) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const TypeSpecification& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__TYPE_SPECIFICATION_HPP_ diff --git a/vda5050_types/include/vda5050_types/value_data_type.hpp b/vda5050_types/include/vda5050_types/value_data_type.hpp new file mode 100644 index 0000000..3041011 --- /dev/null +++ b/vda5050_types/include/vda5050_types/value_data_type.hpp @@ -0,0 +1,48 @@ +/* + * 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_TYPES__VALUE_DATA_TYPE_HPP_ +#define VDA5050_TYPES__VALUE_DATA_TYPE_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for ActionParameterFactsheet +enum class ValueDataType +{ + /// \brief Boolean data type + BOOL, + + /// \brief Number data type + NUMBER, + + /// \brief Integer data type + INTEGER, + + /// \brief Float data type + FLOAT, + + /// \brief Object data type + OBJECT, + + /// \brief Array data type + ARRAY +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__VALUE_DATA_TYPE_HPP_ diff --git a/vda5050_types/include/vda5050_types/vehicle_config.hpp b/vda5050_types/include/vda5050_types/vehicle_config.hpp new file mode 100644 index 0000000..a3b7e40 --- /dev/null +++ b/vda5050_types/include/vda5050_types/vehicle_config.hpp @@ -0,0 +1,65 @@ +/** + * 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_TYPES__VEHICLE_CONFIG_HPP_ +#define VDA5050_TYPES__VEHICLE_CONFIG_HPP_ + +#include +#include +#include "vda5050_types/network.hpp" +#include "vda5050_types/version_info.hpp" + +namespace vda5050_types { + +/// \brief Details the software and hardware versions running on the vehicle, +/// as well as a brief summary of network information. +struct VehicleConfig +{ + /// \brief Array of key-value pair objects containing software and hardware information. + std::optional> versions; + + /// \brief Information about the vehicle's network connection. + /// The listed information shall not be updated while the vehicle is operating. + std::optional network; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const VehicleConfig& other) const + { + if (this->versions != other.versions) return false; + if (this->network != other.network) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const VehicleConfig& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__VEHICLE_CONFIG_HPP_ diff --git a/vda5050_types/include/vda5050_types/velocity.hpp b/vda5050_types/include/vda5050_types/velocity.hpp new file mode 100644 index 0000000..5ac190e --- /dev/null +++ b/vda5050_types/include/vda5050_types/velocity.hpp @@ -0,0 +1,64 @@ +/* + * 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_TYPES__VELOCITY_HPP_ +#define VDA5050_TYPES__VELOCITY_HPP_ + +#include + +namespace vda5050_types { + +/// \brief The AGV's velocity in vehicle coordinate +struct Velocity +{ + /// \brief Velocity in the x direction + std::optional vx; + + /// \brief Velocity in the y direction + std::optional vy; + + /// \brief Turning speed around the z axis + std::optional omega; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Velocity& other) const + { + if (vx != other.vx) return false; + if (vy != other.vy) return false; + if (omega != other.omega) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other the other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Velocity& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__VELOCITY_HPP_ diff --git a/vda5050_types/include/vda5050_types/version_info.hpp b/vda5050_types/include/vda5050_types/version_info.hpp new file mode 100644 index 0000000..397288b --- /dev/null +++ b/vda5050_types/include/vda5050_types/version_info.hpp @@ -0,0 +1,60 @@ +/** + * 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_TYPES__VERSION_INFO_HPP_ +#define VDA5050_TYPES__VERSION_INFO_HPP_ + +#include + +namespace vda5050_types { + +/// \brief Key-value pair containing software/hardware version information. +struct VersionInfo +{ + /// \brief Key of the software/hardware version used (e.g., softwareVersion). + std::string key; + + /// \brief The version corresponding to the key (e.g., v1.12.4-beta). + std::string value; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const VersionInfo& other) const + { + if (this->key != other.key) return false; + if (this->value != other.value) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const VersionInfo& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__VERSION_INFO_HPP_ diff --git a/vda5050_types/include/vda5050_types/visualization.hpp b/vda5050_types/include/vda5050_types/visualization.hpp new file mode 100644 index 0000000..dc02c45 --- /dev/null +++ b/vda5050_types/include/vda5050_types/visualization.hpp @@ -0,0 +1,69 @@ +/** + * 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_TYPES__VISUALIZATION_HPP_ +#define VDA5050_TYPES__VISUALIZATION_HPP_ + +#include +#include "vda5050_types/agv_position.hpp" +#include "vda5050_types/header.hpp" +#include "vda5050_types/velocity.hpp" + +namespace vda5050_types { + +/// \brief A message containing AGV position and/or velocity for visualization +/// Published by the AGV on topic /visualization for near-real time +/// position update. +struct Visualization +{ + /// \brief Message header that contains the common fields required across all messages + Header header; + + /// \brief Position of the AGV + std::optional agv_position; + + /// \brief Velocity of the AGV in vehicle coordinates + std::optional velocity; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const Visualization& other) const + { + if (this->header != other.header) return false; + if (this->agv_position != other.agv_position) return false; + if (this->velocity != other.velocity) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const Visualization& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__VISUALIZATION_HPP_ diff --git a/vda5050_types/include/vda5050_types/wheel_definition.hpp b/vda5050_types/include/vda5050_types/wheel_definition.hpp new file mode 100644 index 0000000..413ab60 --- /dev/null +++ b/vda5050_types/include/vda5050_types/wheel_definition.hpp @@ -0,0 +1,88 @@ +/** + * 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_TYPES__WHEEL_DEFINITION_HPP_ +#define VDA5050_TYPES__WHEEL_DEFINITION_HPP_ + +#include +#include + +#include "vda5050_types/position.hpp" +#include "vda5050_types/wheel_definition_type.hpp" + +namespace vda5050_types { + +/// \brief Struct describing the wheel arrangement and geometry +struct WheelDefinition +{ + /// \brief The type of the wheel + wheelDefinitionType type; + + /// \brief If set to "true", the wheel is actively driven. + bool is_active_driven; + + /// \brief If set to "true", the wheel is actively steered. + bool is_active_steered; + + /// \brief Pose of the AGV according to its coordinate system. + Position position; + + /// \brief Nominal diameter of the wheel [m]. + double diameter; + + /// \brief Nominal width of the wheel [m]. + double width; + + /// \brief Nominal displacement of the wheel's center to the rotation points. This field is necessary for caster wheels. + double center_displacement = 0.0; + + /// \brief Free-form text: can be used by the manufacturer to define constraints. + std::optional constraints; + + /// \brief Equality operator + /// + /// \param other The other object to compare to + /// + /// \return is equal? + inline bool operator==(const WheelDefinition& other) const + { + if (this->type != other.type) return false; + if (this->is_active_driven != other.is_active_driven) return false; + if (this->is_active_steered != other.is_active_steered) return false; + if (this->position != other.position) return false; + if (this->diameter != other.diameter) return false; + if (this->width != other.width) return false; + if (this->center_displacement != other.center_displacement) return false; + if (this->constraints != other.constraints) return false; + return true; + } + + /// \brief Inequality operator + /// + /// \param other The other object to compare to + /// + /// \return is not equal? + inline bool operator!=(const WheelDefinition& other) const + { + return !(this->operator==(other)); + } +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__WHEEL_DEFINITION_HPP_ diff --git a/vda5050_types/include/vda5050_types/wheel_definition_type.hpp b/vda5050_types/include/vda5050_types/wheel_definition_type.hpp new file mode 100644 index 0000000..297d343 --- /dev/null +++ b/vda5050_types/include/vda5050_types/wheel_definition_type.hpp @@ -0,0 +1,35 @@ +/** + * 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_TYPES__WHEEL_DEFINITION_TYPE_HPP_ +#define VDA5050_TYPES__WHEEL_DEFINITION_TYPE_HPP_ + +namespace vda5050_types { + +/// \brief Enum values for wheel definition +enum class wheelDefinitionType +{ + DRIVE, + CASTER, + FIXED, + MECANUM +}; + +} // namespace vda5050_types + +#endif // VDA5050_TYPES__WHEEL_DEFINITION_TYPE_HPP_ diff --git a/vda5050_types/package.xml b/vda5050_types/package.xml new file mode 100644 index 0000000..0d874aa --- /dev/null +++ b/vda5050_types/package.xml @@ -0,0 +1,20 @@ + + + + vda5050_types + 0.0.0 + TODO: Package description + Saurabh Kamat + Apache License 2.0 + + ament_cmake + + ament_cmake_cppcheck + ament_cmake_cpplint + ament_cmake_clang_format + ament_cmake_copyright + + + ament_cmake + +