asebaros.h

Go to the documentation of this file.

#ifndef __ASEBA_ROS_H
#define __ASEBA_ROS_H

#include <dashel/dashel.h>
#include <boost/thread.hpp>
#include <boost/thread/mutex.hpp>

#include <aseba/msg/msg.h>
#include <aseba/msg/descriptions-manager.h>

#include "ros/ros.h"
#include "std_msgs/String.h"

#include "asebaros/LoadScripts.h"
#include "asebaros/GetNodeList.h"
#include "asebaros/GetNodeId.h"
#include "asebaros/GetNodeName.h"
#include "asebaros/GetVariableList.h"
#include "asebaros/SetVariable.h"
#include "asebaros/GetVariable.h"
#include "asebaros/GetEventId.h"
#include "asebaros/GetEventName.h"

#include "asebaros/AsebaEvent.h"
#include "asebaros/AsebaAnonymousEvent.h"

#include <vector>


class AsebaROS;

class AsebaDashelHub: public Dashel::Hub
{
private:
        boost::thread* thread; 
        AsebaROS* asebaROS; 
        bool forward; 
        
public:
        AsebaDashelHub(AsebaROS* asebaROS, unsigned port, bool forward);
        
        void sendMessage(Aseba::Message *message, bool doLock, Dashel::Stream* sourceStream = 0);

        void operator()();
        void startThread();
        void stopThread();
        
protected:
        virtual void connectionCreated(Dashel::Stream *stream);
        virtual void incomingData(Dashel::Stream *stream);
        virtual void connectionClosed(Dashel::Stream *stream, bool abnormal);
};

using namespace asebaros;

typedef std::vector<ros::ServiceServer> ServiceServers;
typedef std::vector<ros::Publisher> Publishers;
typedef std::vector<ros::Subscriber> Subscribers;

class AsebaROS: public Aseba::DescriptionsManager
{
protected:
        typedef std::map<std::string, unsigned> NodesNamesMap;
        typedef std::map<std::string, Aseba::Compiler::VariablesMap> UserDefinedVariablesMap;
        class GetVariableQueryKey
        {
        public:
                GetVariableQueryKey(unsigned nodeId, unsigned pos) : nodeId(nodeId), pos(pos) { }
                bool operator<(const GetVariableQueryKey &that) const {
                        return (nodeId < that.nodeId && pos < that.pos);
                }
                unsigned nodeId;
                unsigned pos;
                
        };
        struct GetVariableQueryValue
        {
                typedef std::vector<sint16> DataVector;
                DataVector data;
                boost::condition_variable cond;
        };
        typedef std::map<GetVariableQueryKey, GetVariableQueryValue*> GetVariableQueryMap;
        
        ros::NodeHandle n; 
        ServiceServers s; 
        
        ros::Publisher anonPub; 
        ros::Subscriber anonSub; 
        Publishers pubs; 
        Subscribers subs; 

        AsebaDashelHub hub; 
        boost::mutex mutex; 
        
        Aseba::CommonDefinitions commonDefinitions; 
        NodesNamesMap nodesNames; 
        UserDefinedVariablesMap userDefinedVariablesMap; 
        GetVariableQueryMap getVariableQueries; 
        
protected:
        bool loadScript(LoadScripts::Request& req, LoadScripts::Response& res);
        
        bool getNodeList(GetNodeList::Request& req, GetNodeList::Response& res);
        bool getNodeId(GetNodeId::Request& req, GetNodeId::Response& res);
        bool getNodeName(GetNodeName::Request& req, GetNodeName::Response& res);
        
        bool getVariableList(GetVariableList::Request& req, GetVariableList::Response& res);
        bool setVariable(SetVariable::Request& req, SetVariable::Response& res);
        bool getVariable(GetVariable::Request& req, GetVariable::Response& res);
        
        bool getEventId(GetEventId::Request& req, GetEventId::Response& res);
        bool getEventName(GetEventName::Request& req, GetEventName::Response& res);
        
        // utility
        bool getNodePosFromNames(const std::string& nodeName, const std::string& variableName, unsigned& nodeId, unsigned& pos) const;
        void sendEventOnROS(const Aseba::UserMessage* asebaMessage);
        
        // callbacks
        void nodeDescriptionReceived(unsigned nodeId);
        void eventReceived(const AsebaAnonymousEventConstPtr& event);
        void knownEventReceived(const uint16 id, const AsebaEventConstPtr& event);
        
public:
        AsebaROS(unsigned port, bool forward);
        ~AsebaROS();
        
        void run();
        
        void processAsebaMessage(Aseba::Message *message);
        
        void connectTarget(const std::string& target) { hub.connect(target); }
};



#endif // __ASEBA_ROS_H