diff --git a/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp new file mode 100644 index 0000000000..24a9c66647 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/speed_limiter.hpp @@ -0,0 +1,106 @@ +// Copyright 2020 PAL Robotics S.L. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* + * Author: Enrique Fernández + */ + +#ifndef DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ +#define DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_ + +// TODO(christophfroehlich) remove this file after the next release + +#include "control_toolbox/speed_limiter.hpp" + +namespace diff_drive_controller +{ +class [[deprecated("Use control_toolbox/speed_limiter instead")]] SpeedLimiter +{ +public: + /** + * \brief Constructor + * \param [in] has_velocity_limits if true, applies velocity limits + * \param [in] has_acceleration_limits if true, applies acceleration limits + * \param [in] has_jerk_limits if true, applies jerk limits + * \param [in] min_velocity Minimum velocity [m/s], usually <= 0 + * \param [in] max_velocity Maximum velocity [m/s], usually >= 0 + * \param [in] min_acceleration Minimum acceleration [m/s^2], usually <= 0 + * \param [in] max_acceleration Maximum acceleration [m/s^2], usually >= 0 + * \param [in] min_jerk Minimum jerk [m/s^3], usually <= 0 + * \param [in] max_jerk Maximum jerk [m/s^3], usually >= 0 + */ + SpeedLimiter( + bool has_velocity_limits = false, bool has_acceleration_limits = false, + bool has_jerk_limits = false, double min_velocity = NAN, double max_velocity = NAN, + double min_acceleration = NAN, double max_acceleration = NAN, double min_jerk = NAN, + double max_jerk = NAN) + : speed_limiter_( + has_velocity_limits, has_acceleration_limits, has_jerk_limits, min_velocity, max_velocity, + min_acceleration, max_acceleration, min_jerk, max_jerk) + { + } + + /** + * \brief Limit the velocity and acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit(v, v0, v1, dt); + } + + /** + * \brief Limit the velocity + * \param [in, out] v Velocity [m/s] + * \return Limiting factor (1.0 if none) + */ + double limit_velocity(double & v) { return speed_limiter_.limit_velocity(v); } + + /** + * \brief Limit the acceleration + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + */ + double limit_acceleration(double & v, double v0, double dt) + { + return speed_limiter_.limit_acceleration(v, v0, dt); + } + + /** + * \brief Limit the jerk + * \param [in, out] v Velocity [m/s] + * \param [in] v0 Previous velocity to v [m/s] + * \param [in] v1 Previous velocity to v0 [m/s] + * \param [in] dt Time step [s] + * \return Limiting factor (1.0 if none) + * \see http://en.wikipedia.org/wiki/Jerk_%28physics%29#Motion_control + */ + double limit_jerk(double & v, double v0, double v1, double dt) + { + return speed_limiter_.limit_jerk(v, v0, v1, dt); + } + +private: + control_toolbox::SpeedLimiter speed_limiter_; // Instance of the new SpeedLimiter +}; + +} // namespace diff_drive_controller + +#endif // DIFF_DRIVE_CONTROLLER__SPEED_LIMITER_HPP_