#include <ros/ros.h>

#include <tutorial_controller/TutorialAction.h>

#include <actionlib/server/simple_action_server.h>

#include "std_msgs/Float64.h"

#include "geometry_msgs/Vector3.h"

#include "sensor_msgs/JointState.h"

#define PI 3.14159265

//Class for containing the server

class ControllerServer{

public:

ControllerServer(std::string name):

as(n, "pid_control", boost::bind(&ControllerServer::executeCB, this, _1), false),

action_name(name)

{

as.registerPreemptCallback(boost::bind(&ControllerServer::preemptCB, this));

//Start the server

as.start();

//Subscriber current positon of servo

positionservosub = n2.subscribe("/sensor/encoder/servo", 1, &ControllerServer::SensorCallBack, this);

//Publisher setpoint, current position and error of control

error_controlpub = n2.advertise<geometry_msgs::Vector3>("/control/error", 1);

//Publisher PID output in servo

positionservopub = n2.advertise<std_msgs::Float64>("/motor/servo", 1);

//Max e Min Output PID Controller

float max = PI;

float min = -PI;

//Initializing PID Controller

Initialize(min,max);

}

//Callback for handling preemption. Reset your helpers here.

//Note that you still have to check for preemption in your work method to break it off

void preemptCB()

{

ROS_INFO("%s got preempted!", action_name.c_str());

result.ok = 0;

as.setPreempted(result, "I got Preempted!");

}

//Callback for processing a goal

void executeCB(const tutorial_controller::TutorialGoalConstPtr& goal)

{

prevTime = ros::Time::now();

//If the server has been killed, don't process

if(!as.isActive()||as.isPreemptRequested()) return;

//Run the processing at 100Hz

ros::Rate rate(100);

//Setup some local variables

bool success = true;

//Loop control

while(1)

{

std_msgs::Float64 msg_pos;

//PID Controller

msg_pos.data = PIDController(goal->position, position_encoder);

//Publishing PID output in servo

positionservopub.publish(msg_pos);

//Auxiliary Message

geometry_msgs::Vector3 msg_error;

msg_error.x = goal->position;

msg_error.y = position_encoder;

msg_error.z = goal->position - position_encoder;

//Publishing setpoint, feedback and error control

error_controlpub.publish(msg_error);

feedback.position = position_encoder;

//Publish feedback to action client

as.publishFeedback(feedback);

//Check for ROS kill

if(!ros::ok())

{

success = false;

ROS_INFO("%s Shutting Down", action_name.c_str());

break;

}

//If the server has been killed/preempted, stop processing

if(!as.isActive()||as.isPreemptRequested()) return;

//Sleep for rate time

rate.sleep();

}

//Publish the result if the goal wasn't preempted

if(success)

{

result.ok = 1;

as.setSucceeded(result);

}

else

{

result.ok = 0;

as.setAborted(result,"I Failed!");

}

}

void Initialize( float min, float max)

{

setOutputLimits(min, max);

lastError = 0;

errSum = 0;

kp = 1.5;

ki = 0.1;

kd = 0;

// kp = 1;

// ki = 2.3;

// kd = 0;

}

void setOutputLimits(float min, float max)

{

if (min > max) return;

minLimit = min;

maxLimit = max;

}

float PIDController(float setpoint, float PV)

{

ros::Time now = ros::Time::now();

ros::Duration change = now - prevTime;

float error = setpoint - PV;

float dErr = error - lastError;

errSum += error*change.toSec();

errSum = std::min(errSum, maxLimit);

errSum = std::max(errSum, minLimit);

dErr = (error - lastError)/change.toSec();

//Do the full calculation

float output = (kp*error) + (ki*errSum) + (kd*dErr);

//Clamp output to bounds

output = std::min(output, maxLimit);

output = std::max(output, minLimit);

//Required values for next round

lastError = error;

return output;

}

void SensorCallBack(const sensor_msgs::JointState& msg)

{

position_encoder = msg.position[0];

}

protected:

ros::NodeHandle n;

ros::NodeHandle n2;

//Subscriber

ros::Subscriber positionservosub;

//Publishers

ros::Publisher positionservopub;

ros::Publisher error_controlpub;

//Actionlib variables

actionlib::SimpleActionServer<tutorial_controller::TutorialAction> as;

tutorial_controller::TutorialFeedback feedback;

tutorial_controller::TutorialResult result;

std::string action_name;

//Control variables

float position_encoder;

float errSum;

float lastError;

float minLimit, maxLimit;

ros::Time prevTime;

float kp;

float ki;

float kd;

};

//Used by ROS to actually create the node. Could theoretically spawn more than one server

int main(int argc, char** argv)

{

ros::init(argc, argv, "pid_server");

//Just a check to make sure the usage was correct

if(argc != 1)

{

ROS_INFO("Usage: pid_server");

return 1;

}

//Spawn the server

ControllerServer server(ros::this_node::getName());

ros::spin();

return 0;

}