PlatformControllerExecutor.cpp

Go to the documentation of this file.

#include "PlatformControllerExecutor.h"
 
#include <algorithm>
#include <filesystem>
#include <utility>
 
#include <ArmarXCore/core/PackagePath.h>
#include <ArmarXCore/core/exceptions/local/ExpressionException.h>
#include <ArmarXCore/core/logging/Logging.h>
#include <ArmarXCore/core/time/Clock.h>
#include <ArmarXCore/core/time/Duration.h>
#include <ArmarXCore/util/CPPUtility/trace.h>
 
#include <armarx/control/client/ComponentPlugin.h>
#include <armarx/navigation/common/controller_types.h>
#include <armarx/navigation/core/Trajectory.h>
#include <armarx/navigation/core/aron_conversions.h>
#include <armarx/navigation/core/types.h>
#include <armarx/navigation/platform_controller/aron/PlatformLocalTrajectoryControllerConfig.aron.generated.h>
#include <armarx/navigation/platform_controller/controller_descriptions.h>
#include <armarx/navigation/trajectory_control/local/aron_conversions.h>
 
namespace armarx::navigation::server
{
 
    PlatformControllerExecutor::PlatformControllerExecutor(
        ControllerComponentPlugin& controllerComponentPlugin,
        const Properties& properties,
        const EmergencyStopInterface* emergencyStop) :
        controllerPlugin_(controllerComponentPlugin),
        properties_(properties),
        emergencyStop_(emergencyStop)
    {
        ARMARX_TRACE;
 
        if (properties_.rtUnitDynamicallyLoadLibraries)
        {
            ARMARX_WARNING
                << "Dynamic loading of the controller library is deactivated (and this "
                   "is also desired). If loading the controllers is failing subsequently, "
                   "make sure that the RobotUnit's property `LoadLibraries` contains "
                   "`armarx_navigation:armarx_navigation_platform_controller`.";
        }
 
        // make default configs available to the memory
        ARMARX_INFO << "Loading default configs";
        {
            // const std::filesystem::path configBasePath =
            //     PackagePath("armarx_navigation", "controller_config").toSystemPath();
 
            ARMARX_TRACE;
            // armarx::control::memory::config::parseAndStoreDefaultConfigs<
            //     armarx::navigation::common::ControllerType::PlatformTrajectory>(
            // "" /*configBasePath*/, controllerComponentPlugin.configMemoryWriter());
 
            ARMARX_TRACE;
        }
 
        // initialize controller
        ARMARX_INFO << "Initializing local trajectory controller";
        {
            ARMARX_TRACE;
            auto builder = controllerPlugin_.createControllerBuilder<
                armarx::navigation::common::ControllerType::PlatformLocalTrajectory>();
 
            ARMARX_TRACE;
 
            const armarx::PackagePath configPath = [&]() -> armarx::PackagePath
            {
                const armarx::PackagePath baseConfigPath(
                    "armarx_navigation", "controller_config/PlatformTrajectory/default.json");
 
                ARMARX_CHECK(std::filesystem::exists(baseConfigPath.toSystemPath()))
                    << baseConfigPath;
 
                const armarx::PackagePath robotSpecificConfigPath(
                    "armarx_navigation",
                    "controller_config/PlatformTrajectory/" + properties_.robotName +
                        "default.json");
 
                if (std::filesystem::exists(robotSpecificConfigPath.toSystemPath()))
                {
                    return robotSpecificConfigPath;
                }
 
                return baseConfigPath;
            }();
 
            auto ctrlWrapper = builder.withNodeSet("PlatformPlanning")
                                   .withConfig(configPath.toSystemPath())
                                   .create();
 
            ARMARX_TRACE;
            ARMARX_CHECK_NOT_NULL(ctrlWrapper);
            localTrajCtrl_ = std::move(ctrlWrapper);
            localTrajCtrlInitialConfig_ = localTrajCtrl_->config;
        }
 
        Clock::WaitFor(Duration::Seconds(1));
 
        ARMARX_INFO << "Initializing global trajectory controller";
        {
            ARMARX_TRACE;
            auto builder = controllerPlugin_.createControllerBuilder<
                armarx::navigation::common::ControllerType::PlatformGlobalTrajectory>();
 
            ARMARX_TRACE;
 
            builder.withNodeSet("PlatformPlanning");
 
 
            // prefer robot-specific config if available
            const armarx::PackagePath robotSpecificConfigPath(
                "armarx_navigation",
                "controller_config/GlobalTrajectory/" + properties.robotName + ".json");
 
            if (std::filesystem::exists(robotSpecificConfigPath.toSystemPath()))
            {
                ARMARX_INFO << "Using robot specific config: " << robotSpecificConfigPath;
                builder.withConfig(robotSpecificConfigPath.toSystemPath());
            }
            else // fallback: use default config
            {
                const armarx::PackagePath configPath(
                    "armarx_navigation", "controller_config/GlobalTrajectory/default.json");
                builder.withConfig(configPath.toSystemPath());
            }
 
            auto ctrlWrapper = builder.create();
 
            ARMARX_TRACE;
            ARMARX_CHECK_NOT_NULL(ctrlWrapper);
            globalTrajCtrl_ = std::move(ctrlWrapper);
            globalTrajCtrlInitialConfig_ = globalTrajCtrl_->config;
        }
 
        Clock::WaitFor(Duration::Seconds(1));
 
        ARMARX_TRACE;
        ARMARX_CHECK_NOT_NULL(localTrajCtrl_);
        ARMARX_VERBOSE << "PlatformControllerExecutor: init done.";
    }
 
