Link.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2012-2016 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.
 *
*/
/* Desc: Link class
 * Author: Nate Koenig
 */

#ifndef _LINK_HH_
#define _LINK_HH_

#ifdef _WIN32
  // Ensure that Winsock2.h is included before Windows.h, which can get
  // pulled in by anybody (e.g., Boost).
  #include <Winsock2.h>
#endif

#include <map>
#include <vector>
#include <string>

#include "gazebo/msgs/msgs.hh"
#include "gazebo/transport/TransportTypes.hh"

#include "gazebo/util/UtilTypes.hh"
#include "gazebo/common/Event.hh"
#include "gazebo/common/CommonTypes.hh"

#include "gazebo/physics/LinkState.hh"
#include "gazebo/physics/Entity.hh"
#include "gazebo/physics/Inertial.hh"
#include "gazebo/physics/Joint.hh"
#include "gazebo/util/system.hh"

namespace gazebo
{
  namespace util
  {
    class OpenALSource;
    class OpenALSink;
  }

  namespace physics
  {
    class Model;
    class Collision;
    class Battery;


    class GZ_PHYSICS_VISIBLE Link : public Entity
    {
      public: explicit Link(EntityPtr _parent);

      public: virtual ~Link();

      public: virtual void Load(sdf::ElementPtr _sdf);

      public: virtual void Init();

      public: void Fini();

      public: void Reset();
      using Entity::Reset;

      public: void ResetPhysicsStates();

      public: virtual void UpdateParameters(sdf::ElementPtr _sdf);

      public: void Update(const common::UpdateInfo &_info);
      using Base::Update;

      public: void SetScale(const math::Vector3 &_scale);

      public: virtual void SetEnabled(bool _enable) const = 0;

      public: virtual bool GetEnabled() const = 0;

      public: virtual bool SetSelected(bool _set);

      public: virtual void SetGravityMode(bool _mode) = 0;

      public: virtual bool GetGravityMode() const = 0;

      public: virtual void SetWindMode(const bool _mode);

      public: virtual bool WindMode() const;

      public: virtual void SetSelfCollide(bool _collide) = 0;

      public: void SetCollideMode(const std::string &_mode);

      public: bool GetSelfCollide() const;

      public: void SetLaserRetro(float _retro);

      public: virtual void SetLinearVel(const math::Vector3 &_vel) = 0;

      public: virtual void SetAngularVel(const math::Vector3 &_vel) = 0;

      public: void SetLinearAccel(const math::Vector3 &_accel);

      public: void SetAngularAccel(const math::Vector3 &_accel);

      public: virtual void SetForce(const math::Vector3 &_force) = 0;

      public: virtual void SetTorque(const math::Vector3 &_torque) = 0;

      public: virtual void AddForce(const math::Vector3 &_force) = 0;

      public: virtual void AddRelativeForce(const math::Vector3 &_force) = 0;

      public: virtual void AddForceAtWorldPosition(const math::Vector3 &_force,
                  const math::Vector3 &_pos) = 0;

      public: virtual void AddForceAtRelativePosition(
                  const math::Vector3 &_force,
                  const math::Vector3 &_relPos) = 0;

      public: virtual void AddLinkForce(const math::Vector3 &_force,
          const math::Vector3 &_offset = math::Vector3::Zero) = 0;

      public: virtual void AddTorque(const math::Vector3 &_torque) = 0;

      public: virtual void AddRelativeTorque(const math::Vector3 &_torque) = 0;

      public: math::Pose GetWorldCoGPose() const;

      public: virtual math::Vector3 GetWorldLinearVel() const
              {return this->GetWorldLinearVel(math::Vector3::Zero);}

      public: virtual math::Vector3 GetWorldLinearVel(
                  const math::Vector3 &_offset) const = 0;

      public: virtual math::Vector3 GetWorldLinearVel(
                  const math::Vector3 &_offset,
                  const math::Quaternion &_q) const = 0;

      public: virtual math::Vector3 GetWorldCoGLinearVel() const = 0;

      public: math::Vector3 GetRelativeLinearVel() const;

      public: math::Vector3 GetRelativeAngularVel() const;

      public: math::Vector3 GetRelativeLinearAccel() const;

      public: math::Vector3 GetWorldLinearAccel() const;

      public: math::Vector3 GetRelativeAngularAccel() const;

      public: math::Vector3 GetWorldAngularMomentum() const;

      public: math::Vector3 GetWorldAngularAccel() const;

      public: math::Vector3 GetRelativeForce() const;

      public: virtual math::Vector3 GetWorldForce() const = 0;

      public: math::Vector3 GetRelativeTorque() const;

      public: virtual math::Vector3 GetWorldTorque() const = 0;

      public: ModelPtr GetModel() const;

      public: InertialPtr GetInertial() const {return this->inertial;}

      public: void SetInertial(const InertialPtr &_inertial);

      public: math::Pose GetWorldInertialPose() const;

      public: math::Matrix3 GetWorldInertiaMatrix() const;

      public: CollisionPtr GetCollisionById(unsigned int _id) const;

      public: CollisionPtr GetCollision(const std::string &_name);

      public: CollisionPtr GetCollision(unsigned int _index) const;

      public: Collision_V GetCollisions() const;

      public: virtual math::Box GetBoundingBox() const;

      public: virtual void SetLinearDamping(double _damping) = 0;

