##

SimpleTrackedVehiclePlugin.hh
Go to the documentation of this file.
/*
* Copyright (C) 2017 Open Source Robotics Foundation
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
*/
#ifndef GAZEBO_SIMPLETRACKEDVEHICLEPLUGIN_HH
#define GAZEBO_SIMPLETRACKEDVEHICLEPLUGIN_HH
#include <string>
#include <unordered_map>
#include <boost/algorithm/string.hpp>
#include <gazebo/physics/ode/ode_inc.h>
#include <gazebo/physics/ode/ODELink.hh>
#include <gazebo/physics/ode/ODECollision.hh>
#include <gazebo/ode/contact.h>
#include "gazebo/common/Plugin.hh"
#include "gazebo/physics/physics.hh"
#include "gazebo/transport/TransportTypes.hh"
#include "plugins/TrackedVehiclePlugin.hh"
namespace gazebo {
class SimpleTrackedVehiclePlugin :
public TrackedVehiclePlugin
{
public: SimpleTrackedVehiclePlugin() = default;
public: virtual ~SimpleTrackedVehiclePlugin();
public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override;
public: void Init() override;
public: void Reset() override;
protected: void SetTrackVelocityImpl(double _left, double _right) override;
protected: void UpdateTrackSurface() override;
protected: physics::LinkPtr body;
protected: std::unordered_map<Tracks, physics::LinkPtr> tracks;
protected: std::unordered_map<Tracks, double> trackVelocity;
protected: void DriveTracks(const common::UpdateInfo &/*_unused*/);
public: size_t GetNumTracks(Tracks side) const;
protected: void SetGeomCategories();
protected: 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;
protected: double ComputeSurfaceMotion(double _beltSpeed,
const ignition::math::Vector3d &_beltDirection,
const ignition::math::Vector3d &_frictionDirection) const;
private: transport::NodePtr node;
private: event::ConnectionPtr beforePhysicsUpdateConnection;
protected: unsigned int collideWithoutContactBitmask;
protected: static const unsigned int ROBOT_CATEGORY = 0x10000000;
protected: static const unsigned int BELT_CATEGORY = 0x20000000;
protected: static const unsigned int LEFT_CATEGORY = 0x40000000;
private: physics::ContactManager *contactManager;
class ContactIterator : std::iterator<std::input_iterator_tag, dContact>
{
private: pointer currentContact;
private: size_t jointIndex;
private: dBodyID body;
private: dGeomID geom1, geom2;
private: bool initialized;
// Constructors.
public: ContactIterator();
public: explicit ContactIterator(bool _initialized);
public: ContactIterator(const ContactIterator &_rhs);
public: ContactIterator(dBodyID _body, dGeomID _geom1, dGeomID _geom2);
public: static ContactIterator begin(dBodyID _body, dGeomID _geom1,
dGeomID _geom2);
public: static ContactIterator end();
public: ContactIterator operator++();
// Operators. It is required to implement them in iterators.
public: bool operator==(const ContactIterator &_rhs);
public: ContactIterator &operator=(const ContactIterator &_rhs);
public: ContactIterator operator++(int _unused);
public: reference operator*();
public: pointer operator->();
public: pointer getPointer();
public: bool operator!=(const ContactIterator &_rhs);
};
};
}
#endif
gazebo::Tracks
Tracks
Enum for distinguishing between left and right tracks.
Definition: TrackedVehiclePlugin.hh:40
gazebo::SimpleTrackedVehiclePlugin
A very fast, but also very accurate model of non-deformable tracks without grousers.
Definition: SimpleTrackedVehiclePlugin.hh:63
gazebo
Forward declarations for the common classes.
Definition: Animation.hh:26
gazebo::physics::ModelPtr
boost::shared_ptr< Model > ModelPtr
Definition: PhysicsTypes.hh:93
gazebo::TrackedVehiclePlugin
An abstract gazebo model plugin for tracked vehicles.
Definition: TrackedVehiclePlugin.hh:79
gazebo::SimpleTrackedVehiclePlugin::BELT_CATEGORY
static const unsigned int BELT_CATEGORY
Category for tracks.
Definition: SimpleTrackedVehiclePlugin.hh:154
TransportTypes.hh
Forward declarations for transport.
gazebo::SimpleTrackedVehiclePlugin::body
physics::LinkPtr body
Body of the robot.
Definition: SimpleTrackedVehiclePlugin.hh:92
gazebo::SimpleTrackedVehiclePlugin::tracks
std::unordered_map< Tracks, physics::LinkPtr > tracks
The tracks controlled by this plugin.
Definition: SimpleTrackedVehiclePlugin.hh:95
TrackedVehiclePlugin.hh
gazebo::transport::NodePtr
boost::shared_ptr< Node > NodePtr
Definition: TransportTypes.hh:57
gazebo::SimpleTrackedVehiclePlugin::SetTrackVelocityImpl
void SetTrackVelocityImpl(double _left, double _right) override
Set new target velocity for the tracks.
ODECollision.hh
gazebo::SimpleTrackedVehiclePlugin::SimpleTrackedVehiclePlugin
SimpleTrackedVehiclePlugin()=default
gazebo::SimpleTrackedVehiclePlugin::ComputeSurfaceMotion
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.
gazebo::SimpleTrackedVehiclePlugin::Init
void Init() override
Initialize the plugin.
gazebo::SimpleTrackedVehiclePlugin::ROBOT_CATEGORY
static const unsigned int ROBOT_CATEGORY
Category for the non-track parts of the robot.
Definition: SimpleTrackedVehiclePlugin.hh:152
gazebo::SimpleTrackedVehiclePlugin::Reset
void Reset() override
Reset the plugin.
gazebo::event::ConnectionPtr
boost::shared_ptr< Connection > ConnectionPtr
Definition: CommonTypes.hh:134
gazebo::SimpleTrackedVehiclePlugin::GetNumTracks
size_t GetNumTracks(Tracks side) const
Return the number of tracks on the given side.
gazebo::SimpleTrackedVehiclePlugin::~SimpleTrackedVehiclePlugin
virtual ~SimpleTrackedVehiclePlugin()
ODELink.hh
gazebo::physics::ContactManager
Aggregates all the contact information generated by the collision detection engine.
Definition: ContactManager.hh:72
gazebo::physics::LinkPtr
boost::shared_ptr< Link > LinkPtr
Definition: PhysicsTypes.hh:109
gazebo::SimpleTrackedVehiclePlugin::trackVelocity
std::unordered_map< Tracks, double > trackVelocity
Desired velocities of the tracks.
Definition: SimpleTrackedVehiclePlugin.hh:98
gazebo::SimpleTrackedVehiclePlugin::SetGeomCategories
void SetGeomCategories()
Set collide categories and bits of all geometries to the required values.
gazebo::SimpleTrackedVehiclePlugin::LEFT_CATEGORY
static const unsigned int LEFT_CATEGORY
Category for all items on the left side.
Definition: SimpleTrackedVehiclePlugin.hh:156
gazebo::SimpleTrackedVehiclePlugin::collideWithoutContactBitmask
unsigned int collideWithoutContactBitmask
This bitmask will be set to the whole vehicle body.
Definition: SimpleTrackedVehiclePlugin.hh:150
Plugin.hh
gazebo::SimpleTrackedVehiclePlugin::UpdateTrackSurface
void UpdateTrackSurface() override
Update surface parameters of the tracks to correspond to the values set in this plugin.
gazebo::SimpleTrackedVehiclePlugin::Load
void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) override
Called when the plugin is loaded.
gazebo::SimpleTrackedVehiclePlugin::ComputeFrictionDirection
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.
gazebo::common::UpdateInfo
Information for use in an update event.
Definition: UpdateInfo.hh:30
ode_inc.h
gazebo::SimpleTrackedVehiclePlugin::DriveTracks
void DriveTracks(const common::UpdateInfo &)
Compute and apply the forces that make the tracks move.