SCITOSManager.h

Go to the documentation of this file.

/*
 * Copyright (C) 2012 MetraLabs GmbH (MLAB), GERMANY.
 * All rights reserved.
 * Contact: info@MetraLabs.com
 *
 * Commercial Usage:
 *   Licensees holding valid commercial licenses may use this file in
 *   accordance with the commercial license agreement provided with the
 *   software or, alternatively, in accordance with the terms contained in
 *   a written agreement between you and MetraLabs.
 *
 * GNU General Public License Usage:
 *   Alternatively, this file may be used under the terms of the GNU
 *   General Public License version 3.0 as published by the Free Software
 *   Foundation and appearing in the file LICENSE.GPL3 included in the
 *   packaging of this file.  Please review the following information to
 *   ensure the GNU General Public License version 3.0 requirements will be
 *   met: http://www.gnu.org/copyleft/gpl.html.
 *   Alternatively you may (at your option) use any later version of the GNU
 *   General Public License if such license has been publicly approved by
 *   MetraLabs (or its successors, if any).
 *
 * IN NO EVENT SHALL "MLAB" BE LIABLE TO ANY PARTY FOR DIRECT, INDIRECT,
 * SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE
 * OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF "MLABS" HAS BEEN
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * "MLAB" SPECIFICALLY DISCLAIMS ANY WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE. THE SOFTWARE PROVIDED HEREUNDER IS ON AN
 * "AS IS" BASIS, AND "MLAB" HAS NO OBLIGATION TO PROVIDE MAINTENANCE,
 * SUPPORT, UPDATES, ENHANCEMENTS OR MODIFICATIONS.
 */

#ifndef _MIRA_SCITOSMANAGER_H_
#define _MIRA_SCITOSMANAGER_H_

#include <utils/ToString.h>
#include <fw/Unit.h>

#include <robot/RobotModelProvider.h>

#include <can/CANChannelInterface.h>
#include <can/CANOpenSDOClient.h>

#include <SCITOSDefines.h>
#include <SCITOSModule.h>

namespace mira { namespace robot {


class MLAB_SCITOS_EXPORT SCITOSManager : public MicroUnit
{
    MIRA_OBJECT(SCITOSManager)
public:

    struct ErrorDescription
    {
        ErrorDescription() {}
        ErrorDescription(const std::string& iText) :
            category("HWError"), text(iText) {}
        ErrorDescription(const std::string& iCategory, const std::string& iText) :
            category(iCategory), text(iText) {}

        std::string category;
        std::string text;
    };

protected:

    static std::string toHexString(uint32 id)
    {
        std::ostringstream ss;
        ss << std::hex << std::showbase << id;
        return ss.str();
    }

    static uint32 fromPrefixedString(const std::string& s)
    {
        std::istringstream ss(s);
        uint32 i;
        ss >> std::setbase(0) >> i;
        return i;
    }

public:

    struct ModuleID
    {
        template<typename Reflector>
        void reflect(Reflector& r)
        {
            r.property("Node",
                       getter<std::string>([&](){return SCITOSManager::toHexString(node);}),
                       setter<std::string>([&](const std::string& s){node = fromPrefixedString(s);}),
                       "Node number (default = 0 = wildcard)", serialization::IgnoreMissing());

            r.property("VendorID",
                       getter<std::string>([&](){return SCITOSManager::toHexString(vendorID);}),
                       setter<std::string>([&](const std::string& s){vendorID = fromPrefixedString(s);}),
                       "VendorID (default = 0 = wildcard)", serialization::IgnoreMissing());

            r.property("ProductID",
                       getter<std::string>([&](){return SCITOSManager::toHexString(productID);}),
                       setter<std::string>([&](const std::string& s){productID = fromPrefixedString(s);}),
                       "ProductID (default = 0 = wildcard)", serialization::IgnoreMissing());
        }

        bool match(uint8 n, uint32 vID, uint32 pID) const
        {
            return (((node == 0) || (node == n)) &&
                    ((vendorID == 0) || (vendorID == vID)) &&
                    ((productID == 0) || (productID == pID)));
        }

        uint8 node = 0;
        uint32 vendorID = 0;
        uint32 productID = 0;
    };

    SCITOSManager() :
        mInitializationFinished(false) {}

