ros2_control icon indicating copy to clipboard operation
ros2_control copied to clipboard

Dynamic memory allocation inside realtime functions / loops

Open AndyZe opened this issue 2 years ago • 5 comments

I thought it was bad practice to declare a variable inside a realtime loop, but I see it a lot in this codebase. For example, here in actuator.cpp write():

return_type Actuator::write()
{
  return_type result = return_type::ERROR;

Compare to the ROS1 ros_control codebase where a lot of effort went into avoiding this practice. For example, here's the equivalent of write():

void setCommand(double command) {assert(cmd_); *cmd_ = command;}

Are we really trusting compilers so much these days or am I missing something?

AndyZe avatar Mar 08 '22 22:03 AndyZe

Here's an even worse example of 3 messages being declared and even resized inside update(). Pointed out by @jbeck28

https://github.com/ros-controls/ros2_controllers/blob/b9ccf04aa95203e37077f297ddcf9f933ef464fa/joint_trajectory_controller/src/joint_trajectory_controller.cpp#L144

  JointTrajectoryPoint state_current, state_desired, state_error;
  const auto joint_num = joint_names_.size();
  resize_joint_trajectory_point(state_current, joint_num);

AndyZe avatar Mar 08 '22 22:03 AndyZe

Anything which goes on the stack isn't really a problem, as it can be allocated deterministically (it's essentially O(1), as it's simple pointer arithmetic).

Memory allocation from a heap is something which is not inherently deterministic (at least not with the default memory allocators), and that's what you should avoid when writing code which should be deterministic.

If any of the classes pointed out have member variables which (might) allocate on the heap (such as std::string and any of the container classes), that might be a problem and would have to be audited.

Alternatively, a deterministic memory allocator could be used (such as TLSF).

But even with that in place, the initialisation of those classes must be deterministic, otherwise having a deterministic allocator will not be sufficient.


Edit: there's quite a bit of logging and use of std::make_shared(..) as well in code paths which should be deterministic. That's also a problem.

gavanderhoorn avatar Mar 09 '22 08:03 gavanderhoorn

We should agree on the best approach/rules in this issue and open a bunch of good-first-issues to clean the code base completely. It is true that some places need a lot of house-keeping.

destogl avatar Mar 10 '22 15:03 destogl

I'm planning to start working on this issue of resizing variables in the joint trajectory controller update(). The only two strategies I'm aware of are:

  • Pre-allocate a large-ish amount of memory ahead of time, then only use what we need
  • Use a deterministic memory allocator (TLSF) as mentioned by gvanderhoorn

Any preferences or other ideas?

AndyZe avatar Apr 26 '22 13:04 AndyZe

I think I'll preallocate based on joint_names_.size(). (that's an upper limit on how large the command can be)

It's already being done that way in some other places :+1:

AndyZe avatar Apr 26 '22 13:04 AndyZe