CoordinatorBase.cpp

Go to the documentation of this file.

#include "CoordinatorBase.h"
 
// #include <VirtualRobot/Nodes/RobotNode.h>
// #include <VirtualRobot/RobotNodeSet.h>
//
// #include <simox/control/environment/CollisionRobot.h>
// #include <simox/control/impl/simox/robot/Robot.h>
//
#include <ArmarXCore/core/logging/Logging.h>
 
// #include <ArmarXCore/util/CPPUtility/trace.h>
 
namespace armarx::control::njoint_controller::core
{
 
    CoordinatorBase::CoordinatorBase()
    {
        ARMARX_INFO << "coordinator constructed";
    }
 
    CoordinatorBase::~CoordinatorBase()
    {
        ARMARX_IMPORTANT << "destruction of coordinator";
    }
 
    //    void
    //    CoordinatorBase::useCoordinator(const std::string& type,
    //                                    const ::armarx::aron::data::dto::DictPtr& dto,
    //                                    const Ice::Current& /*unused*/)
    //    {
    //        ARMARX_INFO << "Adding coordinator of type " << type;
    //        /// TODO think about other types of coordinators
    //        if (!coordinator)
    //        {
    //            /// TODO add check if the current controller has all robotNodeSets
    //            ///  that the coordinator requires. currently, use check below.
    //            coordinator = std::make_shared<common::coordination::SyncCoordination>(dto);
    //        }
    //        else
    //        {
    //            coordinator->updateConfig(dto);
    //        }
 
    //        for (const auto& nodeset : coordinator->getNodesetList())
    //        {
    //            if (limb.find(nodeset) == limb.end())
    //            {
    //                coordinatorEnabled.store(false);
    //                ARMARX_WARNING << "The requested nodeset by coordinator " << nodeset
    //                               << " does not exist in the current controller, disable coordinator";
    //                return;
    //            }
    //        }
    //        coordinatorEnabled.store(true);
    //        // std::map<std::string, Eigen::Matrix4f> initPose;
    //        // for (const auto& nodeset: coord->getNodesetList())
    //        // {
    //        //     auto buff = limb.at(nodeset)->bufferConfigRtToUser.getUpToDateReadBuffer();
    //        //     initPose[nodeset] = buff.desiredPose;
    //        //     ARMARX_INFO << "using node set " << nodeset << " with pose " << buff.desiredPose;
    //        // }
    //        // coord->reset(initPose);
    //    }
 
    //    void
    //    CoordinatorBase::disableCoordinator(const Ice::Current&)
    //    {
    //        for (const auto& nodeset : coordinator->getNodesetList())
    //        {
    //            auto& arm = limb.at(nodeset);
    //            auto& cfgRT = arm->bufferConfigRtToUser.getUpToDateReadBuffer();
    //            arm->bufferConfigUserToNonRt.getWriteBuffer().desiredPose = cfgRT.desiredPose;
    //            arm->bufferConfigUserToNonRt.commitWrite();
    //            usleep(2000);
    //        }
    //        coordinatorEnabled.store(false);
    //        ARMARX_INFO << "Coordinator is disabled!!";
    //    }
 
} // namespace armarx::control::njoint_controller::core