c++ - ROS订阅者一直阻塞回调

Question

我正在尝试使用 ROS 对 PID Controller 进行编程，问题是我无法将任何来自 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;
}

Answer 1

当然:

首先这里有一个拼写错误，因为每次你收到一条消息时，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();
}

那么你应该解决你所有的问题。