00001 #ifndef GAZEBOGRIPPER_H 00002 #define GAZEBOGRIPPER_H 00003 00004 #include <gazebo/common/Plugin.hh> 00005 #include <gazebo/common/Events.hh> 00006 00007 #include <gazebo/physics/Gripper.hh> 00008 00009 #include <ros/ros.h> 00010 #include <sensor_msgs/JointState.h> 00011 00012 #include <gazebo/physics/Model.hh> 00013 #include <gazebo/physics/Joint.hh> 00014 #include <gazebo/physics/World.hh> 00015 #include <gazebo/physics/Link.hh> 00016 #include <gazebo/physics/PhysicsEngine.hh> 00017 #include <gazebo/physics/Collision.hh> 00018 #include <gazebo/physics/Contact.hh> 00019 00020 namespace gazebo 00021 { 00022 class GazeboGripper : public ModelPlugin 00023 { 00024 public: 00025 GazeboGripper(); 00026 ~GazeboGripper(); 00027 00028 virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 00029 virtual void Init(); 00030 00031 // Called by the world update start event 00032 void onUpdate(); 00033 00034 private: 00035 00036 // Check contacts and handle appropriately 00037 void handleContacts(const std::vector<physics::Contact*>& contacts); 00038 00039 // Attach linkPtr to the gripper 00040 void handleAttach(physics::LinkPtr linkPtr); 00041 00042 // Detach an object from the gripper 00043 void handleDetach(); 00044 00045 // Pointer to the model 00046 physics::ModelPtr modelPtr; 00047 // This joint is used to attach the robot to the world 00048 physics::JointPtr fixedJointPtr; 00049 // Event callback for world updates. Used to update contacts 00050 event::ConnectionPtr worldUpdateEvent; 00051 // The collision objects in the robot checked for contact with the world 00052 std::vector<std::string> collisionNames; 00053 00054 // If true, the grippers are attached 00055 bool attached; 00056 // The amount of time required at low relative speed before grippers are attached 00057 double attachWait; 00058 // The amount of time required at high relative speed before grippers are detached 00059 double detachWait; 00060 // The amount of time between checks for contacts. Useful since world updates are very frequent 00061 double updateTime; 00062 // The maximum relative speed between the gripper and the world allowed for contact 00063 double maxRelativeMotionRate; 00064 // The name of the link that is attached to the world (may be different from the gripper) 00065 std::string gripperAttachLink; 00066 00067 // The most recent time that contacts were checked 00068 common::Time prevUpdateTime; 00069 // The amount of time the grippers have been in contact with the world 00070 double contactDuration; 00071 // The amount of time the grippers have not been in contact with the world 00072 double nonContactDuration; 00073 // The time of most recent contact 00074 common::Time prevContactUpdateTime; 00075 // The most recent difference in pose between the robot and the world. Used for speed computation 00076 math::Pose prevDiff; 00077 // The name of this plugin 00078 std::string pluginName; 00079 }; 00080 } 00081 00082 #endif