Revision history [back]

Hi,

First i have the feeling that it would be helpful to you te re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

Second, I would suggest you to check the name of the joints : http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html But I presume that you know well the robot that you are using, and probably it has only one moving joint.

Then I have dared to rearrange your code in that way (it 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 it will works.

#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       

sensor_msgs::JointState msg;

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::measuredValued,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);

  }

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

  }

  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;
}

Hi,

First i have the feeling that it would be helpful to you te to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

Second, I would suggest you to check the name of the joints : http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html But I presume that you know well the robot that you are using, and probably it has only one moving joint.

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 works.

As a previous comment to your question said, ros:spin() or equivalent is 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 when I receive one data wherever he comes from the measurements topic or the given order in topic /pid.

#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       

sensor_msgs::JointState msg;

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::measuredValued,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);

  }

  void measuredValued(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    cout << "Measured value" << endl;
    measuredValue = msg->velocity[0];
    computeControlAndPublish();
  }
  void setPoints(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;
}

Hi,

First i I have the feeling that it would be helpful to you to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

Second, I would suggest you to check the name of the joints : http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html But I presume that you know well the robot that you are using, and probably it has only one moving joint.

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 works.

As a previous comment to your question said, ros:spin()ros::spin() or equivalent is 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 when I receive one data wherever he comes from the measurements topic or the given order in topic /pid.

#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       

sensor_msgs::JointState msg;

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::measuredValued,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);

  }

  void measuredValued(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    cout << "Measured value" << endl;
    measuredValue = msg->velocity[0];
    computeControlAndPublish();
  }
  void setPoints(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;
}

Hi,

First I have the feeling that it would be helpful to you to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

Second, I would suggest you to check the name of the joints : http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html But I presume that you know well the robot that you are using, and probably it has only one moving joint.

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 works.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 when whenever I receive one data wherever he comes from from, the measurements topic or the given order in topic /pid.

#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       

sensor_msgs::JointState msg;

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::measuredValued,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);

  }

  void measuredValued(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    cout << "Measured value" << endl;
    measuredValue = msg->velocity[0];
    computeControlAndPublish();
  }
  void setPoints(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;
}

Hi,

First I have the feeling that it would be helpful to you to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

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

But I presume that you know well the robot that you are using, and probably it has only one moving joint.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.

#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       

sensor_msgs::JointState msg;

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::measuredValued,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);

  }

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

  void computeControlAndPublish(){
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;
}

Hi,

First I have the feeling that it would be helpful to you to re-read this tutorial:
http://wiki.ros.org/ROS/Tutorials/WritingPublisherSubscriber%28c%2B%2B%29

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

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::measuredValued,this);
&MyNicePID::measureCurrentVelocity,this);
    sub_set = n.subscribe("/pid",1,&MyNicePID::setPoints,this);
n.subscribe("/pid",1,&MyNicePID::recordWantedVelocity,this);

  }

  void measuredValued(const measureCurrentVelocity(const sensor_msgs::JointState::ConstPtr& msg)
  {      
    cout << "Measured value" << endl;
    measuredValue = msg->velocity[0];
    computeControlAndPublish();
  }
  void setPoints(const 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;
}