StateAggregator.hpp

Go to the documentation of this file.

#ifndef STATEAGGREGATOR_HPP
#define STATEAGGREGATOR_HPP
#include <vector>
#include <map>
#include <base/Time.hpp>
#include <base/commands/Joints.hpp>
#include <boost/function.hpp>

namespace joint_dispatcher
{

class StateAggregator
{
    class StateInfo
    {
    public:
        size_t stateId;
        size_t outputPosition;
        bool updated;
        base::Time updateTime;
    };
    
    size_t actuatorCnt;
    
    size_t updateCnt;
    
    base::commands::Joints status;
    std::vector<StateInfo *> stateIdToInfo;
    
    std::map<size_t, StateInfo> stateMap;

    base::Time statusInterval;
    
    boost::function<void (const base::commands::Joints &status)> writeStatusCallback;
    
    bool writeState;
protected:
    const base::commands::Joints &getStatus() const 
    {
        return status;
    };
    
public:
    
    const std::vector<size_t> getActuatorIds() const;
    
    StateAggregator(const std::vector< std::pair<size_t, std::string> >& stateMap, boost::function<void (const base::commands::Joints &status)> writeStatusCallback, base::Time statusInterval);
    ~StateAggregator() {};
    
    void setNewStatus(size_t stateId, const base::Time stateTime, const base::JointState& state);

    void checkWriteState();
    
    
    void reset();
};

class CommandDispatcher
{
    class InputMapping
    {
    public:
        int actuatorId;
        int inputId;
    };    
    
    std::vector<InputMapping> inputMap;
    
    boost::function<void (int32_t actuatorId, base::JointState::MODE mode, double value)> setCommandCallback;
    
public:
    const std::vector<int32_t> getActuatorIds() const;
    
    CommandDispatcher(std::vector< boost::int32_t > const & actuatorMap, boost::function<void (int32_t actuatorId, base::JointState::MODE mode, double value)> setCommandCallback);
    virtual ~CommandDispatcher() {};
    
    void processCommand(base::commands::Joints cmd);    
};

}

#endif // STATEAGGREGATOR_HPP