diffdrive_plugin_multi_wheel.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2010, Daniel Hewlett, Antons Rebguns
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  * * Redistributions of source code must retain the above copyright
8  * notice, this list of conditions and the following disclaimer.
9  * * Redistributions in binary form must reproduce the above copyright
10  * notice, this list of conditions and the following disclaimer in the
11  * documentation and/or other materials provided with the distribution.
12  * * Neither the name of the <organization> nor the
13  * names of its contributors may be used to endorse or promote products
14  * derived from this software without specific prior written permission.
15  *
16  * THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
17  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19  * DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
20  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26  *
27  **/
28 
29 /*
30  * \file gazebo_ros_diff_drive.h
31  *
32  * \brief A differential drive plugin for gazebo. Based on the diffdrive plugin
33  * developed for the erratic robot (see copyright notice above). The original
34  * plugin can be found in the ROS package gazebo_erratic_plugins.
35  *
36  * \author Piyush Khandelwal (piyushk@gmail.com)
37  *
38  * $ Id: 06/21/2013 11:23:40 AM piyushk $
39  */
40 
46 /*
47  Copyright (c) 2014, Stefan Kohlbrecher
48  All rights reserved.
49 
50  Redistribution and use in source and binary forms, with or without
51  modification, are permitted provided that the following conditions are met:
52  * Redistributions of source code must retain the above copyright
53  notice, this list of conditions and the following disclaimer.
54  * Redistributions in binary form must reproduce the above copyright
55  notice, this list of conditions and the following disclaimer in the
56  documentation and/or other materials provided with the distribution.
57  * Neither the name of the <organization> nor the
58  names of its contributors may be used to endorse or promote products
59  derived from this software without specific prior written permission.
60 
61  THIS SOFTWARE IS PROVIDED BY Antons Rebguns <email> ''AS IS'' AND ANY
62  EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
63  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
64  DISCLAIMED. IN NO EVENT SHALL Antons Rebguns <email> BE LIABLE FOR ANY
65  DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
66  (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
67  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
68  ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
69  (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
70  SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
71 
72 */
73 
74 #ifndef DIFFDRIVE_PLUGIN_HH
75 #define DIFFDRIVE_PLUGIN_HH
76 
77 #include <map>
78 
79 // Gazebo
80 #include <gazebo/common/common.hh>
81 #include <gazebo/physics/physics.hh>
82 
83 // ROS
84 #include <ros/ros.h>
86 #include <tf/transform_listener.h>
87 #include <geometry_msgs/Twist.h>
88 #include <nav_msgs/Odometry.h>
89 #include <nav_msgs/OccupancyGrid.h>
90 
91 // Custom Callback Queue
92 #include <ros/callback_queue.h>
93 #include <ros/advertise_options.h>
94 
95 // Boost
96 #include <boost/thread.hpp>
97 #include <boost/bind.hpp>
98 
99 namespace gazebo {
100 
101  class Joint;
102  class Entity;
103 
104  class GazeboRosDiffDriveMultiWheel : public ModelPlugin {
105 
106  public:
109  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
110 
111  protected:
112  virtual void UpdateChild();
113  virtual void FiniChild();
114 
115  private:
116  void publishOdometry(double step_time);
117  void getWheelVelocities();
118 
119  physics::WorldPtr world;
120  physics::ModelPtr parent;
121  event::ConnectionPtr update_connection_;
122 
123  std::vector<std::string> joint_names_[2];
124 
127  double torque;
128  double wheel_speed_[2];
129 
130  std::vector<physics::JointPtr> joints_[2];
131 
132  // ROS STUFF
137  nav_msgs::Odometry odom_;
138  std::string tf_prefix_;
139 
140  boost::mutex lock;
141 
142  std::string robot_namespace_;
143  std::string command_topic_;
144  std::string odometry_topic_;
145  std::string odometry_frame_;
146  std::string robot_base_frame_;
147 
148  // Custom Callback Queue
150  boost::thread callback_queue_thread_;
151  void QueueThread();
152 
153  // DiffDrive stuff
154  void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
155 
156  double x_;
157  double rot_;
158  bool alive_;
159 
160  // Update Rate
161  double update_rate_;
163  common::Time last_update_time_;
164 
167 
168  };
169 
170 }
171 
172 #endif
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
std::vector< physics::JointPtr > joints_[2]


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Fri Feb 5 2021 03:48:30