servo_plugin.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 #ifndef SERVO_PLUGIN_H
00023 #define SERVO_PLUGIN_H
00024 
00025 #include <gazebo/common/Plugin.hh>
00026 #include <gazebo/common/Time.hh>
00027 #include <gazebo/math/Quaternion.hh>
00028 
00029 // ROS 
00030 #include <ros/ros.h>
00031 #include <tf/transform_broadcaster.h>
00032 #include <tf/transform_listener.h>
00033 #include <geometry_msgs/QuaternionStamped.h>
00034 #include <sensor_msgs/JointState.h>
00035 
00036 // Boost
00037 #include <boost/thread.hpp>
00038 #include <boost/bind.hpp>
00039 
00040 namespace gazebo
00041 {
00042 
00043 class ServoPlugin : public ModelPlugin
00044 {
00045 
00046 public:
00047   ServoPlugin();
00048   virtual ~ServoPlugin();
00049 
00050 protected:
00051   virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf);
00052   virtual void Init();
00053   virtual void Reset();
00054   virtual void Update();
00055 
00056 private:
00057   void CalculateVelocities();
00058   void publish_joint_states();
00059 
00060 private:
00062   physics::WorldPtr world;
00063 
00064   // Simulation time of the last update
00065   common::Time prevUpdateTime;
00066 
00067   bool enableMotors;
00068 
00069   struct Servo {
00070     std::string name;
00071     math::Vector3 axis;
00072     physics::JointPtr joint;
00073     float velocity;
00074     Servo() : velocity() {}
00075   } servo[3];
00076 
00077   unsigned int countOfServos;
00078   unsigned int orderOfAxes[3];
00079   unsigned int rotationConv;
00080   sensor_msgs::JointState joint_state;
00081 
00082   // parameters
00083   std::string robotNamespace;
00084   std::string topicName;
00085   std::string jointStateName;
00086   common::Time controlPeriod;
00087 
00088   float proportionalControllerGain;
00089   float derivativeControllerGain;
00090   double maximumVelocity;
00091   float maximumTorque;
00092 
00093   // ROS STUFF
00094   ros::NodeHandle* rosnode_;
00095   ros::Publisher jointStatePub_;
00096   ros::Subscriber sub_;
00097   tf::TransformListener* transform_listener_;
00098 
00099   // Custom Callback Queue
00100   ros::CallbackQueue queue_;
00101 //  boost::thread* callback_queue_thread_;
00102 //  void QueueThread();
00103 
00104   // DiffDrive stuff
00105   void cmdCallback(const geometry_msgs::QuaternionStamped::ConstPtr& cmd_msg);
00106 
00107   boost::mutex mutex;
00108   geometry_msgs::QuaternionStamped::ConstPtr current_cmd;
00109   math::Quaternion rotation_;
00110 
00111   // Pointer to the update event connection
00112   event::ConnectionPtr updateConnection;
00113 };
00114 
00115 }
00116 
00117 #endif


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Wed Aug 26 2015 11:44:35