CODEWITHSUNDEEP
c++callbackblockingrace-conditionros
user2691824
user2691824

Reputation:

ROS subscriber keep blocking callback

I am trying to program a PID controller using a ROS, the problem is that i am not able to publish any data from the PID node onto a the desired topic since my subscriber callback keeps blocking for that part of the code to be executed..

Do you guys have any good idea on how I can overcome this issue? 

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

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

void measuredValued(const sensor_msgs::JointState::ConstPtr& msg)
{   
     measuredValue = msg->velocity[0];

}
void setPoints(const sensor_msgs::JointState::ConstPtr& msg)
{   

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

  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 error = 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; 
    error.publish(msg);
    ros::spinOnce();

  return 0;
}

Upvotes: 0

Views: 2443

Answers (1)

Dave
Dave

Reputation: 348

Of course:

First of all there is here a typo error because everytime you receive a message the main is going to be created and executed again:

void setPoints(const sensor_msgs::JointState::ConstPtr& msg)
{   
           <= Here a } is missing !!!!!!
int main(int argc, char **argv)

Your problem is that in you main you didn t define a loop which calls regularly the function

ros::spinOnce();

For this reason the whole program is going to be executed once and then stops.

If you put the above line in a separate loop like the following:

while (ros::ok()) 
{
  publish....

  ros::spinOnce();
}

then you should solve all your problems.

Upvotes: 1

Related Questions