PursuingMessages.h

Go to the documentation of this file.

// ----------------------------------------------------------------------------
//
// $Id$
//
// Copyright 2008, 2009, 2010, 2011, 2012  Antonio Franchi and Paolo Stegagno
//
// This file is part of MIP.
//
// MIP is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// MIP is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with MIP. If not, see <http://www.gnu.org/licenses/>.
//
// Contact info: antonio.franchi@tuebingen.mpg.de stegagno@diag.uniroma1.it
//
// ----------------------------------------------------------------------------

#ifndef __PURSUING_MESSAGES__
#define __PURSUING_MESSAGES__

#define PURSUING_MSG_ID "pursuing"
#define PURSUING_CENTRAL_ID 100

#include <Types.h>
#include <string>
#include <vector>

#include <Message.h>

#define MAX_SCAN_SIZE 1024
#define MAX_POSE_ARRAY_SIZE 10

using namespace std;
using namespace MipBaselib;

enum PursuingHeaders {
    // Central test to robot messages
    PUR_HEADER_GO_TO_POSE,
    PUR_HEADER_REQ_SCAN,
    PUR_HEADER_INITIALIZE,
 
    // Robot to central test messages
    PUR_HEADER_POSE_AND_SCAN,
    PUR_HEADER_POSE,
    PUR_HEADER_POSE_ARRAY,
    PUR_HEADER_PARAMS,
    
    PUR_HEADERS_NUM,
};

static const char* pursuingHeaderNames[PUR_HEADERS_NUM] = {
    // Central test to robot messages
    "go to pose",
    "request scan",
    "initialize",
 
    // Robot to central test messages
    "send pose and scan",
    "send pose",
    "send pose array",
    "send parameters",
};


class PursuingMsg : public MipBaselib::Message{
    protected:
        PursuingMsg(){
            string id(PURSUING_MSG_ID);
            setId(id);
        }
        
//      template <class Class> void constructor(const string& inMessage) {
//          inMessage.copy((char* ) this, sizeof(Class));
//      }
//      
//      template <class Class> void encoder(string& outMessage){
//          outMessage = string((char* ) this, sizeof(Class));
//      }
    
    public:
        int robotId;
        PursuingHeaders header;
        
        PursuingMsg(const string& inMessage){
            inMessage.copy((char* ) this, sizeof(PursuingMsg));
        }
};


    // Central test to robot messages

class PursuingMsgGoToPose : public PursuingMsg{
    public:
    
        double x;
        double y;
        double theta;
        bool bReportScan;

        PursuingMsgGoToPose(int robotId_, double x_,
                                    double y_,
                                    double theta_,
                                    bool bReportScan_){
            robotId = robotId_;
            header = PUR_HEADER_GO_TO_POSE;
            x = x_;
            y = y_;
            theta = theta_;
            bReportScan = bReportScan_;
        }
        
        PursuingMsgGoToPose(const string& inMessage){
            constructor<PursuingMsgGoToPose>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgGoToPose>(outMessage);
            //cout<< "Go to pose " << outMessage << endl;
        }
};

class PursuingMsgReqScan : public PursuingMsg{
    public:

        PursuingMsgReqScan(int robotId_){
            robotId = robotId_;
            header = PUR_HEADER_REQ_SCAN;
        }
        
        PursuingMsgReqScan(const string& inMessage){
            constructor<PursuingMsgReqScan>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgReqScan>(outMessage);
        }
};

class PursuingMsgInitialize : public PursuingMsg{
    public:

        PursuingMsgInitialize(int robotId_){
            robotId = robotId_;
            header = PUR_HEADER_INITIALIZE;
        }
        
        PursuingMsgInitialize(const string& inMessage){
            constructor<PursuingMsgInitialize>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgInitialize>(outMessage);
        }
};

// Robot to central test messages

class PursuingMsgPoseAndScan : public PursuingMsg{
    public:
    
        double x;
        double y;
        double theta;
        double scanLengthArray[MAX_SCAN_SIZE];
        double scanBearingArray[MAX_SCAN_SIZE];
        int scanSize;

