gazebo_ros_skid_steer_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_skid_steer_drive.h
00031  *
00032  * \brief A skid steering drive plugin. Inspired by gazebo_ros_diff_drive and SkidSteerDrivePlugin
00033  *
00034  * \author  Zdenek Materna (imaterna@fit.vutbr.cz)
00035  *
00036  * $ Id: 06/25/2013 11:23:40 AM materna $
00037  */
00038 
00039 #ifndef GAZEBO_ROS_SKID_STEER_DRIVE_H_
00040 #define GAZEBO_ROS_SKID_STEER_DRIVE_H_
00041 
00042 #include <map>
00043 
00044 #include <gazebo/common/common.hh>
00045 #include <gazebo/physics/physics.hh>
00046 
00047 // ROS
00048 #include <ros/ros.h>
00049 #include <tf/transform_broadcaster.h>
00050 #include <tf/transform_listener.h>
00051 #include <geometry_msgs/Twist.h>
00052 #include <nav_msgs/Odometry.h>
00053 #include <nav_msgs/OccupancyGrid.h>
00054 
00055 // Custom Callback Queue
00056 #include <ros/callback_queue.h>
00057 #include <ros/advertise_options.h>
00058 
00059 // Boost
00060 #include <boost/thread.hpp>
00061 #include <boost/bind.hpp>
00062 
00063 namespace gazebo {
00064 
00065   class Joint;
00066   class Entity;
00067 
00068   class GazeboRosSkidSteerDrive : public ModelPlugin {
00069 
00070     public:
00071           GazeboRosSkidSteerDrive();
00072       ~GazeboRosSkidSteerDrive();
00073       void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00074 
00075     protected:
00076       virtual void UpdateChild();
00077       virtual void FiniChild();
00078 
00079     private:
00080       void publishOdometry(double step_time);
00081       void getWheelVelocities();
00082 
00083       physics::WorldPtr world;
00084       physics::ModelPtr parent;
00085       event::ConnectionPtr update_connection_;
00086 
00087       std::string left_front_joint_name_;
00088       std::string right_front_joint_name_;
00089       std::string left_rear_joint_name_;
00090       std::string right_rear_joint_name_;
00091 
00092       double wheel_separation_;
00093       double wheel_diameter_;
00094       double torque;
00095       double wheel_speed_[4];
00096 
00097       physics::JointPtr joints[4];
00098 
00099       // ROS STUFF
00100       ros::NodeHandle* rosnode_;
00101       ros::Publisher odometry_publisher_;
00102       ros::Subscriber cmd_vel_subscriber_;
00103       tf::TransformBroadcaster *transform_broadcaster_;
00104       nav_msgs::Odometry odom_;
00105       std::string tf_prefix_;
00106       bool broadcast_tf_;
00107 
00108       boost::mutex lock;
00109 
00110       std::string robot_namespace_;
00111       std::string command_topic_;
00112       std::string odometry_topic_;
00113       std::string odometry_frame_;
00114       std::string robot_base_frame_;
00115 
00116       // Custom Callback Queue
00117       ros::CallbackQueue queue_;
00118       boost::thread callback_queue_thread_;
00119       void QueueThread();
00120 
00121       // DiffDrive stuff
00122       void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00123 
00124       double x_;
00125       double rot_;
00126       bool alive_;
00127 
00128       // Update Rate
00129       double update_rate_;
00130       double update_period_;
00131       common::Time last_update_time_;
00132 
00133   };
00134 
00135 }
00136 
00137 
00138 #endif /* GAZEBO_ROS_SKID_STEER_DRIVE_H_ */


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25