ROS-абонент продолжает блокировать обратный вызов

Question

Я пытаюсь запрограммировать ПИД-контроллер с использованием ROS, проблема в том, что я не могу опубликовать данные из узла 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

Конечно:

Прежде всего есть здесь ошибка опечатка, потому что каждый раз, когда вы получаете сообщение главным будет создан и выполнен снова:

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

Ваша проблема что в вас основной вы не определяли петлю, которая регулярно вызывает функцию

ros::spinOnce(); 

По этой причине вся программа будет выполняться один раз, а затем останавливается.

Если поместить эту строку в отдельном цикле, как следующее:

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

    ros::spinOnce(); 
} 

, то вы должны решить все ваши проблемы.