RemKhepServerROSPluginFuerte.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_PLG_H_
#define __REM_KHEP_SRV_ROS_PLG_H_

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

// TODO: DECOMMENT THE FOLLOWING LINE
// #ifdef ROS_FUERTE
 // ROS 
 #include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <ros/ros.h>
 #include <tf/transform_broadcaster.h>
 // ROS GAZEBO
 
 #include "gazebo.hh"
 #include <sdf/interface/Param.hh>
 #include <physics/physics.h>
 #include <common/CommonTypes.hh>
 #include <common/common.h>

 // 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 remKhepSrvROSPlugin : public ModelPlugin{
 public:
  remKhepSrvROSPlugin();
  
  virtual ~remKhepSrvROSPlugin();
  
 protected:
  virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
  
  void SaveChild(std::string &prefix, std::ostream &stream);
  void InitChild();
  void ResetChild();
  void UpdateChild();
  void FiniChild();
  
 private:
  // Methods
  void write_position_data();
  void publish_odometry();
  
  // Parameters
  physics::ModelPtr   parent_;
  physics::WorldPtr   world;
  event::ConnectionPtr updateConnection;
  
  // Separation between wheels
  Decimal       wheelSep;
  // Diameter of the wheel
  Decimal       wheelDiam;
  Decimal       torque;
  // Update rate
  Decimal       update_rate;
  Decimal       update_period;
  common::Time    last_update_time;
  
  bool         enableMotors;
  int         debugLevel;
  int         urg04lxActive;
  string       intMethodOdo;
  string       laserTopicName;
  string       topicName;
  string       nodeName;
  string       commType;
  Decimal       encodRes;
  Decimal        wheelSpeed[2];
  string        leftJointName;
  string        rightJointName;
  string        robotNamespace;
  string        topicNameOdom;
  
  // 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
  common::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;
  
  GAZEBO_JOINT_PTR     joints[2];
  physics::PhysicsEngine *physicsEngine;
  
  
  // Distance performed by a wheel in the last step
  Decimal d1;
  Decimal d2;
  Decimal dr;
  Decimal 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;
  
  // 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