diffdrive_plugin_6w.h
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003  
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: ROS interface to a Position2d controller for a Differential drive.
00023  * Author: Daniel Hewlett (adapted from Nathan Koenig)
00024  */
00025 #ifndef DIFFDRIVE_PLUGIN_HH
00026 #define DIFFDRIVE_PLUGIN_HH
00027 
00028 #include <map>
00029 
00030 #include <gazebo/common/Plugin.hh>
00031 #include <gazebo/common/Time.hh>
00032 
00033 // ROS 
00034 #include <ros/ros.h>
00035 #include <tf/transform_broadcaster.h>
00036 #include <tf/transform_listener.h>
00037 #include <geometry_msgs/Twist.h>
00038 #include <nav_msgs/Odometry.h>
00039 
00040 // Custom Callback Queue
00041 #include <ros/callback_queue.h>
00042 #include <ros/advertise_options.h>
00043 
00044 // Boost
00045 #include <boost/thread.hpp>
00046 #include <boost/bind.hpp>
00047 
00048 namespace gazebo
00049 {
00050 
00051 class DiffDrivePlugin6W : public ModelPlugin
00052 {
00053 
00054 public:
00055   DiffDrivePlugin6W();
00056   virtual ~DiffDrivePlugin6W();
00057 
00058 protected:
00059   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00060   virtual void Reset();
00061   virtual void Update();
00062 
00063 private:
00064   void publish_odometry();
00065   void GetPositionCmd();
00066 
00067   physics::LinkPtr link;
00068   physics::WorldPtr world;
00069   physics::JointPtr joints[6];
00070 
00071   float wheelSep;
00072   float wheelDiam;
00073   float torque;
00074   float wheelSpeed[2];
00075 
00076   // Simulation time of the last update
00077   common::Time prevUpdateTime;
00078 
00079   bool enableMotors;
00080   float odomPose[3];
00081   float odomVel[3];
00082 
00083   // ROS STUFF
00084   ros::NodeHandle* rosnode_;
00085   ros::Publisher pub_;
00086   ros::Subscriber sub_;
00087   tf::TransformBroadcaster *transform_broadcaster_;
00088   nav_msgs::Odometry odom_;
00089   std::string tf_prefix_;
00090 
00091   boost::mutex lock;
00092 
00093   std::string namespace_;
00094   std::string topic_;
00095   std::string link_name_;
00096 
00097   // Custom Callback Queue
00098   ros::CallbackQueue queue_;
00099   boost::thread callback_queue_thread_;
00100   void QueueThread();
00101 
00102   // DiffDrive stuff
00103   void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00104 
00105   float x_;
00106   float rot_;
00107   bool alive_;
00108 
00109   // Pointer to the update event connection
00110   event::ConnectionPtr updateConnection;
00111 };
00112 
00113 }
00114 
00115 #endif
00116 


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Mon Jun 27 2016 04:58:09