reem_diffdrive_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 * 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/Param.hh> 00031 #include <gazebo/Controller.hh> 00032 #include <gazebo/Model.hh> 00033 00034 // ROS 00035 #include <ros/ros.h> 00036 #include <tf/transform_broadcaster.h> 00037 #include <tf/transform_listener.h> 00038 #include <geometry_msgs/Twist.h> 00039 #include <nav_msgs/Odometry.h> 00040 00041 // Custom Callback Queue 00042 #include <ros/callback_queue.h> 00043 #include <ros/advertise_options.h> 00044 00045 // Boost 00046 #include <boost/thread.hpp> 00047 #include <boost/bind.hpp> 00048 00049 namespace gazebo 00050 { 00051 class Joint; 00052 class Entity; 00053 00054 class reemDiffdrivePlugin : public Controller 00055 { 00056 00057 public: 00058 reemDiffdrivePlugin(Entity *parent); 00059 virtual ~reemDiffdrivePlugin(); 00060 00061 protected: 00062 virtual void LoadChild(XMLConfigNode *node); 00063 void SaveChild(std::string &prefix, std::ostream &stream); 00064 virtual void InitChild(); 00065 void ResetChild(); 00066 virtual void UpdateChild(); 00067 virtual void FiniChild(); 00068 00069 private: 00070 void write_position_data(); 00071 void publish_odometry(); 00072 void GetPositionCmd(); 00073 00074 libgazebo::PositionIface *pos_iface_; 00075 Model *parent_; 00076 ParamT<float> *wheelSepP; 00077 ParamT<float> *wheelDiamP; 00078 ParamT<float> *torqueP; 00079 float wheelSpeed[2]; 00080 00081 // Simulation time of the last update 00082 Time prevUpdateTime; 00083 00084 bool enableMotors; 00085 float odomPose[3]; 00086 float odomVel[3]; 00087 00088 Joint *joints[2]; 00089 PhysicsEngine *physicsEngine; 00090 ParamT<std::string> *leftJointNameP; 00091 ParamT<std::string> *rightJointNameP; 00092 00093 // ROS STUFF 00094 ros::NodeHandle* rosnode_; 00095 ros::Publisher pub_; 00096 ros::Subscriber sub_; 00097 tf::TransformBroadcaster *transform_broadcaster_; 00098 nav_msgs::Odometry odom_; 00099 std::string tf_prefix_; 00100 00101 boost::mutex lock; 00102 00103 ParamT<std::string> *robotNamespaceP; 00104 std::string robotNamespace; 00105 00106 ParamT<std::string> *topicNameP; 00107 std::string topicName; 00108 00109 // Custom Callback Queue 00110 ros::CallbackQueue queue_; 00111 boost::thread* callback_queue_thread_; 00112 void QueueThread(); 00113 00114 // DiffDrive stuff 00115 void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg); 00116 00117 float x_; 00118 float rot_; 00119 bool alive_; 00120 }; 00121 00122 } 00123 00124 #endif 00125