    PlatformControllerExecutor::~PlatformControllerExecutor() = default;
 
    void
    PlatformControllerExecutor::execute(const core::LocalTrajectory& trajectory,
                                        const bool activateController)
    {
        ARMARX_VERBOSE << "Received trajectory for execution with " << trajectory.points().size()
                       << " points";
 
        toAron(localTrajCtrl_->config.targets.trajectory, trajectory);
 
        // sends the updated config to the controller and stores it in the memory
        localTrajCtrl_->updateConfig();
 
        if (activateController and not localTrajCtrl_->ctrl()->isControllerActive())
        {
            ARMARX_INFO << "Start local trajectory controller";
 
            localTrajCtrl_->activate();
        }
    }
 
    void
    PlatformControllerExecutor::execute(const core::GlobalTrajectory& trajectory,
                                        const bool activateController)
    {
        ARMARX_VERBOSE << "Received trajectory for execution with " << trajectory.points().size()
                       << " points";
 
        toAron(globalTrajCtrl_->config.targets.trajectory, trajectory);
 
        ARMARX_INFO << "Sending trajectory with "
                    << globalTrajCtrl_->config.targets.trajectory.points.size()
                    << " to controller.";
 
        // sends the updated config to the controller and stores it in the memory
        globalTrajCtrl_->updateConfig();
 
        if (activateController and not globalTrajCtrl_->ctrl()->isControllerActive())
        {
            globalTrajCtrl_->activate();
        }
    }
 
    void
    PlatformControllerExecutor::start(const ControllerType controllerType)
    {
        ARMARX_INFO << "Start";
        switch (controllerType)
        {
            case ControllerType::GlobalTrajectory:
                if (globalTrajCtrl_ and not globalTrajCtrl_->ctrl()->isControllerActive())
                {
                    globalTrajCtrl_->activate();
                }
                break;
            case ControllerType::LocalTrajectory:
                if (localTrajCtrl_ and not localTrajCtrl_->ctrl()->isControllerActive())
                {
                    localTrajCtrl_->activate();
                }
                break;
            case ControllerType::None:
                ARMARX_INFO << "No controller selected.";
                break;
        }
    }
 
