RemKhepServerROS.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 __REM_KHEP_SERVER_ROS_H_
#define __REM_KHEP_SERVER_ROS_H_

// MIP include
#include <IPCommModule.h>
#include <CommonOptions.h>
#include <remKhepComm.h>
#include <MotionModule.h>
#include <KorebotROS.h>

// ROS include
#include <ROSNode.h>
#include <sensor_msgs/LaserScan.h>
#ifdef ROS_ELECTRIC
 #include <gazebo/Joint.hh>
#endif

#ifdef ROS_FUERTE
 #include <physics/Joint.hh>
#endif

#include <ros/ros.h>

// Boost include
#include <boost/bind.hpp>

// MACROS
// this is 22500 but means 25000 :)
// #define SAMPLE_TIME_USEC 49500
#define SAMPLE_TIME_USEC 30000
#define WAIT_TIME_USEC 1000

// DEBUG FLAGS
#define KHS_COMM_DEBUG 0
#define KHS_DEBUG 0
#define KHS_EXECUTE_DEBUG 0

using namespace std;

// DEBUG DEFINE
#define KH3_DEBUG(debLevel,_staticLevel,message) \
{ \
  if(debLevel <= _staticLevel) { \
    stringstream KH3_DEBUG_ss; \
    KH3_DEBUG_ss << "\033[01;31m" << "[KH3DEBUG] " << "\033[0m" << message; \
    std::cout << KH3_DEBUG_ss.str() << std::endl; \
  } \
}

namespace MipResources{
enum{
 RIGHT, 
 LEFT,
 NUM_WHEELS
};

enum CommRateTypes {
 FIXED,
 VARIABLE,
 NUM_COMM_RATE_TYPES
};

struct laserScanStr{
  MipBaselib::Time _time;
 vector<Decimal> _ranges;
};

class KheperaServerROSOptions : public Options{
 public:
  IntOption*   urg04lxActive;
  IntOption*   korebotActive;
  StringOption* commRateType;
  
  // Name of the laser topic
  StringOption* laserTopicName;
  // Name of the topic where the khepera will publish data
  StringOption* kh3TopicName;
  // Name of the ROS node assigned to the robot
  StringOption* kh3NodeName;
  
  string getObjectName() const{
   return "KheperaServerROSOptions";
  }
  
  KheperaServerROSOptions();
};

class KheperaServerROSPars{
 private:
  // Name of the robot (base for topic)
  string      _robotName;
  // Actual level of debug
  int       _debugLevel;
  // Check if the laser is active or not
  int       _urg04lxActive;
  // Name of the topic where laser data are publishing (e.g. /base_scan/scan)
  string      _laserTopicName;
  // Name of the topic where one wants to publish and subscribe data (e.g. /msg)
  string     _topicName;
  // Name of the ROS node created by the resource ROSNode
  string     _nodeName;
  // Joints
  GAZEBO_JOINT_PTR _joints[2];
  // Wheel separation
  Decimal     _wheelSep;
  // Wheel diameter
  Decimal     _wheelDiam;
  // Integration method of odometry
  string     _intMethodOdo;
  // Communication type: it might be set to 'fixed' or 'variable'
  string     _commType;
  // Maximum torque to be applied to joints
  Decimal     _torqueMax;
  // Encoder resolution
  Decimal     _encRes;

 public:
  KheperaServerROSPars();
  
  KheperaServerROSPars(const string &robotName, const int &debugLevel, const int &urgAct, const string &ltn, const string &tn, const string& nn, const string &imo, GAZEBO_JOINT_PTR jtr, GAZEBO_JOINT_PTR jtl, const Decimal &ws, const Decimal &wd, const string &ct, const Decimal &tm, const Decimal &er);
  
  KheperaServerROSPars(const KheperaServerROSPars &A);
  
  KheperaServerROSPars operator= (KheperaServerROSPars A);
  
  string getRobotName() const;
  
  int getDebugLevel() const;
  
  int getUrg04lxActive() const;
  
  string getLaserTopicName() const;
  
  string getTopicName() const;
  
  string getNodeName() const;
  
  GAZEBO_JOINT_PTR getLeftWheel();
  
  GAZEBO_JOINT_PTR getRightWheel();
  
  Decimal getWheelSeparation() const;
  
  Decimal getWheelDiameter() const;
  
  string getIntMethodOdo() const;
  
  string getCommunicationType() const;
  
  Decimal getMaxTorque() const;
  
  Decimal getEncoderResolution() const;
};


class KheperaServerROS : public MIPObject/*, public gazebo::Controller*/{
 
 private:
  
  // In order to emulate the real laser, the first and last 15 degrees are erased
  static const int ticksToErase = 44;
  
  // Resource KorebotROS
  KorebotROS*        _korebot;
  
  KheperaServerROSOptions*  _options;
  
  bool            _newTState;
  bool            _newVelocity;
  Decimal          _driveToBeSet;
  Decimal          _turnrateToBeSet;
  MotionModuleTState     _motionModuleTStateToBeSet;
  MotionModuleTState     _lastMotionModuleTState;
  
  // Copy of the most important parameters
  string           _robotName;
  int            _debugLevel;
  int            _urg04lxActive;
  GAZEBO_JOINT_PTR     _leftJoint;
  GAZEBO_JOINT_PTR     _rightJoint;
  
  // Subscriber that take data from topic
  ros::Subscriber      _subscriberURG04LXmm;
  // The real package to send to ROS (this is the struct data that will be send to robot)
  laserScanStr        _laserScan;
  
  // ROSNode to manage all the ROS connections
  ROSNode*         _rosNode;
  // Topic specifier. It will be 0 since it is the first topic on the topic list in ROSNode
  int            topicSpecifier;
  
  // It will include:
  deque<string>       _incomingPackets;
  vector<string>       _outcomingPackets;
  
  char            _stateBuffer[70];
  int            _stateBufferCounter;
  int            _numSentState;
  SimTimer         _execCmdsTimer;
  
  // Laser message
  char           _laserBuffer[5048];
  int            _laserBufferCounter;
  int            _numSentScan;
  
  CommRateTypes       _commRateType;
  
  pthread_mutex_t      _laserMutex;
  
  string getObjectName() const {
   return "KheperaServerROS";
  }
  
  // Callback of the laser scan subscriber
  void laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg);
  
 public:
  
  KheperaServerROS(int argc, const char* argv[]);
  
  KheperaServerROS(KheperaServerROSPars * khPars);
  
  ~KheperaServerROS();
  
  bool searchConnection();
  
  void receiveCommands();
  
  int parseCommands();
  
  void executeCommands();
  
  ROSNode* getRosNode();
  
  KorebotROS* getKorebot();
  
  void getVelocityCommand(Decimal &xVel, Decimal &omega);
  
  void getTStateCommand(MotionModuleTState &state);
  
  void lockLaserMutex();
  
  void unlockLaserMutex();
};

} // End of namespace

#endif