Subscribers blocking for performing calculations on data

edit

Hi,

First I have the feeling that it would be helpful to you to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/Wri...

Second, I would suggest you to check the name of the joints : http://docs.ros.org/api/sensor_msgs/h...

But I presume that you know well the robot that you are using, and you can ensure that the joint states will be published always in the same order !?

Then I have dared to rearrange your code in that way (it could be not the best one), but in purpose to avoid the use of global variables. I did not try to compile this code, but I reasonably hope it will work.

As a previous comment to your question said, ros::spin() or equivalent was missing. It's a kind of infinite loop, which check if no terminate signal is received and allow callback functions to be called when new data is available in subscribed topics.

I choose to publish whenever I receive data wherever he comes from, the measurements topic or the given order in topic /pid. I rename your two fonctions to avoid confusion with your variables.

#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/JointState.h"
#include <vector>
#include <sstream>
#define kp   1  //              P
#define ki   1  //              I
#define kd   1  //              D
#define dt 0.1  //              dt       

using namespace std;

class MyNicePID{

private:

  double previous_error;
  double integral;
  double setpoint;
  double measuredValue;
  double derivative;
  std::vector<double> vel;
  std::vector<double> pos;
  ros::Publisher chatter_pub; 
  ros::Subscriber sub_mes;
  ros::Subscriber sub_set;

public:

  MyNicePID(ros::NodeHandle n):
    previous_error(0.0),
    integral(0.0),
    setpoint(0.0),
    measuredValue(0.0),
    derivative(0.0),
    vel(2, 0.0),
    pos(2, 0.0)
  {
    chatter_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 1);
    sub_mes = n.subscribe("/joint_states", 1, &MyNicePID::measureCurrentVelocity,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::recordWantedVelocity,this);

  }

  void measureCurrentVelocity(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    cout << "Measured value" << endl;
    measuredValue = msg->velocity[0];
    computeControlAndPublish();
  }
  void recordWantedVelocity(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    //cout << "setPoint value" << endl;
    setpoint = msg->velocity[0];
    computeControlAndPublish();
  }

  void computeControlAndPublish()
  {

    double error = setpoint - measuredValue;
    integral = integral + error*dt;
    derivative = (error-previous_error)/dt;
    vel[0] = kp*error + ki*integral +kd*derivative;
    vel[1] = 0;
    previous_error = error;
    sensor_msgs::JointState msg;
    msg.position = pos;
    msg.velocity = vel;

    cout << vel[0] << endl;

    chatter_pub.publish(msg);

  }


};


int main(int argc, char **argv)
{

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

  ros::NodeHandle node;

  MyNicePID johnSmith215(node);

  ros::spin();

  return 0;
}