    void
    PlatformControllerExecutor::ensureIsActive(const ControllerType controllerType)
    {
        // While the emergency stop is engaged the RobotUnit keeps our controller deactivated. We
        // run at the replanning rate, so without this we would see "inactive", activate, and
        // repeat -- fighting the safety system for the whole duration of the stop.
        if (emergencyStop_ != nullptr and emergencyStop_->isActive())
        {
            if (not emergencyStopWasActive_)
            {
                emergencyStopWasActive_ = true;
                emergencyStopReleased_ = false;
 
                // The RobotUnit has already deactivated our controller, but the *request*
                // survives, and its "force switch to reactivate old" path resurrects the
                // controller -- with the trajectory it was holding -- the moment the stop
                // releases. The robot then stands where that profile assigns cruise speed, so
                // the commanded twist steps from zero and the device ramp answers at its own
                // maximum, far above the acceleration the trajectory was parametrized for.
                // Withdrawing the request here leaves nothing to resurrect.
                ARMARX_IMPORTANT << "The emergency stop was engaged. Withdrawing the trajectory "
                                    "controller request so it is not resurrected on release.";
                withdrawControllerRequests();
            }
 
            ARMARX_INFO << deactivateSpam(5)
                        << "The emergency stop is engaged. Not activating the trajectory "
                           "controller.";
            return;
        }
 
        if (emergencyStopWasActive_)
        {
            // Report the falling edge once. The navigator re-anchors the trajectory at the
            // robot's current pose before we are asked to activate again.
            emergencyStopWasActive_ = false;
            emergencyStopReleased_ = true;
 
            ARMARX_IMPORTANT << "The emergency stop was released.";
        }
 
        switch (controllerType)
        {
            case ControllerType::GlobalTrajectory:
                if (globalTrajCtrl_)
                {
                    if (not globalTrajCtrl_->ctrl()->isControllerActive())
                    {
                        ARMARX_INFO << deactivateSpam(5)
                                    << "The global trajectory controller is not active but "
                                       "should be. Activating now.";
                        globalTrajCtrl_->activate();
                    }
                }
                break;
            case ControllerType::LocalTrajectory:
                if (localTrajCtrl_)
                {
                    if (not localTrajCtrl_->ctrl()->isControllerActive())
                    {
                        ARMARX_INFO << deactivateSpam(5)
                                    << "The local trajectory controller is not active but "
                                       "should be. Activating now.";
                        localTrajCtrl_->activate();
                    }
                }
                break;
            case ControllerType::None:
                ARMARX_INFO << "No controller selected.";
                break;
        }
    }
 
    bool
    PlatformControllerExecutor::consumeEmergencyStopRelease()
    {
        const bool released = emergencyStopReleased_;
        emergencyStopReleased_ = false;
 
        return released;
    }
 
    void
    PlatformControllerExecutor::withdrawControllerRequests()
    {
        // Deliberately no `isControllerActive()` guard -- see the declaration. `deactivate()`
        // withdraws the request, which is what matters here; whether the controller is currently
        // running is not the question.
        if (localTrajCtrl_ and localTrajCtrl_->ctrl())
        {
            localTrajCtrl_->deactivate();
        }
 
        if (globalTrajCtrl_ and globalTrajCtrl_->ctrl())
        {
            globalTrajCtrl_->deactivate();
        }
    }
 
    void
    PlatformControllerExecutor::dumpDiagnostics()
    try
    {
        if (globalTrajCtrl_ == nullptr)
        {
            return;
        }
 
        // The controller ignores this unless its own `diagnostics.enabled` is set, so there is
        // nothing to check here -- and nothing to pay when diagnostics are off beyond one config
        // update per navigation request.
        globalTrajCtrl_->config.diagnostics.dumpRequest += 1;
        globalTrajCtrl_->updateConfig();
    }
    catch (const std::exception& e)
    {
        // A debugging aid must never be able to fail a navigation request.
        ARMARX_WARNING << "Could not request the execution diagnostics dump: " << e.what();
    }
 