    template<typename Reflector>
    void reflect(Reflector& r)
    {
        r.property("DeadModuleTimeout", mRemoveNodeTimeout,
                   "Timeout for removing dead modules", Duration::seconds(10));
        r.roproperty("IgnoreModules", mIgnoreModules,
                     "List of CAN modules to be ignored.\n"
                     "Each entry consists of Node+VendorID+ProductID. "
                     "A module is ignored if it matches ALL specified elements, default=0=wildcard.");
        auto it = mModules.begin();
        for(; it != mModules.end(); ++it)
            r.property(it->second->getName().c_str(), it->second, "Module");
    }

    void reflect(XMLDeserializer& r);

    mira::can::CANChannelInterfacePtr getCANInterface()
    {
        return mCANInterface;
    }

    mira::can::CANOpenSDOClientPtr getCANSDOClient()
    {
        return mSDOClient;
    }

    mira::can::CANOpenNMTMasterPtr getNMTMaster()
    {
        return mNMTMaster;
    }

    bool hasHWTime() const
    {
        return mHasHWTimeSync;
    }

    uint32 getHWTime(Time time = Time::now())
    {
        Duration diff = time - mLastHWTime;
        return mLastHWTimeValue + (uint32)diff.totalMilliseconds()/10;
    }

    void addErrorDescription(uint32 errorNr, const ErrorDescription& ed)
    {
        boost::mutex::scoped_lock lock(mErrorMutex);
        mErrorDescriptions[errorNr] = ed;
    }

    ErrorDescription getErrorDescription(uint32 errorNr)
    {
        boost::mutex::scoped_lock lock(mErrorMutex);
        auto errorDesc = mErrorDescriptions.find(errorNr);
        // if this type of error is known set correct text and category
        if (errorDesc != mErrorDescriptions.end())
            return errorDesc->second;
        std::string category("Unknown error");
        std::string text = MakeString() << "Error ID " << std::hex << errorNr;
        return ErrorDescription(category, text);
    }

    Time correctPCTime(Time pcTime, uint32 hwTime)
    {
        // obtain the difference of the given hardware time and the hardware
        // time that corresponds with the given PC time
        uint32 diff = hwTime-getHWTime(pcTime);
        int64 signedDiff = *reinterpret_cast<int32*>(&diff);
        // return the PC time corrected by the hardware time difference
        return pcTime + Duration::milliseconds(signedDiff*10);
    }

    Time correctPCTime(Time pcTime, uint16 hwTime)
    {
        // obtain the difference of the given hardware time and the hardware
        // time that corresponds with the given PC time
        uint16 diff = hwTime-getHWTime(pcTime);
        int32 signedDiff = *reinterpret_cast<int16*>(&diff);
        // return the PC time corrected by the hardware time difference
        return pcTime + Duration::milliseconds(signedDiff*10);
    }

    boost::shared_ptr<robot::RobotModel> getRobotModel()
    {
        return mRobotModelProvider->getRobotModel();
    }

    void updateProperties();

protected:
    virtual void initialize();
    virtual void finalize();

    virtual void resume();
    virtual void pause();

private:
    void onHeartbeat(uint8 node, mira::can::CANOpenNodeStatus status);

    void onPDO(const mira::can::CANMessage& message, const Time& time);

private:

    boost::mutex mErrorMutex;
    boost::mutex mPropertyMutex;
    mira::can::CANChannelInterfacePtr mCANInterface;
    mira::can::CANOpenSDOClientPtr mSDOClient;
    mira::can::CANOpenNMTMasterPtr mNMTMaster;
    std::map<uint8, SCITOSModulePtr> mModules;
    std::map<uint8, std::pair<mira::can::CANOpenNodeInfo, Time>> mNodeInfos;
    Duration mRemoveNodeTimeout;
    std::vector<ModuleID> mIgnoreModules;  // to be ignored (configured)
    std::set<uint8> mIgnoredNodes;         // already ignored (runtime)
    std::string mRobotType;
    uint32 mLastHWTimeValue;
    Time mLastHWTime;
    bool mHasHWTimeSync;
    bool mInitializationFinished;
    XMLDom mState;
    std::map<uint32, ErrorDescription> mErrorDescriptions;
    boost::shared_ptr<robot::RobotModelProvider> mRobotModelProvider;
};


}}

#endif