GazeboGraspFix.h

Go to the documentation of this file.

#ifndef GAZEBO_GAZEBOGRASPFIX_H
#define GAZEBO_GAZEBOGRASPFIX_H

#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <gazebo/transport/TransportTypes.hh>
#include <stdio.h>
#include <gazebo_grasp_plugin/GazeboGraspGripper.h>

namespace gazebo
{

class GazeboGraspFix : public ModelPlugin
{
  public:
    GazeboGraspFix();
    GazeboGraspFix(physics::ModelPtr _model);
    virtual ~GazeboGraspFix();

    virtual void OnAttach(const std::string &objectName,
                          const std::string &armName) {}
    virtual void OnDetach(const std::string &objectName,
                          const std::string &armName) {}

  private:
    virtual void Init();
    virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
    void OnUpdate();

    void InitValues();


    void OnContact(const ConstContactsPtr &ptr);

//    bool CheckGrip(const std::vector<GzVector3> &forces, float minAngleDiff,
//                   float lengthRatio);

    bool IsGripperLink(const std::string &linkName, std::string &gripperName) const;

    std::map<std::string, std::string> GetAttachedObjects() const;

    class ObjectContactInfo;

    bool ObjectAttachedToGripper(const ObjectContactInfo &objContInfo,
                                 std::string &attachedToGripper) const;

    bool ObjectAttachedToGripper(const std::string &gripperName,
                                 std::string &attachedToGripper) const;


    // physics::ModelPtr model;
    // physics::PhysicsEnginePtr physics;
    physics::WorldPtr world;

    // sorted by their name, all grippers of the robot
    std::map<std::string, GazeboGraspGripper> grippers;

    event::ConnectionPtr update_connection;
    transport::NodePtr node;
    transport::SubscriberPtr contactSub; //subscriber to contact updates

    // tolerance (in degrees) between force vectors to
    // beconsidered "opposing"
    float forcesAngleTolerance;

    // when an object is attached, collisions with it may be disabled, in case the
    // robot still keeps wobbling.
    bool disableCollisionsOnAttach;

    // all collisions per gazebo collision link (each entry
    // belongs to a physics::CollisionPtr element). The key
    // is the collision link name, the value is the gripper name
    // this collision link belongs to.
    std::map<std::string, std::string> collisions;


    class CollidingPoint;

    // Contact forces sorted by object name the gripper collides with (first key)
    // and the link colliding (second key).
    std::map<std::string, std::map<std::string, CollidingPoint> > contacts;
    boost::mutex mutexContacts; //mutex protects contacts

    // when an object was first attached, it had these colliding points.
    // First key is object name, second is the link colliding, as in \e contacts.
    // Only the links of *one* gripper are stored here. This indirectly imposes the
    // limitation that no two grippers can grasp the object (while it would be
    // possible, the release condition is tied to only one link, so the object may
    // not be released properly).
    std::map<std::string, std::map<std::string, CollidingPoint> >
    attachGripContacts;


    // Records how many subsequent update calls the grip on that object has been recorded
    // as "holding". Every loop, if a grip is not recorded, this number decreases.
    // When it reaches \e grip_count_threshold, it will be attached.
    // The number won't increase above max_grip_count once it has reached that number.
    std::map<std::string, int> gripCounts;

    // *maximum* number in \e gripCounts to be recorded.
    int maxGripCount;

    // number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts
    // as the object grasped, and when it is lower, as released.
    int gripCountThreshold;

    // once an object is gripped, the relative position of the collision link surface to the
    // object is remembered. As soon as this distance changes more than release_tolerance,
    // the object is released.
    float releaseTolerance;

    //nano seconds between two updates
    common::Time updateRate;

    //last time OnUpdate() was called
    common::Time prevUpdateTime;
};

}

#endif  // GAZEBO_GAZEBOGRASPFIX_H