Velocity Smoother

Source code on Github.

The nav2_velocity_smoother is a package containing a lifecycle-component node for smoothing velocities sent by Nav2 to robot controllers. The aim of this package is to implement velocity, acceleration, and deadband smoothing from Nav2 to reduce wear-and-tear on robot motors and hardware controllers by smoothing out the accelerations/jerky movements that might be present with some local trajectory planners' control efforts. It can also interpolate velocity commands at higher rates than the controller server publishes.

See the package's README for more information.

Velocity Smoother Parameters

`use_realtime_priority`

Type: bool Default: false: Adds soft real-time prioritization to the controller server to better ensure resources to time sensitive portions of the codebase. This will set the controller's execution thread to a higher priority than the rest of the system (90) to meet scheduling deadlines to have less missed loop rates. To use this feature, you use set the following inside of /etc/security/limits.conf to give userspace access to elevated prioritization permissions: <username> soft rtprio 99 <username> hard rtprio 99

`smoothing_frequency`

Type: double Default: 20.0: Frequency (Hz) to use the last received velocity command to smooth by velocity, acceleration, and deadband constraints. If set approximately to the rate of your local trajectory planner, it should smooth by acceleration constraints velocity commands. If set much higher, it will interpolate and provide a smooth set of commands to the hardware controller.

`scale_velocities`

Type: bool Default: false: Whether or not to adjust other components of velocity proportionally to a component's required changes due to acceleration limits. This will try to adjust all components to follow the same direction, but still enforces acceleration limits to guarantee compliance, even if it means deviating off commanded trajectory slightly.

`feedback`

Type: string Default: "OPEN_LOOP": Type of feedback to use for the current state of the robot's velocity. In "OPEN_LOOP", it will use the last commanded velocity as the next iteration's current velocity. When acceleration limits are set appropriately, this is a good assumption. In "CLOSED_LOOP", it will use the odometry from the odom topic to estimate the robot's current speed. In closed loop mode, it is important that the odometry is high rate and low latency, relative to the smoothing frequency.

`max_velocity`

Type: vector<double> Default: [0.5, 0.0, 2.5]: Maximum velocities (m/s) in [x, y, theta] axes or [x, y, z, roll, pitch, yaw] for full 6-DoF support.

`min_velocity`

Type: vector<double> Default: [-0.5, 0.0, -2.5]

Minimum velocities (m/s) in [x, y, theta] axes or [x, y, z, roll, pitch, yaw] for full 6-DoF support. This is signed and thus must be negative to reverse.

Note: Rotational velocities negative direction is a right-hand turn, so this should always be negative regardless of reversing preference.

`deadband_velocity`

Type: vector<double> Default: [0.0, 0.0, 0.0]: Minimum velocities (m/s) in [x, y, theta] axes or [x, y, z, roll, pitch, yaw] for full 6-DoF support to send to the robot hardware controllers, to prevent small commands from damaging hardware controllers if that speed cannot be achieved due to stall torque.

`velocity_timeout`

Type: double Default: 1.0: Timeout (s) after which the velocity smoother will send a zero-ed out Twist command and stop publishing.

`max_accel`

Type: vector<double> Default: [2.5, 0.0, 3.2]: Maximum acceleration to apply to each axis [x, y, theta] or [x, y, z, roll, pitch, yaw] for full 6-DoF support.

`max_decel`

Type: vector<double> Default: [-2.5, 0.0, -3.2]: Minimum acceleration to apply to each axis [x, y, theta] or [x, y, z, roll, pitch, yaw] for full 6-DoF support. This is signed and thus these should generally all be negative.

`odom_topic`

Type: string Default: "odom": Topic to find robot odometry, if in "CLOSED_LOOP" operational mode.

`odom_duration`

Type: double Default: 0.1: Time (s) to buffer odometry commands to estimate the robot speed, if in "CLOSED_LOOP" operational mode.

`enable_stamped_cmd_vel`

Type: bool Default: true: Whether to use geometry_msgs::msg::Twist or geometry_msgs::msg::TwistStamped velocity data. true uses TwistStamped, false uses Twist.

`bond_heartbeat_period`

Type: double Default: 0.25: The lifecycle node bond mechanism publishing period (on the /bond topic). Disabled if inferior or equal to 0.0.

`allow_parameter_qos_overrides`

Type: bool Default: true: Whether to allow QoS profiles to be overwritten with parameterized values.

Example

velocity_smoother:
  ros__parameters:
    smoothing_frequency: 20.0
    scale_velocities: false
    feedback: "OPEN_LOOP"
    max_velocity: [0.5, 0.0, 2.5]
    min_velocity: [-0.5, 0.0, -2.5]
    deadband_velocity: [0.0, 0.0, 0.0]
    velocity_timeout: 1.0
    max_accel: [2.5, 0.0, 3.2]
    max_decel: [-2.5, 0.0, -3.2]
    odom_topic: "odom"
    odom_duration: 0.1
    use_realtime_priority: false
    enable_stamped_cmd_vel: false