RemKhepServerROSController.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_SRV_ROS_CTRL_H_
#define __REM_KHEP_SRV_ROS_CTRL_H_

// MIP headers
#include <CommonOptions.h>
#include <Option.h>
#include "RemKhepServerROS.h"

#ifdef ROS_ELECTRIC
 // ROS 
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 // ROS GAZEBO
 #include <gazebo/Simulator.hh>
 #include <gazebo/Global.hh>
 #include <gazebo/Controller.hh>
 #include <gazebo/ControllerFactory.hh>
 #include <gazebo/Model.hh>

 // Custom Callback Queue
 #include <ros/callback_queue.h>
 #include <ros/advertise_options.h>
#endif
// Boost
#include <boost/thread.hpp>

// Define macros
#define SAMPLE_TIME_USEC 30000
#define WAIT_TIME_USEC 1000
 
#define KHS_COMM_DEBUG 0
#define KHS_TIME_DEBUG 0
#define KHS_EXECUTE_DEBUG 0

namespace gazebo{
class Joint;
class Entity;

class remKhepSrvROSController : public gazebo::Controller{
 public:
  remKhepSrvROSController(Entity *parent);
  virtual ~remKhepSrvROSController();
  
 protected:
  virtual void LoadChild(XMLConfigNode *node);
  void SaveChild(std::string &prefix, std::ostream &stream);
  virtual void InitChild();
  void ResetChild();
  virtual void UpdateChild();
  virtual void FiniChild();
  
 private:
  // Methods
  void write_position_data();
  void publish_odometry();
  
  // Variables
  Model *parent_;
  ParamT<float>*    wheelSepP;
  ParamT<float>*    wheelDiamP;
  ParamT<float>*    torqueP;
  ParamT<bool>*     enableMotors;
  ParamT<int>*      debugLevel;
  ParamT<int>*     urg04lxActive;
  ParamT<string>*    intMethodOdo;
  ParamT<string>*    laserTopicName;
  ParamT<string>*    topicName;
  ParamT<string>*    nodeName;
  ParamT<string>*    commType;
  ParamT<Decimal>*   encodRes;
  float         wheelSpeed[2];

  // Name of the actual robot
//   string _nameRobot;
  KheperaServerROSPars* _khPars;
  
  // Simulation time of the last update
//   Time          prevUpdateTime;
  
  // Timer to avoid too often request of data
  Time         _execTimer;
  
  // Pointer to the remote khepera server
  KheperaServerROS*   _remKhepSrvROS;
  
  // Bool to check if the current iteration is the first one or not
  bool          _firstRun;
  // Boolean to check if an ack message is received (for the starting connection)
  bool          _connected;
  // Integer to select which command is given at this time step
  int          _actCmd;
  // Boolean to set if enables the motors
  bool         _enableMotors;
  // Integer to store actual debugLevel
  int          _debugLevel;
  
  Joint *joints[2];
  PhysicsEngine     *physicsEngine;
  ParamT<std::string>  *leftJointNameP;
  ParamT<std::string>  *rightJointNameP;
  
  // Diameter of the wheel
  Decimal wd;
  // Separation between wheels
  Decimal ws;
  // Distance performed by a wheel in the last step
  double d1;
  double d2;
  double dr;
  double da;
  // Time step from the last time update
  Time stepTime;
  
  // ROS STUFF
//   ros::NodeHandle* rosnode_;
  ros::Publisher  pub_;
  ros::Subscriber sub_;
//   nav_msgs::Odometry odom_;
  std::string tf_prefix_;
  
  boost::mutex lock;
  
  ParamT<std::string> *robotNamespaceP;
  std::string robotNamespace;
  
//   ParamT<std::string> *topicNameP;
//   std::string topicName;
  std::string topicNameOdom;
  
  // Custom Callback Queue
  ros::CallbackQueue queue_;
  boost::thread* callback_queue_thread_;
  tf::TransformBroadcaster br;

  void QueueThread();
  
  // DiffDrive stuff
  void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
  
  Decimal _xVel;
  Decimal _omega;
  bool alive_;
 };
 
}

#endif