RobotiqHandPlugin.h

Go to the documentation of this file.

/*
 * Copyright 2014 Open Source Robotics Foundation
 * Copyright 2015 Clearpath Robotics
 *
 * 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_ROBOTIQ_HAND_PLUGIN_HH
#define GAZEBO_ROBOTIQ_HAND_PLUGIN_HH

#include <robotiq_s_model_articulated_msgs/SModelRobotInput.h>
#include <robotiq_s_model_articulated_msgs/SModelRobotOutput.h>
#include <gazebo_plugins/PubQueue.h>
#include <ros/advertise_options.h>
#include <ros/callback_queue.h>
#include <ros/ros.h>
#include <ros/subscribe_options.h>
#include <sensor_msgs/JointState.h>
#include <string>
#include <vector>
#include <boost/scoped_ptr.hpp>
#include <boost/thread/mutex.hpp>
#include <gazebo/common/Plugin.hh>
#include <gazebo/common/Time.hh>
#include <gazebo/physics/physics.hh>

class RobotiqHandPlugin : public gazebo::ModelPlugin
{


  
  friend class gazebo::common::PID;
  enum State
  {
    Disabled = 0,
    Emergency,
    ICS,
    ICF,
    ChangeModeInProgress,
    Simplified
  };

  enum GraspingMode
  {
    Basic = 0,
    Pinch,
    Wide,
    Scissor
  };

  public: RobotiqHandPlugin();

  public: virtual ~RobotiqHandPlugin();

  // Documentation inherited.
  public: void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf);

  private: void RosQueueThread();

  private: void SetHandleCommand(
    const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr &_msg);

  private: void UpdatePIDControl(double _dt);

  private: void GetAndPublishHandleState();

  private: void GetAndPublishJointState(const gazebo::common::Time &_curTime);

  private: void UpdateStates();

  private: bool FindJoints();

  private: void ReleaseHand();

  private: void StopHand();

  private: bool IsHandFullyOpen();

  private: uint8_t GetObjectDetection(const gazebo::physics::JointPtr &_joint,
                                    int _index, uint8_t _rPR, uint8_t _prevrPR);

  private: uint8_t GetCurrentPosition(const gazebo::physics::JointPtr &_joint);

  private: bool GetAndPushBackJoint(const std::string& _jointName,
                                    gazebo::physics::Joint_V& _joints);

  private: bool VerifyField(const std::string &_label, int _min,
                            int _max, int _v);

  private: bool VerifyCommand(
    const robotiq_s_model_articulated_msgs::SModelRobotOutput::ConstPtr &_command);

  private: static const int NumJoints = 5;

  private: static const double VelTolerance = 0.002;

  private: static const double PoseTolerance = 0.002;

  private: static const double MinVelocity = 0.176;

  private: static const double MaxVelocity = 0.88;

  private: static const std::string DefaultLeftTopicCommand;

  private: static const std::string DefaultLeftTopicState;

  private: static const std::string DefaultRightTopicCommand;

  private: static const std::string DefaultRightTopicState;

  private: boost::scoped_ptr<ros::NodeHandle> rosNode;

  private: ros::CallbackQueue rosQueue;

  private: boost::thread callbackQueueThread;

  // ROS publish multi queue, prevents publish() blocking
  private: PubMultiQueue pmq;

  private: ros::Subscriber subHandleCommand;

  // changing the grasping mode.
  private: robotiq_s_model_articulated_msgs::SModelRobotOutput handleCommand;

  private: robotiq_s_model_articulated_msgs::SModelRobotOutput lastHandleCommand;

  private: robotiq_s_model_articulated_msgs::SModelRobotOutput prevCommand;

  private: robotiq_s_model_articulated_msgs::SModelRobotOutput userHandleCommand;

  private: gazebo::event::ConnectionPtr updateConnection;

  private: gazebo::common::Time lastControllerUpdateTime;

  private: robotiq_s_model_articulated_msgs::SModelRobotInput handleState;

  private: boost::mutex controlMutex;

  private: GraspingMode graspingMode;

  private: State handState;

  private: ros::Publisher pubHandleState;

  private: PubQueue<robotiq_s_model_articulated_msgs::SModelRobotInput>::Ptr pubHandleStateQueue;

  private: ros::Publisher pubJointStates;

  private: PubQueue<sensor_msgs::JointState>::Ptr pubJointStatesQueue;

  private: sensor_msgs::JointState jointStates;

  private: gazebo::physics::WorldPtr world;

  private: gazebo::physics::ModelPtr model;

  private: sdf::ElementPtr sdf;

  private: std::string side;

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

  private: gazebo::physics::Joint_V fingerJoints;

  private: gazebo::physics::Joint_V joints;

  private: gazebo::common::PID posePID[NumJoints];
};

#endif  // GAZEBO_ROBOTIQ_HAND_PLUGIN_HH