RemoteKhepera.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_KHEPERA_H_
#define __REMOTE_KHEPERA_H_

#include <socketUDP.h>
#include <CommonOptions.h>

#include "remKhepComm.h"
#include "URG04LX.h"
#include "MobileRob.h"
#include "RemoteRangeSens.h"
#include <SsUtils.h>
#include <deque>



#ifdef LINK_ROS
 #define CONNECTTOROS
#endif




// #include <mutex>

#ifdef CONNECTTOROS
  #include <ROSNode.h>
#endif

namespace MipResources{
 /* @{ */
 
extern "C" void RKWork(void* p);

extern "C" void RKClean(void* p);

class RemoteKheperaPar : public MobileRobPar{
 private:
 public:
  RemoteKheperaPar(){};
  ~RemoteKheperaPar();
};


class RemoteKheperaVar : public MobileRobVar{
  
 private:
  bool           _lastScan1IsLast;
  
  pthread_mutex_t     _tStateLogMutex;
  
  MotionModuleTStateLog  _tStateLog;
  
 public:

  Scan*          _lastScan1;
  Scan*          _lastScan2;
//   Scan _lastScan;
  Time           _lastScanTime;
  Timer          _scanTimer;
  int           _lastScanNum;
  int           _lastStateNumber;
  
  // Variables to check if a message of the same type is already in the list
  #ifdef CONNECTTOROS
   bool          _alreadyArrivedLaser;
//    bool          _alreadyArrivedTState;
  #endif
  
  bool setLastScan(Scan& newScan);
  
  bool getLastScan(Scan& scan, Time timeout);
  
  
  bool getOldScan(Scan& scan, Time timeout);
  
  
  RemoteKheperaVar();
  
  ~RemoteKheperaVar(){};
  
  pthread_mutex_t* ptStateLogMutex(){
   return &_tStateLogMutex;
  }
  
  void lockTStateLog(){
   pthread_mutex_lock(&_tStateLogMutex);
  }
  
  void unlockTStateLog(){
   pthread_mutex_unlock(&_tStateLogMutex);
  }
  
  MotionModuleTStateLog* ptrTStateLog(){
   return &_tStateLog;
  }
};

class RemoteKheperaOptions : public MobileRobOptions{
 public:
  StringOption* baseName;
  StringOption* topicName;
  StringOption* nodeName;
  RemoteKheperaOptions();
  
  string getObjectName() const {
   return "RemoteKheperaOptions";
  }
  
  OptionGroupsType getGroup();
};


class RemoteKhepera : public MobileRob{
 private:
  static const ResourcePlate _plate = MR_REM_KHP_RES;
  
  URG04LXPar _urg04lxPar;
  
  RemoteKheperaVar *_remoteVar; 
  // onboard sensors
  RemoteRangeSens  *_remoteRangeSens;   
  
  bool _correctLandingStates;
  Pose _stateToCorrect;
  Pose _correctedState;
  
  stringstream _ssWarning;
  stringstream _ssDebug;
  
  Timer _timer;
  int _receivedScan ;
  
  Thread _RRThread;
  SenderUDP*  _sender;
  ListenerUDP* _listener;
  int   _sendPort;
  int   _listPort;
  string _addr;
  bool  _unconnected;

  void _startConnection();
  
  void _startConnectionROS();
  
  void _closeConnection();
  
  void _parse(string &msg,deque<MotionModuleTState>* itListTState=NULL);
  
  void _sendString(string s);
  void _getRangeSensPar();
  
  EnhancedMutEx count_mutex;
  
  // ROS adding: Marco Cognetti
//   #if defined USE_ROS && defined FOUND_ROS_LIBS
  #ifdef CONNECTTOROS
   ROSNode*              _rosNode;
   int                 _topicSpecifier;
   RemoteKheperaOptions*        _options;
   bool                _firstOdomMsg;
   Decimal               _lasTimeTStateIn;
   Decimal               _tempTimeTStateIn;
   int                 _numOdoMsgAdded;
   list<MotionModuleTState>::iterator _itPTrTStateLog;
   Decimal               _lasTimeLaser;
  #endif
 public:
  RemoteKhepera(int argc, const char* argv[]);
  
  ~RemoteKhepera();

  ResourcePlate getPlate() const {
   return _plate;
  }
  
  Scan scan(){
   Scan s;
   return s;
  };
  
  /* Scan */
  void startScan(unsigned int num);
  
  void stopScan();
  
  /* Motion Module */
  UnicyclePar* unicyclePar();
  
  void velCmd(Decimal drive, Decimal turnrate);
  
  void getVel(Decimal& drive, Decimal& turnrate);
  
  Pose getPose(void);
  
  MotionModuleTState getMotionState();
  
  void setPose(Pose p);
  
  bool setMotionState(MotionModuleState state, bool setPastTimeStamp=false, Time pastTimeStamp=Time());
  
  bool storedScan(Scan& s,Time timeout);
  bool updatedScan(Scan& s);
  bool updatedScan(Scan& s, MotionModuleTState& state);
  
  bool updatedState(MotionModuleTState& state, Time &timeStamp);
  
  bool updatedState(MotionModuleTState& state);
  
  bool updatedLastState(MotionModuleTState& state, Time& timeStamp, const Decimal &delta);
  
  bool updatedLastState(MotionModuleTState& state, const Decimal &delta);
  
  void receive();
  
  void receiveUDP();
  
  void receiveROS();
  
  void _syncroTime();
  
  RangeSensPar* rangeSensPar();
};

 /* @} */
 
};// end namespace MipResources{

#endif