First time here? Check out the FAQ!


ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

Subscribers blocking for performing calculations on data

asked May 25 '15

215 gravatar image

updated May 25 '15

Hi guys I've made a ROS node which subscribes on two topics and publishes a value on another topic, the problem is though i am not able to publish data to the topic since my callbacks keep blocking the publisher on doing so.. How can I overcome this issue? The data has to be processed before the publisher can have it...

Here is the code:

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

sensor_msgs::JointState msg;
double previous_error;
double integral;
double setpoint;
double measuredValue;
double derivative;

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

}

int main(int argc, char **argv)
{
  //previous_error = 0;
  //integral = 0;
  //setpoint = 0;
  //measuredValue = 0;
  //derivative = 0;
  sensor_msgs::JointState msg;
  std::vector<double> vel(2);
  std::vector<double> pos(2);
  pos[0] = 0;
  pos[1] = 0;

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


  ros::NodeHandle n;


  ros::Publisher chatter_pub = n.advertise<sensor_msgs::JointState>("/ptu/cmd", 1);




        ros::Subscriber sub_mes = n.subscribe("/joint_states", 1, measuredValued);
    ros::Subscriber sub_set = n.subscribe("/pid",1,setPoints);

    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;
    msg.position = pos;
    msg.velocity = vel;
    cout << vel[0] << endl;
    chatter_pub.publish(msg);     
  return 0;
}

The code itself is a simple PID controller, which reaceive feedback from a topic and a setpoint from another topic..

Preview: (hide)

Comments

Where's your ros.spin()? That is, it's not a matter of 'blocking', it's a matter of exiting...

kramer gravatar image kramer  ( May 25 '15 )edit

1 Answer

Sort by » oldest newest most voted
0

answered May 26 '15

Fleurmond gravatar image

updated May 27 '15

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;
}
Preview: (hide)

Question Tools

1 follower

Stats

Asked: May 25 '15

Seen: 852 times

Last updated: May 27 '15