RTThreadTimingsSensorDevice.h

Go to the documentation of this file.

/*
 * This file is part of ArmarX.
 *
 * ArmarX is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * ArmarX is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program. If not, see <http://www.gnu.org/licenses/>.
 *
 * @package    RobotAPI
 * @author     Raphael Grimm ( raphael dot grimm at kit dot edu )
 * @date       2017
 * @copyright  http://www.gnu.org/licenses/gpl-2.0.txt
 *             GNU General Public License
 */
 
#pragma once
 
#include "../SensorValues/SensorValueRTThreadTimings.h"
#include "../util/RtTiming.h"
#include "SensorDevice.h"
 
namespace armarx::RobotUnitModule
{
    TYPEDEF_PTRS_HANDLE(ControlThread);
}
 
namespace armarx
{
    TYPEDEF_PTRS_SHARED(RTThreadTimingsSensorDevice);
 
    class RTThreadTimingsSensorDevice : virtual public SensorDevice
    {
    public:
        RTThreadTimingsSensorDevice(const std::string& name) : DeviceBase(name), SensorDevice(name)
        {
        }
 
        const SensorValueBase* getSensorValue() const override = 0;
 
        virtual void rtMarkRtLoopStart() = 0;
        virtual void rtMarkRtLoopPreSleep() = 0;
 
        virtual void rtMarkRtBusSendReceiveStart() = 0;
        virtual void rtMarkRtBusSendReceiveEnd() = 0;
 
    protected:
        friend class RobotUnit;
        friend class RobotUnitModule::ControlThread;
 
        virtual void rtMarkRtSwitchControllerSetupStart() = 0;
        virtual void rtMarkRtSwitchControllerSetupEnd() = 0;
 
        virtual void rtMarkRtRunNJointControllersStart() = 0;
        virtual void rtMarkRtRunNJointControllersEnd() = 0;
 
        virtual void rtMarkRtHandleInvalidTargetsStart() = 0;
        virtual void rtMarkRtHandleInvalidTargetsEnd() = 0;
 
        virtual void rtMarkRtRunJointControllersStart() = 0;
        virtual void rtMarkRtRunJointControllersEnd() = 0;
 
 
        virtual void rtMarkRtReadSensorDeviceValuesStart() = 0;
        virtual void rtMarkRtReadSensorDeviceValuesEnd() = 0;
 
        virtual void rtMarkRtUpdateSensorAndControlBufferStart() = 0;
        virtual void rtMarkRtUpdateSensorAndControlBufferEnd() = 0;
 
        virtual void rtMarkRtResetAllTargetsStart() = 0;
        virtual void rtMarkRtResetAllTargetsEnd() = 0;
    };
 
    template <class SensorValueType = SensorValueRTThreadTimings>
    class RTThreadTimingsSensorDeviceImpl : public RTThreadTimingsSensorDevice
    {
    public:
        static_assert(std::is_base_of<SensorValueRTThreadTimings, SensorValueType>::value,
                      "The template parameter of RTThreadTimingsSensorDeviceImpl should be derived "
                      "from SensorValueRTThreadTimings");
 
        RTThreadTimingsSensorDeviceImpl(const std::string& name) :
            DeviceBase(name), SensorDevice(name), RTThreadTimingsSensorDevice(name)
        {
        }
 
        const SensorValueBase*
        getSensorValue() const override
        {
            return &value;
        }
 
        SensorValueType value;
 
        void
        rtMarkRtLoopStart() override
        {
            const auto now = armarx::rtNow();
            value.rtLoopRoundTripTime = now - rtLoopStart;
            rtLoopStart = now;
        }
 
        void
        rtMarkRtLoopPreSleep() override
        {
            value.rtLoopDuration = armarx::rtNow() - rtLoopStart;
        }
 
        void
        rtMarkRtBusSendReceiveStart() override
        {
            const IceUtil::Time now = armarx::rtNow();
            value.rtBusSendReceiveRoundTripTime = now - rtBusSendReceiveStart;
            rtBusSendReceiveStart = now;
        }
 
        void
        rtMarkRtBusSendReceiveEnd() override
        {
            value.rtBusSendReceiveDuration = armarx::rtNow() - rtBusSendReceiveStart;
        }
 
    protected:
#define make_markRT_X_Start_End(name)                                                              \
    virtual void rtMarkRt##name##Start() override                                                  \
    {                                                                                              \
        const IceUtil::Time now = armarx::rtNow();                                                 \
        value.rt##name##RoundTripTime = now - rt##name##Start;                                     \
        rt##name##Start = now;                                                                     \
    }                                                                                              \
    virtual void rtMarkRt##name##End() override                                                    \
    {                                                                                              \
        value.rt##name##Duration = armarx::rtNow() - rt##name##Start;                              \
    }                                                                                              \
    static_assert(true, "force trailing semi-colon")
 
        make_markRT_X_Start_End(SwitchControllerSetup);
        make_markRT_X_Start_End(RunNJointControllers);
        make_markRT_X_Start_End(HandleInvalidTargets);
        make_markRT_X_Start_End(RunJointControllers);
        make_markRT_X_Start_End(ReadSensorDeviceValues);
        make_markRT_X_Start_End(UpdateSensorAndControlBuffer);
        make_markRT_X_Start_End(ResetAllTargets);
#undef make_markRT_X_Start_End
 
    protected:
        IceUtil::Time rtSwitchControllerSetupStart;
        IceUtil::Time rtRunNJointControllersStart;
        IceUtil::Time rtHandleInvalidTargetsStart;
        IceUtil::Time rtRunJointControllersStart;
        IceUtil::Time rtBusSendReceiveStart;
        IceUtil::Time rtReadSensorDeviceValuesStart;
        IceUtil::Time rtUpdateSensorAndControlBufferStart;
        IceUtil::Time rtResetAllTargetsStart;
        IceUtil::Time rtLoopStart;
    };
 
} // namespace armarx