0

我正在尝试运行一个 ROS 节点，该节点从节点订阅数据，然后计算 PID 控制器输出并将其发布到另一个主题上。但是每 10 个周期后我得到一个总线，核心转储错误......我不知道为什么。 ..这是供参考的代码：

namespace youbot {

PidController::PidController(double P, double I, double D, double I1, double I2) :
p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
{

   p_error_last_ = 0.0;
   p_error_ = 0.0;
   d_error_ = 0.0;

   i_error_ = 0.0;
   cmd_ = 0.0;
   last_i_error = 0.0;
}

double PidController::updatePid(double error, boost::posix_time::time_duration dt)
{
    double p_term, d_term, i_term;
    p_error_ = error; //this is pError = pState-pTarget
    double deltatime = (double)dt.total_microseconds()/1000.0; //in milli seconds


   if (deltatime == 0.0 || isnan(error) || isinf(error))
      return 0.0;
   p_term = p_gain_ * p_error_;
   i_error_ = last_i_error + deltatime * p_error_;
   last_i_error = deltatime * p_error_;
   i_term = i_gain_ * i_error_;
   if (i_term > i_max_)
   {  
     i_term = i_max_;
     i_error_=i_term/i_gain_;
   }
   else if (i_term < i_min_)
   {
     i_term = i_min_;
     i_error_=i_term/i_gain_;
   }


  if (deltatime != 0)
  {
     d_error_ = (p_error_ - p_error_last_) / deltatime;
     p_error_last_ = p_error_;
  }
  d_term = d_gain_ * d_error_;
  cmd_ = -p_term - i_term - d_term;

  p_error_, i_error_, p_term, i_term, deltatime, cmd_);

  return cmd_;
}
}

using namespace std;
using namespace youbot;

double currentPos[3];
void arrayCallback(const geometry_msgs::Point::ConstPtr& cord);
double calculateControllerOutput(double pos, double targetPos);


int main(int argc, char **argv)
{

ros::init(argc, argv, "coordinateSubscriber");
ros::NodeHandle n;
ros::Subscriber sub3 = n.subscribe("position", 100, arrayCallback);
ros::spin();

return 0;
}

我无法猜测是哪个部分导致了错误。任何帮助将不胜感激..谢谢！

4

0 回答 0