diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 92fa27a7be..2cdd22ffb1 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -344,17 +344,32 @@ controller_interface::CallbackReturn DiffDriveController::on_configure( const int nr_ref_itfs = 2; reference_interfaces_.resize(nr_ref_itfs, std::numeric_limits::quiet_NaN()); - limiter_linear_ = std::make_unique( - params_.linear.x.min_velocity, params_.linear.x.max_velocity, - params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, - params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, - params_.linear.x.min_jerk, params_.linear.x.max_jerk); - - limiter_angular_ = std::make_unique( - params_.angular.z.min_velocity, params_.angular.z.max_velocity, - params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, - params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, - params_.angular.z.min_jerk, params_.angular.z.max_jerk); + try + { + limiter_linear_ = std::make_unique( + params_.linear.x.min_velocity, params_.linear.x.max_velocity, + params_.linear.x.max_acceleration_reverse, params_.linear.x.max_acceleration, + params_.linear.x.max_deceleration, params_.linear.x.max_deceleration_reverse, + params_.linear.x.min_jerk, params_.linear.x.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(logger, "Failed to configure linear speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } + try + { + limiter_angular_ = std::make_unique( + params_.angular.z.min_velocity, params_.angular.z.max_velocity, + params_.angular.z.max_acceleration_reverse, params_.angular.z.max_acceleration, + params_.angular.z.max_deceleration, params_.angular.z.max_deceleration_reverse, + params_.angular.z.min_jerk, params_.angular.z.max_jerk); + } + catch (const std::invalid_argument & e) + { + RCLCPP_ERROR(logger, "Failed to configure angular speed limiter: %s", e.what()); + return controller_interface::CallbackReturn::ERROR; + } if (!reset()) {