    void
    PlatformControllerExecutor::stop()
    {
        lastActiveController_ = ControllerType::None;
 
        ARMARX_INFO << "Stopping local trajectory controller (if available and active)";
        if (localTrajCtrl_ && localTrajCtrl_->ctrl() &&
            localTrajCtrl_->ctrl()->isControllerActive())
        {
            ARMARX_INFO << "Stopping local trajectory controller.";
            localTrajCtrl_->deactivate();
            lastActiveController_ = ControllerType::LocalTrajectory;
        }
        else
        {
            ARMARX_INFO << "Local trajectory controller was not available or active.";
        }
 
        ARMARX_INFO << "Stopping global trajectory controller (if available and active)";
        if (globalTrajCtrl_ && globalTrajCtrl_->ctrl() &&
            globalTrajCtrl_->ctrl()->isControllerActive())
        {
            ARMARX_INFO << "Stopping global trajectory controller.";
            globalTrajCtrl_->deactivate();
            lastActiveController_ = ControllerType::GlobalTrajectory;
        }
        else
        {
            ARMARX_INFO << "Global trajectory controller was not available or active.";
        }
 
 
        auto robotUnit = controllerPlugin_.getRobotUnitPlugin().getRobotUnit();
        if (robotUnit)
        {
            const auto platformUnitPrx = robotUnit->getPlatformUnit();
            if (platformUnitPrx)
            {
                ARMARX_INFO << "Stopping platform via PlatformUnit";
                platformUnitPrx->stopPlatform();
            }
            else
            {
                ARMARX_WARNING << "PlatformUnit proxy not available, cannot stop platform";
            }
        }
        else
        {
            ARMARX_INFO << "RobotUnit not available, cannot stop platform";
        }
    }
 
    void
    PlatformControllerExecutor::updateVelocityLimits(const core::TwistLimits& limits)
    {
 
        if (localTrajCtrl_->ctrl()->isControllerActive())
        {
            // keep the initial config as the upper limit
            localTrajCtrl_->config.params.limits.linear =
                std::min(limits.linear, localTrajCtrlInitialConfig_.params.limits.linear);
            localTrajCtrl_->config.params.limits.angular =
                std::min(limits.angular, localTrajCtrlInitialConfig_.params.limits.angular);
 
            localTrajCtrl_->updateConfig();
        }
 
        if (globalTrajCtrl_->ctrl()->isControllerActive())
        {
            // keep the initial config as the upper limit
            globalTrajCtrl_->config.params.limits.linear =
                std::min(limits.linear, globalTrajCtrlInitialConfig_.params.limits.linear);
            globalTrajCtrl_->config.params.limits.angular =
                std::min(limits.angular, globalTrajCtrlInitialConfig_.params.limits.angular);
 
            globalTrajCtrl_->updateConfig();
        }
    }
 
    void
    PlatformControllerExecutor::updateVelocityFactor(const float velocityFactor)
    {
        ARMARX_CHECK_POSITIVE(velocityFactor)
            << "Scaling factor may not be negative, but is " << velocityFactor;
        ARMARX_CHECK_LESS_EQUAL(velocityFactor, 1)
            << "Scaling factor may not be > 1, but is " << velocityFactor;
 
        if (localTrajCtrl_->ctrl()->isControllerActive())
        {
            localTrajCtrl_->config.params.velocityFactor = velocityFactor;
 
            localTrajCtrl_->updateConfig();
        }
 
        if (globalTrajCtrl_->ctrl()->isControllerActive())
        {
            globalTrajCtrl_->config.params.velocityFactor = velocityFactor;
 
            globalTrajCtrl_->updateConfig();
        }
    }
 
} // namespace armarx::navigation::server