SimpleTrackedVehiclePlugin.hh
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2017 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 #ifndef GAZEBO_SIMPLETRACKEDVEHICLEPLUGIN_HH
19 #define GAZEBO_SIMPLETRACKEDVEHICLEPLUGIN_HH
20 
21 #include <string>
22 #include <unordered_map>
23 
24 #include <boost/algorithm/string.hpp>
25 
29 #include <gazebo/ode/contact.h>
30 
31 #include "gazebo/common/Plugin.hh"
32 #include "gazebo/physics/physics.hh"
34 
36 
37 
38 namespace gazebo {
39 
62 
63  class GZ_PLUGIN_VISIBLE SimpleTrackedVehiclePlugin :
65  {
66  public: SimpleTrackedVehiclePlugin() = default;
67 
68  public: virtual ~SimpleTrackedVehiclePlugin();
69 
73  public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;
74 
76  public: void Init() override;
77 
79  public: void Reset() override;
80 
85  protected: void SetTrackVelocityImpl(double _left, double _right) override;
86 
89  protected: void UpdateTrackSurface() override;
90 
92  protected: physics::LinkPtr body;
93 
95  protected: std::unordered_map<Tracks, physics::LinkPtr> tracks;
96 
98  protected: std::unordered_map<Tracks, double> trackVelocity;
99 
101  protected: void DriveTracks(const common::UpdateInfo &/*_unused*/);
102 
106  public: size_t GetNumTracks(Tracks side) const;
107 
112  protected: void SetGeomCategories();
113 
128  protected: ignition::math::Vector3d ComputeFrictionDirection(
129  double _linearSpeed, double _angularSpeed,
130  bool _drivingStraight, const ignition::math::Pose3d &_bodyPose,
131  const ignition::math::Vector3d &_bodyYAxisGlobal,
132  const ignition::math::Vector3d &_centerOfRotation,
133  const ignition::math::Vector3d &_contactWorldPosition,
134  const ignition::math::Vector3d &_contactNormal,
135  const ignition::math::Vector3d &_beltDirection) const;
136 
141  protected: double ComputeSurfaceMotion(double _beltSpeed,
142  const ignition::math::Vector3d &_beltDirection,
143  const ignition::math::Vector3d &_frictionDirection) const;
144 
145  private: transport::NodePtr node;
146 
147  private: event::ConnectionPtr beforePhysicsUpdateConnection;
148 
150  protected: unsigned int collideWithoutContactBitmask;
152  protected: static const unsigned int ROBOT_CATEGORY = 0x10000000;
154  protected: static const unsigned int BELT_CATEGORY = 0x20000000;
156  protected: static const unsigned int LEFT_CATEGORY = 0x40000000;
157 
158  private: physics::ContactManager *contactManager;
159 
162  class ContactIterator : std::iterator<std::input_iterator_tag, dContact>
163  {
165  private: pointer currentContact;
167  private: size_t jointIndex;
169  private: dBodyID body;
171  private: dGeomID geom1, geom2;
173  private: bool initialized;
174 
175  // Constructors.
176  public: ContactIterator();
177  public: explicit ContactIterator(bool _initialized);
178  public: ContactIterator(const ContactIterator &_rhs);
179  public: ContactIterator(dBodyID _body, dGeomID _geom1, dGeomID _geom2);
180 
182  public: static ContactIterator begin(dBodyID _body, dGeomID _geom1,
183  dGeomID _geom2);
184  public: static ContactIterator end();
185 
187  public: ContactIterator operator++();
188 
189  // Operators. It is required to implement them in iterators.
190  public: bool operator==(const ContactIterator &_rhs);
191  public: ContactIterator &operator=(const ContactIterator &_rhs);
192  public: ContactIterator operator++(int _unused);
193  public: reference operator*();
194  public: pointer operator->();
195  public: pointer getPointer();
196  public: bool operator!=(const ContactIterator &_rhs);
197  };
198  };
199 }
200 
201 
202 #endif
Forward declarations for transport.
A very fast, but also very accurate model of non-deformable tracks without grousers.
Definition: SimpleTrackedVehiclePlugin.hh:65
std::unordered_map< Tracks, physics::LinkPtr > tracks
The tracks controlled by this plugin.
Definition: SimpleTrackedVehiclePlugin.hh:95
double ComputeSurfaceMotion(double _beltSpeed, const ignition::math::Vector3d &_beltDirection, const ignition::math::Vector3d &_frictionDirection) const
Compute the velocity of the surface motion in all contact points.
void Init() override
Initialize the plugin.
void SetGeomCategories()
Set collide categories and bits of all geometries to the required values.
unsigned int collideWithoutContactBitmask
This bitmask will be set to the whole vehicle body.
Definition: SimpleTrackedVehiclePlugin.hh:150
size_t GetNumTracks(Tracks side) const
Return the number of tracks on the given side.
physics::LinkPtr body
Body of the robot.
Definition: SimpleTrackedVehiclePlugin.hh:92
void SetTrackVelocityImpl(double _left, double _right) override
Set new target velocity for the tracks.
void DriveTracks(const common::UpdateInfo &)
Compute and apply the forces that make the tracks move.
std::unordered_map< Tracks, double > trackVelocity
Desired velocities of the tracks.
Definition: SimpleTrackedVehiclePlugin.hh:98
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override
Called when the plugin is loaded.
ignition::math::Vector3d ComputeFrictionDirection(double _linearSpeed, double _angularSpeed, bool _drivingStraight, const ignition::math::Pose3d &_bodyPose, const ignition::math::Vector3d &_bodyYAxisGlobal, const ignition::math::Vector3d &_centerOfRotation, const ignition::math::Vector3d &_contactWorldPosition, const ignition::math::Vector3d &_contactNormal, const ignition::math::Vector3d &_beltDirection) const
Compute the direction of friction force in given contact point.
void UpdateTrackSurface() override
Update surface parameters of the tracks to correspond to the values set in this plugin.
void Reset() override
Reset the plugin.
An abstract gazebo model plugin for tracked vehicles.
Definition: TrackedVehiclePlugin.hh:80
Information for use in an update event.
Definition: UpdateInfo.hh:31
Aggregates all the contact information generated by the collision detection engine.
Definition: ContactManager.hh:73
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:110
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:94
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:57
Forward declarations for the common classes.
Definition: Animation.hh:27
Tracks
Enum for distinguishing between left and right tracks.
Definition: TrackedVehiclePlugin.hh:40