      public: virtual void SetAngularDamping(double _damping) = 0;

      public: double GetLinearDamping() const;

      public: double GetAngularDamping() const;

      public: virtual void SetKinematic(const bool &_kinematic);

      public: virtual bool GetKinematic() const {return false;}

      public: unsigned int GetSensorCount() const;

      public: std::string GetSensorName(unsigned int _index) const;

      public: template<typename T>
              event::ConnectionPtr ConnectEnabled(T _subscriber)
              {return enabledSignal.Connect(_subscriber);}

      public: void DisconnectEnabled(event::ConnectionPtr &_conn)
              {enabledSignal.Disconnect(_conn);}

      public: void FillMsg(msgs::Link &_msg);

      public: void ProcessMsg(const msgs::Link &_msg);

      public: void AddChildJoint(JointPtr _joint);

      public: void AddParentJoint(JointPtr _joint);

      public: void RemoveParentJoint(const std::string &_jointName);

      public: void RemoveChildJoint(const std::string &_jointName);

      // Documentation inherited.
      public: virtual void RemoveChild(EntityPtr _child);
      using Base::RemoveChild;

      public: void AttachStaticModel(ModelPtr &_model,
                                     const math::Pose &_offset);

      public: void DetachStaticModel(const std::string &_modelName);

      public: void DetachAllStaticModels();

      public: virtual void OnPoseChange();

      public: void SetState(const LinkState &_state);

      public: virtual void UpdateMass() {}

      public: virtual void UpdateSurface() {}

      public: virtual void SetAutoDisable(bool _disable) = 0;

      public: Link_V GetChildJointsLinks() const;

      public: Link_V GetParentJointsLinks() const;

      public: void SetPublishData(bool _enable);

      public: Joint_V GetParentJoints() const;

      public: Joint_V GetChildJoints() const;

      public: void RemoveCollision(const std::string &_name);

      public: double GetWorldEnergyPotential() const;

      public: double GetWorldEnergyKinetic() const;

      public: double GetWorldEnergy() const;

      public: msgs::Visual GetVisualMessage(const std::string &_name) const;

      public: virtual void SetLinkStatic(bool _static) = 0;

      public: void MoveFrame(const math::Pose &_worldReferenceFrameSrc,
                        const math::Pose &_worldReferenceFrameDst);

      public: bool FindAllConnectedLinksHelper(
        const LinkPtr &_originalParentLink,
        Link_V &_connectedLinks, bool _fistLink = false);

      public: void SetWindEnabled(const bool _enable);

      public: const ignition::math::Vector3d WorldWindLinearVel() const;

      public: const ignition::math::Vector3d RelativeWindLinearVel() const;

      public: void UpdateWind(const common::UpdateInfo &_info);

      public: common::BatteryPtr Battery(const std::string &_name) const;

      public: common::BatteryPtr Battery(const size_t _index) const;

      public: size_t BatteryCount() const;

      public: bool VisualId(const std::string &_visName, uint32_t &_visualId)
        const;

      public: bool VisualPose(const uint32_t _id,
                  ignition::math::Pose3d &_pose) const;

      public: bool SetVisualPose(const uint32_t _id,
                                 const ignition::math::Pose3d &_pose);

      private: void PublishData();

      private: void LoadCollision(sdf::ElementPtr _sdf);

      private: void SetInertialFromCollisions();

      private: void OnCollision(ConstContactsPtr &_msg);

      private: void ParseVisuals();

      private: bool ContainsLink(const Link_V &_vector, const LinkPtr &_value);

      private: void UpdateVisualGeomSDF(const math::Vector3 &_scale);

      private: void UpdateVisualMsg();

      private: void OnWrenchMsg(ConstWrenchPtr &_msg);

      private: void ProcessWrenchMsg(const msgs::Wrench &_msg);

      private: void LoadBattery(const sdf::ElementPtr _sdf);

      protected: InertialPtr inertial;

      protected: std::vector<std::string> cgVisuals;

      typedef std::map<uint32_t, msgs::Visual> Visuals_M;

      protected: Visuals_M visuals;

      protected: math::Vector3 linearAccel;

      protected: math::Vector3 angularAccel;

      protected: std::vector<math::Pose> attachedModelsOffset;

      protected: bool initialized;

      private: event::EventT<void (bool)> enabledSignal;

      private: bool enabled;

      private: std::vector<std::string> sensors;

      private: std::vector<JointPtr> parentJoints;

      private: std::vector<JointPtr> childJoints;

      private: std::vector<ModelPtr> attachedModels;

      private: transport::PublisherPtr dataPub;

      private: msgs::LinkData linkDataMsg;

      private: bool publishData;

      private: boost::recursive_mutex *publishDataMutex;

      private: Collision_V collisions;

      private: transport::SubscriberPtr wrenchSub;

      private: std::vector<msgs::Wrench> wrenchMsgs;

      private: boost::mutex wrenchMsgMutex;

      private: ignition::math::Vector3d windLinearVel;

      private: event::ConnectionPtr updateConnection;

      private: std::vector<common::BatteryPtr> batteries;

#ifdef HAVE_OPENAL
      private: std::vector<util::OpenALSourcePtr> audioSources;

      private: util::OpenALSinkPtr audioSink;

      private: transport::SubscriberPtr audioContactsSub;
#endif
    };
  }
}
#endif