ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question
0

"Flush"/discard accumulating write() and read() calls in ros2_control hardware component/interface

asked 2022-05-02 07:41:47 -0600

eldavio gravatar image

I want to implement a triggered / on-demand / on-request communication with hardware using ros2_control framework.

In relation to a previous question and this answer on a similar problem, i wrote a controller following forward_command_controller implementation for ros2 galactic, which uses a "settings" command interface type and calls LoanedCommandInterface.set_value(msg->data) whenever a new message msg arrives on the controller's subscribed topic.

On the hardware component side, I exported the "settings" command interface and, inside the write() method I check if a new "setting" has been commanded like so:

if (!std::isnan(hw_command_settings_))
{
  int setting = hw_command_settings_;
  switch (setting)
  {
    case MY_FIRST_SETTING:
      /***********************
       *perform setting command
       ***********************/
      break;
  }
  hw_command_settings_ = std::numeric_limits<double>::quiet_NaN(); //reset setting request
  return hardware_interface::return_type::OK;
}
//write() function continues with the other command interfaces...

If the implementation is real-time, there's no problem: the hardware component writes using the other standard interfaces ("position", "velocity"...), and only if I send a message on the "/settings_controller/command" topic the setting command is performed.

However, the nature of these "settings" commands is not really real-time. I have a homing/calibrating command that needs to wait until the motor reaches a defined position before it's completed. So the write() function is blocked until this action is finished. However, when the normal control cycle is restored, i see a list of write() and read() calls that has been accumulating during this halting period (e.g. 5s halting with 10Hz control cycle results in 50 instant write() and read() calls after the halt). I believe this is like a default policy of ros2_control_node, but I'm very new to the framework and I don't really know.

Is there a way to avoid accumulating read() and write() calls when one of them is halted by a long procedure? If not, is there a way to isolate the "settings" interface so that the write() method is kept real-time? Any other suggestion is really helpful.

Thanks!

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-06-01 13:11:57 -0600

Bence Magyar gravatar image

We have recently extended read/write with time parameters. This should allow you to implement some mechanism to skip writing certain interfaces by keeping accounts of the last time you accessed them.

https://github.com/ros-controls/ros2_...

Available in Humble and Rolling

edit flag offensive delete link more

Question Tools

2 followers

Stats

Asked: 2022-05-02 07:41:47 -0600

Seen: 334 times

Last updated: Jun 01 '22