56 std::size_t error = 0;
62 clock_gettime(CLOCK_MONOTONIC, &last_time_);
67 void GenericHWControlLoop::run()
77 void GenericHWControlLoop::update()
80 clock_gettime(CLOCK_MONOTONIC, ¤t_time_);
82 ros::Duration(current_time_.tv_sec - last_time_.tv_sec + (current_time_.tv_nsec - last_time_.tv_nsec) /
BILLION);
83 last_time_ = current_time_;
89 const double cycle_time_error = (elapsed_time_ - desired_update_period_).toSec();
90 if (cycle_time_error > cycle_time_error_threshold_)
93 << cycle_time_error <<
", cycle time: " << elapsed_time_
94 <<
", threshold: " << cycle_time_error_threshold_);
98 hardware_interface_->read(now, elapsed_time_);
101 controller_manager_->update(now, elapsed_time_);
104 hardware_interface_->write(now, elapsed_time_);