Question

我正在尝试使用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;
}

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();
}

然后你应该解决所有问题。

ROS用户继续阻止回调

1 个答案: