gripper_plugin.cc

#ifndef _VELODYNE_PLUGIN_HH_
#define _VELODYNE_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>

#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"

namespace gazebo
{
	/// \brief A plugin to control the gripper.	
	class GripperPlugin : public ModelPlugin {
	
	/// \brief Constructor	
	public: GripperPlugin() {}

	public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
	{
		this->model = _model; 					// Store model pointer
		this->sdf = _sdf;					// Store sdf pointer

		this->joint1 = this->model->GetJoint("cylinder_grip1");
		this->joint2 = this->model->GetJoint("cylinder_grip2");
		this->joint3 = this->model->GetJoint("cylinder_grip3");
		this->joint4 = this->model->GetJoint("cylinder_grip4");
		this->joint5 = this->model->GetJoint("cylinder_grip5");
		this->joint6 = this->model->GetJoint("cylinder_grip6");
		
		this->pid1 = common::PID(10, 1, 1.1);
		this->pid2 = common::PID(10, 1, 1.1);
		this->pid3 = common::PID(10, 1, 1.1);
		this->pid4 = common::PID(10, 1, 1.1);
		this->pid5 = common::PID(10, 1, 1.1);
		this->pid6 = common::PID(10, 1, 1.1);

		this->model->GetJointController()->SetPositionPID(this->joint1->GetScopedName(), this->pid1);
		this->model->GetJointController()->SetPositionPID(this->joint2->GetScopedName(), this->pid2);
		this->model->GetJointController()->SetPositionPID(this->joint3->GetScopedName(), this->pid3);
		this->model->GetJointController()->SetPositionPID(this->joint4->GetScopedName(), this->pid4);
		this->model->GetJointController()->SetPositionPID(this->joint5->GetScopedName(), this->pid5);
		this->model->GetJointController()->SetPositionPID(this->joint6->GetScopedName(), this->pid6);

		this->model->GetJointController()->SetPositionTarget(this->joint1->GetScopedName(), 0);
		this->model->GetJointController()->SetPositionTarget(this->joint2->GetScopedName(), 0);
		this->model->GetJointController()->SetPositionTarget(this->joint5->GetScopedName(), 0);

		this->model->GetJointController()->SetPositionTarget(this->joint3->GetScopedName(), 0);
		this->model->GetJointController()->SetPositionTarget(this->joint4->GetScopedName(), 0);
		this->model->GetJointController()->SetPositionTarget(this->joint6->GetScopedName(), 0);
		
// ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤ ROS-related ¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤¤
		if (!ros::isInitialized())					// Initialize ros, if it has not already been initialized.
		{	
			int argc = 0;
			char **argv = NULL;
			ros::init(argc, argv, "gazebo_client",
			ros::init_options::NoSigintHandler);
		}

		this->rosNode.reset(new ros::NodeHandle("gazebo_client"));	// Create a ROS node

		ros::SubscribeOptions so1 =					// Create a ROS topic
			ros::SubscribeOptions::create<std_msgs::Float32>(
			"/" + this->model->GetName() + "/grip_rad", 1,