19 #ifndef GAZEBO_ROBOTIQ_HAND_PLUGIN_HH 20 #define GAZEBO_ROBOTIQ_HAND_PLUGIN_HH 22 #include <robotiq_3f_gripper_articulated_msgs/Robotiq3FGripperRobotInput.h> 23 #include <robotiq_3f_gripper_articulated_msgs/Robotiq3FGripperRobotOutput.h> 29 #include <sensor_msgs/JointState.h> 32 #include <boost/scoped_ptr.hpp> 33 #include <boost/thread/mutex.hpp> 34 #include <gazebo/common/Plugin.hh> 35 #include <gazebo/common/Time.hh> 36 #include <gazebo/physics/physics.hh> 89 public:
void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf);
97 const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_msg);
137 int _index, uint8_t _rPR, uint8_t _prevrPR);
151 gazebo::physics::Joint_V& _joints);
159 private:
bool VerifyField(
const std::string &_label,
int _min,
167 const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_command);
202 private: boost::scoped_ptr<ros::NodeHandle>
rosNode;
219 private: robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput
handleCommand;
227 private: robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput
prevCommand;
239 private: robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput
handleState;
266 private: gazebo::physics::WorldPtr
world;
269 private: gazebo::physics::ModelPtr
model;
272 private: sdf::ElementPtr
sdf;
284 private: gazebo::physics::Joint_V
joints;
290 #endif // GAZEBO_ROBOTIQ_HAND_PLUGIN_HH robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput prevCommand
Previous command received. We know if the hand is opening or closing by comparing the current command...
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput handleState
Robotiq Hand State.
gazebo::common::Time lastControllerUpdateTime
keep track of controller update sim-time.
void Load(gazebo::physics::ModelPtr _parent, sdf::ElementPtr _sdf)
static const int NumJoints
Number of joints in the hand. The three fingers can do abduction/adduction. Fingers 1 and 2 can do ci...
ros::Publisher pubHandleState
ROS publisher for Robotiq Hand state.
bool VerifyField(const std::string &_label, int _min, int _max, int _v)
Verify that one command field is within the correct range.
void UpdatePIDControl(double _dt)
Update PID Joint controllers.
static constexpr double MinVelocity
Min. joint speed (rad/s). Finger is 125mm and tip speed is 22mm/s.
sdf::ElementPtr sdf
Pointer to the SDF of this plugin.
std::string side
Used to select between 'left' or 'right' hand.
friend class gazebo::common::PID
gazebo::event::ConnectionPtr updateConnection
gazebo world update connection.
void GetAndPublishHandleState()
Publish Robotiq Hand state.
gazebo::physics::Joint_V joints
Vector containing all the joints.
static constexpr double PoseTolerance
Position tolerance. If the difference between target position and current position is within this val...
static constexpr double MaxVelocity
Max. joint speed (rad/s). Finger is 125mm and tip speed is 110mm/s.
boost::mutex controlMutex
Controller update mutex.
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput userHandleCommand
Original HandleControl message (published by user and unmodified).
std::vector< std::string > jointNames
Vector containing all the joint names.
gazebo::physics::WorldPtr world
World pointer.
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput lastHandleCommand
HandleControl message. Last command received before changing the grasping mode.
gazebo::physics::ModelPtr model
Parent model of the hand.
void SetHandleCommand(const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_msg)
ROS topic callback to update Robotiq Hand Control Commands.
gazebo::physics::Joint_V fingerJoints
Vector containing all the actuated finger joints.
sensor_msgs::JointState jointStates
ROS joint state message.
static const std::string DefaultRightTopicCommand
Default topic name for sending control updates to the right hand.
GraspingMode
Different grasping modes.
bool FindJoints()
Grab pointers to all the joints.
PubQueue< robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInput >::Ptr pubHandleStateQueue
ROS publisher queue for Robotiq Hand state.
static const std::string DefaultLeftTopicState
Default topic name for receiving state updates from the left hand.
GraspingMode graspingMode
Grasping mode.
uint8_t GetObjectDetection(const gazebo::physics::JointPtr &_joint, int _index, uint8_t _rPR, uint8_t _prevrPR)
Internal helper to get the object detection value.
static constexpr double VelTolerance
Velocity tolerance. Below this value we assume that the joint is stopped (rad/s). ...
State handState
Hand state.
ros::CallbackQueue rosQueue
ROS callback queue.
ros::Publisher pubJointStates
Joint state publisher (rviz visualization).
void GetAndPublishJointState(const gazebo::common::Time &_curTime)
Publish Robotiq Joint state.
A plugin that implements the Robotiq 3-Finger Adaptative Gripper. The plugin exposes the next paramet...
boost::scoped_ptr< ros::NodeHandle > rosNode
ROS NodeHandle.
static const std::string DefaultLeftTopicCommand
Default topic name for sending control updates to the left hand.
bool IsHandFullyOpen()
Checks if the hand is fully open. return True when all the fingers are fully open or false otherwise...
static const std::string DefaultRightTopicState
Default topic name for receiving state updates from the right hand.
bool GetAndPushBackJoint(const std::string &_jointName, gazebo::physics::Joint_V &_joints)
Internal helper to reduce code duplication. If the joint name is found, a pointer to the joint is add...
boost::thread callbackQueueThread
ROS callback queue thread.
void RosQueueThread()
ROS callback queue thread.
ros::Subscriber subHandleCommand
ROS control interface.
PubQueue< sensor_msgs::JointState >::Ptr pubJointStatesQueue
ROS publisher queue for joint states.
void StopHand()
Stop the fingers.
virtual ~RobotiqHandPlugin()
Destructor.
gazebo::common::PID posePID[NumJoints]
PIDs used to control the finger positions.
uint8_t GetCurrentPosition(const gazebo::physics::JointPtr &_joint)
Internal helper to get the actual position of the finger.
bool VerifyCommand(const robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput::ConstPtr &_command)
Verify that all the command fields are within the correct range.
void UpdateStates()
Update the controller.
void ReleaseHand()
Fully open the hand at half of the maximum speed.
RobotiqHandPlugin()
Constructor.
robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput handleCommand
HandleControl message. Originally published by user but some of the fields might be internally modifi...