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