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
32#include "gazebo/physics/physics.hh"
34
36
37
38namespace gazebo {
39
62
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
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.
static const unsigned int ROBOT_CATEGORY
Category for the non-track parts of the robot.
Definition SimpleTrackedVehiclePlugin.hh:152
static const unsigned int LEFT_CATEGORY
Category for all items on the left side.
Definition SimpleTrackedVehiclePlugin.hh:156
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.
static const unsigned int BELT_CATEGORY
Category for tracks.
Definition SimpleTrackedVehiclePlugin.hh:154
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:109
boost::shared_ptr< Model > ModelPtr
Definition PhysicsTypes.hh:93
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