Fix reference in chained diff drive controller

[tpoignonec]

Hi,

There seems to be an issue with the new chained diff drive implementation from PR #1485.

I initially had an issue when chaining this (malloc(): unaligned tcache chunk detected after loading the upstream controller). However, while hunting down the error, I found that the cleanup phase is a more easily replicable scenario to get the same problem.
The bug can be reproduced following these steps), but it boils down to dereferencing of the reference_interfaces_ data in DiffDriveController::reset_buffers():

ros2_controllers/diff_drive_controller/src/diff_drive_controller.cpp

Lines 619 to 627 in 5486fcd

	void DiffDriveController::reset_buffers()
	{
	reference_interfaces_ = std::vector<double>(2, std::numeric_limits<double>::quiet_NaN());
	// Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations.
	std::queue<std::array<double, 2>> empty;
	std::swap(previous_two_commands_, empty);
	previous_two_commands_.push({{0.0, 0.0}});
	previous_two_commands_.push({{0.0, 0.0}});

This method is called at different points in the controller: on_configure(), on_error(), and on_cleanup. Each time, the previously exported reference interface is invalidated.

The reset_buffer method should not assign a new vector, at it invalidates the references.
As far as I can see, there are only two options:

// Keeps size, just updates values
reference_interfaces_.assign(
  reference_interfaces_.size(),
  std::numeric_limits<double>::quiet_NaN()); 

// or just use std::fill
std::fill(
  reference_interfaces_.begin(),
  reference_interfaces_.end(),
  std::numeric_limits<double>::quiet_NaN());

In the PR, I went with the std::fill(...) option, not sure if there is any argument for one against the other...

Additionally, I moved the resize to the on_configure() method to make sure the memory is allocated right away. Otherwise, some tests break (reference_interfaces_ not allocated at first use).
Are there any guidelines about that?
That seems to be the way it is done in ros2_control_demos (example_12), but not in the steering_controllers_library.cpp:

ros2_controllers/steering_controllers_library/src/steering_controllers_library.cpp

Lines 311 to 328 in 5486fcd

	std::vector<hardware_interface::CommandInterface>
	SteeringControllersLibrary::on_export_reference_interfaces()
	{
	reference_interfaces_.resize(nr_ref_itfs_, std::numeric_limits<double>::quiet_NaN());
	std::vector<hardware_interface::CommandInterface> reference_interfaces;
	reference_interfaces.reserve(nr_ref_itfs_);
	reference_interfaces.push_back(hardware_interface::CommandInterface(
	get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,
	&reference_interfaces_[0]));
	reference_interfaces.push_back(hardware_interface::CommandInterface(
	get_node()->get_name(), std::string("angular/") + hardware_interface::HW_IF_VELOCITY,
	&reference_interfaces_[1]));
	return reference_interfaces;
	}

[saikishor]

Additionally, I moved the resize to the on_configure() method to make sure the memory is allocated right away. Otherwise, some tests break (reference_interfaces_ not allocated at first use).
Are there any guidelines about that?

Yes, it should be either done in on_configure or the export interfaces methods. The method on_export_reference_interfaces is called while configuring the controller, so the allocation should happen in the non-RT loop.

[saikishor]

Thanks for fixing it.
LGTM

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 84.26%. Comparing base (28845e6) to head (1a49b28).
Report is 2 commits behind head on master.

Additional details and impacted files

@@           Coverage Diff           @@
##           master    #1529   +/-   ##
=======================================
  Coverage   84.26%   84.26%           
=======================================
  Files         123      123           
  Lines       11358    11360    +2     
  Branches      961      962    +1     
=======================================
+ Hits         9571     9573    +2     
  Misses       1471     1471           
  Partials      316      316           

Flag	Coverage Δ
unittests	84.26% <100.00%> (+<0.01%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
...iff_drive_controller/src/diff_drive_controller.cpp	72.80% <100.00%> (+0.07%)	⬆️

... and 2 files with indirect coverage changes