gazebo_ros_tricycle_drive.h
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the copyright holder nor the names of its 
00018  *     contributors may be used to endorse or promote products derived from 
00019  *     this software without specific prior written permission.
00020  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  *  POSSIBILITY OF SUCH DAMAGE.
00032  *********************************************************************/
00033 
00041 #ifndef TRICYCLE_DRIVE_PLUGIN_HH
00042 #define TRICYCLE_DRIVE_PLUGIN_HH
00043 
00044 #include <map>
00045 
00046 // Gazebo
00047 #include <gazebo_plugins/gazebo_ros_utils.h>
00048 
00049 // ROS
00050 #include <ros/ros.h>
00051 #include <tf/transform_broadcaster.h>
00052 #include <tf/transform_listener.h>
00053 #include <geometry_msgs/Twist.h>
00054 #include <geometry_msgs/Pose2D.h>
00055 #include <nav_msgs/Odometry.h>
00056 #include <nav_msgs/OccupancyGrid.h>
00057 #include <sensor_msgs/JointState.h>
00058 
00059 // Custom Callback Queue
00060 #include <ros/callback_queue.h>
00061 #include <ros/advertise_options.h>
00062 
00063 // Boost
00064 #include <boost/thread.hpp>
00065 #include <boost/bind.hpp>
00066 
00067 namespace gazebo {
00068 
00069 class Joint;
00070 class Entity;
00071 
00072 
00073 class GazeboRosTricycleDrive : public ModelPlugin {
00074 
00075     class TricycleDriveCmd {
00076     public:
00077         TricycleDriveCmd():speed(0), angle(0) {};
00078         double speed;
00079         double angle;
00080     };
00081 
00082     enum OdomSource
00083     {
00084         ENCODER = 0,
00085         WORLD = 1,
00086     };
00087 public:
00088     GazeboRosTricycleDrive();
00089     ~GazeboRosTricycleDrive();
00090     void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00091 
00092 protected:
00093     virtual void UpdateChild();
00094     virtual void FiniChild();
00095 
00096 private:
00097     GazeboRosPtr gazebo_ros_;
00098     physics::ModelPtr parent;
00099     void publishOdometry(double step_time);
00100     void publishWheelTF(); 
00101     void publishWheelJointState();
00102     void motorController(double target_speed, double target_angle, double dt);
00103 
00104     event::ConnectionPtr update_connection_;
00105 
00106     physics::JointPtr joint_steering_;
00107     physics::JointPtr joint_wheel_actuated_;
00108     physics::JointPtr joint_wheel_encoder_left_;
00109     physics::JointPtr joint_wheel_encoder_right_;
00110 
00111     double diameter_encoder_wheel_;
00112     double diameter_actuated_wheel_;
00113     double wheel_acceleration_;
00114     double wheel_deceleration_;
00115     double wheel_speed_tolerance_;
00116     double steering_angle_tolerance_;
00117     double steering_speed_;
00118     double separation_encoder_wheel_;
00119 
00120     OdomSource odom_source_;
00121     double wheel_torque_;
00122 
00123     std::string robot_namespace_;
00124     std::string command_topic_;
00125     std::string odometry_topic_;
00126     std::string odometry_frame_;
00127     std::string robot_base_frame_;
00128 
00129     // ROS STUFF
00130     ros::Publisher odometry_publisher_;
00131     ros::Subscriber cmd_vel_subscriber_;
00132     boost::shared_ptr<tf::TransformBroadcaster> transform_broadcaster_;
00133     sensor_msgs::JointState joint_state_;
00134     ros::Publisher joint_state_publisher_;
00135     nav_msgs::Odometry odom_;
00136 
00137     boost::mutex lock;
00138 
00139 
00140     // Custom Callback Queue
00141     ros::CallbackQueue queue_;
00142     boost::thread callback_queue_thread_;
00143     void QueueThread();
00144 
00145     // DiffDrive stuff
00146     void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
00147 
00149     void UpdateOdometryEncoder();
00150 
00151     TricycleDriveCmd cmd_;
00152     bool alive_;
00153     geometry_msgs::Pose2D pose_encoder_;
00154     common::Time last_odom_update_;
00155 
00156     // Update Rate
00157     double update_rate_;
00158     double update_period_;
00159     common::Time last_actuator_update_;
00160 
00161     // Flags
00162     bool publishWheelTF_;
00163     bool publishWheelJointState_;
00164 
00165 };
00166 
00167 }
00168 
00169 #endif
00170 


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