        PursuingMsgPoseAndScan(int robotId_, double x_,
                                    double y_,
                                    double theta_, 
                                    Scan &scan){
            robotId = robotId_;
            header = PUR_HEADER_POSE_AND_SCAN;
            x = x_;
            y = y_;
            theta = theta_;
            while( !scan.askExclusiveAccess(Time(1,0)) ) { usleep(1000); };
            while( !scan.getSize(scanSize) ){ usleep(1000); };
            scanSize = min( MAX_SCAN_SIZE, scanSize );
            
            cout<< " sending scan size " << scanSize << endl;
            
            Ray ray;
            for( int i = 0; i < scanSize; i++ )
            {
                while( !scan.getRay( ray, i ) ) { usleep(1000); };
                
                scanLengthArray[i] = ray.reading();
                scanBearingArray[i] = ray.bearing().dCast2Pi();
                
                //if( i % 100 == 0 ) cout<< "  " << robotId << ", ray " << i << " " << ray.reading() << "," << ray.bearing().print() << endl;
            }
            scan.leaveExclusiveAccess();
        }
        
        PursuingMsgPoseAndScan(const string& inMessage){
            constructor<PursuingMsgPoseAndScan>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgPoseAndScan>(outMessage);
        }
};


class PursuingMsgPose : public PursuingMsg{
    public:
    
        double x;
        double y;
        double theta;

        PursuingMsgPose(int robotId_, double x_,
                                    double y_,
                                    double theta_){
            robotId = robotId_;
            header = PUR_HEADER_POSE;
            x = x_;
            y = y_;
            theta = theta_;
        }
        
        PursuingMsgPose(const string& inMessage){
            constructor<PursuingMsgPose>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgPose>(outMessage);
        }
};

class PursuingMsgPoseArray : public PursuingMsg{
    public:
    
        double x[MAX_POSE_ARRAY_SIZE];
        double y[MAX_POSE_ARRAY_SIZE];
        double theta[MAX_POSE_ARRAY_SIZE];

        PursuingMsgPoseArray(int robotId_, PoseFeatures poseFeatures){
            robotId = robotId_;
            header = PUR_HEADER_POSE_ARRAY;
            
            for( int i = 0; i < MAX_POSE_ARRAY_SIZE; i++ )
            {
                x[i] = -99;
                y[i] = -99;
                theta[i] = -99;
            }
            
            for( int i = 0; i < poseFeatures.size(); i++ )
            {
                Pose pose = poseFeatures[i].getObj();
                int id = poseFeatures[i].getAsso();
                
                if( id < MAX_POSE_ARRAY_SIZE && id >= 0 )
                {
                    x[id] = pose.pos().x();
                    y[id] = pose.pos().y();
                    theta[id] = pose.ori().dCast2Pi();
                }
            }
        }
        
        PursuingMsgPoseArray(const string& inMessage){
            constructor<PursuingMsgPoseArray>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgPoseArray>(outMessage);
        }
};

class PursuingMsgParams : public PursuingMsg{
    public:
    
        double sensorLinMaxRange;
        double sensorFieldOfView;
        int sensorCount;
        double sensorResolution;
        
        double robotRadius;
        
        double x;
        double y;
        double theta;

        PursuingMsgParams(int robotId_, double sensorLinMaxRange_, 
                                        double sensorFieldOfView_,
                                        int sensorCount_,
                                        double sensorResolution_,
                                        double robotRadius_,
                                        double x_,
                                        double y_,
                                        double theta_)
        {
            robotId = robotId_;
            header = PUR_HEADER_PARAMS;
            sensorLinMaxRange = sensorLinMaxRange_;
            sensorFieldOfView = sensorFieldOfView_;
            sensorCount = sensorCount_;
            sensorResolution = sensorResolution_;
            
            robotRadius = robotRadius_;
            
            x = x_;
            y = y_;
            theta = theta_;
        }
        
        PursuingMsgParams(const string& inMessage){
            constructor<PursuingMsgParams>(inMessage);
        }
    
        void encode(string& outMessage){
            encoder<PursuingMsgParams>(outMessage);
        }
};

    
#endif