|
| 1 | +/* |
| 2 | + * Copyright (c) Contributors to the Open 3D Engine Project. |
| 3 | + * For complete copyright and license terms please see the LICENSE at the root of this distribution. |
| 4 | + * |
| 5 | + * SPDX-License-Identifier: Apache-2.0 OR MIT |
| 6 | + * |
| 7 | + */ |
| 8 | + |
| 9 | +#pragma once |
| 10 | + |
| 11 | +#include <ROS2/ROS2Bus.h> |
| 12 | +#include <ROS2/Sensor/Events/SensorEventSource.h> |
| 13 | +#include <ROS2/Sensor/SensorConfiguration.h> |
| 14 | + |
| 15 | +namespace ROS2 |
| 16 | +{ |
| 17 | + namespace Internal |
| 18 | + { |
| 19 | + template< |
| 20 | + template< |
| 21 | + template<typename...> |
| 22 | + class, // EventType |
| 23 | + template<typename...> |
| 24 | + class, // EventHandlerType |
| 25 | + typename...> // Event parameters |
| 26 | + class C, |
| 27 | + class T> |
| 28 | + struct is_specialization_of : AZStd::false_type |
| 29 | + { |
| 30 | + }; |
| 31 | + |
| 32 | + template< |
| 33 | + template< |
| 34 | + template<typename...> |
| 35 | + class, // EventType |
| 36 | + template<typename...> |
| 37 | + class, // EventHandlerType |
| 38 | + typename...> // Event parameters |
| 39 | + class Base, |
| 40 | + template<typename...> |
| 41 | + class EventType, |
| 42 | + template<typename...> |
| 43 | + class EventHandlerType, |
| 44 | + typename... Args> |
| 45 | + struct is_specialization_of<Base, Base<EventType, EventHandlerType, Args...>> : AZStd::true_type |
| 46 | + { |
| 47 | + }; |
| 48 | + } // namespace Internal |
| 49 | + |
| 50 | + //! Class adapting event source (ROS2::SensorEventSource) to configurable working frequency. This is handled via adapted event, in |
| 51 | + //! a similar manner like it is done in SensorEventSource. EventSourceAdapter has its internal handler that connects to |
| 52 | + //! SensorEventSource source event, and signals adapted event according to frequency set (ROS2::EventSourceAdapter::SetFrequency). |
| 53 | + //! User can connect to this event using ROS2::EventSourceAdapter::ConnectToAdaptedEvent method. This class should be used, instead |
| 54 | + //! of using directly a class derived from SensorEventSource, when specific working frequency is required. Following this path, user can |
| 55 | + //! still use source event - ROS2::EventSourceAdapter::ConnectToSourceEvent. This template has to be resolved using a class derived from |
| 56 | + //! SensorEventSource specialization. |
| 57 | + //! @see ROS2::SensorEventSource |
| 58 | + template<class EventSourceT> |
| 59 | + class EventSourceAdapter |
| 60 | + { |
| 61 | + public: |
| 62 | + // Require non-abstract type derived from SensorEventSource specialization. |
| 63 | + static_assert(Internal::is_specialization_of<SensorEventSource, typename EventSourceT::SourceBaseType>::value); |
| 64 | + static_assert(AZStd::is_base_of<typename EventSourceT::SourceBaseType, EventSourceT>::value); |
| 65 | + static_assert(AZStd::is_abstract<EventSourceT>::value == false); |
| 66 | + |
| 67 | + static void Reflect(AZ::ReflectContext* context) |
| 68 | + { |
| 69 | + EventSourceT::Reflect(context); |
| 70 | + |
| 71 | + if ( auto* serializeContext = azrtti_cast<AZ::SerializeContext*>(context)) |
| 72 | + { |
| 73 | + serializeContext->Class<EventSourceAdapter<EventSourceT>>() |
| 74 | + ->Version(1) |
| 75 | + ->Field("Adapted frequency", &EventSourceAdapter<EventSourceT>::m_adaptedFrequency) |
| 76 | + ->Field("Event source configuration", &EventSourceAdapter<EventSourceT>::m_eventSource); |
| 77 | + |
| 78 | + if (auto* editContext = serializeContext->GetEditContext()) |
| 79 | + { |
| 80 | + editContext |
| 81 | + ->Class<EventSourceAdapter<EventSourceT>>( |
| 82 | + "Event Source Adapter", "Adapts sensor event source to specified working frequency") |
| 83 | + ->ClassElement(AZ::Edit::ClassElements::EditorData, "") |
| 84 | + ->ElementAttribute(AZ::Edit::Attributes::AutoExpand, true) |
| 85 | + ->DataElement( |
| 86 | + AZ::Edit::UIHandlers::Default, |
| 87 | + &EventSourceAdapter<EventSourceT>::m_adaptedFrequency, |
| 88 | + "Adapted frequency", |
| 89 | + "Adapter event signalling frequency"); |
| 90 | + } |
| 91 | + } |
| 92 | + } |
| 93 | + |
| 94 | + //! Starts event source adapter - assigns internal adapted event handler and starts managed event source. Adapted frequency can be |
| 95 | + //! set using ROS2::EventSourceAdapter::SetFrequency method. |
| 96 | + void Start() |
| 97 | + { |
| 98 | + m_sourceAdaptingEventHandler = typename EventSourceT::SourceEventHandlerType( |
| 99 | + [this](auto&&... args) |
| 100 | + { |
| 101 | + const float sourceDeltaTime = m_eventSource.GetDeltaTime(AZStd::forward<decltype(args)>(args)...); |
| 102 | + m_adaptedDeltaTime += sourceDeltaTime; |
| 103 | + |
| 104 | + if (!IsPublicationDeadline(sourceDeltaTime)) |
| 105 | + { |
| 106 | + return; |
| 107 | + } |
| 108 | + |
| 109 | + m_sensorAdaptedEvent.Signal(m_adaptedDeltaTime, AZStd::forward<decltype(args)>(args)...); |
| 110 | + m_adaptedDeltaTime = 0.0f; |
| 111 | + }); |
| 112 | + m_eventSource.ConnectToSourceEvent(m_sourceAdaptingEventHandler); |
| 113 | + m_eventSource.Start(); |
| 114 | + } |
| 115 | + |
| 116 | + //! Stops event source adapter - stops event source and disconnects internal adapted event handler from source event. If it will be |
| 117 | + //! necessary, this implementation allows multiple consecutive calls of Start / Stop, however user must also investigate specific |
| 118 | + //! event source implementation with such case in mind. |
| 119 | + void Stop() |
| 120 | + { |
| 121 | + m_eventSource.Stop(); |
| 122 | + m_sourceAdaptingEventHandler.Disconnect(); |
| 123 | + } |
| 124 | + |
| 125 | + //! Sets adapter working frequency. By design, adapter will not work correctly, if this frequency will be greater than used event |
| 126 | + //! source frequency - e.g. adapter will be requested to work in 60Hz, when using event source working in 30Hz. In general, adapted |
| 127 | + //! frequency should be equal or lower than event source frequency - this is forced internally |
| 128 | + //! (ROS2::EventSourceAdapter::IsPublicationDeadline). Optimal (highest precision in timing events) working conditions take place |
| 129 | + //! when event source frequency is an integer multiple of adapted frequency. |
| 130 | + //! @param adaptedFrequency Adapter working frequency. When set to zero or less adapter will be assumed to work in 1Hz. |
| 131 | + void SetFrequency(float adaptedFrequency) |
| 132 | + { |
| 133 | + m_adaptedFrequency = adaptedFrequency; |
| 134 | + } |
| 135 | + |
| 136 | + //! Connects given event handler to source event (ROS2::SensorEventSource). That event is signalled regardless of adapted frequency |
| 137 | + //! set for event source adapter (ROS2::EventSourceAdapter::SetFrequency). Its frequency depends only on specific event source |
| 138 | + //! implementation. If different working frequency is required (main purpose of ROS2::EventSourceAdapter), user should see |
| 139 | + //! ROS2::EventSourceAdapter::ConnectToAdaptedEvent method. |
| 140 | + //! @param sourceEventHandler Event handler for source event (frequency not managed by event source adapter). |
| 141 | + //! @see ROS2::SensorEventSource |
| 142 | + void ConnectToSourceEvent(typename EventSourceT::SourceEventHandlerType& sourceEventHandler) |
| 143 | + { |
| 144 | + m_eventSource.ConnectToSourceEvent(sourceEventHandler); |
| 145 | + } |
| 146 | + |
| 147 | + //! Connects given event handler to adapted event (ROS2::EventSourceAdapter). This event is signalled with a frequency set with |
| 148 | + //! ROS2::EventSourceAdapter::SetFrequency method. |
| 149 | + //! @param adaptedEventHandler Event handler for adapted event. |
| 150 | + void ConnectToAdaptedEvent(typename EventSourceT::AdaptedEventHandlerType& adaptedEventHandler) |
| 151 | + { |
| 152 | + adaptedEventHandler.Connect(m_sensorAdaptedEvent); |
| 153 | + } |
| 154 | + |
| 155 | + private: |
| 156 | + //! Uses: |
| 157 | + //! - internal tick counter, |
| 158 | + //! - last delta time of event source and |
| 159 | + //! - frequency set for adapter |
| 160 | + //! to support managing calls from event source. In other words, uses delta time of event source to calculate average number of |
| 161 | + //! source event calls per adapted event call. |
| 162 | + //! @param sourceDeltaTime Delta time of event source. |
| 163 | + //! @return Whether it is time to signal adapted event. |
| 164 | + [[nodiscard]] bool IsPublicationDeadline(float sourceDeltaTime) |
| 165 | + { |
| 166 | + if (--m_tickCounter > 0) |
| 167 | + { |
| 168 | + return false; |
| 169 | + } |
| 170 | + |
| 171 | + const float sourceFrequencyEstimation = 1.0f / sourceDeltaTime; |
| 172 | + const float numberOfFrames = |
| 173 | + m_adaptedFrequency <= sourceFrequencyEstimation ? (sourceFrequencyEstimation / m_adaptedFrequency) : 1.0f; |
| 174 | + m_tickCounter = aznumeric_cast<int>(AZStd::round(numberOfFrames)); |
| 175 | + return true; |
| 176 | + } |
| 177 | + |
| 178 | + EventSourceT m_eventSource{}; ///< Event source managed by this adapter. |
| 179 | + |
| 180 | + //! Event handler for adapting event source to specific frequency. |
| 181 | + typename EventSourceT::SourceEventHandlerType m_sourceAdaptingEventHandler{}; |
| 182 | + |
| 183 | + //! Adapted event that is called with specific frequency. |
| 184 | + typename EventSourceT::AdaptedEventType m_sensorAdaptedEvent{}; |
| 185 | + |
| 186 | + float m_adaptedFrequency{ 30.0f }; ///< Adapted frequency value. |
| 187 | + float m_adaptedDeltaTime{ 0.0f }; ///< Accumulator for calculating adapted delta time. |
| 188 | + int m_tickCounter{ 0 }; ///< Internal counter for controlling adapter frequency. |
| 189 | + }; |
| 190 | + |
| 191 | + AZ_TYPE_INFO_TEMPLATE(EventSourceAdapter, "{DC8BB5F7-8E0E-42A1-BD82-5FCD9D31B9DD}", AZ_TYPE_INFO_CLASS) |
| 192 | +} // namespace ROS2 |
0 commit comments