diff --git a/atos/modules/ObjectControl/inc/objectcontrol.hpp b/atos/modules/ObjectControl/inc/objectcontrol.hpp index d85db375f..76d71eba5 100644 --- a/atos/modules/ObjectControl/inc/objectcontrol.hpp +++ b/atos/modules/ObjectControl/inc/objectcontrol.hpp @@ -78,6 +78,8 @@ namespace AbsoluteKinematics { class RemoteControlled; } +class SmImpl; + /*! * \brief The ObjectControl class is intended as an overarching device * used to control a scenario. No behaviour is implemented in it @@ -86,6 +88,9 @@ namespace AbsoluteKinematics { */ class ObjectControl : public Module { + // Forward declare state machine + class Sm; + friend class SmImpl; friend class ObjectControlState; friend class AbstractKinematics::Idle; friend class AbstractKinematics::Initialized; @@ -361,7 +366,7 @@ class ObjectControl : public Module void injectObjectData(const MonitorMessage& monr); //! \brief TODO OsiHandler::LocalObjectGroundTruth_t buildOSILocalGroundTruth(const MonitorMessage&) const; - + std::unique_ptr sm; void publishScenarioInfoToJournal(); }; diff --git a/atos/modules/ObjectControl/inc/sm_impl.hpp b/atos/modules/ObjectControl/inc/sm_impl.hpp new file mode 100644 index 000000000..0cb251da7 --- /dev/null +++ b/atos/modules/ObjectControl/inc/sm_impl.hpp @@ -0,0 +1,204 @@ +#include "sml.hpp" +#include "objectcontrol.hpp" +#include + +#pragma once + +// State transitions +class SmImpl { +public: + // States + constexpr static auto idle = boost::sml::state; + constexpr static auto initialized = boost::sml::state; + constexpr static auto connecting = boost::sml::state; // Is this really a state? could be a transition, if we wait for all objects to connect + constexpr static auto armed = boost::sml::state; + constexpr static auto disarming = boost::sml::state; // Same with this one + constexpr static auto aborting = boost::sml::state; // Same with this one + constexpr static auto ready = boost::sml::state; + constexpr static auto testlive = boost::sml::state; + + // Events + struct InitializeRequest {}; + struct ConnectRequest {}; + struct DisconnectRequest {}; + struct ArmRequest {}; + struct DisarmRequest {}; + struct StartRequest {}; + struct AbortRequest {}; + struct StartObjectRequest { + uint32_t id; + std::chrono::system_clock::time_point startTime; + }; + struct ConnectedToObject {}; + struct ConnectedToLiveObject {}; + struct DisconnectedFromObject {}; + struct ObjectDisarmed {}; + struct AllObjectsConnected {}; + struct AllObjectsDisarmed {}; + struct ObjectAborting {}; + + // Guards + struct scenarioLoaded { + auto operator()(ObjectControl* handler) { + return handler->loadScenario(); + } + } scenarioLoaded; + + struct allObjectsConnected{ + auto operator()(ObjectControl* handler) { + handler->areAllObjectsIn(OBJECT_STATE_DISARMED); + }; + } allObjectsConnected; + + struct anyObjectInArmedState{ + auto operator()(ObjectControl* handler) { + handler->isAnyObjectIn(OBJECT_STATE_ARMED); + }; + } anyObjectInArmedState; + + struct allObjectsInArmedState{ + auto operator()(ObjectControl* handler) { + handler->areAllObjectsIn(OBJECT_STATE_ARMED); + }; + } allObjectsInArmedState; + + struct anyObjectInRunningState{ + auto operator()(ObjectControl* handler) { + handler->isAnyObjectIn(OBJECT_STATE_RUNNING); + }; + } anyObjectInRunningState; + + struct allObjectsDisarmedOrDisconnected{ + auto operator()(ObjectControl* handler) { + static auto disarmedOrDisconnected = [](const std::shared_ptr obj) { + return obj->getState() == OBJECT_STATE_DISARMED || !obj->isConnected(); + }; + handler->areAllObjects(disarmedOrDisconnected); + }; + } allObjectsDisarmedOrDisconnected; + + struct allObjectsDisarmed{ + auto operator()(ObjectControl* handler) { + handler->areAllObjectsIn(OBJECT_STATE_DISARMED); + }; + } allObjectsDisarmed; + + // Actions + struct setKinematicsMode { + auto operator()(ObjectControl* handler) { + //JournalRecordData(JournalRecordType::JOURNAL_RECORD_EVENT, "INIT successful"); + try { + auto anchorID = handler->getAnchorObjectID(); + handler->transformScenarioRelativeTo(anchorID); + handler->controlMode = ObjectControl::RELATIVE_KINEMATICS; + RCLCPP_INFO(handler->get_logger(), "Relative control mode enabled"); + } catch (std::invalid_argument&) { + handler->controlMode = ObjectControl::ABSOLUTE_KINEMATICS; + RCLCPP_INFO(handler->get_logger(), "Absolute control mode enabled"); + } + } + } setKinematicsMode; + + struct connectToTestObjects{ + auto operator()(ObjectControl* handler) { + //RCLCPP_INFO(handler->get_logger(), "Handling connect request"); + //JournalRecordData(JOURNAL_RECORD_EVENT, "CONNECT received"); + handler->beginConnectionAttempt(); + }; + } connectToTestObjects; + + struct clearScenario{ + auto operator()(ObjectControl* handler) { + handler->clearScenario(); + }; + } clearScenario; + + struct disconnectFromTestObjects{ + auto operator()(ObjectControl* handler) { + //JournalRecordData(JOURNAL_RECORD_EVENT, "DISCONNECT received"); + handler->abortConnectionAttempt(); + handler->disconnectObjects(); + }; + } disconnectFromTestObjects; + + struct armObjects{ + auto operator()(ObjectControl* handler) { + //JournalRecordData(JOURNAL_RECORD_EVENT, "ARM received"); + handler->armObjects(); + }; + } armObjects; + + struct startListeners{ + auto operator()(ObjectControl* handler) { + handler->startListeners(); + handler->notifyObjectsConnected(); + }; + } startListeners; + + struct disarmObjects{ + auto operator()(ObjectControl* handler) { + //JournalRecordData(JOURNAL_RECORD_EVENT, "DISARM received"); + handler->disarmObjects(); + }; + } disarmObjects; + + struct startObjectRequest{ + auto operator()(ObjectControl* handler, StartObjectRequest const& request) { + handler->startObject(request.id, request.startTime); + }; + } startObjectRequest; + + auto operator()() const noexcept { + using namespace boost::sml; + return make_transition_table( + // Idle states + *idle + event [scenarioLoaded] / setKinematicsMode = initialized, + *idle + event [!scenarioLoaded] = idle, + + // Initialized can go back to idle or connecting + initialized + event = connecting, + initialized + event / clearScenario = idle, + + // Try to connect + connecting + event / disconnectFromTestObjects = idle, + connecting + event [anyObjectInArmedState] = disarming, + connecting + event [anyObjectInRunningState] = aborting, + connecting + event [!allObjectsConnected] = connecting, + connecting + event [allObjectsConnected] / armObjects = armed, + connecting + event / startListeners = ready, + + // Try to disarm + disarming + event / disconnectFromTestObjects = idle, + disarming + event [allObjectsDisarmedOrDisconnected] = ready, + disarming + event [!allObjectsDisarmedOrDisconnected] = disarming, + disarming + event = ready, + + // Armed state (some transitions missing here..?) + armed + event [allObjectsInArmedState] = testlive, + armed + event = disarming, + armed + event = ready, + armed + event = disarming, + + // TestLive state + testlive + event = aborting, + testlive + event / startObjectRequest = testlive, + + // Abort when object requests it, when the operator request it, or when something goes wrong + connecting + event = aborting, + connecting + event = aborting, + connecting + event = aborting, + disarming + event = aborting, + disarming + event = aborting, + disarming + event = aborting, + armed + event = aborting, + armed + event = aborting, + armed + event = aborting, + + // on_entry functions for states + idle + on_entry<_> / clearScenario, + connecting + on_entry<_> / connectToTestObjects, + armed + on_entry<_> / armObjects, + disarming + on_entry<_> / disarmObjects + ); + } +}; \ No newline at end of file diff --git a/atos/modules/ObjectControl/inc/sml.hpp b/atos/modules/ObjectControl/inc/sml.hpp new file mode 100644 index 000000000..33f474e93 --- /dev/null +++ b/atos/modules/ObjectControl/inc/sml.hpp @@ -0,0 +1,2900 @@ +// +// Copyright (c) 2016-2024 Kris Jusiak (kris at jusiak dot net) +// +// Distributed under the Boost Software License, Version 1.0. +// (See accompanying file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) +// +#ifndef BOOST_SML_HPP +#define BOOST_SML_HPP +#if (__cplusplus < 201305L && _MSC_VER < 1900) +#error "[Boost::ext].SML requires C++14 support (Clang-3.4+, GCC-5.1+, MSVC-2015+)" +#else +#if defined(__ICCARM__) && __IAR_SYSTEMS_ICC__ < 8 +#error "[Boost::ext].SML requires C++14 support (IAR C/C++ ARM 8.1+)" +#endif +#define BOOST_SML_VERSION 1'1'11 +#define BOOST_SML_NAMESPACE_BEGIN \ + namespace boost { \ + inline namespace ext { \ + namespace sml { \ + inline namespace v1_1_11 { +#define BOOST_SML_NAMESPACE_END \ + } \ + } \ + } \ + } +#if defined(__clang__) +#define __BOOST_SML_UNUSED __attribute__((unused)) +#define __BOOST_SML_VT_INIT \ + {} +#if !defined(BOOST_SML_CFG_DISABLE_MIN_SIZE) +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) __VA_ARGS__ _[0] +#else +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) +#endif +#define __BOOST_SML_ZERO_SIZE_ARRAY_CREATE(...) +#define __BOOST_SML_TEMPLATE_KEYWORD template +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wgnu-string-literal-operator-template" +#pragma clang diagnostic ignored "-Wzero-length-array" +#elif defined(__GNUC__) +#if !defined(__has_builtin) +#define __BOOST_SML_DEFINED_HAS_BUILTIN +#define __has_builtin(...) 0 +#endif +#define __BOOST_SML_UNUSED __attribute__((unused)) +#define __BOOST_SML_VT_INIT \ + {} +#if !defined(BOOST_SML_CFG_DISABLE_MIN_SIZE) +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) __VA_ARGS__ _[0]{} +#else +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) +#endif +#define __BOOST_SML_ZERO_SIZE_ARRAY_CREATE(...) __VA_ARGS__ ? __VA_ARGS__ : 1 +#define __BOOST_SML_TEMPLATE_KEYWORD template +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wpedantic" +#if defined(__GNUC__) && (__GNUC__ >= 10) +#pragma GCC diagnostic ignored "-Wsubobject-linkage" +#endif +#elif defined(_MSC_VER) && !defined(__clang__) +#define __BOOST_SML_DEFINED_HAS_BUILTIN +#define __has_builtin(...) __has_builtin##__VA_ARGS__ +#define __has_builtin__make_integer_seq(...) 1 +#define __BOOST_SML_UNUSED +#define __BOOST_SML_VT_INIT +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) +#define __BOOST_SML_ZERO_SIZE_ARRAY_CREATE(...) __VA_ARGS__ ? __VA_ARGS__ : 1 +#if defined(_MSC_VER) && !defined(__clang__) && _MSC_VER >= 1910 // MSVC 2017 +#define __BOOST_SML_TEMPLATE_KEYWORD template +#else +#define __BOOST_SML_TEMPLATE_KEYWORD +#endif +#pragma warning(disable : 4503) +#pragma warning(disable : 4200) +#elif defined(__ICCARM__) +#if !defined(__has_builtin) +#define __BOOST_SML_DEFINED_HAS_BUILTIN +#define __has_builtin(...) 0 +#endif +/* Needs IAR language extensions */ +#define __BOOST_SML_UNUSED __attribute__((unused)) +#define __BOOST_SML_VT_INIT \ + {} +#define __BOOST_SML_ZERO_SIZE_ARRAY(...) +#define __BOOST_SML_ZERO_SIZE_ARRAY_CREATE(...) __VA_ARGS__ ? __VA_ARGS__ : 1 +#define __BOOST_SML_TEMPLATE_KEYWORD template +#endif +BOOST_SML_NAMESPACE_BEGIN +#define __BOOST_SML_REQUIRES(...) typename aux::enable_if<__VA_ARGS__, int>::type = 0 +namespace aux { +using byte = unsigned char; +struct none_type {}; +template +struct type {}; +template +struct non_type {}; +template +struct pair {}; +template +struct type_list { + using type = type_list; +}; +template +struct bool_list { + using type = bool_list; +}; +template +struct inherit : Ts... { + using type = inherit; +}; +template +struct identity { + using type = T; +}; +template +T &&declval(); +template +struct integral_constant { + using type = integral_constant; + static constexpr T value = V; +}; +using true_type = integral_constant; +using false_type = integral_constant; +template +using void_t = void; +template +struct always : true_type {}; +template +struct never : false_type {}; +namespace detail { +template +struct conditional; +template <> +struct conditional { + template + using fn = T; +}; +template <> +struct conditional { + template + using fn = T; +}; +} // namespace detail +template +struct conditional { + using type = typename detail::conditional::template fn; +}; +template +using conditional_t = typename detail::conditional::template fn; +template +struct enable_if {}; +template +struct enable_if { + using type = T; +}; +template +using enable_if_t = typename enable_if::type; +template +struct is_same : false_type {}; +template +struct is_same : true_type {}; +template +#if defined(_MSC_VER) && !defined(__clang__) +struct is_base_of : integral_constant { +}; +#else +using is_base_of = integral_constant; +#endif +template +decltype(T(declval()...), true_type{}) test_is_constructible(int); +template +false_type test_is_constructible(...); +template +#if defined(_MSC_VER) && !defined(__clang__) +struct is_constructible : decltype(test_is_constructible(0)) { +}; +#else +using is_constructible = decltype(test_is_constructible(0)); +#endif +template +struct is_empty_base : T { + U _; +}; +template +struct is_empty : aux::integral_constant) == sizeof(none_type)> {}; +template +struct function_traits; +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +#if defined(__cpp_noexcept_function_type) +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +template +struct function_traits { + using args = type_list; +}; +#endif +template +using function_traits_t = typename function_traits::args; +template +struct remove_const { + using type = T; +}; +template +struct remove_const { + using type = T; +}; +template +using remove_const_t = typename remove_const::type; +template +struct remove_reference { + using type = T; +}; +template +struct remove_reference { + using type = T; +}; +template +struct remove_reference { + using type = T; +}; +template +using remove_reference_t = typename remove_reference::type; +template +struct remove_pointer { + using type = T; +}; +template +struct remove_pointer { + using type = T; +}; +template +using remove_pointer_t = typename remove_pointer::type; +} // namespace aux +namespace aux { +using swallow = int[]; +template +struct index_sequence { + using type = index_sequence; +}; +#if __has_builtin(__make_integer_seq) +template +struct integer_sequence; +template +struct integer_sequence { + using type = index_sequence; +}; +template +struct make_index_sequence_impl { + using type = typename __make_integer_seq::type; +}; +#else +template +struct concat; +template +struct concat, index_sequence> : index_sequence {}; +template +struct make_index_sequence_impl + : concat::type, typename make_index_sequence_impl::type>::type {}; +template <> +struct make_index_sequence_impl<0> : index_sequence<> {}; +template <> +struct make_index_sequence_impl<1> : index_sequence<0> {}; +#endif +template +using make_index_sequence = typename make_index_sequence_impl::type; +template +struct join { + using type = type_list<>; +}; +template +struct join { + using type = T; +}; +template +struct join> : type_list {}; +template +struct join, type_list> : type_list {}; +template +struct join, type_list, type_list> : type_list {}; +template +struct join, type_list, Ts...> : join, Ts...> {}; +template +struct join, type_list, type_list, type_list, type_list, type_list, + type_list, type_list, type_list, type_list, type_list, type_list, + type_list, type_list, type_list, type_list, type_list, Us...> + : join, + Us...> {}; +template +using join_t = typename join::type; +template +struct unique_impl; +template +struct unique_impl, T, Ts...> : conditional_t, inherit...>>::value, + unique_impl, Ts...>, unique_impl, Ts...>> { +}; +template +struct unique_impl> : type_list {}; +template +struct unique : unique_impl, Ts...> {}; +template +struct unique : type_list {}; +template +using unique_t = typename unique::type; +template +struct is_unique; +template +struct is_unique : true_type {}; +template +struct is_unique, T, Ts...> + : conditional_t, inherit...>>::value, false_type, is_unique, Ts...>> {}; +template +using is_unique_t = is_unique, Ts...>; +template