ros2_control_node.cpp real time problem

[yeyanlei]

Describe the bug
There are is a problem about ros2_control, i found that the ROS_Control_node would suddenly jitter (like 3ms), the set period is 1ms. The system environment is ubuntu 22.04, ros2 humble, kernel 5.15.55-rt with Preempt_rt patch. Fifo mode, priority is 90. i have posted serveral pictures. system max jitter is less than 50us.
print result
[ros2_control_node-1] ==============================measured_period.nanoseconds ===========6116648
[ros2_control_node-1] ==============================measured_period.nanoseconds ===========89013
[ros2_control_node-1] ==============================measured_period.nanoseconds ===========26079

To Reproduce
Steps to reproduce the behavior:

1. This is code, just print jitter of measured_period.

while (rclcpp::ok())
{
// calculate measured period
auto const current_time = cm->now();
auto const measured_period = current_time - previous_time;
previous_time = current_time;
rcl_duration_value_t base_time=1000000;
if (base_time-measured_period.nanoseconds()>50000 || base_time-measured_period.nanoseconds()<-50000)
{
std::cout<<"==============================measured_period.nanoseconds ==========="<<measured_period.nanoseconds()<<std::endl;
}

    // execute update loop
    cm->read(cm->now(), measured_period);
    cm->update(cm->now(), measured_period);
    cm->write(cm->now(), measured_period);

    // wait until we hit the end of the period
    next_iteration_time += period;
    std::this_thread::sleep_until(next_iteration_time);
  }
});

Expected behavior
A clear and concise description of what you expected to happen.

Screenshots
If applicable, add screenshots to help explain your problem.

Environment (please complete the following information):

• OS: [ubuntu 22.04]

• Version [humble]
  

• kernel Linux c 5.15.55-rt48 Import ros2_control #1 SMP PREEMPT_RT

Additional context
Add any other context about the problem here, especially include any modifications to ros2_control that relate to this issue.

ros2_control_node.txt

[yeyanlei]

@bmagyar @destogl Can you help me check this problem or give me some suggestions

[yeyanlei]

I find that update() causes a large time delay (like 2ms, even bigger, the cycle time is 1ms).

[destogl]

Hi @yeyanlei,

the way you are measuring time could cause jitter, since you are using "print" functions on each run. Do you get output that Preempt Kernel is used? It should be automatically recognized in the master version. Also, what controllers are you running? We would need your exact system setup to know what could cause the issue. This mean, which hardware drivers are used, which controllers and in which configuration.

[yeyanlei]

@destogl I have located this problem, latency occurs in update (), controller_manager.cpp, update() function(). I post printed time series. Large time delay will occur in t2-t1 (like400us). using controller is joint_trajectory_controller. Preept Kernel is used. The timing of read() and write() functions can be guaranteed.

this is code from controller_manager.cpp

auto t1 = rclcpp::Clock().now();
auto duaration_t1_t0=t1-t0;

for (auto loaded_controller : rt_controller_list)
{
// TODO(v-lopez) we could cache this information
// #153
if (is_controller_active(*loaded_controller.c))
{
const auto controller_update_rate = loaded_controller.c->get_update_rate();

  bool controller_go =
    controller_update_rate == 0 || ((update_loop_counter_ % controller_update_rate) == 0);
  RCLCPP_DEBUG(
    get_logger(), "update_loop_counter: '%d ' controller_go: '%s ' controller_name: '%s '",
    update_loop_counter_, controller_go ? "True" : "False",
    loaded_controller.info.name.c_str());

  if (controller_go)
  {
    auto controller_ret = loaded_controller.c->update(
      time, (controller_update_rate != update_rate_ && controller_update_rate != 0)
              ? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
              : period);

    if (controller_ret != controller_interface::return_type::OK)
    {
      ret = controller_ret;
    }
  }
}

}

auto t2 = rclcpp::Clock().now();
auto duaration_t2_t1=t2-t1;

if (duaration_t2_t1.nanoseconds()>100000)
{
std::cout<<"=========================update t2-t1 duaration.nanoseconds() ==========="<<duaration_t2_t1.nanoseconds()<<std::endl;
}

[yeyanlei]

@destogl joint_trajectory_controller(arm_controller) generates a large delay.
the system includes three controllers, arm_controller(joint_trajectory_controller), hand_controller(joint_trajectory_controller), and joint_state_broadcaster.

[yeyanlei]

@destogl At present, the problem has been preliminarily solved. I directly use joint_ trajectory_ The controller source file， obviously reduces the robot jitter. I suspect that “sudo apt install ros humble joint trajectory controller” has some problems.

[yeyanlei]

Using sudo apt install ros-humble-joint-trajectory-controller, sometimes the program execution time is 2000-3000us. using source file joint-trajectory-controller master, sometimes the program execution time less than 500us.

[destogl]

Using sudo apt install ros-humble-joint-trajectory-controller, sometimes the program execution time is 2000-3000us. using source file joint-trajectory-controller master, sometimes the program execution time less than 500us.

Ok, great! Yes, we did some improvements recently, and they are not yet synced to the humble release. Glad that you solved the problem.

I'll close this. You are welcome to reopen is needed.