gazebo_ros_diff_drive.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 
00041 #ifndef DIFFDRIVE_PLUGIN_HH
00042 #define DIFFDRIVE_PLUGIN_HH
00043 
00044 #include <map>
00045 
00046 // Gazebo
00047 #include <gazebo/common/common.hh>
00048 #include <gazebo/physics/physics.hh>
00049 #include <gazebo_plugins/gazebo_ros_utils.h>
00050 
00051 // ROS
00052 #include <ros/ros.h>
00053 #include <tf/transform_broadcaster.h>
00054 #include <tf/transform_listener.h>
00055 #include <geometry_msgs/Twist.h>
00056 #include <geometry_msgs/Pose2D.h>
00057 #include <nav_msgs/Odometry.h>
00058 #include <sensor_msgs/JointState.h>
00059 
00060 // Custom Callback Queue
00061 #include <ros/callback_queue.h>
00062 #include <ros/advertise_options.h>
00063 
00064 // Boost
00065 #include <boost/thread.hpp>
00066 #include <boost/bind.hpp>
00067 
00068 namespace gazebo {
00069 
00070   class Joint;
00071   class Entity;
00072 
00073   class GazeboRosDiffDrive : public ModelPlugin {
00074 
00075     enum OdomSource
00076     {
00077         ENCODER = 0,
00078         WORLD = 1,
00079     };
00080     public:
00081       GazeboRosDiffDrive();
00082       ~GazeboRosDiffDrive();
00083       void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00084       void Reset();
00085 
00086     protected:
00087       virtual void UpdateChild();
00088       virtual void FiniChild();
00089 
00090     private:
00091       void publishOdometry(double step_time);
00092       void getWheelVelocities();
00093       void publishWheelTF(); 
00094       void publishWheelJointState();
00095       void UpdateOdometryEncoder();
00096 
00097 
00098       GazeboRosPtr gazebo_ros_;
00099       physics::ModelPtr parent;
00100       event::ConnectionPtr update_connection_;
00101 
00102       double wheel_separation_;
00103       double wheel_diameter_;
00104       double wheel_torque;
00105       double wheel_speed_[2];
00106           double wheel_accel;
00107       double wheel_speed_instr_[2];
00108 
00109       std::vector<physics::JointPtr> joints_;
00110 
00111       // ROS STUFF
00112       ros::Publisher odometry_publisher_;
00113       ros::Subscriber cmd_vel_subscriber_;
00114       boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
00115       sensor_msgs::JointState joint_state_;
00116       ros::Publisher joint_state_publisher_;      
00117       nav_msgs::Odometry odom_;
00118       std::string tf_prefix_;
00119 
00120       boost::mutex lock;
00121 
00122       std::string robot_namespace_;
00123       std::string command_topic_;
00124       std::string odometry_topic_;
00125       std::string odometry_frame_;
00126       std::string robot_base_frame_;
00127       bool publish_tf_;
00128       bool legacy_mode_;
00129       // Custom Callback Queue
00130       ros::CallbackQueue queue_;
00131       boost::thread callback_queue_thread_;
00132       void QueueThread();
00133 
00134       // DiffDrive stuff
00135       void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00136 
00137       double x_;
00138       double rot_;
00139       bool alive_;
00140 
00141       // Update Rate
00142       double update_rate_;
00143       double update_period_;
00144       common::Time last_update_time_;
00145       
00146       OdomSource odom_source_;
00147       geometry_msgs::Pose2D pose_encoder_;
00148       common::Time last_odom_update_;
00149       
00150     // Flags
00151     bool publishWheelTF_;
00152     bool publishWheelJointState_;
00153 
00154   };
00155 
00156 }
00157 
00158 #endif
00159 


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:22