From adc7fb266db7b396e0f749dd22b82ba100761c11 Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Thu, 6 Feb 2025 15:13:45 +0100 Subject: [PATCH 1/2] fix memory dereferencing bug in DiffDriveController::reset_buffers() --- diff_drive_controller/src/diff_drive_controller.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 7356d80219..c4ba9a71cd 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -618,7 +618,9 @@ bool DiffDriveController::reset() void DiffDriveController::reset_buffers() { - reference_interfaces_ = std::vector(2, std::numeric_limits::quiet_NaN()); + std::fill( + reference_interfaces_.begin(), reference_interfaces_.end(), + std::numeric_limits::quiet_NaN()); // Empty out the old queue. Fill with zeros (not NaN) to catch early accelerations. std::queue> empty; std::swap(previous_two_commands_, empty); From 1a49b284bf7ef5db033206a4c4c15018a864427a Mon Sep 17 00:00:00 2001 From: Thibault Poignonec Date: Thu, 6 Feb 2025 15:37:28 +0100 Subject: [PATCH 2/2] move memory allocation (i.e., resize) to on_configure() --- diff_drive_controller/src/diff_drive_controller.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index c4ba9a71cd..3156d0d80f 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/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::quiet_NaN()); + // TODO(christophfroehlich) remove deprecated parameters // START DEPRECATED if (!params_.linear.x.has_velocity_limits) @@ -716,10 +720,8 @@ bool DiffDriveController::on_set_chained_mode(bool chained_mode) std::vector DiffDriveController::on_export_reference_interfaces() { - const int nr_ref_itfs = 2; - reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); std::vector 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,