RemoteRobTask.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 __REMOTE_ROB_TASK_H_
#define __REMOTE_ROB_TASK_H_

#include <Task.h>
#include <Resource.h>
#include <MobileRob.h>
#include <Time.h>

#include <CommonOptions.h>
#include <socketUDP.h>
#include <SsUtils.h>
#include <Thread.h>
#include <Trajectory.h>
#include <SharedMem.h>

using namespace MipResources;

namespace MipTasks{
 
 /* @{ */
 
 
 enum RemoteRobotCommPacketsTypes{
 RRB_SCAN_UP,
 RRB_SCAN_DOWN,
 RRB_SCAN_GET_ONE,
 RRB_SCAN_GET_N,
 RRB_IR_UP,
 RRB_IR_DOWN,
 RRB_IR_GET_ONE,
 RRB_IR_GET_N,
 RRB_GP_UP,
 RRB_GP_DOWN,
 RRB_GP_GET_ONE,
 RRB_GP_GET_N,
 RRB_VG_UP,
 RRB_VG_DOWN,
 RRB_VG_GET_ONE,
 RRB_VG_GET_N,
 RRB_COMM_PACKETS_TYPES_NUM
 };
 static const char* RemoteRobotCommPackets[RRB_COMM_PACKETS_TYPES_NUM] = {
 "scu",
 "scd",
 "scg",
 "scn",
 "iru",
 "ird",
 "irg",
 "irn",
 "gpu",
 "gpd",
 "gpg",
 "gpn",
 "gvu",
 "gvd",
 "gvg",
 "gvn"
 };
 
 
 class RemoteRobTaskOptions : public Options {
  public:
   IntOption* scanSendingRate;
   IntOption* raysSendingRate;
   IntOption* sendBearing;
   
   RemoteRobTaskOptions();

   string getObjectName() const {
    return "RemoteRobTaskOptions";
   }
 };
 
 
 class RemoteRobSharedMem : public SharedMem{
  public:
   void initSharedMem(){
   }
 };
 
 
 extern "C" void scanSenderWork(void* t);
 
 extern "C" void scanSenderClean(void* p);
 
 extern "C" void poseVeloSenderWork(void* t);
 
 extern "C" void poseVeloSenderClean(void* p);
 
 
 class RemoteRobTask : public Task{
  protected:
   Thread _scanSenderThread;
   Thread _poseVeloSenderThread;
  private:
   
   RemoteRobTaskOptions options;
   
   static const TaskPlate _plate = RRB_TASK;
   static const long int  _maxDurSec  = 0,     _maxDurUsec = 100000;
   static const long int  _maxSamplPeriodSec = 0, _maxSamplPeriodUsec = 100000;
   static const long int  _minSamplPeriodSec = 0, _minSamplPeriodUsec = 1000;
   
   ScanTypes rangeSensorType;
   
   MobileRob* _mobileRob;  
   RemoteRobSharedMem* _sharedMem;
   Position   robPosition; 
   Pose       robPose;     
   
   bool connected;
   
   int myId;
   
   int lastTakenScan;
   
   Time lastTimeStamp;
   int lastTotalScans;
   
   ListenerUDP*  listener;
   SenderUDP*    sender;
   
   vector<string> incomingPackets;
   vector<string> incomingPacketsSenders;
   vector<string> outcomingPackets;
   
   int sendPort;
   string senderAddr;
   int listPort;
   
   bool _sendRangeSensParFlag;
   
   bool sendRawScan;
   int _sendState;
   int _sendScan;
   int _sendIR;
   
   int _sendPoseGoal;
   int _sendVeloGoal;
   
  public:
   RemoteRobTask(ResourcePointers resources,int argc, const char* argv[]);
   
   ~RemoteRobTask(){
    delete listener;
    delete sender;
   }
   
   TaskPlate getPlate() const {
    return _plate;
   }
   Time getMaxDuration(){
    return Time(_maxDurSec,_maxDurUsec);
   }
   Time getMaxSamplPeriod(){
    return Time(_maxSamplPeriodSec,_maxSamplPeriodUsec);
   }
   Time getMinSamplPeriod(){
    return Time(_minSamplPeriodSec,_minSamplPeriodUsec);
   }
   TaskOutputs run();
   
   void doScanSenderWork();
   
   void doPoseVeloSenderWork();
  
  private:
   
   // Network functions
   bool searchConnection();
   void unconnect(){
    connected = false;
    delete sender;
   }
   void receiveCommands();
   void executeCommands();
   
   
   // Pose send functions
   void upState(){ _sendState = -1; }
   void downState(){ _sendState = 0; }
   void sendOneState(){ _sendState = 1; }
   void sendNState(stringstream &s);
   
   
   // Goal Pose send functions
   void upPoseGoal(){ _sendPoseGoal = -1; }
   void downPoseGoal(){ _sendPoseGoal = 0; }
   void sendOnePoseGoal(){ _sendPoseGoal = 1; }
   void sendNPoseGoal(stringstream &s);
   
   // Velocity Goal send functions
   void upVeloGoal(){ _sendVeloGoal = -1; }
   void downVeloGoal(){ _sendVeloGoal = 0; }
   void sendOneVeloGoal(){ _sendVeloGoal = 1; }
   void sendNVeloGoal(stringstream &s);
   
   
   // Scan send functions
   void upScan(){ _sendScan = -1; }
   void downScan(){ _sendScan = 0; }
   void sendOneScan(){ _sendScan = 1; }
   void sendNScan(stringstream &s);
   void _sendRangeSensPar(){ _sendRangeSensParFlag = true; }
   
   
   // IR send functions
   void upIR(){ _sendIR = -1; }
   void downIR(){ _sendIR = 0; }
   void sendOneIR(){ _sendIR = 1; }
   void sendNIR(stringstream &s);
   
   
   // Set functions
   void setStateNoTime(stringstream &s);
   void setState(stringstream &s);
   void setVel(stringstream &s);
   void setPoseGoal(stringstream &s);
   void setVeloGoal(stringstream &s);
   
   
   // low level send functions
   void sendState();
   void sendPoseGoal();
   void sendVeloGoal();
   void sendScan();
   void sendRangeSensPar();
   void sendRangeSensmmPar();
   void sendIR();
   
   
   
   void updateScan(){
    Scan scan;
    _mobileRob->updatedScan(scan);
   }
   
   void startScan(){
    _mobileRob->startScan(0);
   }
   
   void getNScan(stringstream &s){
    int num;
    s >> num;
    _mobileRob->startScan(num);
   }
   
   void stopScan(){
    _mobileRob->stopScan();
   }
 };
 
 /* @} */
 
};// end namespace MipTasks


#endif