Subscribers blocking for performing calculations on data
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..
Asked by 215 on 2015-05-24 19:44:20 UTC
Answers
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::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;
}
Asked by Fleurmond on 2015-05-26 12:22:24 UTC
Comments
Where's your
ros.spin()
? That is, it's not a matter of 'blocking', it's a matter of exiting...Asked by kramer on 2015-05-25 08:14:30 UTC