diffdrive_plugin.h
Go to the documentation of this file.
00001 /*
00002     Copyright (c) 2010, Daniel Hewlett, Antons Rebguns
00003     All rights reserved.
00004 
00005     Redistribution and use in source and binary forms, with or without
00006     modification, are permitted provided that the following conditions are met:
00007         * Redistributions of source code must retain the above copyright
00008         notice, this list of conditions and the following disclaimer.
00009         * Redistributions in binary form must reproduce the above copyright
00010         notice, this list of conditions and the following disclaimer in the
00011         documentation and/or other materials provided with the distribution.
00012         * Neither the name of the <organization> nor the
00013         names of its contributors may be used to endorse or promote products
00014         derived from this software without specific prior written permission.
00015 
00016     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00017     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00020     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 */
00027 
00028 #ifndef DIFFDRIVE_PLUGIN_HH
00029 #define DIFFDRIVE_PLUGIN_HH
00030 
00031 #include <map>
00032 
00033 #include <common/common.h>
00034 #include <physics/physics.h>
00035 
00036 // ROS
00037 #include <ros/ros.h>
00038 #include <tf/transform_broadcaster.h>
00039 #include <tf/transform_listener.h>
00040 #include <geometry_msgs/Twist.h>
00041 #include <nav_msgs/Odometry.h>
00042 
00043 // Custom Callback Queue
00044 #include <ros/callback_queue.h>
00045 #include <ros/advertise_options.h>
00046 
00047 // Boost
00048 #include <boost/thread.hpp>
00049 #include <boost/bind.hpp>
00050 
00051 namespace gazebo
00052 {
00053 class Joint;
00054 class Entity;
00055 
00056 class DiffDrivePlugin : public ModelPlugin
00057 {
00058   public: DiffDrivePlugin();
00059   public: ~DiffDrivePlugin();
00060   public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00061   protected: virtual void UpdateChild();
00062   protected: virtual void FiniChild();
00063 
00064 private:
00065   void write_position_data();
00066   void publish_odometry();
00067   void GetPositionCmd();
00068 
00069   physics::WorldPtr world;
00070   physics::ModelPtr parent;
00071   event::ConnectionPtr updateConnection;
00072 
00073   std::string leftJointName;
00074   std::string rightJointName;
00075 
00076   double wheelSeparation;
00077   double wheelDiameter;
00078   double torque;
00079   double wheelSpeed[2];
00080 
00081   double odomPose[3];
00082   double odomVel[3];
00083 
00084   physics::JointPtr joints[2];
00085   physics::PhysicsEnginePtr physicsEngine;
00086 
00087   // ROS STUFF
00088   ros::NodeHandle* rosnode_;
00089   ros::Publisher pub_;
00090   ros::Subscriber sub_;
00091   tf::TransformBroadcaster *transform_broadcaster_;
00092   nav_msgs::Odometry odom_;
00093   std::string tf_prefix_;
00094 
00095   boost::mutex lock;
00096 
00097   std::string robotNamespace;
00098   std::string topicName;
00099 
00100   // Custom Callback Queue
00101   ros::CallbackQueue queue_;
00102   boost::thread callback_queue_thread_;
00103   void QueueThread();
00104 
00105   // DiffDrive stuff
00106   void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00107 
00108   double x_;
00109   double rot_;
00110   bool alive_;
00111 };
00112 
00113 }
00114 
00115 #endif
00116 


erratic_gazebo_plugins
Author(s): Daniel Hewlett, Antons Rebguns
autogenerated on Sat Dec 28 2013 16:58:15