PlatformControllerExecutor.h

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <string>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/control/client/ControllerWrapper.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/server/execution/EmergencyStopInterface.h>
#include <armarx/navigation/server/execution/ExecutorInterface.h>
 
namespace armarx::control::client
{
    class ComponentPlugin;
}
 
namespace armarx::navigation::server
{
 
    /**
     * @brief The PlatformUnitExecutor class
     *
     * TODO: Should be renamed to whatever the new unit will be called which takes Mat4f
     * as input instead of X/Y/Yaw, so that we have a generic interface.
     */
    class PlatformControllerExecutor : virtual public ExecutorInterface
    {
 
    public:
        using ControllerComponentPlugin = armarx::control::client::ComponentPlugin;
 
        struct Properties
        {
            std::string robotName;
            bool rtUnitDynamicallyLoadLibraries = false;
        };
 
        /**
         * @param emergencyStop  Consulted before re-activating a controller. `nullptr` means
         *                       this deployment has no emergency-stop source, which is not the
         *                       same as one that has gone silent: absent proceeds, broken
         *                       refuses.
         */
        PlatformControllerExecutor(ControllerComponentPlugin& controllerComponentPlugin,
                                   const Properties& properties,
                                   const EmergencyStopInterface* emergencyStop = nullptr);
        ~PlatformControllerExecutor() override;
 
        void execute(const core::LocalTrajectory& trajectory,
                     bool activateController = false) override;
        void execute(const core::GlobalTrajectory& trajectory,
                     bool activateController = false) override;
 
        void start(ControllerType controllerType) override;
 
        void ensureIsActive(ControllerType controllerType) override;
 
        bool consumeEmergencyStopRelease() override;
 
        void stop() override;
 
        /// Bumps the controller's `diagnostics.dumpRequest`, which makes it write the episode it
        /// has recorded and begin the next one. Does nothing if diagnostics are off.
        void dumpDiagnostics() override;
 
        void updateVelocityLimits(const core::TwistLimits& limits) override;
 
        void updateVelocityFactor(float velocityFactor) override;
 
 
    private:
        /**
         * @brief Deactivate both controllers regardless of whether they currently *run*.
         *
         * `stop()` cannot serve here: it only deactivates a controller that reports active, and
         * during an emergency stop the RobotUnit has already deactivated ours -- so it would
         * leave the request standing, which is the very thing that has to go. It also stops the
         * platform through the PlatformUnit, which is meaningless while the drives are held.
         */
        void withdrawControllerRequests();
 
        ControllerComponentPlugin& controllerPlugin_;
 
        const Properties properties_;
 
        /// Non-owning; outlives this object. Null when the deployment has no emergency stop.
        const EmergencyStopInterface* emergencyStop_;
 
 
        std::unique_ptr<armarx::control::client::ControllerWrapper<
            armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>>
            globalTrajCtrl_;
 
        std::unique_ptr<armarx::control::client::ControllerWrapper<
            armarx::navigation::common::ControllerType::PlatformLocalTrajectory>>
            localTrajCtrl_;
 
        control::client::ControllerDescription<
            armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>::AronDTO
            globalTrajCtrlInitialConfig_;
 
        control::client::ControllerDescription<
            armarx::navigation::common::ControllerType::PlatformLocalTrajectory>::AronDTO
            localTrajCtrlInitialConfig_;
 
 
        ControllerType lastActiveController_ = ControllerType::None;
 
        /// Edge detection on `emergencyStop_->isActive()`, both read and written only from the
        /// navigator's periodic task via `ensureIsActive` / `consumeEmergencyStopRelease`.
        bool emergencyStopWasActive_ = false;
        bool emergencyStopReleased_ = false;
    };
 
} // namespace armarx::navigation::server