ROS订阅者一直阻塞回调
ROS subscriber keep blocking callback
我正在尝试使用 ROS 对 PID 控制器进行编程,问题是我无法将任何来自 PID 节点的数据发布到所需的主题上,因为我的订阅者回调一直阻塞那部分代码将被执行..
你们对我如何克服这个问题有什么好主意吗?
#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;
}
当然:
首先,这里有一个拼写错误,因为每次您收到一条消息时,都会再次创建并执行 main:
void setPoints(const sensor_msgs::JointState::ConstPtr& msg)
{
<= Here a } is missing !!!!!!
int main(int argc, char **argv)
你的问题是在你的 main 中你没有定义一个定期调用函数的循环
ros::spinOnce();
因此整个程序将执行一次然后停止。
如果将上面的行放在一个单独的循环中,如下所示:
while (ros::ok())
{
publish....
ros::spinOnce();
}
那么你应该解决你所有的问题。
我正在尝试使用 ROS 对 PID 控制器进行编程,问题是我无法将任何来自 PID 节点的数据发布到所需的主题上,因为我的订阅者回调一直阻塞那部分代码将被执行..
你们对我如何克服这个问题有什么好主意吗?
#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;
}
当然:
首先,这里有一个拼写错误,因为每次您收到一条消息时,都会再次创建并执行 main:
void setPoints(const sensor_msgs::JointState::ConstPtr& msg)
{
<= Here a } is missing !!!!!!
int main(int argc, char **argv)
你的问题是在你的 main 中你没有定义一个定期调用函数的循环
ros::spinOnce();
因此整个程序将执行一次然后停止。
如果将上面的行放在一个单独的循环中,如下所示:
while (ros::ok())
{
publish....
ros::spinOnce();
}
那么你应该解决你所有的问题。