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 
00032 // ROS 
00033 #include <ros/ros.h>
00034 #include <tf/transform_broadcaster.h>
00035 #include <tf/transform_listener.h>
00036 #include <geometry_msgs/Twist.h>
00037 #include <nav_msgs/Odometry.h>
00038 
00039 // Custom Callback Queue
00040 #include <ros/callback_queue.h>
00041 #include <ros/advertise_options.h>
00042 
00043 // Boost
00044 #include <boost/thread.hpp>
00045 #include <boost/bind.hpp>
00046 
00047 namespace gazebo
00048 {
00049 
00050 class DiffDrivePlugin6W : public ModelPlugin
00051 {
00052 
00053 public:
00054   DiffDrivePlugin6W();
00055   virtual ~DiffDrivePlugin6W();
00056 
00057 protected:
00058   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00059   virtual void Reset();
00060   virtual void Update();
00061 
00062 private:
00063   void publish_odometry();
00064   void GetPositionCmd();
00065 
00066   physics::LinkPtr link;
00067   physics::WorldPtr world;
00068   physics::JointPtr joints[6];
00069 
00070   float wheelSep;
00071   float wheelDiam;
00072   float torque;
00073   float wheelSpeed[2];
00074 
00075   // Simulation time of the last update
00076   common::Time prevUpdateTime;
00077 
00078   bool enableMotors;
00079   float odomPose[3];
00080   float odomVel[3];
00081 
00082   // ROS STUFF
00083   ros::NodeHandle* rosnode_;
00084   ros::Publisher pub_;
00085   ros::Subscriber sub_;
00086   tf::TransformBroadcaster *transform_broadcaster_;
00087   nav_msgs::Odometry odom_;
00088   std::string tf_prefix_;
00089 
00090   boost::mutex lock;
00091 
00092   std::string robotNamespace;
00093   std::string topicName;
00094   std::string linkName;
00095 
00096   // Custom Callback Queue
00097   ros::CallbackQueue queue_;
00098   boost::thread callback_queue_thread_;
00099   void QueueThread();
00100 
00101   // DiffDrive stuff
00102   void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00103 
00104   float x_;
00105   float rot_;
00106   bool alive_;
00107 
00108   // Pointer to the update event connection
00109   event::ConnectionPtr updateConnection;
00110 };
00111 
00112 }
00113 
00114 #endif
00115 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30