Fix reference in chained diff drive controller (ros-controls#1529)

Author: tpoignonec

diff_drive_controller/src/diff_drive_controller.cpp

@@ -335,6 +335,10 @@ controller_interface::CallbackReturn DiffDriveController::on_configure(
   cmd_vel_timeout_ = rclcpp::Duration::from_seconds(params_.cmd_vel_timeout);
   publish_limited_velocity_ = params_.publish_limited_velocity;
 
+  // Allocate reference interfaces if needed
+  const int nr_ref_itfs = 2;
+  reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits<double>::quiet_NaN());
+
   // TODO(christophfroehlich) remove deprecated parameters
   // START DEPRECATED
   if (!params_.linear.x.has_velocity_limits)
@@ -618,7 +622,9 @@ bool DiffDriveController::reset()
 
 void DiffDriveController::reset_buffers()
 {
-  reference_interfaces_ = std::vector<double>(2, std::numeric_limits<double>::quiet_NaN());
+  std::fill(
+    reference_interfaces_.begin(), reference_interfaces_.end(),
+    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);
@@ -714,10 +720,8 @@ bool DiffDriveController::on_set_chained_mode(bool chained_mode)
 std::vector<hardware_interface::CommandInterface>
 DiffDriveController::on_export_reference_interfaces()
 {
-  const int nr_ref_itfs = 2;
-  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.reserve(reference_interfaces_.size());
 
   reference_interfaces.push_back(hardware_interface::CommandInterface(
     get_node()->get_name(), std::string("linear/") + hardware_interface::HW_IF_VELOCITY,