Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Nav2 Velocity Smoother #2964

Merged
merged 17 commits into from
Jun 13, 2022
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 21 additions & 3 deletions nav2_bringup/launch/navigation_launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ def generate_launch_description():
'planner_server',
'behavior_server',
'bt_navigator',
'waypoint_follower']
'waypoint_follower',
'velocity_smoother']

# Map fully qualified names to relative ones so the node's namespace can be prepended.
# In case of the transforms (tf), currently, there doesn't seem to be a better alternative
Expand Down Expand Up @@ -109,7 +110,7 @@ def generate_launch_description():
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
remappings=remappings),
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
Node(
package='nav2_smoother',
executable='smoother_server',
Expand Down Expand Up @@ -155,6 +156,16 @@ def generate_launch_description():
respawn_delay=2.0,
parameters=[configured_params],
remappings=remappings),
Node(
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
package='nav2_velocity_smoother',
executable='velocity_smoother',
name='velocity_smoother',
output='screen',
respawn=use_respawn,
respawn_delay=2.0,
parameters=[configured_params],
remappings=remappings +
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
Node(
package='nav2_lifecycle_manager',
executable='lifecycle_manager',
Expand All @@ -175,7 +186,7 @@ def generate_launch_description():
plugin='nav2_controller::ControllerServer',
name='controller_server',
parameters=[configured_params],
remappings=remappings),
remappings=remappings + [('cmd_vel', 'cmd_vel_nav')]),
ComposableNode(
package='nav2_smoother',
plugin='nav2_smoother::SmootherServer',
Expand Down Expand Up @@ -206,6 +217,13 @@ def generate_launch_description():
name='waypoint_follower',
parameters=[configured_params],
remappings=remappings),
ComposableNode(
package='nav2_velocity_smoother',
plugin='nav2_velocity_smoother::VelocitySmoother',
name='velocity_smoother',
parameters=[configured_params],
remappings=remappings +
[('cmd_vel', 'cmd_vel_nav'), ('cmd_vel_smoothed', 'cmd_vel')]),
ComposableNode(
package='nav2_lifecycle_manager',
plugin='nav2_lifecycle_manager::LifecycleManager',
Expand Down
16 changes: 16 additions & 0 deletions nav2_bringup/params/nav2_params.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -339,10 +339,26 @@ robot_state_publisher:

waypoint_follower:
ros__parameters:
use_sim_time: True
loop_rate: 20
stop_on_failure: false
waypoint_task_executor_plugin: "wait_at_waypoint"
wait_at_waypoint:
plugin: "nav2_waypoint_follower::WaitAtWaypoint"
enabled: True
waypoint_pause_duration: 200

velocity_smoother:
ros__parameters:
use_sim_time: True
smoothing_frequency: 20.0
scale_velocities: False
feedback: "OPEN_LOOP"
max_velocity: [0.26, 0.0, 1.0]
min_velocity: [-0.26, 0.0, -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
deadband_velocity: [0.0, 0.0, 0.0]
velocity_timeout: 1.0
84 changes: 84 additions & 0 deletions nav2_velocity_smoother/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,84 @@
cmake_minimum_required(VERSION 3.5)
project(nav2_velocity_smoother)

find_package(ament_cmake REQUIRED)
find_package(nav2_common REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_components REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(nav2_util REQUIRED)

if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 17)
endif()

nav2_package()

include_directories(
include
)

set(executable_name velocity_smoother)
set(library_name ${executable_name}_core)

set(dependencies
rclcpp
rclcpp_components
geometry_msgs
nav2_util
)

# Main library
add_library(${library_name} SHARED
src/velocity_smoother.cpp
)
ament_target_dependencies(${library_name}
${dependencies}
)

# Main executable
add_executable(${executable_name}
src/main.cpp
)
ament_target_dependencies(${executable_name}
${dependencies}
)
target_link_libraries(${executable_name} ${library_name})

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
set(ament_cmake_copyright_FOUND TRUE)
ament_lint_auto_find_test_dependencies()

find_package(ament_cmake_gtest REQUIRED)
endif()

rclcpp_components_register_nodes(${library_name} "nav2_velocity_smoother::VelocitySmoother")

install(
TARGETS ${library_name}
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)

install(TARGETS ${executable_name}
RUNTIME DESTINATION lib/${PROJECT_NAME}
)

install(DIRECTORY include/
DESTINATION include/
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
find_package(ament_cmake_gtest REQUIRED)
add_subdirectory(test)
endif()

ament_export_include_directories(include)
ament_export_libraries(${library_name})
ament_export_dependencies(${dependencies})
ament_package()
87 changes: 87 additions & 0 deletions nav2_velocity_smoother/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,87 @@
# Velocity Smoother

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 supports differential drive and omnidirectional robot platforms primarily, but is applicable to ackermann as well with some intepretations of ``Twist``. It was built by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/) while at [Samsung Research](https://www.sra.samsung.com/).

See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-velocity-smoother.html) for additional parameter descriptions.

## Features

This package was created to do the following:

- Limit velocity commands by kinematic constraints, including velocity and acceleration
- Limit velocities based on deadband regions
- Stop sending velocities after a given timeout duration of no new commands (due to stopped navigation)
- Send a zero-velocity command at velocity timeout to stop the robot, in case not properly handled
- Support Omni and differential drive robots (e.g. X, Y, Theta)
- Smooth velocities proportionally in the same direction as commanded, whenever possible within kinematic limits
- Provide open loop and closed loop options
- Component nodes for use in single-process systems and stand-alone node format
- Dynamically reconfigurable parameters

## Design

This is a lifecycle-component node, using the lifecycle manager for state management and composition for process management.
It is designed to take in a command from Nav2's controller server and smooth it for use on robot hardware controllers.
Thusly, it takes in a command via the `cmd_vel` topic and produces a smoothed output on `smoothed_cmd_vel`.

The node is designed on a regular timer running at a configurable rate.
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
This is in contrast to simply computing a smoothed velocity command in the callback of each `cmd_vel` input from Nav2.
This allows us to interpolate commands at a higher frequency than Nav2's local trajectory planners can provide.
For example, if a local trajectory planner is running at 20hz, the velocity smoother can run at 100hz to provide approximately 5 messages to a robot controller which will be smoothed by kinematic limits at each timestep.
This provides a more regular stream of commands to a robot base and interpolates commands between the current velocity and the desired velocity more finely for smoother acceleration / motion profiles.
While this is not required, it is a nice design feature.
It is possible to also simply run the smoother at `cmd_vel` rate to smooth velocities alone without interpolation.

There are two primary operation modes: open and closed loop.
In open-loop, the node assumes that the robot was able to achieve the velocity send to it in the last command which was smoothed (which should be a good assumption if acceleration limits are set properly).
This is useful when robot odometry is not particularly accurate or has significant latency relative to `smoothing_frequency` so there isn't a delay in the feedback loop.
In closed-loop, the node will read from the odometry topic and apply a smoother over it to obtain the robot's current speed.
This will be used to determine the robot's current velocity and therefore achivable velocity targets by the velocity, acceleration, and deadband constraints using live data.

## Parameters

See inline description of parameters in the `VelocitySmoother`.

```
velocity_smoother:
ros__parameters:
smoothing_frequency: 20.0 # Rate to run smoother
scale_velocities: false # scale velocities proportionally if any axis is outside of acceleration range to follow same vector, if possible
feedback: "OPEN_LOOP" # Type of feedback for current speed. Open loop uses the last smoothed output. Closed loop uses robot odometry
max_velocity: [0.5, 0.0, 2.5] # Maximum velocities, ordered [Vx, Vy, Vw]
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
min_velocity: [-0.5, 0.0, -2.5] # Minimum velocities, ordered [Vx, Vy, Vw]
deadband_velocity: [0.0, 0.0, 0.0] # A deadband of velocities below which they should be zero-ed out for sending to the robot base controller, ordered [Vx, Vy, Vw]
velocity_timeout: 1.0 # Time (s) after which if no new velocity commands are received to zero out and stop
max_accel: [2.5, 0.0, 3.2] # Maximum acceleration, ordered [Ax, Ay, Aw]
max_decel: [-2.5, 0.0, -3.2] # Maximum deceleration, ordered [Ax, Ay, Aw]
odom_topic: "odom" # Topic of odometry to use for estimating current velocities
odom_duration: 0.1 # Period of time (s) to sample odometry information in for velocity estimation
```

## Topics

| Topic | Type | Use |
|------------------|-------------------------|-------------------------------|
| smoothed_cmd_vel | geometry_msgs/Twist | Publish smoothed velocities |
| cmd_vel | geometry_msgs/Twist | Subscribe to input velocities |


## Install

```
sudo apt-get install ros-<ros2-distro>-nav2-velocity-smoother
```

## Etc (Important Side Notes)

Typically: if you have low rate odometry, you should use open-loop mode or set the smoothing frequency relatively similar to that of your `cmd_vel` topic. If you have high rate odometry, you can use closed-loop mode with a higher smoothing frequency since you'll have more up to date information to smooth based off of. Do not exceed the smoothing frequency to your odometry frequency in closed loop mode. Also consider the ``odom_duration`` to use relative to your odometry publication rate and noise characteristics.
If the smoothing frequency out paces odometry or poorly selected ``odom_duration``s are used, the robot can oscillate and/or accelerate slowly due to latency in closed loop mode.

When in doubt, open-loop is a reasonable choice for most users.

The minimum and maximum velocities for rotation (e.g. ``Vw``) represent left and right turns. While we make it possible to specify these separately, most users would be wise to set these values the same (but signed) for rotation. Additionally, the parameters are signed, so it is important to specify maximum deceleration with negative signs to represent deceleration. Minimum velocities with negatives when moving backward, so backward movement can be restricted by setting this to ``0``.

Deadband velocities are minimum thresholds, below which we set its value to `0`. This can be useful when your robot's breaking torque from stand still is non-trivial so sending very small values will pull high amounts of current.
Loading