| #include <BaSyx/controlcomponent/simple/ControlComponent.h> |
| #include <BaSyx/controlcomponent/enumerations/ControlComponentConstants.h> |
| |
| namespace basyx { |
| namespace controlcomponent { |
| namespace simple { |
| |
| using constants = basyx::controlcomponent::ControlComponentConstants; |
| using constants_ = basyx::controlcomponent::ControlComponentConstants_; |
| |
| ControlComponent::ControlComponent() |
| : savedOccupierId {""} |
| , occupationState{OccupationState::free} |
| , occupier{""} |
| , lastOccupier{""} |
| , exMode{ExecutionMode::Auto} |
| , exState{ExecutionState::idle} |
| , opMode{""} |
| , workState{""} |
| , errorState{""} |
| , prevError{""} |
| , cmd{""} |
| , localOverwrite{""} |
| , localOverwriteFree{""} |
| , service_operations{object::make_map()} |
| { |
| this->init_service_operations(); |
| } |
| |
| void ControlComponent::addControlComponentChangeListener(const std::shared_ptr<IControlComponentChangeListener> listener) |
| { |
| this->componentChangeListeners.emplace(listener->getUniqueID(), listener); |
| } |
| |
| void ControlComponent::removeControlComponentChangeListener(const std::shared_ptr<IControlComponentChangeListener> listener) |
| { |
| this->componentChangeListeners.erase(listener->getUniqueID()); |
| } |
| |
| const basyx::object ControlComponent::getServiceOperationMap() |
| { |
| return this->service_operations; |
| } |
| |
| void ControlComponent::finishState() |
| { |
| switch (this->getExecutionState()) |
| { |
| case ExecutionState::starting: |
| { |
| this->setExecutionState(ExecutionState::execute); |
| return; |
| } |
| case ExecutionState::execute: |
| { |
| this->setExecutionState(ExecutionState::completing); |
| return; |
| } |
| case ExecutionState::completing: |
| { |
| this->setExecutionState(ExecutionState::complete); |
| return; |
| } |
| case ExecutionState::resetting: |
| { |
| this->setExecutionState(ExecutionState::idle); |
| return; |
| } |
| case ExecutionState::holding: |
| { |
| this->setExecutionState(ExecutionState::held); |
| return; |
| } |
| case ExecutionState::unholding: |
| { |
| this->setExecutionState(ExecutionState::execute); |
| return; |
| } |
| case ExecutionState::suspending: |
| { |
| this->setExecutionState(ExecutionState::suspended); |
| return; |
| } |
| case ExecutionState::unsuspending: |
| { |
| this->setExecutionState(ExecutionState::execute); |
| return; |
| } |
| case ExecutionState::stopping: |
| { |
| this->setExecutionState(ExecutionState::stopped); |
| return; |
| } |
| case ExecutionState::stopped: |
| { |
| this->setExecutionState(ExecutionState::idle); |
| return; |
| } |
| case ExecutionState::aborting: |
| { |
| this->setExecutionState(ExecutionState::aborted); |
| return; |
| } |
| case ExecutionState::clearing: |
| { |
| this->setExecutionState(ExecutionState::stopped); |
| return; |
| } |
| } |
| } |
| |
| const std::vector<std::string> & ControlComponent::getOrderList() |
| { |
| return this->orderList; |
| } |
| |
| void ControlComponent::addOrder(const std::string &newOrder) |
| { |
| this->orderList.push_back(newOrder); |
| } |
| |
| void ControlComponent::clearOrder() |
| { |
| this->orderList.clear(); |
| } |
| |
| OccupationState ControlComponent::getOccupationState() |
| { |
| return static_cast<OccupationState>(this->occupationState); |
| } |
| |
| void ControlComponent::setOccupationState(const OccupationState &occState) |
| { |
| this->occupationState = occState; |
| this->notify_change_listeners_on_variable_change(constants::occupationState, (int) occState); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onNewOccupationState(occState); |
| } |
| } |
| |
| std::string ControlComponent::getOccupierID() |
| { |
| return this->occupier; |
| } |
| |
| void ControlComponent::setOccupierID(const std::string &occId) |
| { |
| this->occupier = occId; |
| this->notify_change_listeners_on_variable_change(constants::occupier, occId); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onNewOccupier(occId); |
| } |
| } |
| |
| std::string ControlComponent::getLastOccupierID() |
| { |
| return this->lastOccupier; |
| } |
| |
| void ControlComponent::setLastOccupierID(const std::string &occId) |
| { |
| this->lastOccupier = occId; |
| this->notify_change_listeners_on_variable_change(constants::lastOccupier, occId); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onLastOccupier(occId); |
| } |
| } |
| |
| ExecutionMode ControlComponent::getExecutionMode() |
| { |
| return this->exMode; |
| } |
| |
| void ControlComponent::setExecutionMode(const ExecutionMode &exMode) |
| { |
| this->exMode = exMode; |
| this->notify_change_listeners_on_variable_change(constants::exMode, (int) exMode); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedExecutionMode(exMode); |
| } |
| } |
| |
| ExecutionState ControlComponent::getExecutionState() |
| { |
| return this->exState; |
| } |
| |
| void ControlComponent::setExecutionState(const ExecutionState &newSt) |
| { |
| this->exState = newSt; |
| this->notify_change_listeners_on_variable_change(constants::exState, ExecutionState_::to_string(newSt)); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedExecutionState(newSt); |
| } |
| } |
| |
| std::string ControlComponent::getOperationMode() |
| { |
| return this->opMode; |
| } |
| |
| void ControlComponent::setOperationMode(const std::string &opMode) |
| { |
| this->opMode = opMode; |
| this->notify_change_listeners_on_variable_change(constants::opMode, opMode); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedOperationMode(opMode); |
| } |
| } |
| |
| std::string ControlComponent::getWorkState() |
| { |
| return this->workState; |
| } |
| |
| void ControlComponent::setWorkState(const std::string &workState) |
| { |
| this->workState = workState; |
| this->notify_change_listeners_on_variable_change(constants::workState, workState); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedWorkState(workState); |
| } |
| } |
| |
| std::string ControlComponent::getErrorState() |
| { |
| return this->errorState; |
| } |
| |
| void ControlComponent::setErrorState(const std::string &errorState) |
| { |
| this->errorState = errorState; |
| this->notify_change_listeners_on_variable_change(constants::errorState, errorState); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedErrorState(errorState); |
| } |
| } |
| |
| std::string ControlComponent::getLastErrorState() |
| { |
| return this->prevError; |
| } |
| |
| void ControlComponent::setLastErrorState(const std::string &lastErrorState) |
| { |
| this->errorState = lastErrorState; |
| this->notify_change_listeners_on_variable_change(constants::errorState, lastErrorState); |
| |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onChangedPrevError(lastErrorState); |
| } |
| } |
| |
| std::string ControlComponent::getCommand() |
| { |
| return this->cmd; |
| } |
| |
| void ControlComponent::setCommand(const std::string &cmd) |
| { |
| this->cmd = cmd; |
| } |
| |
| std::string ControlComponent::getLocalOverwrite() |
| { |
| return this->localOverwrite; |
| } |
| |
| void ControlComponent::setLocalOverwrite(const std::string &cmd) |
| { |
| this->localOverwrite = cmd; |
| } |
| |
| std::string ControlComponent::getLocalOverwriteFree() |
| { |
| return this->localOverwriteFree; |
| } |
| |
| void ControlComponent::setLocalOverwriteFree(const std::string &cmd) |
| { |
| this->localOverwriteFree = cmd; |
| } |
| |
| void ControlComponent::changeExecutionState(const ExecutionOrder &ex_order) |
| { |
| // Check if execution order leads to valid state in current state |
| switch (this->getExecutionState()) |
| { |
| case ExecutionState::idle: |
| // Process expected orders |
| if (ex_order == ExecutionOrder::start) |
| { |
| this->setExecutionState(ExecutionState::starting); |
| return; |
| } |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::starting: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::execute: |
| // Process expected orders |
| if (ex_order == ExecutionOrder::complete) |
| { |
| this->setExecutionState(ExecutionState::completing); |
| return; |
| } |
| if (ex_order == ExecutionOrder::hold) |
| { |
| this->setExecutionState(ExecutionState::holding); |
| return; |
| } |
| if (ex_order == ExecutionOrder::suspend) |
| { |
| this->setExecutionState(ExecutionState::suspending); |
| return; |
| } |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::completing: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::complete: |
| if (ex_order == ExecutionOrder::reset) |
| { |
| this->setExecutionState(ExecutionState::resetting); |
| return; |
| } |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::resetting: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::holding: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::held: |
| if (ex_order == ExecutionOrder::unhold) |
| { |
| this->setExecutionState(ExecutionState::unholding); |
| return; |
| } |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::unholding: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::suspending: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::suspended: |
| if (ex_order == ExecutionOrder::unsuspend) |
| { |
| this->setExecutionState(ExecutionState::unsuspending); |
| return; |
| } |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::unsuspending: |
| if (ex_order == ExecutionOrder::stop) |
| { |
| this->setExecutionState(ExecutionState::stopping); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::stopping: |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| |
| case ExecutionState::stopped: |
| if (ex_order == ExecutionOrder::reset) |
| { |
| this->setExecutionState(ExecutionState::resetting); |
| return; |
| } |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| case ExecutionState::aborted: |
| if (ex_order == ExecutionOrder::clear) |
| { |
| this->setExecutionState(ExecutionState::clearing); |
| return; |
| } |
| |
| case ExecutionState::clearing: |
| if (ex_order == ExecutionOrder::abort) |
| { |
| this->setExecutionState(ExecutionState::aborting); |
| return; |
| } |
| } |
| |
| } |
| |
| void ControlComponent::invokeLocalOverwrite() |
| { |
| // Store current occupier because we need to restore it later |
| this->savedOccupierId = this->getOccupierID(); |
| |
| // Enter local overwrite state |
| this->setOccupationState(OccupationState::local); |
| this->setOccupierID(constants_::to_string(constants::LOCAL)); |
| } |
| |
| void ControlComponent::clearLocalOverwrite() |
| { |
| this->setOccupierID(this->savedOccupierId); |
| |
| // Restore occupier state based on variables |
| if (this->savedOccupierId.empty()) |
| this->setOccupationState(OccupationState::free); |
| else if (this->getLastOccupierID().empty()) |
| this->setOccupationState(OccupationState::occupied); |
| else |
| this->setOccupationState(OccupationState::priority); |
| } |
| |
| template<typename T> void ControlComponent::notify_change_listeners_on_variable_change(const ControlComponentConstants &status_key, T status) |
| { |
| for (auto listener : this->componentChangeListeners) |
| { |
| listener.second->onVariableChange(constants_::to_string(status_key), status); |
| } |
| } |
| |
| void ControlComponent::init_service_operations() |
| { |
| // All lambdas returning bool since this is supported by vab |
| this->service_operations.insertKey(constants_::to_string(constants::free), object::make_function([this](std::string senderId) { |
| this->freeControlComponent(senderId); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::occupy), object::make_function([this](std::string occupier) { |
| this->occupyControlComponent(occupier); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::priority), object::make_function([this](std::string occupier) { |
| this->priorityOccupation(occupier); |
| return true; |
| })); |
| |
| this->service_operations.insertKey(constants_::to_string(constants::Auto), object::make_function([this]() { |
| this->setExecutionMode(ExecutionMode::Auto); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::semiauto), object::make_function([this]() { |
| this->setExecutionMode(ExecutionMode::Semiauto); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::manual), object::make_function([this]() { |
| this->setExecutionMode(ExecutionMode::Manual); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::simulation), object::make_function([this]() { |
| this->setExecutionMode(ExecutionMode::Simulation); |
| return true; |
| })); |
| |
| this->service_operations.insertKey(constants_::to_string(constants::start), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::start); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::reset), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::reset); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::hold), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::hold); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::unhold), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::unhold); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::suspend), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::suspend); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::unsuspend), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::unsuspend); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::abort), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::abort); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::stop), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::stop); |
| return true; |
| })); |
| this->service_operations.insertKey(constants_::to_string(constants::clear), object::make_function([this]() { |
| this->changeExecutionState(ExecutionOrder::clear); |
| return true; |
| })); |
| |
| this->service_operations.insertKey(constants_::to_string(constants::bstate), object::make_function([this]() { |
| this->setOperationMode(std::string {"BSTATE"}); |
| return true; |
| })); |
| } |
| |
| void ControlComponent::freeControlComponent(const std::string &senderId) |
| { |
| if (this->getOccupierID().compare(senderId) == 0) |
| { |
| this->setOccupierID(this->getLastOccupierID()); |
| this->setLastOccupierID(""); |
| if (this->getOccupierID().empty()) |
| this->setOccupationState(OccupationState::free); |
| else |
| this->setOccupationState(OccupationState::occupied); |
| } |
| } |
| |
| void ControlComponent::occupyControlComponent(const std::string &occupier) |
| { |
| if (this->getOccupationState() == OccupationState::free) |
| { |
| this->setOccupierID(occupier); |
| this->setOccupationState(OccupationState::occupied); |
| } |
| } |
| |
| void ControlComponent::priorityOccupation(const std::string &occupier) |
| { |
| if ((this->getOccupationState() == OccupationState::free) or (this->getOccupationState() == OccupationState::occupied)) |
| { |
| this->setLastOccupierID(this->getOccupierID()); |
| this->setOccupierID(occupier); |
| this->setOccupationState(OccupationState::priority); |
| } |
| } |
| |
| } |
| } |
| } |