diffdrive_plugin_multi_wheel.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 
00029 /*
00030  * \file  gazebo_ros_diff_drive.h
00031  *
00032  * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin 
00033  * developed for the erratic robot (see copyright notice above). The original
00034  * plugin can be found in the ROS package gazebo_erratic_plugins.
00035  *
00036  * \author  Piyush Khandelwal (piyushk@gmail.com)
00037  *
00038  * $ Id: 06/21/2013 11:23:40 AM piyushk $
00039  */
00040 
00046 /*
00047     Copyright (c) 2014, Stefan Kohlbrecher
00048     All rights reserved.
00049 
00050     Redistribution and use in source and binary forms, with or without
00051     modification, are permitted provided that the following conditions are met:
00052         * Redistributions of source code must retain the above copyright
00053         notice, this list of conditions and the following disclaimer.
00054         * Redistributions in binary form must reproduce the above copyright
00055         notice, this list of conditions and the following disclaimer in the
00056         documentation and/or other materials provided with the distribution.
00057         * Neither the name of the <organization> nor the
00058         names of its contributors may be used to endorse or promote products
00059         derived from this software without specific prior written permission.
00060 
00061     THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
00062     EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00063     WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00064     DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
00065     DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00066     (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00067     LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00068     ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00069     (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00070     SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00071 
00072 */
00073 
00074 #ifndef DIFFDRIVE_PLUGIN_HH
00075 #define DIFFDRIVE_PLUGIN_HH
00076 
00077 #include <map>
00078 
00079 // Gazebo
00080 #include <gazebo/common/common.hh>
00081 #include <gazebo/physics/physics.hh>
00082 
00083 // ROS
00084 #include <ros/ros.h>
00085 #include <tf/transform_broadcaster.h>
00086 #include <tf/transform_listener.h>
00087 #include <geometry_msgs/Twist.h>
00088 #include <nav_msgs/Odometry.h>
00089 #include <nav_msgs/OccupancyGrid.h>
00090 
00091 // Custom Callback Queue
00092 #include <ros/callback_queue.h>
00093 #include <ros/advertise_options.h>
00094 
00095 // Boost
00096 #include <boost/thread.hpp>
00097 #include <boost/bind.hpp>
00098 
00099 namespace gazebo {
00100 
00101   class Joint;
00102   class Entity;
00103 
00104   class GazeboRosDiffDriveMultiWheel : public ModelPlugin {
00105 
00106     public:
00107       GazeboRosDiffDriveMultiWheel();
00108       ~GazeboRosDiffDriveMultiWheel();
00109       void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00110 
00111     protected:
00112       virtual void UpdateChild();
00113       virtual void FiniChild();
00114 
00115     private:
00116       void publishOdometry(double step_time);
00117       void getWheelVelocities();
00118 
00119       physics::WorldPtr world;
00120       physics::ModelPtr parent;
00121       event::ConnectionPtr update_connection_;
00122 
00123       std::vector<std::string> joint_names_[2];
00124 
00125       double wheel_separation_;
00126       double wheel_diameter_;
00127       double torque;
00128       double wheel_speed_[2];
00129 
00130       std::vector<physics::JointPtr> joints_[2];
00131 
00132       // ROS STUFF
00133       ros::NodeHandle* rosnode_;
00134       ros::Publisher odometry_publisher_;
00135       ros::Subscriber cmd_vel_subscriber_;
00136       tf::TransformBroadcaster *transform_broadcaster_;
00137       nav_msgs::Odometry odom_;
00138       std::string tf_prefix_;
00139 
00140       boost::mutex lock;
00141 
00142       std::string robot_namespace_;
00143       std::string command_topic_;
00144       std::string odometry_topic_;
00145       std::string odometry_frame_;
00146       std::string robot_base_frame_;
00147 
00148       // Custom Callback Queue
00149       ros::CallbackQueue queue_;
00150       boost::thread callback_queue_thread_;
00151       void QueueThread();
00152 
00153       // DiffDrive stuff
00154       void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00155 
00156       double x_;
00157       double rot_;
00158       bool alive_;
00159 
00160       // Update Rate
00161       double update_rate_;
00162       double update_period_;
00163       common::Time last_update_time_;
00164 
00165       bool publish_odometry_tf_;
00166       bool publish_odometry_msg_;
00167 
00168   };
00169 
00170 }
00171 
00172 #endif


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