gazebo_ros_tricycle_drive.h
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2013, Markus Bader <markus.bader@tuwien.ac.at>
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
27  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *********************************************************************/
33 
41 #ifndef TRICYCLE_DRIVE_PLUGIN_HH
42 #define TRICYCLE_DRIVE_PLUGIN_HH
43 
44 #include <map>
45 
46 // Gazebo
48 
49 // ROS
50 #include <ros/ros.h>
52 #include <tf/transform_listener.h>
53 #include <geometry_msgs/Twist.h>
54 #include <geometry_msgs/Pose2D.h>
55 #include <nav_msgs/Odometry.h>
56 #include <nav_msgs/OccupancyGrid.h>
57 #include <sensor_msgs/JointState.h>
58 
59 // Custom Callback Queue
60 #include <ros/callback_queue.h>
61 #include <ros/advertise_options.h>
62 
63 // Boost
64 #include <boost/thread.hpp>
65 #include <boost/bind.hpp>
66 
67 namespace gazebo {
68 
69 class Joint;
70 class Entity;
71 
72 
73 class GazeboRosTricycleDrive : public ModelPlugin {
74 
76  public:
78  double speed;
79  double angle;
80  };
81 
83  {
84  ENCODER = 0,
85  WORLD = 1,
86  };
87 public:
90  void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
91 
92 protected:
93  virtual void UpdateChild();
94  virtual void FiniChild();
95 
96 private:
98  physics::ModelPtr parent;
99  void publishOdometry(double step_time);
100  void publishWheelTF();
101  void publishWheelJointState();
102  void motorController(double target_speed, double target_angle, double dt);
103 
104  event::ConnectionPtr update_connection_;
105 
106  physics::JointPtr joint_steering_;
107  physics::JointPtr joint_wheel_actuated_;
108  physics::JointPtr joint_wheel_encoder_left_;
109  physics::JointPtr joint_wheel_encoder_right_;
110 
119 
122 
123  std::string robot_namespace_;
124  std::string command_topic_;
125  std::string odometry_topic_;
126  std::string odometry_frame_;
127  std::string robot_base_frame_;
128 
129  // ROS STUFF
133  sensor_msgs::JointState joint_state_;
135  nav_msgs::Odometry odom_;
136 
137  boost::mutex lock;
138 
139 
140  // Custom Callback Queue
142  boost::thread callback_queue_thread_;
143  void QueueThread();
144 
145  // DiffDrive stuff
146  void cmdVelCallback(const geometry_msgs::Twist::ConstPtr& cmd_msg);
147 
149  void UpdateOdometryEncoder();
150 
152  bool alive_;
153  geometry_msgs::Pose2D pose_encoder_;
154  common::Time last_odom_update_;
155 
156  // Update Rate
157  double update_rate_;
159  common::Time last_actuator_update_;
160 
161  // Flags
164 
165 };
166 
167 }
168 
169 #endif
170 
void UpdateOdometryEncoder()
updates the relative robot pose based on the wheel encoders
void publishWheelJointState()
publishes the wheel tf&#39;s
boost::shared_ptr< tf::TransformBroadcaster > transform_broadcaster_
void cmdVelCallback(const geometry_msgs::Twist::ConstPtr &cmd_msg)
void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
void motorController(double target_speed, double target_angle, double dt)


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52