add humble-navigation2
This commit is contained in:
X-lanni
@@ -0,0 +1,129 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(nav2_mppi_controller)
|
||||
|
||||
add_definitions(-DXTENSOR_ENABLE_XSIMD)
|
||||
add_definitions(-DXTENSOR_USE_XSIMD)
|
||||
|
||||
set(XTENSOR_USE_TBB 0)
|
||||
set(XTENSOR_USE_OPENMP 0)
|
||||
set(XTENSOR_USE_XSIMD 1)
|
||||
|
||||
# set(XTENSOR_DEFAULT_LAYOUT column_major) # row_major, column_major
|
||||
# set(XTENSOR_DEFAULT_TRAVERSAL row_major) # row_major, column_major
|
||||
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(xtensor REQUIRED)
|
||||
find_package(xsimd REQUIRED)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
)
|
||||
|
||||
set(dependencies_pkgs
|
||||
rclcpp
|
||||
nav2_common
|
||||
pluginlib
|
||||
tf2
|
||||
geometry_msgs
|
||||
visualization_msgs
|
||||
nav_msgs
|
||||
nav2_core
|
||||
nav2_costmap_2d
|
||||
nav2_util
|
||||
tf2_geometry_msgs
|
||||
tf2_eigen
|
||||
tf2_ros
|
||||
)
|
||||
|
||||
foreach(pkg IN LISTS dependencies_pkgs)
|
||||
find_package(${pkg} REQUIRED)
|
||||
endforeach()
|
||||
|
||||
nav2_package()
|
||||
|
||||
include(CheckCXXCompilerFlag)
|
||||
|
||||
check_cxx_compiler_flag("-mno-avx512f" COMPILER_SUPPORTS_AVX512)
|
||||
check_cxx_compiler_flag("-msse4.2" COMPILER_SUPPORTS_SSE4)
|
||||
check_cxx_compiler_flag("-mavx2" COMPILER_SUPPORTS_AVX2)
|
||||
check_cxx_compiler_flag("-mfma" COMPILER_SUPPORTS_FMA)
|
||||
|
||||
if(COMPILER_SUPPORTS_AVX512)
|
||||
add_compile_options(-mno-avx512f)
|
||||
endif()
|
||||
|
||||
if(COMPILER_SUPPORTS_SSE4)
|
||||
add_compile_options(-msse4.2)
|
||||
endif()
|
||||
|
||||
if(COMPILER_SUPPORTS_AVX2)
|
||||
add_compile_options(-mavx2)
|
||||
endif()
|
||||
|
||||
if(COMPILER_SUPPORTS_FMA)
|
||||
add_compile_options(-mfma)
|
||||
endif()
|
||||
|
||||
# If building one the same hardware to be deployed on, try `-march=native`!
|
||||
add_compile_options(-O3 -finline-limit=10000000 -ffp-contract=fast -ffast-math -mtune=generic)
|
||||
|
||||
add_library(mppi_controller SHARED
|
||||
src/controller.cpp
|
||||
src/optimizer.cpp
|
||||
src/critic_manager.cpp
|
||||
src/trajectory_visualizer.cpp
|
||||
src/path_handler.cpp
|
||||
src/parameters_handler.cpp
|
||||
src/noise_generator.cpp
|
||||
)
|
||||
|
||||
add_library(mppi_critics SHARED
|
||||
src/critics/obstacles_critic.cpp
|
||||
src/critics/cost_critic.cpp
|
||||
src/critics/goal_critic.cpp
|
||||
src/critics/goal_angle_critic.cpp
|
||||
src/critics/path_align_critic.cpp
|
||||
src/critics/path_align_legacy_critic.cpp
|
||||
src/critics/path_follow_critic.cpp
|
||||
src/critics/path_angle_critic.cpp
|
||||
src/critics/prefer_forward_critic.cpp
|
||||
src/critics/twirling_critic.cpp
|
||||
src/critics/constraint_critic.cpp
|
||||
src/critics/velocity_deadband_critic.cpp
|
||||
)
|
||||
|
||||
set(libraries mppi_controller mppi_critics)
|
||||
|
||||
foreach(lib IN LISTS libraries)
|
||||
target_compile_options(${lib} PUBLIC -fconcepts)
|
||||
target_include_directories(${lib} PUBLIC ${xsimd_INCLUDE_DIRS}) # ${OpenMP_INCLUDE_DIRS}
|
||||
target_link_libraries(${lib} xtensor xtensor::optimize xtensor::use_xsimd)
|
||||
ament_target_dependencies(${lib} ${dependencies_pkgs})
|
||||
endforeach()
|
||||
|
||||
install(TARGETS mppi_controller mppi_critics
|
||||
ARCHIVE DESTINATION lib
|
||||
LIBRARY DESTINATION lib
|
||||
RUNTIME DESTINATION bin
|
||||
)
|
||||
|
||||
install(DIRECTORY include/
|
||||
DESTINATION include/
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
add_subdirectory(test)
|
||||
# add_subdirectory(benchmark)
|
||||
endif()
|
||||
|
||||
ament_export_libraries(${libraries})
|
||||
ament_export_dependencies(${dependencies_pkgs})
|
||||
ament_export_include_directories(include)
|
||||
pluginlib_export_plugin_description_file(nav2_core mppic.xml)
|
||||
pluginlib_export_plugin_description_file(nav2_mppi_controller critics.xml)
|
||||
|
||||
ament_package()
|
||||
@@ -0,0 +1,23 @@
|
||||
MIT License
|
||||
|
||||
Copyright (c) 2021-2022 Fast Sense Studio
|
||||
Copyright (c) 2022-2023 Samsung Research America
|
||||
Copyright (c) 2023 Open Navigation LLC
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
of this software and associated documentation files (the "Software"), to deal
|
||||
in the Software without restriction, including without limitation the rights
|
||||
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
copies of the Software, and to permit persons to whom the Software is
|
||||
furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included in all
|
||||
copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
||||
SOFTWARE.
|
||||
@@ -0,0 +1,323 @@
|
||||
# Model Predictive Path Integral Controller
|
||||
|
||||
|
||||
|
||||
## Overview
|
||||
|
||||
This is a predictive controller (local trajectory planner) that implements the [Model Predictive Path Integral (MPPI)](https://ieeexplore.ieee.org/document/7487277) algorithm to track a path with adaptive collision avoidance. It contains plugin-based critic functions to impact the behavior of the algorithm. It was created by [Aleksei Budyakov](https://www.linkedin.com/in/aleksei-budyakov-334889224/) and adapted & developed for Nav2 by [Steve Macenski](https://www.linkedin.com/in/steve-macenski-41a985101/).
|
||||
|
||||
This plugin implements the ``nav2_core::Controller`` interface allowing it to be used across the navigation stack as a local trajectory planner in the controller server's action server (``controller_server``).
|
||||
|
||||
This controller is measured to run at 50+ Hz on a modest Intel processor (4th gen i5). See its Configuration Guide Page for additional parameter descriptions.
|
||||
|
||||
It works currently with Differential, Omnidirectional, and Ackermann robots.
|
||||
|
||||
## MPPI Description
|
||||
|
||||
The MPPI algorithm is an MPC variant that finds a control velocity for the robot using an iterative approach. Using the previous time step's best control solution and the robot's current state, a set of randomly sampled perturbations from a Gaussian distribution are applied. These noised controls are forward simulated to generate a set of trajectories within the robot's motion model.
|
||||
|
||||
Next, these trajectories are scored using a set of plugin-based critic functions to find the best trajectory in the batch. The output scores are used to set the best control with a soft max function.
|
||||
|
||||
This process is then repeated a number of times and returns a converged solution. This solution is then used as the basis of the next time step's initial control.
|
||||
|
||||
## Features
|
||||
|
||||
- Predictive MPC trajectory planner
|
||||
- Utilizes plugin-based critics which can be swapped out, tuned, or replaced easily by the user
|
||||
- Highly optimized CPU-only performance using vectorization and tensor operations
|
||||
- Supports a number of common motion models, including Ackermann, Differential-Drive, and Omni-directional
|
||||
- Includes fallback mechanisms to handle soft-failures before escalating to recovery behaviors
|
||||
- High-quality code implementation with Doxygen, high unit test coverage, documentation, and parameter guide
|
||||
- Easily extensible to support modern research variants of MPPI
|
||||
- Comes pre-tuned for good out-of-the-box behavior
|
||||
|
||||
## Configuration
|
||||
|
||||
### Controller
|
||||
| Parameter | Type | Definition |
|
||||
| --------------------- | ------ | -------------------------------------------------------------------------------------------------------- |
|
||||
| motion_model | string | Default: DiffDrive. Type of model [DiffDrive, Omni, Ackermann]. |
|
||||
| critics | string | Default: None. Critics (plugins) names |
|
||||
| iteration_count | int | Default 1. Iteration count in MPPI algorithm. Recommend to keep as 1 and prefer more batches. |
|
||||
| batch_size | int | Default 1000. Count of randomly sampled candidate trajectories |
|
||||
| time_steps | int | Default 56. Number of time steps (points) in each sampled trajectory |
|
||||
| model_dt | double | Default: 0.05. Time interval (s) between two sampled points in trajectories. |
|
||||
| vx_std | double | Default 0.2. Sampling standard deviation for VX |
|
||||
| vy_std | double | Default 0.2. Sampling standard deviation for VY |
|
||||
| wz_std | double | Default 0.4. Sampling standard deviation for Wz |
|
||||
| vx_max | double | Default 0.5. Max VX (m/s) |
|
||||
| vy_max | double | Default 0.5. Max VY in either direction, if holonomic. (m/s) |
|
||||
| vx_min | double | Default -0.35. Min VX (m/s) |
|
||||
| wz_max | double | Default 1.9. Max WZ (rad/s) |
|
||||
| temperature | double | Default: 0.3. Selectiveness of trajectories by their costs (The closer this value to 0, the "more" we take in consideration controls with less cost), 0 mean use control with best cost, huge value will lead to just taking mean of all trajectories without cost consideration |
|
||||
| gamma | double | Default: 0.015. A trade-off between smoothness (high) and low energy (low). This is a complex parameter that likely won't need to be changed from the default of `0.1` which works well for a broad range of cases. See Section 3D-2 in "Information Theoretic Model Predictive Control: Theory and Applications to Autonomous Driving" for detailed information. |
|
||||
| visualize | bool | Default: false. Publish visualization of trajectories, which can slow down the controller significantly. Use only for debugging. |
|
||||
| retry_attempt_limit | int | Default 1. Number of attempts to find feasible trajectory on failure for soft-resets before reporting failure. |
|
||||
| reset_period | double | Default 1.0. required time of inactivity to reset optimizer (only in Humble due to backport ABI policies) |
|
||||
| regenerate_noises | bool | Default false. Whether to regenerate noises each iteration or use single noise distribution computed on initialization and reset. Practically, this is found to work fine since the trajectories are being sampled stochastically from a normal distribution and reduces compute jittering at run-time due to thread wake-ups to resample normal distribution. |
|
||||
|
||||
#### Trajectory Visualizer
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| trajectory_step | int | Default: 5. The step between trajectories to visualize to downsample candidate trajectory pool. |
|
||||
| time_step | int | Default: 3. The step between points on trajectories to visualize to downsample trajectory density. |
|
||||
|
||||
#### Path Handler
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| max_robot_pose_search_dist | double | Default: Costmap half-size. Max integrated distance ahead of robot pose to search for nearest path point in case of path looping. |
|
||||
| prune_distance | double | Default: 1.5. Distance ahead of nearest point on path to robot to prune path to. |
|
||||
| transform_tolerance | double | Default: 0.1. Time tolerance for data transformations with TF. |
|
||||
| enforce_path_inversion | double | Default: False. If true, it will prune paths containing cusping points for segments changing directions (e.g. path inversions) such that the controller will be forced to change directions at or very near the planner's requested inversion point. This is targeting Smac Planner users with feasible paths who need their robots to switch directions where specifically requested. |
|
||||
| inversion_xy_tolerance | double | Default: 0.2. Cartesian proximity (m) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. |
|
||||
| inversion_yaw_tolerance | double | Default: 0.4. Angular proximity (radians) to path inversion point to be considered "achieved" to pass on the rest of the path after path inversion. 0.4 rad = 23 deg. |
|
||||
|
||||
#### Ackermann Motion Model
|
||||
| Parameter | Type | Definition |
|
||||
| -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| min_turning_r | double | minimum turning radius for ackermann motion model |
|
||||
|
||||
#### Constraint Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 4.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term.
|
||||
|
||||
#### Goal Angle Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 3.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| threshold_to_consider | double | Default 0.5. Minimal distance between robot and goal above which angle goal cost considered. |
|
||||
|
||||
#### Goal Critic
|
||||
| Parameter | Type | Definition |
|
||||
| -------------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 5.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| threshold_to_consider | double | Default 1.4. Distance between robot and goal above which goal cost starts being considered |
|
||||
|
||||
|
||||
#### Obstacles Critic
|
||||
|
||||
Uses estimated distances from obstacles using cost and inflation parameters to avoid obstacles
|
||||
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
|
||||
| critical_weight | double | Default 20.0. Weight to apply to critic for near collisions closer than `collision_margin_distance` to prevent near collisions **only** as a method of virtually inflating the footprint. This should not be used to generally influence obstacle avoidance away from critical collisions. |
|
||||
| repulsion_weight | double | Default 1.5. Weight to apply to critic for generally preferring routes in lower cost space. This is separated from the critical term to allow for fine tuning of obstacle behaviors with path alignment for dynamic scenes without impacting actions which may directly lead to near-collisions. This is applied within the `inflation_radius` distance from obstacles. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| collision_cost | double | Default 10000.0. Cost to apply to a true collision in a trajectory. |
|
||||
| collision_margin_distance | double | Default 0.10. Margin distance from collision to apply severe penalty, similar to footprint inflation. Between 0.05-0.2 is reasonable. |
|
||||
| near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.
|
||||
| cost_scaling_factor | double | Default 10.0. Exponential decay factor across inflation radius. This should be the same as for your inflation layer (Humble only)
|
||||
| inflation_radius | double | Default 0.55. Radius to inflate costmap around lethal obstacles. This should be the same as for your inflation layer (Humble only)
|
||||
|
||||
#### Cost Critic
|
||||
|
||||
Uses inflated costmap cost directly to avoid obstacles
|
||||
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| consider_footprint | bool | Default: False. Whether to use point cost (if robot is circular or low compute power) or compute SE2 footprint cost. |
|
||||
| cost_weight | double | Default 3.81. Wight to apply to critic to avoid obstacles. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| collision_cost | double | Default 1000000.0. Cost to apply to a true collision in a trajectory. |
|
||||
| critical_cost | double | Default 300.0. Cost to apply to a pose with any point in in inflated space to prefer distance from obstacles. |
|
||||
| near_goal_distance | double | Default 0.5. Distance near goal to stop applying preferential obstacle term to allow robot to smoothly converge to goal pose in close proximity to obstacles.
|
||||
| inflation_layer_name | string | Default "". Name of the inflation layer. If empty, it uses the last inflation layer in the costmap. If you have multiple inflation layers, you may want to specify the name of the layer to use. |
|
||||
|
||||
#### Path Align Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 10.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path align cost stops being considered |
|
||||
| offset_from_furthest | double | Default 20. Checks that the candidate trajectories are sufficiently far along their way tracking the path to apply the alignment critic. This ensures that path alignment is only considered when actually tracking the path, preventing awkward initialization motions preventing the robot from leaving the path to achieve the appropriate heading. |
|
||||
| trajectory_point_step | double | Default 4. Step of trajectory points to evaluate for path distance to reduce compute time. Between 1-10 is typically reasonable. |
|
||||
| max_path_occupancy_ratio | double | Default 0.07 (7%). Maximum proportion of the path that can be occupied before this critic is not considered to allow the obstacle and path follow critics to avoid obstacles while following the path's intent in presence of dynamic objects in the scene. |
|
||||
| use_path_orientations | bool | Default false. Whether to consider path's orientations in path alignment, which can be useful when paired with feasible smac planners to incentivize directional changes only where/when the smac planner requests them. If you want the robot to deviate and invert directions where the controller sees fit, keep as false. If your plans do not contain orientation information (e.g. navfn), keep as false. |
|
||||
|
||||
Note: There is a "Legacy" version of this critic also available with the same parameters of an old formulation pre-October 2023.
|
||||
|
||||
#### Path Angle Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 2.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which path angle cost stops being considered |
|
||||
| offset_from_furthest | int | Default 4. Number of path points after furthest one any trajectory achieves to compute path angle relative to. |
|
||||
| max_angle_to_furthest | double | Default 1.2. Angular distance between robot and goal above which path angle cost starts being considered |
|
||||
| forward_preference | bool | Default true. Whether or not your robot has a preference for which way is forward in motion. Different from if reversing is generally allowed, but if you robot contains *no* particular preference one way or another. |
|
||||
|
||||
|
||||
#### Path Follow Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 5.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| offset_from_furthest | int | Default 6. Number of path points after furthest one any trajectory achieves to drive path tracking relative to. |
|
||||
| threshold_to_consider | float | Default 1.4. Distance between robot and goal above which path follow cost stops being considered |
|
||||
|
||||
#### Prefer Forward Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 5.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| threshold_to_consider | double | Default 0.5. Distance between robot and goal above which prefer forward cost stops being considered |
|
||||
|
||||
|
||||
#### Twirling Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 10.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
|
||||
|
||||
#### Velocity Deadband Critic
|
||||
| Parameter | Type | Definition |
|
||||
| --------------- | ------ | ----------------------------------------------------------------------------------------------------------- |
|
||||
| cost_weight | double | Default 35.0. Weight to apply to critic term. |
|
||||
| cost_power | int | Default 1. Power order to apply to term. |
|
||||
| deadband_velocities | double[] | Default [0.0, 0.0, 0.0]. The array of deadband velocities [vx, vz, wz]. A zero array indicates that the critic will take no action. |
|
||||
|
||||
|
||||
### XML configuration example
|
||||
```
|
||||
controller_server:
|
||||
ros__parameters:
|
||||
controller_frequency: 30.0
|
||||
FollowPath:
|
||||
plugin: "nav2_mppi_controller::MPPIController"
|
||||
time_steps: 56
|
||||
model_dt: 0.05
|
||||
batch_size: 2000
|
||||
vx_std: 0.2
|
||||
vy_std: 0.2
|
||||
wz_std: 0.4
|
||||
vx_max: 0.5
|
||||
vx_min: -0.35
|
||||
vy_max: 0.5
|
||||
wz_max: 1.9
|
||||
iteration_count: 1
|
||||
prune_distance: 1.7
|
||||
transform_tolerance: 0.1
|
||||
temperature: 0.3
|
||||
gamma: 0.015
|
||||
motion_model: "DiffDrive"
|
||||
visualize: false
|
||||
TrajectoryVisualizer:
|
||||
trajectory_step: 5
|
||||
time_step: 3
|
||||
AckermannConstraints:
|
||||
min_turning_r: 0.2
|
||||
critics: ["ConstraintCritic", "CostCritic", "GoalCritic", "GoalAngleCritic", "PathAlignCritic", "PathFollowCritic", "PathAngleCritic", "PreferForwardCritic"]
|
||||
ConstraintCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 4.0
|
||||
GoalCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
threshold_to_consider: 1.4
|
||||
GoalAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.0
|
||||
threshold_to_consider: 0.5
|
||||
PreferForwardCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
threshold_to_consider: 0.5
|
||||
# ObstaclesCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# repulsion_weight: 1.5
|
||||
# critical_weight: 20.0
|
||||
# consider_footprint: false
|
||||
# collision_cost: 10000.0
|
||||
# collision_margin_distance: 0.1
|
||||
# near_goal_distance: 0.5
|
||||
CostCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 3.81
|
||||
critical_cost: 300.0
|
||||
consider_footprint: true
|
||||
collision_cost: 1000000.0
|
||||
near_goal_distance: 1.0
|
||||
PathAlignCritic:
|
||||
PathAlignCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 14.0
|
||||
max_path_occupancy_ratio: 0.05
|
||||
trajectory_point_step: 3
|
||||
threshold_to_consider: 0.5
|
||||
offset_from_furthest: 20
|
||||
use_path_orientations: false
|
||||
PathFollowCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 5.0
|
||||
offset_from_furthest: 5
|
||||
threshold_to_consider: 1.4
|
||||
PathAngleCritic:
|
||||
enabled: true
|
||||
cost_power: 1
|
||||
cost_weight: 2.0
|
||||
offset_from_furthest: 4
|
||||
threshold_to_consider: 0.5
|
||||
max_angle_to_furthest: 1.0
|
||||
forward_preference: true
|
||||
# VelocityDeadbandCritic:
|
||||
# enabled: true
|
||||
# cost_power: 1
|
||||
# cost_weight: 35.0
|
||||
# deadband_velocities: [0.05, 0.05, 0.05]
|
||||
# TwirlingCritic:
|
||||
# enabled: true
|
||||
# twirling_cost_power: 1
|
||||
# twirling_cost_weight: 10.0
|
||||
```
|
||||
## Topics
|
||||
|
||||
| Topic | Type | Description |
|
||||
|---------------------------|----------------------------------|-----------------------------------------------------------------------|
|
||||
| `trajectories` | `visualization_msgs/MarkerArray` | Randomly generated trajectories, including resulting control sequence |
|
||||
| `transformed_global_plan` | `nav_msgs/Path` | Part of global plan considered by local planner |
|
||||
|
||||
## Notes to Users
|
||||
|
||||
### General Words of Wisdom
|
||||
|
||||
The `model_dt` parameter generally should be set to the duration of your control frequency. So if your control frequency is 20hz, this should be `0.05`. However, you may also set it lower **but not larger**.
|
||||
|
||||
Visualization of the trajectories using `visualize` uses compute resources to back out trajectories for visualization and therefore slows compute time. It is not suggested that this parameter is set to `true` during a deployed use, but is a useful debug instrument while tuning the system, but use sparingly. Visualizing 2000 batches @ 56 points at 30 hz is _a lot_.
|
||||
|
||||
The most common parameters you might want to start off changing are the velocity profiles (`vx_max`, `vx_min`, `wz_max`, and `vy_max` if holonomic) and the `motion_model` to correspond to your vehicle. Its wise to consider the `prune_distance` of the path plan in proportion to your maximum velocity and prediction horizon. The only deeper parameter that will likely need to be adjusted for your particular settings is the Obstacle critics' `repulsion_weight` since the tuning of this is proportional to your inflation layer's radius. Higher radii should correspond to reduced `repulsion_weight` due to the penalty formation (e.g. `inflation_radius - min_dist_to_obstacle`). If this penalty is too high, the robot will slow significantly when entering cost-space from non-cost space or jitter in narrow corridors. It is noteworthy, but likely not necessary to be changed, that the Obstacle critic may use the full footprint information if `consider_footprint = true`, though comes at an increased compute cost.
|
||||
|
||||
If you don't require path following behavior (e.g. just want to follow a goal pose and let the model predictive elements decide the best way to accomplish that), you may easily remove the PathAlign, PathFollow and PathAngle critics.
|
||||
|
||||
By default, the controller is tuned and has the capabilities established in the PathAlign/Obstacle critics to generally follow the path closely when no obstacles prevent it, but able to deviate from the path when blocked. See `PathAlignCritic::score()` for details, but it is disabled when the local path is blocked so the obstacle critic takes over in that state.
|
||||
|
||||
### Prediction Horizon, Costmap Sizing, and Offsets
|
||||
|
||||
As this is a predictive planner, there is some relationship between maximum speed, prediction times, and costmap size that users should keep in mind while tuning for their application. If a controller server costmap is set to 3.0m in size, that means that with the robot in the center, there is 1.5m of information on either side of the robot. When your prediction horizon (time_steps * model_dt) at maximum speed (vx_max) is larger than this, then your robot will be artificially limited in its maximum speeds and behavior by the costmap limitation. For example, if you predict forward 3 seconds (60 steps @ 0.05s per step) at 0.5m/s maximum speed, the **minimum** required costmap radius is 1.5m - or 3m total width.
|
||||
|
||||
The same applies to the Path Follow and Align offsets from furthest. In the same example if the furthest point we can consider is already at the edge of the costmap, then further offsets are thresholded because they're unusable. So its important while selecting these parameters to make sure that the theoretical offsets can exist on the costmap settings selected with the maximum prediction horizon and velocities desired. Setting the threshold for consideration in the path follower + goal critics as the same as your prediction horizon can make sure you have clean hand-offs between them, as the path follower will otherwise attempt to slow slightly once it reaches the final goal pose as its marker.
|
||||
|
||||
The Path Follow critic cannot drive velocities greater than the projectable distance of that velocity on the available path on the rolling costmap. The Path Align critic `offset_from_furthest` represents the number of path points a trajectory passes through while tracking the path. If this is set either absurdly low (e.g. 5) it can trigger when a robot is simply trying to start path tracking causing some suboptimal behaviors and local minima while starting a task. If it is set absurdly high (e.g. 50) relative to the path resolution and costmap size, then the critic may never trigger or only do so when at full-speed. A balance here is wise. A selection of this value to be ~30% of the maximum velocity distance projected is good (e.g. if a planner produces points every 2.5cm, 60 can fit on the 1.5m local costmap radius. If the max speed is 0.5m/s with a 3s prediction time, then 20 points represents 33% of the maximum speed projected over the prediction horizon onto the path). When in doubt, `prediction_horizon_s * max_speed / path_resolution / 3.0` is a good baseline.
|
||||
|
||||
### Obstacle, Inflation Layer, and Path Following
|
||||
|
||||
There also exists a relationship between the costmap configurations and the Obstacle critic configurations. If the Obstacle critic is not well tuned with the costmap parameters (inflation radius, scale) it can cause the robot to wobble significantly as it attempts to take finitely lower-cost trajectories with a slightly lower cost in exchange for jerky motion. The default behavior was tuned for small AMRs (e.g. turtlebots or similar), so if using a larger robot, you may want to reduce the `repulsion_weight` in kind. It may also perform awkward maneuvers when in free-space to try to maximize time in a small pocket of 0-cost over a more natural motion which involves moving into some low-costed region. Finally, it may generally refuse to go into costed space at all when starting in a free 0-cost space if the gain is set disproportionately higher than the Path Follow scoring to encourage the robot to move along the path. This is due to the critic cost of staying in free space becoming more attractive than entering even lightly costed space in exchange for progression along the task.
|
||||
|
||||
Thus, care should be taken to select weights of the obstacle critic in conjunction with the costmap inflation radius and scale so that a robot does not have such issues. How I (Steve, your friendly neighborhood navigator) tuned this was to first create the appropriate obstacle critic behavior desirable in conjunction with the inflation layer parameters. Its worth noting that the Obstacle critic converts the cost into a distance from obstacles, so the nature of the distribution of costs in the inflation isn't overly significant. However, the inflation radius and the scale will define the cost at the end of the distribution where free-space meets the lowest cost value within the radius. So testing for quality behavior when going over that threshold should be considered.
|
||||
|
||||
As you increase or decrease your weights on the Obstacle, you may notice the aforementioned behaviors (e.g. won't overcome free to non-free threshold). To overcome them, increase the FollowPath critic cost to increase the desire for the trajectory planner to continue moving towards the goal. Make sure to not overshoot this though, keep them balanced. A desirable outcome is smooth motion roughly in the center of spaces without significant close interactions with obstacles. It shouldn't be perfectly following a path yet nor should the output velocity be wobbling jaggedly.
|
||||
|
||||
Once you have your obstacle avoidance behavior tuned and matched with an appropriate path following penalty, tune the Path Align critic to align with the path. If you design exact-path-alignment behavior, its possible to skip the obstacle critic step as highly tuning the system to follow the path will give it less ability to deviate to avoid obstacles (though it'll slow and stop). Tuning the critic weight for the Obstacle critic high will do the job to avoid near-collisions but the repulsion weight is largely unnecessary to you. For others wanting more dynamic behavior, it _can_ be beneficial to slowly lower the weight on the obstacle critic to give the path alignment critic some more room to work. If your path was generated with a cost-aware planner (like all provided by Nav2) and providing paths sufficiently far from obstacles for your satisfaction, the impact of a slightly reduced Obstacle critic with a Path Alignment critic will do you well. Not over-weighting the path align critic will allow the robot to deviate from the path to get around dynamic obstacles in the scene or other obstacles not previous considered during path planning. It is subjective as to the best behavior for your application, but it has been shown that MPPI can be an exact path tracker and/or avoid dynamic obstacles very fluidly and everywhere in between. The defaults provided are in the generally right regime for a balanced initial trade-off.
|
||||
@@ -0,0 +1,22 @@
|
||||
find_package(benchmark REQUIRED)
|
||||
|
||||
set(BENCHMARK_NAMES
|
||||
optimizer_benchmark
|
||||
controller_benchmark
|
||||
)
|
||||
|
||||
foreach(name IN LISTS BENCHMARK_NAMES)
|
||||
add_executable(${name}
|
||||
${name}.cpp
|
||||
)
|
||||
ament_target_dependencies(${name}
|
||||
${dependencies_pkgs}
|
||||
)
|
||||
target_link_libraries(${name}
|
||||
mppi_controller mppi_critics benchmark
|
||||
)
|
||||
|
||||
target_include_directories(${name} PRIVATE
|
||||
${PROJECT_SOURCE_DIR}/test/utils
|
||||
)
|
||||
endforeach()
|
||||
@@ -0,0 +1,238 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <benchmark/benchmark.h>
|
||||
#include <string>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <nav2_costmap_2d/cost_values.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
|
||||
#include <nav2_core/goal_checker.hpp>
|
||||
|
||||
#include <xtensor/xarray.hpp>
|
||||
#include <xtensor/xio.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/controller.hpp"
|
||||
|
||||
#include "utils.hpp"
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
void prepareAndRunBenchmark(
|
||||
bool consider_footprint, std::string motion_model,
|
||||
std::vector<std::string> critics, benchmark::State & state)
|
||||
{
|
||||
bool visualize = false;
|
||||
|
||||
int batch_size = 300;
|
||||
int time_steps = 12;
|
||||
unsigned int path_points = 50u;
|
||||
int iteration_count = 2;
|
||||
double lookahead_distance = 10.0;
|
||||
|
||||
TestCostmapSettings costmap_settings{};
|
||||
auto costmap_ros = getDummyCostmapRos(costmap_settings);
|
||||
auto costmap = costmap_ros->getCostmap();
|
||||
|
||||
TestPose start_pose = costmap_settings.getCenterPose();
|
||||
double path_step = costmap_settings.resolution;
|
||||
|
||||
TestPathSettings path_settings{start_pose, path_points, path_step, path_step};
|
||||
TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count,
|
||||
lookahead_distance, motion_model, consider_footprint};
|
||||
|
||||
unsigned int offset = 4;
|
||||
unsigned int obstacle_size = offset * 2;
|
||||
|
||||
unsigned char obstacle_cost = 250;
|
||||
|
||||
auto [obst_x, obst_y] = costmap_settings.getCenterIJ();
|
||||
|
||||
obst_x = obst_x - offset;
|
||||
obst_y = obst_y - offset;
|
||||
addObstacle(costmap, {obst_x, obst_y, obstacle_size, obstacle_cost});
|
||||
|
||||
printInfo(optimizer_settings, path_settings, critics);
|
||||
|
||||
rclcpp::NodeOptions options;
|
||||
std::vector<rclcpp::Parameter> params;
|
||||
setUpControllerParams(visualize, params);
|
||||
setUpOptimizerParams(optimizer_settings, critics, params);
|
||||
options.parameter_overrides(params);
|
||||
auto node = getDummyNode(options);
|
||||
|
||||
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
tf_buffer->setUsingDedicatedThread(true); // One-thread broadcasting-listening model
|
||||
|
||||
auto broadcaster =
|
||||
std::make_shared<tf2_ros::TransformBroadcaster>(node);
|
||||
auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
|
||||
|
||||
auto map_odom_broadcaster = std::async(
|
||||
std::launch::async, sendTf, "map", "odom", broadcaster, node,
|
||||
20);
|
||||
|
||||
auto odom_base_link_broadcaster = std::async(
|
||||
std::launch::async, sendTf, "odom", "base_link", broadcaster, node,
|
||||
20);
|
||||
|
||||
auto controller = getDummyController(node, tf_buffer, costmap_ros);
|
||||
|
||||
// evalControl args
|
||||
auto pose = getDummyPointStamped(node, start_pose);
|
||||
auto velocity = getDummyTwist();
|
||||
auto path = getIncrementalDummyPath(node, path_settings);
|
||||
|
||||
controller->setPlan(path);
|
||||
|
||||
nav2_core::GoalChecker * dummy_goal_checker{nullptr};
|
||||
|
||||
for (auto _ : state) {
|
||||
controller->computeVelocityCommands(pose, velocity, dummy_goal_checker);
|
||||
}
|
||||
map_odom_broadcaster.wait();
|
||||
odom_base_link_broadcaster.wait();
|
||||
}
|
||||
|
||||
static void BM_DiffDrivePointFootprint(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "DiffDrive";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_DiffDrive(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "DiffDrive";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
|
||||
static void BM_Omni(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Omni";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_Ackermann(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_GoalCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_GoalAngleCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalAngleCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_ObstaclesCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"ObstaclesCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_ObstaclesCriticPointFootprint(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = false;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"ObstaclesCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_TwilringCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"TwirlingCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_PathFollowCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"PathFollowCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_PathAngleCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"PathAngleCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond);
|
||||
|
||||
BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);
|
||||
|
||||
BENCHMARK_MAIN();
|
||||
@@ -0,0 +1,213 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <benchmark/benchmark.h>
|
||||
#include <string>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <nav2_costmap_2d/cost_values.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
|
||||
#include <nav2_core/goal_checker.hpp>
|
||||
|
||||
#include <xtensor/xarray.hpp>
|
||||
#include <xtensor/xio.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
|
||||
#include "utils.hpp"
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
void prepareAndRunBenchmark(
|
||||
bool consider_footprint, std::string motion_model,
|
||||
std::vector<std::string> critics, benchmark::State & state)
|
||||
{
|
||||
int batch_size = 300;
|
||||
int time_steps = 12;
|
||||
unsigned int path_points = 50u;
|
||||
int iteration_count = 2;
|
||||
double lookahead_distance = 10.0;
|
||||
|
||||
TestCostmapSettings costmap_settings{};
|
||||
auto costmap_ros = getDummyCostmapRos(costmap_settings);
|
||||
auto costmap = costmap_ros->getCostmap();
|
||||
|
||||
TestPose start_pose = costmap_settings.getCenterPose();
|
||||
double path_step = costmap_settings.resolution;
|
||||
|
||||
TestPathSettings path_settings{start_pose, path_points, path_step, path_step};
|
||||
TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count,
|
||||
lookahead_distance, motion_model, consider_footprint};
|
||||
|
||||
unsigned int offset = 4;
|
||||
unsigned int obstacle_size = offset * 2;
|
||||
|
||||
unsigned char obstacle_cost = 250;
|
||||
|
||||
auto [obst_x, obst_y] = costmap_settings.getCenterIJ();
|
||||
|
||||
obst_x = obst_x - offset;
|
||||
obst_y = obst_y - offset;
|
||||
addObstacle(costmap, {obst_x, obst_y, obstacle_size, obstacle_cost});
|
||||
|
||||
printInfo(optimizer_settings, path_settings, critics);
|
||||
auto node = getDummyNode(optimizer_settings, critics);
|
||||
auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node);
|
||||
auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get());
|
||||
|
||||
// evalControl args
|
||||
auto pose = getDummyPointStamped(node, start_pose);
|
||||
auto velocity = getDummyTwist();
|
||||
auto path = getIncrementalDummyPath(node, path_settings);
|
||||
nav2_core::GoalChecker * dummy_goal_checker{nullptr};
|
||||
|
||||
for (auto _ : state) {
|
||||
optimizer->evalControl(pose, velocity, path, dummy_goal_checker);
|
||||
}
|
||||
}
|
||||
|
||||
static void BM_DiffDrivePointFootprint(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "DiffDrive";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_DiffDrive(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "DiffDrive";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
|
||||
static void BM_Omni(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Omni";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_Ackermann(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_GoalCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_GoalAngleCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"GoalAngleCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_ObstaclesCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"ObstaclesCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_ObstaclesCriticPointFootprint(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = false;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"ObstaclesCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_TwilringCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"TwirlingCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_PathFollowCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"PathFollowCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
static void BM_PathAngleCritic(benchmark::State & state)
|
||||
{
|
||||
bool consider_footprint = true;
|
||||
std::string motion_model = "Ackermann";
|
||||
std::vector<std::string> critics = {{"PathAngleCritic"}};
|
||||
|
||||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
|
||||
}
|
||||
|
||||
BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond);
|
||||
|
||||
BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond);
|
||||
BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond);
|
||||
|
||||
BENCHMARK_MAIN();
|
||||
@@ -0,0 +1,53 @@
|
||||
<class_libraries>
|
||||
<library path="mppi_critics">
|
||||
|
||||
<class type="mppi::critics::ObstaclesCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for obstacle avoidance</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::CostCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for obstacle avoidance using costmap score</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::GoalCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for driving towards the goal</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::GoalAngleCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for achieving the goal heading angle</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::PathAlignCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for aligning to path</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::PathAlignLegacyCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for aligning to path (legacy)</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::PathAngleCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for tracking the path in the correct heading</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::PathFollowCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for driving towards the goal that is furthest among trajectories nearest path points</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::PreferForwardCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description></description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::TwirlingCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for preventing twirling behavior when using omnidirectional models</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::ConstraintCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for incentivizing moving within kinematic and dynamic bounds</description>
|
||||
</class>
|
||||
|
||||
<class type="mppi::critics::VelocityDeadbandCritic" base_class_type="mppi::critics::CriticFunction">
|
||||
<description>mppi critic for restricting command velocities in deadband range</description>
|
||||
</class>
|
||||
|
||||
</library>
|
||||
</class_libraries>
|
||||
@@ -0,0 +1,132 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CONTROLLER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CONTROLLER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "nav2_mppi_controller/tools/path_handler.hpp"
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
|
||||
#include "nav2_mppi_controller/models/constraints.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
#include "nav2_core/controller.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
|
||||
namespace nav2_mppi_controller
|
||||
{
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
/**
|
||||
* @class mppi::MPPIController
|
||||
* @brief Main plugin controller for MPPI Controller
|
||||
*/
|
||||
class MPPIController : public nav2_core::Controller
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::MPPIController
|
||||
*/
|
||||
MPPIController() = default;
|
||||
|
||||
/**
|
||||
* @brief Configure controller on bringup
|
||||
* @param parent WeakPtr to node
|
||||
* @param name Name of plugin
|
||||
* @param tf TF buffer to use
|
||||
* @param costmap_ros Costmap2DROS object of environment
|
||||
*/
|
||||
void configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
|
||||
|
||||
/**
|
||||
* @brief Cleanup resources
|
||||
*/
|
||||
void cleanup() override;
|
||||
|
||||
/**
|
||||
* @brief Activate controller
|
||||
*/
|
||||
void activate() override;
|
||||
|
||||
/**
|
||||
* @brief Deactivate controller
|
||||
*/
|
||||
void deactivate() override;
|
||||
|
||||
/**
|
||||
* @brief Reset the controller state between tasks
|
||||
*/
|
||||
void reset();
|
||||
|
||||
/**
|
||||
* @brief Main method to compute velocities using the optimizer
|
||||
* @param robot_pose Robot pose
|
||||
* @param robot_speed Robot speed
|
||||
* @param goal_checker Pointer to the goal checker for awareness if completed task
|
||||
*/
|
||||
geometry_msgs::msg::TwistStamped computeVelocityCommands(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
nav2_core::GoalChecker * goal_checker) override;
|
||||
|
||||
/**
|
||||
* @brief Set new reference path to track
|
||||
* @param path Path to track
|
||||
*/
|
||||
void setPlan(const nav_msgs::msg::Path & path) override;
|
||||
|
||||
/**
|
||||
* @brief Set new speed limit from callback
|
||||
* @param speed_limit Speed limit to use
|
||||
* @param percentage Bool if the speed limit is absolute or relative
|
||||
*/
|
||||
void setSpeedLimit(const double & speed_limit, const bool & percentage) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Visualize trajectories
|
||||
* @param transformed_plan Transformed input plan
|
||||
*/
|
||||
void visualize(nav_msgs::msg::Path transformed_plan);
|
||||
|
||||
std::string name_;
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
|
||||
rclcpp::Clock::SharedPtr clock_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
|
||||
std::unique_ptr<ParametersHandler> parameters_handler_;
|
||||
Optimizer optimizer_;
|
||||
PathHandler path_handler_;
|
||||
TrajectoryVisualizer trajectory_visualizer_;
|
||||
|
||||
bool visualize_;
|
||||
|
||||
double reset_period_;
|
||||
// Last time computeVelocityCommands was called
|
||||
rclcpp::Time last_time_called_;
|
||||
};
|
||||
|
||||
} // namespace nav2_mppi_controller
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CONTROLLER_HPP_
|
||||
@@ -0,0 +1,57 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/models/trajectories.hpp"
|
||||
#include "nav2_mppi_controller/models/path.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::CriticData
|
||||
* @brief Data to pass to critics for scoring, including state, trajectories,
|
||||
* pruned path, global goal, costs, and important parameters to share
|
||||
*/
|
||||
struct CriticData
|
||||
{
|
||||
const models::State & state;
|
||||
const models::Trajectories & trajectories;
|
||||
const models::Path & path;
|
||||
const geometry_msgs::msg::Pose & goal;
|
||||
|
||||
xt::xtensor<float, 1> & costs;
|
||||
float & model_dt;
|
||||
|
||||
bool fail_flag;
|
||||
nav2_core::GoalChecker * goal_checker;
|
||||
std::shared_ptr<MotionModel> motion_model;
|
||||
std::optional<std::vector<bool>> path_pts_valid;
|
||||
std::optional<size_t> furthest_reached_path_point;
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_
|
||||
@@ -0,0 +1,118 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITIC_FUNCTION_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITIC_FUNCTION_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/critic_data.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::CollisionCost
|
||||
* @brief Utility for storing cost information
|
||||
*/
|
||||
struct CollisionCost
|
||||
{
|
||||
float cost{0};
|
||||
bool using_footprint{false};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class mppi::critics::CriticFunction
|
||||
* @brief Abstract critic objective function to score trajectories
|
||||
*/
|
||||
class CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::critics::CriticFunction
|
||||
*/
|
||||
CriticFunction() = default;
|
||||
|
||||
/**
|
||||
* @brief Destructor for mppi::critics::CriticFunction
|
||||
*/
|
||||
virtual ~CriticFunction() = default;
|
||||
|
||||
/**
|
||||
* @brief Configure critic on bringup
|
||||
* @param parent WeakPtr to node
|
||||
* @param parent_name name of the controller
|
||||
* @param name Name of plugin
|
||||
* @param costmap_ros Costmap2DROS object of environment
|
||||
* @param dynamic_parameter_handler Parameter handler object
|
||||
*/
|
||||
void on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent,
|
||||
const std::string & parent_name,
|
||||
const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
|
||||
ParametersHandler * param_handler)
|
||||
{
|
||||
parent_ = parent;
|
||||
logger_ = parent_.lock()->get_logger();
|
||||
name_ = name;
|
||||
parent_name_ = parent_name;
|
||||
costmap_ros_ = costmap_ros;
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
parameters_handler_ = param_handler;
|
||||
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(enabled_, "enabled", true);
|
||||
|
||||
initialize();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Main function to score trajectory
|
||||
* @param data Critic data to use in scoring
|
||||
*/
|
||||
virtual void score(CriticData & data) = 0;
|
||||
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
virtual void initialize() = 0;
|
||||
|
||||
/**
|
||||
* @brief Get name of critic
|
||||
*/
|
||||
std::string getName()
|
||||
{
|
||||
return name_;
|
||||
}
|
||||
|
||||
protected:
|
||||
bool enabled_;
|
||||
std::string name_, parent_name_;
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
nav2_costmap_2d::Costmap2D * costmap_{nullptr};
|
||||
|
||||
ParametersHandler * parameters_handler_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITIC_FUNCTION_HPP_
|
||||
@@ -0,0 +1,103 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITIC_MANAGER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITIC_MANAGER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
#include "nav2_mppi_controller/critic_data.hpp"
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::CriticManager
|
||||
* @brief Manager of objective function plugins for scoring trajectories
|
||||
*/
|
||||
class CriticManager
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::CriticManager
|
||||
*/
|
||||
CriticManager() = default;
|
||||
|
||||
/**
|
||||
* @brief Virtual Destructor for mppi::CriticManager
|
||||
*/
|
||||
virtual ~CriticManager() = default;
|
||||
|
||||
/**
|
||||
* @brief Configure critic manager on bringup and load plugins
|
||||
* @param parent WeakPtr to node
|
||||
* @param name Name of plugin
|
||||
* @param costmap_ros Costmap2DROS object of environment
|
||||
* @param dynamic_parameter_handler Parameter handler object
|
||||
*/
|
||||
void on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS>, ParametersHandler *);
|
||||
|
||||
/**
|
||||
* @brief Score trajectories by the set of loaded critic functions
|
||||
* @param CriticData Struct of necessary information to pass to the critic functions
|
||||
*/
|
||||
void evalTrajectoriesScores(CriticData & data) const;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Get parameters (critics to load)
|
||||
*/
|
||||
void getParams();
|
||||
|
||||
/**
|
||||
* @brief Load the critic plugins
|
||||
*/
|
||||
virtual void loadCritics();
|
||||
|
||||
/**
|
||||
* @brief Get full-name namespaced critic IDs
|
||||
*/
|
||||
std::string getFullName(const std::string & name);
|
||||
|
||||
protected:
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
std::string name_;
|
||||
|
||||
ParametersHandler * parameters_handler_;
|
||||
std::vector<std::string> critic_names_;
|
||||
std::unique_ptr<pluginlib::ClassLoader<critics::CriticFunction>> loader_;
|
||||
std::vector<std::unique_ptr<critics::CriticFunction>> critics_;
|
||||
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITIC_MANAGER_HPP_
|
||||
+56
@@ -0,0 +1,56 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__CONSTRAINT_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__CONSTRAINT_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for enforcing feasible constraints
|
||||
*/
|
||||
class ConstraintCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to goal following
|
||||
*
|
||||
* @param costs [out] add reference cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
float getMaxVelConstraint() {return max_vel_;}
|
||||
float getMinVelConstraint() {return min_vel_;}
|
||||
|
||||
protected:
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
float min_vel_;
|
||||
float max_vel_;
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__CONSTRAINT_CRITIC_HPP_
|
||||
@@ -0,0 +1,98 @@
|
||||
// Copyright (c) 2023 Robocc Brice Renaudeau
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
|
||||
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
|
||||
#include "nav2_costmap_2d/inflation_layer.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::CostCritic
|
||||
* @brief Critic objective function for avoiding obstacles using costmap's inflated cost
|
||||
*/
|
||||
class CostCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to obstacle avoidance
|
||||
*
|
||||
* @param costs [out] add obstacle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Checks if cost represents a collision
|
||||
* @param cost Point cost at pose center
|
||||
* @param x X of pose
|
||||
* @param y Y of pose
|
||||
* @param theta theta of pose
|
||||
* @return bool if in collision
|
||||
*/
|
||||
bool inCollision(float cost, float x, float y, float theta);
|
||||
|
||||
/**
|
||||
* @brief cost at a robot pose
|
||||
* @param x X of pose
|
||||
* @param y Y of pose
|
||||
* @return Collision information at pose
|
||||
*/
|
||||
float costAtPose(float x, float y);
|
||||
|
||||
/**
|
||||
* @brief Find the min cost of the inflation decay function for which the robot MAY be
|
||||
* in collision in any orientation
|
||||
* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)
|
||||
* @return double circumscribed cost, any higher than this and need to do full footprint collision checking
|
||||
* since some element of the robot could be in collision
|
||||
*/
|
||||
float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
|
||||
|
||||
protected:
|
||||
nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
|
||||
collision_checker_{nullptr};
|
||||
float possibly_inscribed_cost_;
|
||||
|
||||
bool consider_footprint_{true};
|
||||
float circumscribed_radius_{0};
|
||||
float circumscribed_cost_{0};
|
||||
float collision_cost_{0};
|
||||
float critical_cost_{0};
|
||||
float weight_{0};
|
||||
|
||||
float near_goal_distance_;
|
||||
std::string inflation_layer_name_;
|
||||
|
||||
unsigned int power_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__COST_CRITIC_HPP_
|
||||
+53
@@ -0,0 +1,53 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__GOAL_ANGLE_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__GOAL_ANGLE_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for driving towards goal orientation
|
||||
*/
|
||||
class GoalAngleCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to robot orientation at goal pose
|
||||
* (considered only if robot near last goal in current plan)
|
||||
*
|
||||
* @param costs [out] add goal angle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
float threshold_to_consider_{0};
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__GOAL_ANGLE_CRITIC_HPP_
|
||||
@@ -0,0 +1,53 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__GOAL_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__GOAL_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for driving towards goal
|
||||
*/
|
||||
class GoalCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to goal following
|
||||
*
|
||||
* @param costs [out] add reference cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
float threshold_to_consider_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__GOAL_CRITIC_HPP_
|
||||
+103
@@ -0,0 +1,103 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include "nav2_costmap_2d/footprint_collision_checker.hpp"
|
||||
#include "nav2_costmap_2d/inflation_layer.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for avoiding obstacles, allowing it to deviate off
|
||||
* the planned path. This is important to tune in tandem with PathAlign to make a balance
|
||||
* between path-tracking and dynamic obstacle avoidance capabilities as desirable for a
|
||||
* particular application
|
||||
*/
|
||||
class ObstaclesCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to obstacle avoidance
|
||||
*
|
||||
* @param costs [out] add obstacle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Checks if cost represents a collision
|
||||
* @param cost Costmap cost
|
||||
* @return bool if in collision
|
||||
*/
|
||||
inline bool inCollision(float cost) const;
|
||||
|
||||
/**
|
||||
* @brief cost at a robot pose
|
||||
* @param x X of pose
|
||||
* @param y Y of pose
|
||||
* @param theta theta of pose
|
||||
* @return Collision information at pose
|
||||
*/
|
||||
inline CollisionCost costAtPose(float x, float y, float theta);
|
||||
|
||||
/**
|
||||
* @brief Distance to obstacle from cost
|
||||
* @param cost Costmap cost
|
||||
* @return float Distance to the obstacle represented by cost
|
||||
*/
|
||||
inline float distanceToObstacle(const CollisionCost & cost);
|
||||
|
||||
/**
|
||||
* @brief Find the min cost of the inflation decay function for which the robot MAY be
|
||||
* in collision in any orientation
|
||||
* @param costmap Costmap2DROS to get minimum inscribed cost (e.g. 128 in inflation layer documentation)
|
||||
* @return double circumscribed cost, any higher than this and need to do full footprint collision checking
|
||||
* since some element of the robot could be in collision
|
||||
*/
|
||||
float findCircumscribedCost(std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap);
|
||||
|
||||
protected:
|
||||
nav2_costmap_2d::FootprintCollisionChecker<nav2_costmap_2d::Costmap2D *>
|
||||
collision_checker_{nullptr};
|
||||
|
||||
bool consider_footprint_{true};
|
||||
float collision_cost_{0};
|
||||
float inflation_scale_factor_{0}, inflation_radius_{0};
|
||||
|
||||
float possibly_inscribed_cost_;
|
||||
float collision_margin_distance_;
|
||||
float near_goal_distance_;
|
||||
float circumscribed_cost_{0}, circumscribed_radius_{0};
|
||||
|
||||
unsigned int power_{0};
|
||||
float repulsion_weight_, critical_weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_
|
||||
+59
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for aligning to the path. Note:
|
||||
* High settings of this will follow the path more precisely, but also makes it
|
||||
* difficult (or impossible) to deviate in the presence of dynamic obstacles.
|
||||
* This is an important critic to tune and consider in tandem with Obstacle.
|
||||
*/
|
||||
class PathAlignCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to trajectories path alignment
|
||||
*
|
||||
* @param costs [out] add reference cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
size_t offset_from_furthest_{0};
|
||||
int trajectory_point_step_{0};
|
||||
float threshold_to_consider_{0};
|
||||
float max_path_occupancy_ratio_{0};
|
||||
bool use_path_orientations_{false};
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::PathAlignLegacyCritic
|
||||
* @brief Critic objective function for aligning to the path. Note:
|
||||
* High settings of this will follow the path more precisely, but also makes it
|
||||
* difficult (or impossible) to deviate in the presence of dynamic obstacles.
|
||||
* This is an important critic to tune and consider in tandem with Obstacle.
|
||||
* This is the initial 'Legacy' implementation before replacement Oct 2023.
|
||||
*/
|
||||
class PathAlignLegacyCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to trajectories path alignment
|
||||
*
|
||||
* @param costs [out] add reference cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
size_t offset_from_furthest_{0};
|
||||
int trajectory_point_step_{0};
|
||||
float threshold_to_consider_{0};
|
||||
float max_path_occupancy_ratio_{0};
|
||||
bool use_path_orientations_{false};
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_LEGACY_CRITIC_HPP_
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for aligning to path in cases of extreme misalignment
|
||||
* or turning
|
||||
*/
|
||||
class PathAngleCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to robot orientation at goal pose
|
||||
* (considered only if robot near last goal in current plan)
|
||||
*
|
||||
* @param costs [out] add goal angle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
float max_angle_to_furthest_{0};
|
||||
float threshold_to_consider_{0};
|
||||
|
||||
size_t offset_from_furthest_{0};
|
||||
bool reversing_allowed_{true};
|
||||
bool forward_preference_{true};
|
||||
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ANGLE_CRITIC_HPP_
|
||||
+60
@@ -0,0 +1,60 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PATH_FOLLOW_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__PATH_FOLLOW_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for following the path approximately
|
||||
* To allow for deviation from path in case of dynamic obstacles. Path Align
|
||||
* is what aligns the trajectories to the path more or less precisely, if desireable.
|
||||
* A higher weight here with an offset > 1 will accelerate the samples to full speed
|
||||
* faster and push the follow point further ahead, creating some shortcutting.
|
||||
*/
|
||||
class PathFollowCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to robot orientation at goal pose
|
||||
* (considered only if robot near last goal in current plan)
|
||||
*
|
||||
* @param costs [out] add goal angle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
float threshold_to_consider_{0};
|
||||
size_t offset_from_furthest_{0};
|
||||
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_FOLLOW_CRITIC_HPP_
|
||||
+52
@@ -0,0 +1,52 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__PREFER_FORWARD_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__PREFER_FORWARD_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for preferring forward motion
|
||||
*/
|
||||
class PreferForwardCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to robot orientation at goal pose
|
||||
* (considered only if robot near last goal in current plan)
|
||||
*
|
||||
* @param costs [out] add goal angle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
float threshold_to_consider_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__PREFER_FORWARD_CRITIC_HPP_
|
||||
+51
@@ -0,0 +1,51 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__TWIRLING_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__TWIRLING_CRITIC_HPP_
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::ConstraintCritic
|
||||
* @brief Critic objective function for penalizing wiggling/twirling
|
||||
*/
|
||||
class TwirlingCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to robot orientation at goal pose
|
||||
* (considered only if robot near last goal in current plan)
|
||||
*
|
||||
* @param costs [out] add goal angle cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__TWIRLING_CRITIC_HPP_
|
||||
+54
@@ -0,0 +1,54 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_mppi_controller/critic_function.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::critics::VelocityDeadbandCritic
|
||||
* @brief Critic objective function for enforcing feasible constraints
|
||||
*/
|
||||
class VelocityDeadbandCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Initialize critic
|
||||
*/
|
||||
void initialize() override;
|
||||
|
||||
/**
|
||||
* @brief Evaluate cost related to goal following
|
||||
*
|
||||
* @param costs [out] add reference cost values to this tensor
|
||||
*/
|
||||
void score(CriticData & data) override;
|
||||
|
||||
protected:
|
||||
unsigned int power_{0};
|
||||
float weight_{0};
|
||||
std::vector<float> deadband_velocities_{0.0f, 0.0f, 0.0f};
|
||||
};
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__CRITICS__VELOCITY_DEADBAND_CRITIC_HPP_
|
||||
@@ -0,0 +1,46 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::models::ControlConstraints
|
||||
* @brief Constraints on control
|
||||
*/
|
||||
struct ControlConstraints
|
||||
{
|
||||
float vx_max;
|
||||
float vx_min;
|
||||
float vy;
|
||||
float wz;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct mppi::models::SamplingStd
|
||||
* @brief Noise parameters for sampling trajectories
|
||||
*/
|
||||
struct SamplingStd
|
||||
{
|
||||
float vx;
|
||||
float vy;
|
||||
float wz;
|
||||
};
|
||||
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_
|
||||
+52
@@ -0,0 +1,52 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::models::Control
|
||||
* @brief A set of controls
|
||||
*/
|
||||
struct Control
|
||||
{
|
||||
float vx, vy, wz;
|
||||
};
|
||||
|
||||
/**
|
||||
* @struct mppi::models::ControlSequence
|
||||
* @brief A control sequence over time (e.g. trajectory)
|
||||
*/
|
||||
struct ControlSequence
|
||||
{
|
||||
xt::xtensor<float, 1> vx;
|
||||
xt::xtensor<float, 1> vy;
|
||||
xt::xtensor<float, 1> wz;
|
||||
|
||||
void reset(unsigned int time_steps)
|
||||
{
|
||||
vx = xt::zeros<float>({time_steps});
|
||||
vy = xt::zeros<float>({time_steps});
|
||||
wz = xt::zeros<float>({time_steps});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
|
||||
+45
@@ -0,0 +1,45 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_
|
||||
|
||||
#include <cstddef>
|
||||
#include "nav2_mppi_controller/models/constraints.hpp"
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::models::OptimizerSettings
|
||||
* @brief Settings for the optimizer to use
|
||||
*/
|
||||
struct OptimizerSettings
|
||||
{
|
||||
models::ControlConstraints base_constraints{0, 0, 0, 0};
|
||||
models::ControlConstraints constraints{0, 0, 0, 0};
|
||||
models::SamplingStd sampling_std{0, 0, 0};
|
||||
float model_dt{0};
|
||||
float temperature{0};
|
||||
float gamma{0};
|
||||
unsigned int batch_size{0};
|
||||
unsigned int time_steps{0};
|
||||
unsigned int iteration_count{0};
|
||||
bool shift_control_sequence{false};
|
||||
size_t retry_attempt_limit{0};
|
||||
};
|
||||
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__OPTIMIZER_SETTINGS_HPP_
|
||||
@@ -0,0 +1,46 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::models::Path
|
||||
* @brief Path represented as a tensor
|
||||
*/
|
||||
struct Path
|
||||
{
|
||||
xt::xtensor<float, 1> x;
|
||||
xt::xtensor<float, 1> y;
|
||||
xt::xtensor<float, 1> yaws;
|
||||
|
||||
/**
|
||||
* @brief Reset path data
|
||||
*/
|
||||
void reset(unsigned int size)
|
||||
{
|
||||
x = xt::zeros<float>({size});
|
||||
y = xt::zeros<float>({size});
|
||||
yaws = xt::zeros<float>({size});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
|
||||
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @struct mppi::models::State
|
||||
* @brief State information: velocities, controls, poses, speed
|
||||
*/
|
||||
struct State
|
||||
{
|
||||
xt::xtensor<float, 2> vx;
|
||||
xt::xtensor<float, 2> vy;
|
||||
xt::xtensor<float, 2> wz;
|
||||
|
||||
xt::xtensor<float, 2> cvx;
|
||||
xt::xtensor<float, 2> cvy;
|
||||
xt::xtensor<float, 2> cwz;
|
||||
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
geometry_msgs::msg::Twist speed;
|
||||
|
||||
/**
|
||||
* @brief Reset state data
|
||||
*/
|
||||
void reset(unsigned int batch_size, unsigned int time_steps)
|
||||
{
|
||||
vx = xt::zeros<float>({batch_size, time_steps});
|
||||
vy = xt::zeros<float>({batch_size, time_steps});
|
||||
wz = xt::zeros<float>({batch_size, time_steps});
|
||||
|
||||
cvx = xt::zeros<float>({batch_size, time_steps});
|
||||
cvy = xt::zeros<float>({batch_size, time_steps});
|
||||
cwz = xt::zeros<float>({batch_size, time_steps});
|
||||
}
|
||||
};
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
|
||||
@@ -0,0 +1,47 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
namespace mppi::models
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::models::Trajectories
|
||||
* @brief Candidate Trajectories
|
||||
*/
|
||||
struct Trajectories
|
||||
{
|
||||
xt::xtensor<float, 2> x;
|
||||
xt::xtensor<float, 2> y;
|
||||
xt::xtensor<float, 2> yaws;
|
||||
|
||||
/**
|
||||
* @brief Reset state data
|
||||
*/
|
||||
void reset(unsigned int batch_size, unsigned int time_steps)
|
||||
{
|
||||
x = xt::zeros<float>({batch_size, time_steps});
|
||||
y = xt::zeros<float>({batch_size, time_steps});
|
||||
yaws = xt::zeros<float>({batch_size, time_steps});
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mppi::models
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_
|
||||
@@ -0,0 +1,175 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
|
||||
|
||||
#include <cstdint>
|
||||
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include <xtensor/xmath.hpp>
|
||||
#include <xtensor/xmasked_view.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
#include <xtensor/xnoalias.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::MotionModel
|
||||
* @brief Abstract motion model for modeling a vehicle
|
||||
*/
|
||||
class MotionModel
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::MotionModel
|
||||
*/
|
||||
MotionModel() = default;
|
||||
|
||||
/**
|
||||
* @brief Destructor for mppi::MotionModel
|
||||
*/
|
||||
virtual ~MotionModel() = default;
|
||||
|
||||
/**
|
||||
* @brief With input velocities, find the vehicle's output velocities
|
||||
* @param state Contains control velocities to use to populate vehicle velocities
|
||||
*/
|
||||
virtual void predict(models::State & state)
|
||||
{
|
||||
using namespace xt::placeholders; // NOLINT
|
||||
xt::noalias(xt::view(state.vx, xt::all(), xt::range(1, _))) =
|
||||
xt::view(state.cvx, xt::all(), xt::range(0, -1));
|
||||
|
||||
xt::noalias(xt::view(state.wz, xt::all(), xt::range(1, _))) =
|
||||
xt::view(state.cwz, xt::all(), xt::range(0, -1));
|
||||
|
||||
if (isHolonomic()) {
|
||||
xt::noalias(xt::view(state.vy, xt::all(), xt::range(1, _))) =
|
||||
xt::view(state.cvy, xt::all(), xt::range(0, -1));
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Whether the motion model is holonomic, using Y axis
|
||||
* @return Bool If holonomic
|
||||
*/
|
||||
virtual bool isHolonomic() = 0;
|
||||
|
||||
/**
|
||||
* @brief Apply hard vehicle constraints to a control sequence
|
||||
* @param control_sequence Control sequence to apply constraints to
|
||||
*/
|
||||
virtual void applyConstraints(models::ControlSequence & /*control_sequence*/) {}
|
||||
};
|
||||
|
||||
/**
|
||||
* @class mppi::AckermannMotionModel
|
||||
* @brief Ackermann motion model
|
||||
*/
|
||||
class AckermannMotionModel : public MotionModel
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::AckermannMotionModel
|
||||
*/
|
||||
explicit AckermannMotionModel(ParametersHandler * param_handler, const std::string & name)
|
||||
{
|
||||
auto getParam = param_handler->getParamGetter(name + ".AckermannConstraints");
|
||||
getParam(min_turning_r_, "min_turning_r", 0.2);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Whether the motion model is holonomic, using Y axis
|
||||
* @return Bool If holonomic
|
||||
*/
|
||||
bool isHolonomic() override
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Apply hard vehicle constraints to a control sequence
|
||||
* @param control_sequence Control sequence to apply constraints to
|
||||
*/
|
||||
void applyConstraints(models::ControlSequence & control_sequence) override
|
||||
{
|
||||
auto & vx = control_sequence.vx;
|
||||
auto & wz = control_sequence.wz;
|
||||
|
||||
auto view = xt::masked_view(wz, (xt::fabs(vx) / xt::fabs(wz)) < min_turning_r_);
|
||||
view = xt::sign(wz) * xt::fabs(vx) / min_turning_r_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Get minimum turning radius of ackermann drive
|
||||
* @return Minimum turning radius
|
||||
*/
|
||||
float getMinTurningRadius() {return min_turning_r_;}
|
||||
|
||||
private:
|
||||
float min_turning_r_{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @class mppi::DiffDriveMotionModel
|
||||
* @brief Differential drive motion model
|
||||
*/
|
||||
class DiffDriveMotionModel : public MotionModel
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::DiffDriveMotionModel
|
||||
*/
|
||||
DiffDriveMotionModel() = default;
|
||||
|
||||
/**
|
||||
* @brief Whether the motion model is holonomic, using Y axis
|
||||
* @return Bool If holonomic
|
||||
*/
|
||||
bool isHolonomic() override
|
||||
{
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @class mppi::OmniMotionModel
|
||||
* @brief Omnidirectional motion model
|
||||
*/
|
||||
class OmniMotionModel : public MotionModel
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::OmniMotionModel
|
||||
*/
|
||||
OmniMotionModel() = default;
|
||||
|
||||
/**
|
||||
* @brief Whether the motion model is holonomic, using Y axis
|
||||
* @return Bool If holonomic
|
||||
*/
|
||||
bool isHolonomic() override
|
||||
{
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__MOTION_MODELS_HPP_
|
||||
@@ -0,0 +1,267 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/models/optimizer_settings.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/critic_manager.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/models/trajectories.hpp"
|
||||
#include "nav2_mppi_controller/models/path.hpp"
|
||||
#include "nav2_mppi_controller/tools/noise_generator.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::Optimizer
|
||||
* @brief Main algorithm optimizer of the MPPI Controller
|
||||
*/
|
||||
class Optimizer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::Optimizer
|
||||
*/
|
||||
Optimizer() = default;
|
||||
|
||||
/**
|
||||
* @brief Destructor for mppi::Optimizer
|
||||
*/
|
||||
~Optimizer() {shutdown();}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Initializes optimizer on startup
|
||||
* @param parent WeakPtr to node
|
||||
* @param name Name of plugin
|
||||
* @param costmap_ros Costmap2DROS object of environment
|
||||
* @param dynamic_parameter_handler Parameter handler object
|
||||
*/
|
||||
void initialize(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
|
||||
ParametersHandler * dynamic_parameters_handler);
|
||||
|
||||
/**
|
||||
* @brief Shutdown for optimizer at process end
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* @brief Compute control using MPPI algorithm
|
||||
* @param robot_pose Pose of the robot at given time
|
||||
* @param robot_speed Speed of the robot at given time
|
||||
* @param plan Path plan to track
|
||||
* @param goal_checker Object to check if goal is completed
|
||||
* @return TwistStamped of the MPPI control
|
||||
*/
|
||||
geometry_msgs::msg::TwistStamped evalControl(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed, const nav_msgs::msg::Path & plan,
|
||||
const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
|
||||
|
||||
/**
|
||||
* @brief Get the trajectories generated in a cycle for visualization
|
||||
* @return Set of trajectories evaluated in cycle
|
||||
*/
|
||||
models::Trajectories & getGeneratedTrajectories();
|
||||
|
||||
/**
|
||||
* @brief Get the optimal trajectory for a cycle for visualization
|
||||
* @return Optimal trajectory
|
||||
*/
|
||||
xt::xtensor<float, 2> getOptimizedTrajectory();
|
||||
|
||||
/**
|
||||
* @brief Set the maximum speed based on the speed limits callback
|
||||
* @param speed_limit Limit of the speed for use
|
||||
* @param percentage Whether the speed limit is absolute or relative
|
||||
*/
|
||||
void setSpeedLimit(double speed_limit, bool percentage);
|
||||
|
||||
/**
|
||||
* @brief Reset the optimization problem to initial conditions
|
||||
*/
|
||||
void reset();
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Main function to generate, score, and return trajectories
|
||||
*/
|
||||
void optimize();
|
||||
|
||||
/**
|
||||
* @brief Prepare state information on new request for trajectory rollouts
|
||||
* @param robot_pose Pose of the robot at given time
|
||||
* @param robot_speed Speed of the robot at given time
|
||||
* @param plan Path plan to track
|
||||
* @param goal_checker Object to check if goal is completed
|
||||
*/
|
||||
void prepare(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
const nav_msgs::msg::Path & plan,
|
||||
const geometry_msgs::msg::Pose & goal, nav2_core::GoalChecker * goal_checker);
|
||||
|
||||
/**
|
||||
* @brief Obtain the main controller's parameters
|
||||
*/
|
||||
void getParams();
|
||||
|
||||
/**
|
||||
* @brief Set the motion model of the vehicle platform
|
||||
* @param model Model string to use
|
||||
*/
|
||||
void setMotionModel(const std::string & model);
|
||||
|
||||
/**
|
||||
* @brief Shift the optimal control sequence after processing for
|
||||
* next iterations initial conditions after execution
|
||||
*/
|
||||
void shiftControlSequence();
|
||||
|
||||
/**
|
||||
* @brief updates generated trajectories with noised trajectories
|
||||
* from the last cycle's optimal control
|
||||
*/
|
||||
void generateNoisedTrajectories();
|
||||
|
||||
/**
|
||||
* @brief Apply hard vehicle constraints on control sequence
|
||||
*/
|
||||
void applyControlSequenceConstraints();
|
||||
|
||||
/**
|
||||
* @brief Update velocities in state
|
||||
* @param state fill state with velocities on each step
|
||||
*/
|
||||
void updateStateVelocities(models::State & state) const;
|
||||
|
||||
/**
|
||||
* @brief Update initial velocity in state
|
||||
* @param state fill state
|
||||
*/
|
||||
void updateInitialStateVelocities(models::State & state) const;
|
||||
|
||||
/**
|
||||
* @brief predict velocities in state using model
|
||||
* for time horizon equal to timesteps
|
||||
* @param state fill state
|
||||
*/
|
||||
void propagateStateVelocitiesFromInitials(models::State & state) const;
|
||||
|
||||
/**
|
||||
* @brief Rollout velocities in state to poses
|
||||
* @param trajectories to rollout
|
||||
* @param state fill state
|
||||
*/
|
||||
void integrateStateVelocities(
|
||||
models::Trajectories & trajectories,
|
||||
const models::State & state) const;
|
||||
|
||||
/**
|
||||
* @brief Rollout velocities in state to poses
|
||||
* @param trajectories to rollout
|
||||
* @param state fill state
|
||||
*/
|
||||
void integrateStateVelocities(
|
||||
xt::xtensor<float, 2> & trajectories,
|
||||
const xt::xtensor<float, 2> & state) const;
|
||||
|
||||
/**
|
||||
* @brief Update control sequence with state controls weighted by costs
|
||||
* using softmax function
|
||||
*/
|
||||
void updateControlSequence();
|
||||
|
||||
/**
|
||||
* @brief Convert control sequence to a twist commant
|
||||
* @param stamp Timestamp to use
|
||||
* @return TwistStamped of command to send to robot base
|
||||
*/
|
||||
geometry_msgs::msg::TwistStamped
|
||||
getControlFromSequenceAsTwist(const builtin_interfaces::msg::Time & stamp);
|
||||
|
||||
/**
|
||||
* @brief Whether the motion model is holonomic
|
||||
* @return Bool if holonomic to populate `y` axis of state
|
||||
*/
|
||||
bool isHolonomic() const;
|
||||
|
||||
/**
|
||||
* @brief Using control frequence and time step size, determine if trajectory
|
||||
* offset should be used to populate initial state of the next cycle
|
||||
*/
|
||||
void setOffset(double controller_frequency);
|
||||
|
||||
/**
|
||||
* @brief Perform fallback behavior to try to recover from a set of trajectories in collision
|
||||
* @param fail Whether the system failed to recover from
|
||||
*/
|
||||
bool fallback(bool fail);
|
||||
|
||||
protected:
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros_;
|
||||
nav2_costmap_2d::Costmap2D * costmap_;
|
||||
std::string name_;
|
||||
|
||||
std::shared_ptr<MotionModel> motion_model_;
|
||||
|
||||
ParametersHandler * parameters_handler_;
|
||||
CriticManager critic_manager_;
|
||||
NoiseGenerator noise_generator_;
|
||||
|
||||
models::OptimizerSettings settings_;
|
||||
|
||||
models::State state_;
|
||||
models::ControlSequence control_sequence_;
|
||||
std::array<mppi::models::Control, 4> control_history_;
|
||||
models::Trajectories generated_trajectories_;
|
||||
models::Path path_;
|
||||
geometry_msgs::msg::Pose goal_;
|
||||
xt::xtensor<float, 1> costs_;
|
||||
|
||||
CriticData critics_data_ = {
|
||||
state_, generated_trajectories_, path_, goal_,
|
||||
costs_, settings_.model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__OPTIMIZER_HPP_
|
||||
+112
@@ -0,0 +1,112 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_
|
||||
|
||||
#include <string>
|
||||
#include <memory>
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
#include <condition_variable>
|
||||
|
||||
#include <xtensor/xtensor.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/models/optimizer_settings.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::NoiseGenerator
|
||||
* @brief Generates noise trajectories from optimal trajectory
|
||||
*/
|
||||
class NoiseGenerator
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::NoiseGenerator
|
||||
*/
|
||||
NoiseGenerator() = default;
|
||||
|
||||
/**
|
||||
* @brief Initialize noise generator with settings and model types
|
||||
* @param settings Settings of controller
|
||||
* @param is_holonomic If base is holonomic
|
||||
* @param name Namespace for configs
|
||||
* @param param_handler Get parameters util
|
||||
*/
|
||||
void initialize(
|
||||
mppi::models::OptimizerSettings & settings,
|
||||
bool is_holonomic, const std::string & name, ParametersHandler * param_handler);
|
||||
|
||||
/**
|
||||
* @brief Shutdown noise generator thread
|
||||
*/
|
||||
void shutdown();
|
||||
|
||||
/**
|
||||
* @brief Signal to the noise thread the controller is ready to generate a new
|
||||
* noised control for the next iteration
|
||||
*/
|
||||
void generateNextNoises();
|
||||
|
||||
/**
|
||||
* @brief set noised control_sequence to state controls
|
||||
* @return noises vx, vy, wz
|
||||
*/
|
||||
void setNoisedControls(models::State & state, const models::ControlSequence & control_sequence);
|
||||
|
||||
/**
|
||||
* @brief Reset noise generator with settings and model types
|
||||
* @param settings Settings of controller
|
||||
* @param is_holonomic If base is holonomic
|
||||
*/
|
||||
void reset(mppi::models::OptimizerSettings & settings, bool is_holonomic);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Thread to execute noise generation process
|
||||
*/
|
||||
void noiseThread();
|
||||
|
||||
/**
|
||||
* @brief Generate random controls by gaussian noise with mean in
|
||||
* control_sequence_
|
||||
*
|
||||
* @return tensor of shape [ batch_size_, time_steps_, 2]
|
||||
* where 2 stands for v, w
|
||||
*/
|
||||
void generateNoisedControls();
|
||||
|
||||
xt::xtensor<float, 2> noises_vx_;
|
||||
xt::xtensor<float, 2> noises_vy_;
|
||||
xt::xtensor<float, 2> noises_wz_;
|
||||
|
||||
mppi::models::OptimizerSettings settings_;
|
||||
bool is_holonomic_;
|
||||
|
||||
std::thread noise_thread_;
|
||||
std::condition_variable noise_cond_;
|
||||
std::mutex noise_lock_;
|
||||
bool active_{false}, ready_{false}, regenerate_noises_{false};
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_
|
||||
+267
@@ -0,0 +1,267 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PARAMETERS_HANDLER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__TOOLS__PARAMETERS_HANDLER_HPP_
|
||||
|
||||
#include <functional>
|
||||
#include <string>
|
||||
#include <type_traits>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp/parameter_value.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
/**
|
||||
* @class Parameter Type enum
|
||||
*/
|
||||
enum class ParameterType { Dynamic, Static };
|
||||
|
||||
/**
|
||||
* @class mppi::ParametersHandler
|
||||
* @brief Handles getting parameters and dynamic parmaeter changes
|
||||
*/
|
||||
class ParametersHandler
|
||||
{
|
||||
public:
|
||||
using get_param_func_t = void (const rclcpp::Parameter & param);
|
||||
using post_callback_t = void ();
|
||||
using pre_callback_t = void ();
|
||||
|
||||
/**
|
||||
* @brief Constructor for mppi::ParametersHandler
|
||||
*/
|
||||
ParametersHandler() = default;
|
||||
|
||||
/**
|
||||
* @brief Constructor for mppi::ParametersHandler
|
||||
* @param parent Weak ptr to node
|
||||
*/
|
||||
explicit ParametersHandler(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent);
|
||||
|
||||
/**
|
||||
* @brief Starts processing dynamic parameter changes
|
||||
*/
|
||||
void start();
|
||||
|
||||
/**
|
||||
* @brief Dynamic parameter callback
|
||||
* @param parameter Parameter changes to process
|
||||
* @return Set Parameter Result
|
||||
*/
|
||||
rcl_interfaces::msg::SetParametersResult dynamicParamsCallback(
|
||||
std::vector<rclcpp::Parameter> parameters);
|
||||
|
||||
/**
|
||||
* @brief Get an object to retreive parameters
|
||||
* @param ns Namespace to get parameters within
|
||||
* @return Parameter getter object
|
||||
*/
|
||||
inline auto getParamGetter(const std::string & ns);
|
||||
|
||||
/**
|
||||
* @brief Set a callback to process after parameter changes
|
||||
* @param callback Callback function
|
||||
*/
|
||||
template<typename T>
|
||||
void addPostCallback(T && callback);
|
||||
|
||||
/**
|
||||
* @brief Set a callback to process before parameter changes
|
||||
* @param callback Callback function
|
||||
*/
|
||||
template<typename T>
|
||||
void addPreCallback(T && callback);
|
||||
|
||||
/**
|
||||
* @brief Set a parameter to a dynamic parameter callback
|
||||
* @param setting Parameter
|
||||
* @param name Name of parameter
|
||||
*/
|
||||
template<typename T>
|
||||
void setDynamicParamCallback(T & setting, const std::string & name);
|
||||
|
||||
/**
|
||||
* @brief Get mutex lock for changing parameters
|
||||
* @return Pointer to mutex
|
||||
*/
|
||||
std::mutex * getLock()
|
||||
{
|
||||
return ¶meters_change_mutex_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Set a parameter to a dynamic parameter callback
|
||||
* @param name Name of parameter
|
||||
* @param callback Parameter callback
|
||||
*/
|
||||
template<typename T>
|
||||
void addDynamicParamCallback(const std::string & name, T && callback);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Gets parameter
|
||||
* @param setting Return Parameter type
|
||||
* @param name Parameter name
|
||||
* @param default_value Default parameter value
|
||||
* @param param_type Type of parameter (dynamic or static)
|
||||
*/
|
||||
template<typename SettingT, typename ParamT>
|
||||
void getParam(
|
||||
SettingT & setting, const std::string & name, ParamT default_value,
|
||||
ParameterType param_type = ParameterType::Dynamic);
|
||||
|
||||
/**
|
||||
* @brief Set a parameter
|
||||
* @param setting Return Parameter type
|
||||
* @param name Parameter name
|
||||
* @param node Node to set parameter via
|
||||
*/
|
||||
template<typename ParamT, typename SettingT, typename NodeT>
|
||||
void setParam(SettingT & setting, const std::string & name, NodeT node) const;
|
||||
|
||||
/**
|
||||
* @brief Converts parameter type to real types
|
||||
* @param parameter Parameter to convert into real type
|
||||
* @return parameter as a functional type
|
||||
*/
|
||||
template<typename T>
|
||||
static auto as(const rclcpp::Parameter & parameter);
|
||||
|
||||
std::mutex parameters_change_mutex_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr
|
||||
on_set_param_handler_;
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
|
||||
std::string node_name_;
|
||||
|
||||
bool verbose_{false};
|
||||
|
||||
std::unordered_map<std::string, std::function<get_param_func_t>>
|
||||
get_param_callbacks_;
|
||||
|
||||
std::vector<std::function<pre_callback_t>> pre_callbacks_;
|
||||
std::vector<std::function<post_callback_t>> post_callbacks_;
|
||||
};
|
||||
|
||||
inline auto ParametersHandler::getParamGetter(const std::string & ns)
|
||||
{
|
||||
return [this, ns](
|
||||
auto & setting, const std::string & name, auto default_value,
|
||||
ParameterType param_type = ParameterType::Dynamic) {
|
||||
getParam(
|
||||
setting, ns.empty() ? name : ns + "." + name,
|
||||
std::move(default_value), param_type);
|
||||
};
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback)
|
||||
{
|
||||
get_param_callbacks_[name] = callback;
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void ParametersHandler::addPostCallback(T && callback)
|
||||
{
|
||||
post_callbacks_.push_back(callback);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void ParametersHandler::addPreCallback(T && callback)
|
||||
{
|
||||
pre_callbacks_.push_back(callback);
|
||||
}
|
||||
|
||||
template<typename SettingT, typename ParamT>
|
||||
void ParametersHandler::getParam(
|
||||
SettingT & setting, const std::string & name,
|
||||
ParamT default_value,
|
||||
ParameterType param_type)
|
||||
{
|
||||
auto node = node_.lock();
|
||||
|
||||
nav2_util::declare_parameter_if_not_declared(
|
||||
node, name, rclcpp::ParameterValue(default_value));
|
||||
|
||||
setParam<ParamT>(setting, name, node);
|
||||
|
||||
if (param_type == ParameterType::Dynamic) {
|
||||
setDynamicParamCallback(setting, name);
|
||||
}
|
||||
}
|
||||
|
||||
template<typename ParamT, typename SettingT, typename NodeT>
|
||||
void ParametersHandler::setParam(
|
||||
SettingT & setting, const std::string & name, NodeT node) const
|
||||
{
|
||||
ParamT param_in{};
|
||||
node->get_parameter(name, param_in);
|
||||
setting = static_cast<SettingT>(param_in);
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name)
|
||||
{
|
||||
if (get_param_callbacks_.find(name) != get_param_callbacks_.end()) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto callback = [this, &setting, name](const rclcpp::Parameter & param) {
|
||||
setting = as<T>(param);
|
||||
|
||||
if (verbose_) {
|
||||
RCLCPP_INFO(logger_, "Dynamic parameter changed: %s", std::to_string(param).c_str());
|
||||
}
|
||||
};
|
||||
|
||||
addDynamicParamCallback(name, callback);
|
||||
|
||||
if (verbose_) {
|
||||
RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
template<typename T>
|
||||
auto ParametersHandler::as(const rclcpp::Parameter & parameter)
|
||||
{
|
||||
if constexpr (std::is_same_v<T, bool>) {
|
||||
return parameter.as_bool();
|
||||
} else if constexpr (std::is_integral_v<T>) {
|
||||
return parameter.as_int();
|
||||
} else if constexpr (std::is_floating_point_v<T>) {
|
||||
return parameter.as_double();
|
||||
} else if constexpr (std::is_same_v<T, std::string>) {
|
||||
return parameter.as_string();
|
||||
} else if constexpr (std::is_same_v<T, std::vector<int64_t>>) {
|
||||
return parameter.as_integer_array();
|
||||
} else if constexpr (std::is_same_v<T, std::vector<double>>) {
|
||||
return parameter.as_double_array();
|
||||
} else if constexpr (std::is_same_v<T, std::vector<std::string>>) {
|
||||
return parameter.as_string_array();
|
||||
} else if constexpr (std::is_same_v<T, std::vector<bool>>) {
|
||||
return parameter.as_bool_array();
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__TOOLS__PARAMETERS_HANDLER_HPP_
|
||||
@@ -0,0 +1,167 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Dexory
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
|
||||
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "tf2_ros/buffer.h"
|
||||
#include "geometry_msgs/msg/pose_stamped.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
#include "nav2_util/geometry_utils.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
using PathIterator = std::vector<geometry_msgs::msg::PoseStamped>::iterator;
|
||||
using PathRange = std::pair<PathIterator, PathIterator>;
|
||||
|
||||
/**
|
||||
* @class mppi::PathHandler
|
||||
* @brief Manager of incoming reference paths for transformation and processing
|
||||
*/
|
||||
|
||||
class PathHandler
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::PathHandler
|
||||
*/
|
||||
PathHandler() = default;
|
||||
|
||||
/**
|
||||
* @brief Destructor for mppi::PathHandler
|
||||
*/
|
||||
~PathHandler() = default;
|
||||
|
||||
/**
|
||||
* @brief Initialize path handler on bringup
|
||||
* @param parent WeakPtr to node
|
||||
* @param name Name of plugin
|
||||
* @param costmap_ros Costmap2DROS object of environment
|
||||
* @param tf TF buffer for transformations
|
||||
* @param dynamic_parameter_handler Parameter handler object
|
||||
*/
|
||||
void initialize(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS>,
|
||||
std::shared_ptr<tf2_ros::Buffer>, ParametersHandler *);
|
||||
|
||||
/**
|
||||
* @brief Set new reference path
|
||||
* @param Plan Path to use
|
||||
*/
|
||||
void setPath(const nav_msgs::msg::Path & plan);
|
||||
|
||||
/**
|
||||
* @brief Get reference path
|
||||
* @return Path
|
||||
*/
|
||||
nav_msgs::msg::Path & getPath();
|
||||
|
||||
/**
|
||||
* @brief transform global plan to local applying constraints,
|
||||
* then prune global plan
|
||||
* @param robot_pose Pose of robot
|
||||
* @return global plan in local frame
|
||||
*/
|
||||
nav_msgs::msg::Path transformPath(const geometry_msgs::msg::PoseStamped & robot_pose);
|
||||
|
||||
/**
|
||||
* @brief Get the global goal pose transformed to the local frame
|
||||
* @param stamp Time to get the goal pose at
|
||||
* @return Transformed goal pose
|
||||
*/
|
||||
geometry_msgs::msg::PoseStamped getTransformedGoal(const builtin_interfaces::msg::Time & stamp);
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Transform a pose to another frame
|
||||
* @param frame Frame to transform to
|
||||
* @param in_pose Input pose
|
||||
* @param out_pose Output pose
|
||||
* @return Bool if successful
|
||||
*/
|
||||
bool transformPose(
|
||||
const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
|
||||
geometry_msgs::msg::PoseStamped & out_pose) const;
|
||||
|
||||
/**
|
||||
* @brief Get largest dimension of costmap (radially)
|
||||
* @return Max distance from center of costmap to edge
|
||||
*/
|
||||
double getMaxCostmapDist();
|
||||
|
||||
/**
|
||||
* @brief Transform a pose to the global reference frame
|
||||
* @param pose Current pose
|
||||
* @return output poose in global reference frame
|
||||
*/
|
||||
geometry_msgs::msg::PoseStamped
|
||||
transformToGlobalPlanFrame(const geometry_msgs::msg::PoseStamped & pose);
|
||||
|
||||
/**
|
||||
* @brief Get global plan within window of the local costmap size
|
||||
* @param global_pose Robot pose
|
||||
* @return plan transformed in the costmap frame and iterator to the first pose of the global
|
||||
* plan (for pruning)
|
||||
*/
|
||||
std::pair<nav_msgs::msg::Path, PathIterator> getGlobalPlanConsideringBoundsInCostmapFrame(
|
||||
const geometry_msgs::msg::PoseStamped & global_pose);
|
||||
|
||||
/**
|
||||
* @brief Prune a path to only interesting portions
|
||||
* @param plan Plan to prune
|
||||
* @param end Final path iterator
|
||||
*/
|
||||
void prunePlan(nav_msgs::msg::Path & plan, const PathIterator end);
|
||||
|
||||
/**
|
||||
* @brief Check if the robot pose is within the set inversion tolerances
|
||||
* @param robot_pose Robot's current pose to check
|
||||
* @return bool If the robot pose is within the set inversion tolerances
|
||||
*/
|
||||
bool isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose);
|
||||
|
||||
std::string name_;
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
ParametersHandler * parameters_handler_;
|
||||
|
||||
nav_msgs::msg::Path global_plan_;
|
||||
nav_msgs::msg::Path global_plan_up_to_inversion_;
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
|
||||
double max_robot_pose_search_dist_{0};
|
||||
double prune_distance_{0};
|
||||
double transform_tolerance_{0};
|
||||
float inversion_xy_tolerance_{0.2};
|
||||
float inversion_yaw_tolerance{0.4};
|
||||
bool enforce_path_inversion_{false};
|
||||
unsigned int inversion_locale_{0u};
|
||||
};
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_
|
||||
+114
@@ -0,0 +1,114 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <xtensor/xtensor.hpp>
|
||||
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
#include "nav2_mppi_controller/models/trajectories.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
/**
|
||||
* @class mppi::TrajectoryVisualizer
|
||||
* @brief Visualizes trajectories for debugging
|
||||
*/
|
||||
class TrajectoryVisualizer
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Constructor for mppi::TrajectoryVisualizer
|
||||
*/
|
||||
TrajectoryVisualizer() = default;
|
||||
|
||||
/**
|
||||
* @brief Configure trajectory visualizer
|
||||
* @param parent WeakPtr to node
|
||||
* @param name Name of plugin
|
||||
* @param frame_id Frame to publish trajectories in
|
||||
* @param dynamic_parameter_handler Parameter handler object
|
||||
*/
|
||||
void on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
const std::string & frame_id, ParametersHandler * parameters_handler);
|
||||
|
||||
/**
|
||||
* @brief Cleanup object on shutdown
|
||||
*/
|
||||
void on_cleanup();
|
||||
|
||||
/**
|
||||
* @brief Activate object
|
||||
*/
|
||||
void on_activate();
|
||||
|
||||
/**
|
||||
* @brief Deactivate object
|
||||
*/
|
||||
void on_deactivate();
|
||||
|
||||
/**
|
||||
* @brief Add an optimal trajectory to visualize
|
||||
* @param trajectory Optimal trajectory
|
||||
*/
|
||||
void add(const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace);
|
||||
|
||||
/**
|
||||
* @brief Add candidate trajectories to visualize
|
||||
* @param trajectories Candidate trajectories
|
||||
*/
|
||||
void add(const models::Trajectories & trajectories, const std::string & marker_namespace);
|
||||
|
||||
/**
|
||||
* @brief Visualize the plan
|
||||
* @param plan Plan to visualize
|
||||
*/
|
||||
void visualize(const nav_msgs::msg::Path & plan);
|
||||
|
||||
/**
|
||||
* @brief Reset object
|
||||
*/
|
||||
void reset();
|
||||
|
||||
protected:
|
||||
std::string frame_id_;
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::MarkerArray>>
|
||||
trajectories_publisher_;
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::Path>> transformed_path_pub_;
|
||||
|
||||
std::unique_ptr<visualization_msgs::msg::MarkerArray> points_;
|
||||
int marker_id_ = 0;
|
||||
|
||||
ParametersHandler * parameters_handler_;
|
||||
|
||||
size_t trajectory_step_{0};
|
||||
size_t time_step_{0};
|
||||
|
||||
rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
|
||||
};
|
||||
|
||||
} // namespace mppi
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__TOOLS__TRAJECTORY_VISUALIZER_HPP_
|
||||
@@ -0,0 +1,679 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#ifndef NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
|
||||
#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <string>
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#include <xtensor/xarray.hpp>
|
||||
#include <xtensor/xnorm.hpp>
|
||||
#include <xtensor/xmath.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "angles/angles.h"
|
||||
|
||||
#include "tf2/utils.h"
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "nav_msgs/msg/path.hpp"
|
||||
#include "visualization_msgs/msg/marker_array.hpp"
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
|
||||
#include "nav2_util/node_utils.hpp"
|
||||
#include "nav2_core/goal_checker.hpp"
|
||||
|
||||
#include "nav2_mppi_controller/models/optimizer_settings.hpp"
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
#include "nav2_mppi_controller/models/path.hpp"
|
||||
#include "builtin_interfaces/msg/time.hpp"
|
||||
#include "nav2_mppi_controller/critic_data.hpp"
|
||||
|
||||
namespace mppi::utils
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
/**
|
||||
* @brief Convert data into pose
|
||||
* @param x X position
|
||||
* @param y Y position
|
||||
* @param z Z position
|
||||
* @return Pose object
|
||||
*/
|
||||
inline geometry_msgs::msg::Pose createPose(double x, double y, double z)
|
||||
{
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position.x = x;
|
||||
pose.position.y = y;
|
||||
pose.position.z = z;
|
||||
pose.orientation.w = 1;
|
||||
pose.orientation.x = 0;
|
||||
pose.orientation.y = 0;
|
||||
pose.orientation.z = 0;
|
||||
return pose;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert data into scale
|
||||
* @param x X scale
|
||||
* @param y Y scale
|
||||
* @param z Z scale
|
||||
* @return Scale object
|
||||
*/
|
||||
inline geometry_msgs::msg::Vector3 createScale(double x, double y, double z)
|
||||
{
|
||||
geometry_msgs::msg::Vector3 scale;
|
||||
scale.x = x;
|
||||
scale.y = y;
|
||||
scale.z = z;
|
||||
return scale;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert data into color
|
||||
* @param r Red component
|
||||
* @param g Green component
|
||||
* @param b Blue component
|
||||
* @param a Alpha component (transparency)
|
||||
* @return Color object
|
||||
*/
|
||||
inline std_msgs::msg::ColorRGBA createColor(float r, float g, float b, float a)
|
||||
{
|
||||
std_msgs::msg::ColorRGBA color;
|
||||
color.r = r;
|
||||
color.g = g;
|
||||
color.b = b;
|
||||
color.a = a;
|
||||
return color;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert data into a Maarker
|
||||
* @param id Marker ID
|
||||
* @param pose Marker pose
|
||||
* @param scale Marker scale
|
||||
* @param color Marker color
|
||||
* @param frame Reference frame to use
|
||||
* @return Visualization Marker
|
||||
*/
|
||||
inline visualization_msgs::msg::Marker createMarker(
|
||||
int id, const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & scale,
|
||||
const std_msgs::msg::ColorRGBA & color, const std::string & frame_id, const std::string & ns)
|
||||
{
|
||||
using visualization_msgs::msg::Marker;
|
||||
Marker marker;
|
||||
marker.header.frame_id = frame_id;
|
||||
marker.header.stamp = rclcpp::Time(0, 0);
|
||||
marker.ns = ns;
|
||||
marker.id = id;
|
||||
marker.type = Marker::SPHERE;
|
||||
marker.action = Marker::ADD;
|
||||
|
||||
marker.pose = pose;
|
||||
marker.scale = scale;
|
||||
marker.color = color;
|
||||
return marker;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert data into TwistStamped
|
||||
* @param vx X velocity
|
||||
* @param wz Angular velocity
|
||||
* @param stamp Timestamp
|
||||
* @param frame Reference frame to use
|
||||
*/
|
||||
inline geometry_msgs::msg::TwistStamped toTwistStamped(
|
||||
float vx, float wz, const builtin_interfaces::msg::Time & stamp, const std::string & frame)
|
||||
{
|
||||
geometry_msgs::msg::TwistStamped twist;
|
||||
twist.header.frame_id = frame;
|
||||
twist.header.stamp = stamp;
|
||||
twist.twist.linear.x = vx;
|
||||
twist.twist.angular.z = wz;
|
||||
|
||||
return twist;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert data into TwistStamped
|
||||
* @param vx X velocity
|
||||
* @param vy Y velocity
|
||||
* @param wz Angular velocity
|
||||
* @param stamp Timestamp
|
||||
* @param frame Reference frame to use
|
||||
*/
|
||||
inline geometry_msgs::msg::TwistStamped toTwistStamped(
|
||||
float vx, float vy, float wz, const builtin_interfaces::msg::Time & stamp,
|
||||
const std::string & frame)
|
||||
{
|
||||
auto twist = toTwistStamped(vx, wz, stamp, frame);
|
||||
twist.twist.linear.y = vy;
|
||||
|
||||
return twist;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert path to a tensor
|
||||
* @param path Path to convert
|
||||
* @return Path tensor
|
||||
*/
|
||||
inline models::Path toTensor(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
auto result = models::Path{};
|
||||
result.reset(path.poses.size());
|
||||
|
||||
for (size_t i = 0; i < path.poses.size(); ++i) {
|
||||
result.x(i) = path.poses[i].pose.position.x;
|
||||
result.y(i) = path.poses[i].pose.position.y;
|
||||
result.yaws(i) = tf2::getYaw(path.poses[i].pose.orientation);
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if the robot pose is within the Goal Checker's tolerances to goal
|
||||
* @param global_checker Pointer to the goal checker
|
||||
* @param robot Pose of robot
|
||||
* @param goal Goal pose
|
||||
* @return bool If robot is within goal checker tolerances to the goal
|
||||
*/
|
||||
inline bool withinPositionGoalTolerance(
|
||||
nav2_core::GoalChecker * goal_checker,
|
||||
const geometry_msgs::msg::Pose & robot,
|
||||
const geometry_msgs::msg::Pose & goal)
|
||||
{
|
||||
if (goal_checker) {
|
||||
geometry_msgs::msg::Pose pose_tolerance;
|
||||
geometry_msgs::msg::Twist velocity_tolerance;
|
||||
goal_checker->getTolerances(pose_tolerance, velocity_tolerance);
|
||||
|
||||
const auto pose_tolerance_sq = pose_tolerance.position.x * pose_tolerance.position.x;
|
||||
|
||||
auto dx = robot.position.x - goal.position.x;
|
||||
auto dy = robot.position.y - goal.position.y;
|
||||
|
||||
auto dist_sq = dx * dx + dy * dy;
|
||||
|
||||
if (dist_sq < pose_tolerance_sq) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if the robot pose is within tolerance to the goal
|
||||
* @param pose_tolerance Pose tolerance to use
|
||||
* @param robot Pose of robot
|
||||
* @param goal Goal pose
|
||||
* @return bool If robot is within tolerance to the goal
|
||||
*/
|
||||
inline bool withinPositionGoalTolerance(
|
||||
float pose_tolerance,
|
||||
const geometry_msgs::msg::Pose & robot,
|
||||
const geometry_msgs::msg::Pose & goal)
|
||||
{
|
||||
const double & dist_sq =
|
||||
std::pow(goal.position.x - robot.position.x, 2) +
|
||||
std::pow(goal.position.y - robot.position.y, 2);
|
||||
|
||||
const float pose_tolerance_sq = pose_tolerance * pose_tolerance;
|
||||
|
||||
if (dist_sq < pose_tolerance_sq) {
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief normalize
|
||||
* Normalizes the angle to be -M_PI circle to +M_PI circle
|
||||
* It takes and returns radians.
|
||||
* @param angles Angles to normalize
|
||||
* @return normalized angles
|
||||
*/
|
||||
template<typename T>
|
||||
auto normalize_angles(const T & angles)
|
||||
{
|
||||
auto && theta = xt::eval(xt::fmod(angles + M_PI, 2.0 * M_PI));
|
||||
return xt::eval(xt::where(theta <= 0.0, theta + M_PI, theta - M_PI));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief shortest_angular_distance
|
||||
*
|
||||
* Given 2 angles, this returns the shortest angular
|
||||
* difference. The inputs and ouputs are of course radians.
|
||||
*
|
||||
* The result
|
||||
* would always be -pi <= result <= pi. Adding the result
|
||||
* to "from" will always get you an equivelent angle to "to".
|
||||
* @param from Start angle
|
||||
* @param to End angle
|
||||
* @return Shortest distance between angles
|
||||
*/
|
||||
template<typename F, typename T>
|
||||
auto shortest_angular_distance(
|
||||
const F & from,
|
||||
const T & to)
|
||||
{
|
||||
return normalize_angles(to - from);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Evaluate furthest point idx of data.path which is
|
||||
* nearset to some trajectory in data.trajectories
|
||||
* @param data Data to use
|
||||
* @return Idx of furthest path point reached by a set of trajectories
|
||||
*/
|
||||
inline size_t findPathFurthestReachedPoint(const CriticData & data)
|
||||
{
|
||||
const auto traj_x = xt::view(data.trajectories.x, xt::all(), -1, xt::newaxis());
|
||||
const auto traj_y = xt::view(data.trajectories.y, xt::all(), -1, xt::newaxis());
|
||||
|
||||
const auto dx = data.path.x - traj_x;
|
||||
const auto dy = data.path.y - traj_y;
|
||||
|
||||
const auto dists = dx * dx + dy * dy;
|
||||
|
||||
size_t max_id_by_trajectories = 0, min_id_by_path = 0;
|
||||
float min_distance_by_path = std::numeric_limits<float>::max();
|
||||
float cur_dist = 0.0f;
|
||||
|
||||
for (size_t i = 0; i < dists.shape(0); i++) {
|
||||
min_id_by_path = 0;
|
||||
min_distance_by_path = std::numeric_limits<float>::max();
|
||||
for (size_t j = 0; j < dists.shape(1); j++) {
|
||||
cur_dist = dists(i, j);
|
||||
if (cur_dist < min_distance_by_path) {
|
||||
min_distance_by_path = cur_dist;
|
||||
min_id_by_path = j;
|
||||
}
|
||||
}
|
||||
max_id_by_trajectories = std::max(max_id_by_trajectories, min_id_by_path);
|
||||
}
|
||||
return max_id_by_trajectories;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Evaluate closest point idx of data.path which is
|
||||
* nearset to the start of the trajectory in data.trajectories
|
||||
* @param data Data to use
|
||||
* @return Idx of closest path point at start of the trajectories
|
||||
*/
|
||||
inline size_t findPathTrajectoryInitialPoint(const CriticData & data)
|
||||
{
|
||||
// First point should be the same for all trajectories from initial conditions
|
||||
const auto dx = data.path.x - data.trajectories.x(0, 0);
|
||||
const auto dy = data.path.y - data.trajectories.y(0, 0);
|
||||
const auto dists = dx * dx + dy * dy;
|
||||
|
||||
float min_distance_by_path = std::numeric_limits<float>::max();
|
||||
size_t min_id = 0;
|
||||
for (size_t j = 0; j < dists.shape(0); j++) {
|
||||
if (dists(j) < min_distance_by_path) {
|
||||
min_distance_by_path = dists(j);
|
||||
min_id = j;
|
||||
}
|
||||
}
|
||||
|
||||
return min_id;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief evaluate path furthest point if it is not set
|
||||
* @param data Data to use
|
||||
*/
|
||||
inline void setPathFurthestPointIfNotSet(CriticData & data)
|
||||
{
|
||||
if (!data.furthest_reached_path_point) {
|
||||
data.furthest_reached_path_point = findPathFurthestReachedPoint(data);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief evaluate path costs
|
||||
* @param data Data to use
|
||||
*/
|
||||
inline void findPathCosts(
|
||||
CriticData & data,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
auto * costmap = costmap_ros->getCostmap();
|
||||
unsigned int map_x, map_y;
|
||||
const size_t path_segments_count = data.path.x.shape(0) - 1;
|
||||
data.path_pts_valid = std::vector<bool>(path_segments_count, false);
|
||||
for (unsigned int idx = 0; idx < path_segments_count; idx++) {
|
||||
const auto path_x = data.path.x(idx);
|
||||
const auto path_y = data.path.y(idx);
|
||||
if (!costmap->worldToMap(path_x, path_y, map_x, map_y)) {
|
||||
(*data.path_pts_valid)[idx] = false;
|
||||
continue;
|
||||
}
|
||||
|
||||
switch (costmap->getCost(map_x, map_y)) {
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
case (LETHAL_OBSTACLE):
|
||||
(*data.path_pts_valid)[idx] = false;
|
||||
continue;
|
||||
case (INSCRIBED_INFLATED_OBSTACLE):
|
||||
(*data.path_pts_valid)[idx] = false;
|
||||
continue;
|
||||
case (NO_INFORMATION):
|
||||
const bool is_tracking_unknown =
|
||||
costmap_ros->getLayeredCostmap()->isTrackingUnknown();
|
||||
(*data.path_pts_valid)[idx] = is_tracking_unknown ? true : false;
|
||||
continue;
|
||||
}
|
||||
|
||||
(*data.path_pts_valid)[idx] = true;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief evaluate path costs if it is not set
|
||||
* @param data Data to use
|
||||
*/
|
||||
inline void setPathCostsIfNotSet(
|
||||
CriticData & data,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
if (!data.path_pts_valid) {
|
||||
findPathCosts(data, costmap_ros);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief evaluate angle from pose (have angle) to point (no angle)
|
||||
* @param pose pose
|
||||
* @param point_x Point to find angle relative to X axis
|
||||
* @param point_y Point to find angle relative to Y axis
|
||||
* @param forward_preference If reversing direction is valid
|
||||
* @return Angle between two points
|
||||
*/
|
||||
inline float posePointAngle(
|
||||
const geometry_msgs::msg::Pose & pose, double point_x, double point_y, bool forward_preference)
|
||||
{
|
||||
float pose_x = pose.position.x;
|
||||
float pose_y = pose.position.y;
|
||||
float pose_yaw = tf2::getYaw(pose.orientation);
|
||||
|
||||
float yaw = atan2f(point_y - pose_y, point_x - pose_x);
|
||||
|
||||
// If no preference for forward, return smallest angle either in heading or 180 of heading
|
||||
if (!forward_preference) {
|
||||
return std::min(
|
||||
fabs(angles::shortest_angular_distance(yaw, pose_yaw)),
|
||||
fabs(angles::shortest_angular_distance(yaw, angles::normalize_angle(pose_yaw + M_PI))));
|
||||
}
|
||||
|
||||
return fabs(angles::shortest_angular_distance(yaw, pose_yaw));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Apply Savisky-Golay filter to optimal trajectory
|
||||
* @param control_sequence Sequence to apply filter to
|
||||
* @param control_history Recent set of controls for edge-case handling
|
||||
* @param Settings Settings to use
|
||||
*/
|
||||
inline void savitskyGolayFilter(
|
||||
models::ControlSequence & control_sequence,
|
||||
std::array<mppi::models::Control, 4> & control_history,
|
||||
const models::OptimizerSettings & settings)
|
||||
{
|
||||
// Savitzky-Golay Quadratic, 9-point Coefficients
|
||||
xt::xarray<float> filter = {-21.0, 14.0, 39.0, 54.0, 59.0, 54.0, 39.0, 14.0, -21.0};
|
||||
filter /= 231.0;
|
||||
|
||||
const unsigned int num_sequences = control_sequence.vx.shape(0) - 1;
|
||||
|
||||
// Too short to smooth meaningfully
|
||||
if (num_sequences < 20) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto applyFilter = [&](const xt::xarray<float> & data) -> float {
|
||||
return xt::sum(data * filter, {0}, immediate)();
|
||||
};
|
||||
|
||||
auto applyFilterOverAxis =
|
||||
[&](xt::xtensor<float, 1> & sequence,
|
||||
const float hist_0, const float hist_1, const float hist_2, const float hist_3) -> void
|
||||
{
|
||||
unsigned int idx = 0;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
hist_0,
|
||||
hist_1,
|
||||
hist_2,
|
||||
hist_3,
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 4)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
hist_1,
|
||||
hist_2,
|
||||
hist_3,
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 4)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
hist_2,
|
||||
hist_3,
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 4)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
hist_3,
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 4)});
|
||||
|
||||
for (idx = 4; idx != num_sequences - 4; idx++) {
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
sequence(idx - 4),
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 4)});
|
||||
}
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
sequence(idx - 4),
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 3),
|
||||
sequence(idx + 3)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
sequence(idx - 4),
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 2),
|
||||
sequence(idx + 2)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
sequence(idx - 4),
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 1),
|
||||
sequence(idx + 1)});
|
||||
|
||||
idx++;
|
||||
sequence(idx) = applyFilter(
|
||||
{
|
||||
sequence(idx - 4),
|
||||
sequence(idx - 3),
|
||||
sequence(idx - 2),
|
||||
sequence(idx - 1),
|
||||
sequence(idx),
|
||||
sequence(idx),
|
||||
sequence(idx),
|
||||
sequence(idx),
|
||||
sequence(idx)});
|
||||
};
|
||||
|
||||
// Filter trajectories
|
||||
applyFilterOverAxis(
|
||||
control_sequence.vx, control_history[0].vx,
|
||||
control_history[1].vx, control_history[2].vx, control_history[3].vx);
|
||||
applyFilterOverAxis(
|
||||
control_sequence.vy, control_history[0].vy,
|
||||
control_history[1].vy, control_history[2].vy, control_history[3].vy);
|
||||
applyFilterOverAxis(
|
||||
control_sequence.wz, control_history[0].wz,
|
||||
control_history[1].wz, control_history[2].wz, control_history[3].wz);
|
||||
|
||||
// Update control history
|
||||
unsigned int offset = settings.shift_control_sequence ? 1 : 0;
|
||||
control_history[0] = control_history[1];
|
||||
control_history[1] = control_history[2];
|
||||
control_history[2] = control_history[3];
|
||||
control_history[3] = {
|
||||
control_sequence.vx(offset),
|
||||
control_sequence.vy(offset),
|
||||
control_sequence.wz(offset)};
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Find the iterator of the first pose at which there is an inversion on the path,
|
||||
* @param path to check for inversion
|
||||
* @return the first point after the inversion found in the path
|
||||
*/
|
||||
inline unsigned int findFirstPathInversion(nav_msgs::msg::Path & path)
|
||||
{
|
||||
// At least 3 poses for a possible inversion
|
||||
if (path.poses.size() < 3) {
|
||||
return path.poses.size();
|
||||
}
|
||||
|
||||
// Iterating through the path to determine the position of the path inversion
|
||||
for (unsigned int idx = 1; idx < path.poses.size() - 1; ++idx) {
|
||||
// We have two vectors for the dot product OA and AB. Determining the vectors.
|
||||
float oa_x = path.poses[idx].pose.position.x -
|
||||
path.poses[idx - 1].pose.position.x;
|
||||
float oa_y = path.poses[idx].pose.position.y -
|
||||
path.poses[idx - 1].pose.position.y;
|
||||
float ab_x = path.poses[idx + 1].pose.position.x -
|
||||
path.poses[idx].pose.position.x;
|
||||
float ab_y = path.poses[idx + 1].pose.position.y -
|
||||
path.poses[idx].pose.position.y;
|
||||
|
||||
// Checking for the existance of cusp, in the path, using the dot product.
|
||||
float dot_product = (oa_x * ab_x) + (oa_y * ab_y);
|
||||
if (dot_product < 0.0) {
|
||||
return idx + 1;
|
||||
}
|
||||
}
|
||||
|
||||
return path.poses.size();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Find and remove poses after the first inversion in the path
|
||||
* @param path to check for inversion
|
||||
* @return The location of the inversion, return 0 if none exist
|
||||
*/
|
||||
inline unsigned int removePosesAfterFirstInversion(nav_msgs::msg::Path & path)
|
||||
{
|
||||
nav_msgs::msg::Path cropped_path = path;
|
||||
const unsigned int first_after_inversion = findFirstPathInversion(cropped_path);
|
||||
if (first_after_inversion == path.poses.size()) {
|
||||
return 0u;
|
||||
}
|
||||
|
||||
cropped_path.poses.erase(
|
||||
cropped_path.poses.begin() + first_after_inversion, cropped_path.poses.end());
|
||||
path = cropped_path;
|
||||
return first_after_inversion;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compare to trajectory points to find closest path point along integrated distances
|
||||
* @param vec Vect to check
|
||||
* @return dist Distance to look for
|
||||
*/
|
||||
inline size_t findClosestPathPt(const std::vector<float> & vec, float dist, size_t init = 0)
|
||||
{
|
||||
auto iter = std::lower_bound(vec.begin() + init, vec.end(), dist);
|
||||
if (iter == vec.begin() + init) {
|
||||
return 0;
|
||||
}
|
||||
if (dist - *(iter - 1) < *iter - dist) {
|
||||
return iter - 1 - vec.begin();
|
||||
}
|
||||
return iter - vec.begin();
|
||||
}
|
||||
|
||||
} // namespace mppi::utils
|
||||
|
||||
#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 8.2 MiB |
@@ -0,0 +1,7 @@
|
||||
<class_libraries>
|
||||
<library path="mppi_controller">
|
||||
<class type="nav2_mppi_controller::MPPIController" base_class_type="nav2_core::Controller">
|
||||
<description>MPPI Controller for Nav2</description>
|
||||
</class>
|
||||
</library>
|
||||
</class_libraries>
|
||||
@@ -0,0 +1,42 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>nav2_mppi_controller</name>
|
||||
<version>1.1.18</version>
|
||||
<description>nav2_mppi_controller</description>
|
||||
<maintainer email="stevenmacenski@gmail.com">Steve Macenski</maintainer>
|
||||
<maintainer email="budyakov.aleksei@gmail.com">Aleksei Budyakov</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<buildtool_depend>ament_cmake_ros</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>nav2_common</depend>
|
||||
<depend>nav2_core</depend>
|
||||
<depend>nav2_util</depend>
|
||||
<depend>nav2_costmap_2d</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>nav2_msgs</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>xtensor</depend>
|
||||
<depend>libomp-dev</depend>
|
||||
<depend>benchmark</depend>
|
||||
<depend>xsimd</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
<nav2_core plugin="${prefix}/mppic.xml" />
|
||||
<nav2_mppi_controller plugin="${prefix}/critics.xml" />
|
||||
</export>
|
||||
|
||||
</package>
|
||||
@@ -0,0 +1,138 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <stdint.h>
|
||||
#include <chrono>
|
||||
#include "nav2_mppi_controller/controller.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
|
||||
// #define BENCHMARK_TESTING
|
||||
|
||||
namespace nav2_mppi_controller
|
||||
{
|
||||
|
||||
void MPPIController::configure(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent,
|
||||
std::string name, const std::shared_ptr<tf2_ros::Buffer> tf,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
|
||||
{
|
||||
parent_ = parent;
|
||||
costmap_ros_ = costmap_ros;
|
||||
tf_buffer_ = tf;
|
||||
name_ = name;
|
||||
parameters_handler_ = std::make_unique<ParametersHandler>(parent);
|
||||
|
||||
auto node = parent_.lock();
|
||||
clock_ = node->get_clock();
|
||||
last_time_called_ = clock_->now();
|
||||
// Get high-level controller parameters
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(visualize_, "visualize", false);
|
||||
getParam(reset_period_, "reset_period", 1.0);
|
||||
|
||||
// Configure composed objects
|
||||
optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get());
|
||||
path_handler_.initialize(parent_, name_, costmap_ros_, tf_buffer_, parameters_handler_.get());
|
||||
trajectory_visualizer_.on_configure(
|
||||
parent_, name_,
|
||||
costmap_ros_->getGlobalFrameID(), parameters_handler_.get());
|
||||
|
||||
RCLCPP_INFO(logger_, "Configured MPPI Controller: %s", name_.c_str());
|
||||
}
|
||||
|
||||
void MPPIController::cleanup()
|
||||
{
|
||||
optimizer_.shutdown();
|
||||
trajectory_visualizer_.on_cleanup();
|
||||
parameters_handler_.reset();
|
||||
RCLCPP_INFO(logger_, "Cleaned up MPPI Controller: %s", name_.c_str());
|
||||
}
|
||||
|
||||
void MPPIController::activate()
|
||||
{
|
||||
trajectory_visualizer_.on_activate();
|
||||
parameters_handler_->start();
|
||||
RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
|
||||
}
|
||||
|
||||
void MPPIController::deactivate()
|
||||
{
|
||||
trajectory_visualizer_.on_deactivate();
|
||||
RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
|
||||
}
|
||||
|
||||
void MPPIController::reset()
|
||||
{
|
||||
optimizer_.reset();
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
nav2_core::GoalChecker * goal_checker)
|
||||
{
|
||||
#ifdef BENCHMARK_TESTING
|
||||
auto start = std::chrono::system_clock::now();
|
||||
#endif
|
||||
|
||||
if (clock_->now() - last_time_called_ > rclcpp::Duration::from_seconds(reset_period_)) {
|
||||
reset();
|
||||
}
|
||||
last_time_called_ = clock_->now();
|
||||
|
||||
std::lock_guard<std::mutex> param_lock(*parameters_handler_->getLock());
|
||||
geometry_msgs::msg::Pose goal = path_handler_.getTransformedGoal(robot_pose.header.stamp).pose;
|
||||
|
||||
nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose);
|
||||
|
||||
nav2_costmap_2d::Costmap2D * costmap = costmap_ros_->getCostmap();
|
||||
std::unique_lock<nav2_costmap_2d::Costmap2D::mutex_t> costmap_lock(*(costmap->getMutex()));
|
||||
|
||||
geometry_msgs::msg::TwistStamped cmd =
|
||||
optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal, goal_checker);
|
||||
|
||||
#ifdef BENCHMARK_TESTING
|
||||
auto end = std::chrono::system_clock::now();
|
||||
auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(end - start).count();
|
||||
RCLCPP_INFO(logger_, "Control loop execution time: %ld [ms]", duration);
|
||||
#endif
|
||||
|
||||
if (visualize_) {
|
||||
visualize(std::move(transformed_plan));
|
||||
}
|
||||
|
||||
return cmd;
|
||||
}
|
||||
|
||||
void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
|
||||
{
|
||||
trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
|
||||
trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
|
||||
trajectory_visualizer_.visualize(std::move(transformed_plan));
|
||||
}
|
||||
|
||||
void MPPIController::setPlan(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
path_handler_.setPath(path);
|
||||
}
|
||||
|
||||
void MPPIController::setSpeedLimit(const double & speed_limit, const bool & percentage)
|
||||
{
|
||||
optimizer_.setSpeedLimit(speed_limit, percentage);
|
||||
}
|
||||
|
||||
} // namespace nav2_mppi_controller
|
||||
|
||||
#include "pluginlib/class_list_macros.hpp"
|
||||
PLUGINLIB_EXPORT_CLASS(nav2_mppi_controller::MPPIController, nav2_core::Controller)
|
||||
@@ -0,0 +1,78 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critic_manager.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
void CriticManager::on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros, ParametersHandler * param_handler)
|
||||
{
|
||||
parent_ = parent;
|
||||
costmap_ros_ = costmap_ros;
|
||||
name_ = name;
|
||||
auto node = parent_.lock();
|
||||
logger_ = node->get_logger();
|
||||
parameters_handler_ = param_handler;
|
||||
|
||||
getParams();
|
||||
loadCritics();
|
||||
}
|
||||
|
||||
void CriticManager::getParams()
|
||||
{
|
||||
auto node = parent_.lock();
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(critic_names_, "critics", std::vector<std::string>{}, ParameterType::Static);
|
||||
}
|
||||
|
||||
void CriticManager::loadCritics()
|
||||
{
|
||||
if (!loader_) {
|
||||
loader_ = std::make_unique<pluginlib::ClassLoader<critics::CriticFunction>>(
|
||||
"nav2_mppi_controller", "mppi::critics::CriticFunction");
|
||||
}
|
||||
|
||||
critics_.clear();
|
||||
for (auto name : critic_names_) {
|
||||
std::string fullname = getFullName(name);
|
||||
auto instance = std::unique_ptr<critics::CriticFunction>(
|
||||
loader_->createUnmanagedInstance(fullname));
|
||||
critics_.push_back(std::move(instance));
|
||||
critics_.back()->on_configure(
|
||||
parent_, name_, name_ + "." + name, costmap_ros_,
|
||||
parameters_handler_);
|
||||
RCLCPP_INFO(logger_, "Critic loaded : %s", fullname.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
std::string CriticManager::getFullName(const std::string & name)
|
||||
{
|
||||
return "mppi::critics::" + name;
|
||||
}
|
||||
|
||||
void CriticManager::evalTrajectoriesScores(
|
||||
CriticData & data) const
|
||||
{
|
||||
for (size_t q = 0; q < critics_.size(); q++) {
|
||||
if (data.fail_flag) {
|
||||
break;
|
||||
}
|
||||
critics_[q]->score(data);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,81 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/constraint_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void ConstraintCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
|
||||
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 4.0);
|
||||
RCLCPP_INFO(
|
||||
logger_, "ConstraintCritic instantiated with %d power and %f weight.",
|
||||
power_, weight_);
|
||||
|
||||
float vx_max, vy_max, vx_min;
|
||||
getParentParam(vx_max, "vx_max", 0.5);
|
||||
getParentParam(vy_max, "vy_max", 0.0);
|
||||
getParentParam(vx_min, "vx_min", -0.35);
|
||||
|
||||
const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0;
|
||||
max_vel_ = sqrtf(vx_max * vx_max + vy_max * vy_max);
|
||||
min_vel_ = min_sgn * sqrtf(vx_min * vx_min + vy_max * vy_max);
|
||||
}
|
||||
|
||||
void ConstraintCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto sgn = xt::where(data.state.vx > 0.0, 1.0, -1.0);
|
||||
auto vel_total = sgn * xt::sqrt(data.state.vx * data.state.vx + data.state.vy * data.state.vy);
|
||||
auto out_of_max_bounds_motion = xt::maximum(vel_total - max_vel_, 0);
|
||||
auto out_of_min_bounds_motion = xt::maximum(min_vel_ - vel_total, 0);
|
||||
|
||||
auto acker = dynamic_cast<AckermannMotionModel *>(data.motion_model.get());
|
||||
if (acker != nullptr) {
|
||||
auto & vx = data.state.vx;
|
||||
auto & wz = data.state.wz;
|
||||
auto out_of_turning_rad_motion = xt::maximum(
|
||||
acker->getMinTurningRadius() - (xt::fabs(vx) / xt::fabs(wz)), 0.0);
|
||||
|
||||
data.costs += xt::pow(
|
||||
xt::sum(
|
||||
(std::move(out_of_max_bounds_motion) +
|
||||
std::move(out_of_min_bounds_motion) +
|
||||
std::move(out_of_turning_rad_motion)) *
|
||||
data.model_dt, {1}, immediate) * weight_, power_);
|
||||
return;
|
||||
}
|
||||
|
||||
data.costs += xt::pow(
|
||||
xt::sum(
|
||||
(std::move(out_of_max_bounds_motion) +
|
||||
std::move(out_of_min_bounds_motion)) *
|
||||
data.model_dt, {1}, immediate) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(mppi::critics::ConstraintCritic, mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,231 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
#include "nav2_mppi_controller/critics/cost_critic.hpp"
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void CostCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(consider_footprint_, "consider_footprint", false);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 3.81);
|
||||
getParam(critical_cost_, "critical_cost", 300.0);
|
||||
getParam(collision_cost_, "collision_cost", 1000000.0);
|
||||
getParam(near_goal_distance_, "near_goal_distance", 0.5);
|
||||
getParam(inflation_layer_name_, "inflation_layer_name", std::string(""));
|
||||
|
||||
// Normalized by cost value to put in same regime as other weights
|
||||
weight_ /= 254.0f;
|
||||
|
||||
// Normalize weight when parameter is changed dynamically as well
|
||||
auto weightDynamicCb = [&](const rclcpp::Parameter & weight) {
|
||||
weight_ = weight.as_double() / 254.0f;
|
||||
};
|
||||
parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb);
|
||||
|
||||
collision_checker_.setCostmap(costmap_);
|
||||
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
|
||||
|
||||
if (possibly_inscribed_cost_ < 1.0f) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"Inflation layer either not found or inflation is not set sufficiently for "
|
||||
"optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
|
||||
" the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
|
||||
"github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
|
||||
" for full instructions. This will substantially impact run-time performance.");
|
||||
}
|
||||
|
||||
if (costmap_ros_->getUseRadius() == consider_footprint_) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Inconsistent configuration in collision checking. Please verify the robot's shape settings "
|
||||
"in both the costmap and the cost critic.");
|
||||
if (costmap_ros_->getUseRadius()) {
|
||||
throw nav2_core::PlannerException(
|
||||
"Considering footprint in collision checking but no robot footprint provided in the "
|
||||
"costmap.");
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"InflationCostCritic instantiated with %d power and %f / %f weights. "
|
||||
"Critic will collision check based on %s cost.",
|
||||
power_, critical_cost_, weight_, consider_footprint_ ?
|
||||
"footprint" : "circular");
|
||||
}
|
||||
|
||||
float CostCritic::findCircumscribedCost(
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
|
||||
{
|
||||
bool inflation_layer_found = false;
|
||||
double result = -1.0;
|
||||
const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
|
||||
if (static_cast<float>(circum_radius) == circumscribed_radius_) {
|
||||
// early return if footprint size is unchanged
|
||||
return circumscribed_cost_;
|
||||
}
|
||||
|
||||
// check if the costmap has an inflation layer
|
||||
for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
|
||||
layer != costmap->getLayeredCostmap()->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
|
||||
if (!inflation_layer ||
|
||||
(!inflation_layer_name_.empty() &&
|
||||
inflation_layer->getName() != inflation_layer_name_))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
inflation_layer_found = true;
|
||||
const double resolution = costmap->getCostmap()->getResolution();
|
||||
result = inflation_layer->computeCost(circum_radius / resolution);
|
||||
}
|
||||
|
||||
if (!inflation_layer_found) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"No inflation layer found in costmap configuration. "
|
||||
"If this is an SE2-collision checking plugin, it cannot use costmap potential "
|
||||
"field to speed up collision checking by only checking the full footprint "
|
||||
"when robot is within possibly-inscribed radius of an obstacle. This may "
|
||||
"significantly slow down planning times and not avoid anything but absolute collisions!");
|
||||
}
|
||||
|
||||
circumscribed_radius_ = static_cast<float>(circum_radius);
|
||||
circumscribed_cost_ = static_cast<float>(result);
|
||||
|
||||
return circumscribed_cost_;
|
||||
}
|
||||
|
||||
void CostCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (consider_footprint_) {
|
||||
// footprint may have changed since initialization if user has dynamic footprints
|
||||
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
|
||||
}
|
||||
|
||||
// If near the goal, don't apply the preferential term since the goal is near obstacles
|
||||
bool near_goal = false;
|
||||
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
|
||||
near_goal = true;
|
||||
}
|
||||
|
||||
auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
|
||||
repulsive_cost.fill(0.0);
|
||||
|
||||
const size_t traj_len = data.trajectories.x.shape(1);
|
||||
bool all_trajectories_collide = true;
|
||||
for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
|
||||
bool trajectory_collide = false;
|
||||
const auto & traj = data.trajectories;
|
||||
float pose_cost;
|
||||
|
||||
for (size_t j = 0; j < traj_len; j++) {
|
||||
// The costAtPose doesn't use orientation
|
||||
// The footprintCostAtPose will always return "INSCRIBED" if footprint is over it
|
||||
// So the center point has more information than the footprint
|
||||
pose_cost = costAtPose(traj.x(i, j), traj.y(i, j));
|
||||
if (pose_cost < 1.0f) {continue;} // In free space
|
||||
|
||||
if (inCollision(pose_cost, traj.x(i, j), traj.y(i, j), traj.yaws(i, j))) {
|
||||
trajectory_collide = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// Let near-collision trajectory points be punished severely
|
||||
// Note that we collision check based on the footprint actual,
|
||||
// but score based on the center-point cost regardless
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
if (pose_cost >= INSCRIBED_INFLATED_OBSTACLE) {
|
||||
repulsive_cost[i] += critical_cost_;
|
||||
} else if (!near_goal) { // Generally prefer trajectories further from obstacles
|
||||
repulsive_cost[i] += pose_cost;
|
||||
}
|
||||
}
|
||||
|
||||
if (!trajectory_collide) {
|
||||
all_trajectories_collide = false;
|
||||
} else {
|
||||
repulsive_cost[i] = collision_cost_;
|
||||
}
|
||||
}
|
||||
|
||||
data.costs += xt::pow((weight_ * repulsive_cost / traj_len), power_);
|
||||
data.fail_flag = all_trajectories_collide;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if cost represents a collision
|
||||
* @param cost Costmap cost
|
||||
* @return bool if in collision
|
||||
*/
|
||||
bool CostCritic::inCollision(float cost, float x, float y, float theta)
|
||||
{
|
||||
bool is_tracking_unknown =
|
||||
costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
|
||||
|
||||
// If consider_footprint_ check footprint scort for collision
|
||||
if (consider_footprint_ &&
|
||||
(cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
|
||||
{
|
||||
cost = static_cast<float>(collision_checker_.footprintCostAtPose(
|
||||
x, y, theta, costmap_ros_->getRobotFootprint()));
|
||||
}
|
||||
|
||||
switch (static_cast<unsigned char>(cost)) {
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
case (LETHAL_OBSTACLE):
|
||||
return true;
|
||||
case (INSCRIBED_INFLATED_OBSTACLE):
|
||||
return consider_footprint_ ? false : true;
|
||||
case (NO_INFORMATION):
|
||||
return is_tracking_unknown ? false : true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
float CostCritic::costAtPose(float x, float y)
|
||||
{
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
unsigned int x_i, y_i;
|
||||
if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
|
||||
return nav2_costmap_2d::NO_INFORMATION;
|
||||
}
|
||||
|
||||
return collision_checker_.pointCost(x_i, y_i);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::CostCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,58 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void GoalAngleCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 3.0);
|
||||
|
||||
getParam(threshold_to_consider_, "threshold_to_consider", 0.5);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"GoalAngleCritic instantiated with %d power, %f weight, and %f "
|
||||
"angular threshold.",
|
||||
power_, weight_, threshold_to_consider_);
|
||||
}
|
||||
|
||||
void GoalAngleCritic::score(CriticData & data)
|
||||
{
|
||||
if (!enabled_ || !utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const auto goal_idx = data.path.x.shape(0) - 1;
|
||||
const float goal_yaw = data.path.yaws(goal_idx);
|
||||
|
||||
data.costs += xt::pow(
|
||||
xt::mean(xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, goal_yaw)), {1}) *
|
||||
weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::GoalAngleCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,61 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/goal_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
void GoalCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 5.0);
|
||||
getParam(threshold_to_consider_, "threshold_to_consider", 1.4);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_, "GoalCritic instantiated with %d power and %f weight.",
|
||||
power_, weight_);
|
||||
}
|
||||
|
||||
void GoalCritic::score(CriticData & data)
|
||||
{
|
||||
if (!enabled_ || !utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const auto & goal_x = data.goal.position.x;
|
||||
const auto & goal_y = data.goal.position.y;
|
||||
|
||||
const auto traj_x = xt::view(data.trajectories.x, xt::all(), xt::all());
|
||||
const auto traj_y = xt::view(data.trajectories.y, xt::all(), xt::all());
|
||||
|
||||
auto dists = xt::sqrt(
|
||||
xt::pow(traj_x - goal_x, 2) +
|
||||
xt::pow(traj_y - goal_y, 2));
|
||||
|
||||
data.costs += xt::pow(xt::mean(dists, {1}, immediate) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(mppi::critics::GoalCritic, mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,245 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <cmath>
|
||||
#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void ObstaclesCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(consider_footprint_, "consider_footprint", false);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(repulsion_weight_, "repulsion_weight", 1.5);
|
||||
getParam(critical_weight_, "critical_weight", 20.0);
|
||||
getParam(collision_cost_, "collision_cost", 10000.0);
|
||||
getParam(collision_margin_distance_, "collision_margin_distance", 0.10);
|
||||
getParam(near_goal_distance_, "near_goal_distance", 0.5);
|
||||
|
||||
collision_checker_.setCostmap(costmap_);
|
||||
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
|
||||
|
||||
if (possibly_inscribed_cost_ < 1.0f) {
|
||||
RCLCPP_ERROR(
|
||||
logger_,
|
||||
"Inflation layer either not found or inflation is not set sufficiently for "
|
||||
"optimized non-circular collision checking capabilities. It is HIGHLY recommended to set"
|
||||
" the inflation radius to be at MINIMUM half of the robot's largest cross-section. See "
|
||||
"github.com/ros-planning/navigation2/tree/main/nav2_smac_planner#potential-fields"
|
||||
" for full instructions. This will substantially impact run-time performance.");
|
||||
}
|
||||
|
||||
if (costmap_ros_->getUseRadius() == consider_footprint_) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Inconsistent configuration in collision checking. Please verify the robot's shape settings "
|
||||
"in both the costmap and the obstacle critic.");
|
||||
if (costmap_ros_->getUseRadius()) {
|
||||
throw nav2_core::PlannerException(
|
||||
"Considering footprint in collision checking but no robot footprint provided in the "
|
||||
"costmap.");
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"ObstaclesCritic instantiated with %d power and %f / %f weights. "
|
||||
"Critic will collision check based on %s cost.",
|
||||
power_, critical_weight_, repulsion_weight_, consider_footprint_ ?
|
||||
"footprint" : "circular");
|
||||
}
|
||||
|
||||
float ObstaclesCritic::findCircumscribedCost(
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap)
|
||||
{
|
||||
double result = -1.0;
|
||||
bool inflation_layer_found = false;
|
||||
|
||||
const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius();
|
||||
if (static_cast<float>(circum_radius) == circumscribed_radius_) {
|
||||
// early return if footprint size is unchanged
|
||||
return circumscribed_cost_;
|
||||
}
|
||||
|
||||
// check if the costmap has an inflation layer
|
||||
for (auto layer = costmap->getLayeredCostmap()->getPlugins()->begin();
|
||||
layer != costmap->getLayeredCostmap()->getPlugins()->end();
|
||||
++layer)
|
||||
{
|
||||
auto inflation_layer = std::dynamic_pointer_cast<nav2_costmap_2d::InflationLayer>(*layer);
|
||||
if (!inflation_layer) {
|
||||
continue;
|
||||
}
|
||||
|
||||
inflation_layer_found = true;
|
||||
const double resolution = costmap->getCostmap()->getResolution();
|
||||
result = inflation_layer->computeCost(circum_radius / resolution);
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(inflation_scale_factor_, "cost_scaling_factor", 10.0);
|
||||
getParam(inflation_radius_, "inflation_radius", 0.55);
|
||||
}
|
||||
|
||||
if (!inflation_layer_found) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"No inflation layer found in costmap configuration. "
|
||||
"If this is an SE2-collision checking plugin, it cannot use costmap potential "
|
||||
"field to speed up collision checking by only checking the full footprint "
|
||||
"when robot is within possibly-inscribed radius of an obstacle. This may "
|
||||
"significantly slow down planning times and not avoid anything but absolute collisions!");
|
||||
}
|
||||
|
||||
circumscribed_radius_ = static_cast<float>(circum_radius);
|
||||
circumscribed_cost_ = static_cast<float>(result);
|
||||
|
||||
return circumscribed_cost_;
|
||||
}
|
||||
|
||||
float ObstaclesCritic::distanceToObstacle(const CollisionCost & cost)
|
||||
{
|
||||
const float scale_factor = inflation_scale_factor_;
|
||||
const float min_radius = costmap_ros_->getLayeredCostmap()->getInscribedRadius();
|
||||
float dist_to_obj = (scale_factor * min_radius - log(cost.cost) + log(253.0f)) / scale_factor;
|
||||
|
||||
// If not footprint collision checking, the cost is using the center point cost and
|
||||
// needs the radius subtracted to obtain the closest distance to the object
|
||||
if (!cost.using_footprint) {
|
||||
dist_to_obj -= min_radius;
|
||||
}
|
||||
|
||||
return dist_to_obj;
|
||||
}
|
||||
|
||||
void ObstaclesCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (consider_footprint_) {
|
||||
// footprint may have changed since initialization if user has dynamic footprints
|
||||
possibly_inscribed_cost_ = findCircumscribedCost(costmap_ros_);
|
||||
}
|
||||
|
||||
// If near the goal, don't apply the preferential term since the goal is near obstacles
|
||||
bool near_goal = false;
|
||||
if (utils::withinPositionGoalTolerance(near_goal_distance_, data.state.pose.pose, data.goal)) {
|
||||
near_goal = true;
|
||||
}
|
||||
|
||||
auto && raw_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
|
||||
raw_cost.fill(0.0f);
|
||||
auto && repulsive_cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
|
||||
repulsive_cost.fill(0.0f);
|
||||
|
||||
const size_t traj_len = data.trajectories.x.shape(1);
|
||||
bool all_trajectories_collide = true;
|
||||
for (size_t i = 0; i < data.trajectories.x.shape(0); ++i) {
|
||||
bool trajectory_collide = false;
|
||||
float traj_cost = 0.0f;
|
||||
const auto & traj = data.trajectories;
|
||||
CollisionCost pose_cost;
|
||||
|
||||
for (size_t j = 0; j < traj_len; j++) {
|
||||
pose_cost = costAtPose(traj.x(i, j), traj.y(i, j), traj.yaws(i, j));
|
||||
if (pose_cost.cost < 1.0f) {continue;} // In free space
|
||||
|
||||
if (inCollision(pose_cost.cost)) {
|
||||
trajectory_collide = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// Cannot process repulsion if inflation layer does not exist
|
||||
if (inflation_radius_ == 0.0f || inflation_scale_factor_ == 0.0f) {
|
||||
continue;
|
||||
}
|
||||
|
||||
const float dist_to_obj = distanceToObstacle(pose_cost);
|
||||
|
||||
// Let near-collision trajectory points be punished severely
|
||||
if (dist_to_obj < collision_margin_distance_) {
|
||||
traj_cost += (collision_margin_distance_ - dist_to_obj);
|
||||
} else if (!near_goal) { // Generally prefer trajectories further from obstacles
|
||||
repulsive_cost[i] += (inflation_radius_ - dist_to_obj);
|
||||
}
|
||||
}
|
||||
|
||||
if (!trajectory_collide) {all_trajectories_collide = false;}
|
||||
raw_cost[i] = trajectory_collide ? collision_cost_ : traj_cost;
|
||||
}
|
||||
|
||||
data.costs += xt::pow(
|
||||
(critical_weight_ * raw_cost) +
|
||||
(repulsion_weight_ * repulsive_cost / traj_len),
|
||||
power_);
|
||||
data.fail_flag = all_trajectories_collide;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Checks if cost represents a collision
|
||||
* @param cost Costmap cost
|
||||
* @return bool if in collision
|
||||
*/
|
||||
bool ObstaclesCritic::inCollision(float cost) const
|
||||
{
|
||||
bool is_tracking_unknown =
|
||||
costmap_ros_->getLayeredCostmap()->isTrackingUnknown();
|
||||
|
||||
switch (static_cast<unsigned char>(cost)) {
|
||||
using namespace nav2_costmap_2d; // NOLINT
|
||||
case (LETHAL_OBSTACLE):
|
||||
return true;
|
||||
case (INSCRIBED_INFLATED_OBSTACLE):
|
||||
return consider_footprint_ ? false : true;
|
||||
case (NO_INFORMATION):
|
||||
return is_tracking_unknown ? false : true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
CollisionCost ObstaclesCritic::costAtPose(float x, float y, float theta)
|
||||
{
|
||||
CollisionCost collision_cost;
|
||||
float & cost = collision_cost.cost;
|
||||
collision_cost.using_footprint = false;
|
||||
unsigned int x_i, y_i;
|
||||
if (!collision_checker_.worldToMap(x, y, x_i, y_i)) {
|
||||
cost = nav2_costmap_2d::NO_INFORMATION;
|
||||
return collision_cost;
|
||||
}
|
||||
cost = collision_checker_.pointCost(x_i, y_i);
|
||||
|
||||
if (consider_footprint_ &&
|
||||
(cost >= possibly_inscribed_cost_ || possibly_inscribed_cost_ < 1.0f))
|
||||
{
|
||||
cost = static_cast<float>(collision_checker_.footprintCostAtPose(
|
||||
x, y, theta, costmap_ros_->getRobotFootprint()));
|
||||
collision_cost.using_footprint = true;
|
||||
}
|
||||
|
||||
return collision_cost;
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::ObstaclesCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,144 @@
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/path_align_critic.hpp"
|
||||
|
||||
#include <xtensor/xfixed.hpp>
|
||||
#include <xtensor/xmath.hpp>
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
using namespace xt::placeholders; // NOLINT
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
void PathAlignCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 10.0);
|
||||
|
||||
getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07);
|
||||
getParam(offset_from_furthest_, "offset_from_furthest", 20);
|
||||
getParam(trajectory_point_step_, "trajectory_point_step", 4);
|
||||
getParam(
|
||||
threshold_to_consider_,
|
||||
"threshold_to_consider", 0.5);
|
||||
getParam(use_path_orientations_, "use_path_orientations", false);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"ReferenceTrajectoryCritic instantiated with %d power and %f weight",
|
||||
power_, weight_);
|
||||
}
|
||||
|
||||
void PathAlignCritic::score(CriticData & data)
|
||||
{
|
||||
// Don't apply close to goal, let the goal critics take over
|
||||
if (!enabled_ || utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't apply when first getting bearing w.r.t. the path
|
||||
utils::setPathFurthestPointIfNotSet(data);
|
||||
const size_t path_segments_count = *data.furthest_reached_path_point; // up to furthest only
|
||||
if (path_segments_count < offset_from_furthest_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't apply when dynamic obstacles are blocking significant proportions of the local path
|
||||
utils::setPathCostsIfNotSet(data, costmap_ros_);
|
||||
const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data);
|
||||
unsigned int invalid_ctr = 0;
|
||||
const float range = *data.furthest_reached_path_point - closest_initial_path_point;
|
||||
for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) {
|
||||
if (!(*data.path_pts_valid)[i]) {invalid_ctr++;}
|
||||
if (static_cast<float>(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points
|
||||
const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points
|
||||
const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points
|
||||
|
||||
const size_t batch_size = data.trajectories.x.shape(0);
|
||||
const size_t time_steps = data.trajectories.x.shape(1);
|
||||
auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
|
||||
|
||||
// Find integrated distance in the path
|
||||
std::vector<float> path_integrated_distances(path_segments_count, 0.0f);
|
||||
float dx = 0.0f, dy = 0.0f;
|
||||
for (unsigned int i = 1; i != path_segments_count; i++) {
|
||||
dx = P_x(i) - P_x(i - 1);
|
||||
dy = P_y(i) - P_y(i - 1);
|
||||
float curr_dist = sqrtf(dx * dx + dy * dy);
|
||||
path_integrated_distances[i] = path_integrated_distances[i - 1] + curr_dist;
|
||||
}
|
||||
|
||||
float traj_integrated_distance = 0.0f;
|
||||
float summed_path_dist = 0.0f, dyaw = 0.0f;
|
||||
float num_samples = 0.0f;
|
||||
float Tx = 0.0f, Ty = 0.0f;
|
||||
size_t path_pt = 0;
|
||||
for (size_t t = 0; t < batch_size; ++t) {
|
||||
traj_integrated_distance = 0.0f;
|
||||
summed_path_dist = 0.0f;
|
||||
num_samples = 0.0f;
|
||||
path_pt = 0u;
|
||||
const auto T_x = xt::view(data.trajectories.x, t, xt::all());
|
||||
const auto T_y = xt::view(data.trajectories.y, t, xt::all());
|
||||
for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) {
|
||||
Tx = T_x(p);
|
||||
Ty = T_y(p);
|
||||
dx = Tx - T_x(p - trajectory_point_step_);
|
||||
dy = Ty - T_y(p - trajectory_point_step_);
|
||||
traj_integrated_distance += sqrtf(dx * dx + dy * dy);
|
||||
path_pt = utils::findClosestPathPt(
|
||||
path_integrated_distances, traj_integrated_distance, path_pt);
|
||||
|
||||
// The nearest path point to align to needs to be not in collision, else
|
||||
// let the obstacle critic take over in this region due to dynamic obstacles
|
||||
if ((*data.path_pts_valid)[path_pt]) {
|
||||
dx = P_x(path_pt) - Tx;
|
||||
dy = P_y(path_pt) - Ty;
|
||||
num_samples += 1.0f;
|
||||
if (use_path_orientations_) {
|
||||
const auto T_yaw = xt::view(data.trajectories.yaws, t, xt::all());
|
||||
dyaw = angles::shortest_angular_distance(P_yaw(path_pt), T_yaw(p));
|
||||
summed_path_dist += sqrtf(dx * dx + dy * dy + dyaw * dyaw);
|
||||
} else {
|
||||
summed_path_dist += sqrtf(dx * dx + dy * dy);
|
||||
}
|
||||
}
|
||||
}
|
||||
if (num_samples > 0) {
|
||||
cost[t] = summed_path_dist / num_samples;
|
||||
} else {
|
||||
cost[t] = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
data.costs += xt::pow(std::move(cost) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::PathAlignCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,137 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp"
|
||||
|
||||
#include <xtensor/xfixed.hpp>
|
||||
#include <xtensor/xmath.hpp>
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
using namespace xt::placeholders; // NOLINT
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
void PathAlignLegacyCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 10.0);
|
||||
|
||||
getParam(max_path_occupancy_ratio_, "max_path_occupancy_ratio", 0.07);
|
||||
getParam(offset_from_furthest_, "offset_from_furthest", 20);
|
||||
getParam(trajectory_point_step_, "trajectory_point_step", 4);
|
||||
getParam(
|
||||
threshold_to_consider_,
|
||||
"threshold_to_consider", 0.5);
|
||||
getParam(use_path_orientations_, "use_path_orientations", false);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"ReferenceTrajectoryCritic instantiated with %d power and %f weight",
|
||||
power_, weight_);
|
||||
}
|
||||
|
||||
void PathAlignLegacyCritic::score(CriticData & data)
|
||||
{
|
||||
// Don't apply close to goal, let the goal critics take over
|
||||
if (!enabled_ || utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't apply when first getting bearing w.r.t. the path
|
||||
utils::setPathFurthestPointIfNotSet(data);
|
||||
if (*data.furthest_reached_path_point < offset_from_furthest_) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Don't apply when dynamic obstacles are blocking significant proportions of the local path
|
||||
utils::setPathCostsIfNotSet(data, costmap_ros_);
|
||||
const size_t closest_initial_path_point = utils::findPathTrajectoryInitialPoint(data);
|
||||
unsigned int invalid_ctr = 0;
|
||||
const float range = *data.furthest_reached_path_point - closest_initial_path_point;
|
||||
for (size_t i = closest_initial_path_point; i < *data.furthest_reached_path_point; i++) {
|
||||
if (!(*data.path_pts_valid)[i]) {invalid_ctr++;}
|
||||
if (static_cast<float>(invalid_ctr) / range > max_path_occupancy_ratio_ && invalid_ctr > 2) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
const auto & T_x = data.trajectories.x;
|
||||
const auto & T_y = data.trajectories.y;
|
||||
const auto & T_yaw = data.trajectories.yaws;
|
||||
|
||||
const auto P_x = xt::view(data.path.x, xt::range(_, -1)); // path points
|
||||
const auto P_y = xt::view(data.path.y, xt::range(_, -1)); // path points
|
||||
const auto P_yaw = xt::view(data.path.yaws, xt::range(_, -1)); // path points
|
||||
|
||||
const size_t batch_size = T_x.shape(0);
|
||||
const size_t time_steps = T_x.shape(1);
|
||||
const size_t traj_pts_eval = floor(time_steps / trajectory_point_step_);
|
||||
const size_t path_segments_count = data.path.x.shape(0) - 1;
|
||||
auto && cost = xt::xtensor<float, 1>::from_shape({data.costs.shape(0)});
|
||||
|
||||
if (path_segments_count < 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
float dist_sq = 0.0f, dx = 0.0f, dy = 0.0f, dyaw = 0.0f, summed_dist = 0.0f;
|
||||
float min_dist_sq = std::numeric_limits<float>::max();
|
||||
size_t min_s = 0;
|
||||
|
||||
for (size_t t = 0; t < batch_size; ++t) {
|
||||
summed_dist = 0.0f;
|
||||
for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) {
|
||||
min_dist_sq = std::numeric_limits<float>::max();
|
||||
min_s = 0;
|
||||
|
||||
// Find closest path segment to the trajectory point
|
||||
for (size_t s = 0; s < path_segments_count - 1; s++) {
|
||||
xt::xtensor_fixed<float, xt::xshape<2>> P;
|
||||
dx = P_x(s) - T_x(t, p);
|
||||
dy = P_y(s) - T_y(t, p);
|
||||
if (use_path_orientations_) {
|
||||
dyaw = angles::shortest_angular_distance(P_yaw(s), T_yaw(t, p));
|
||||
dist_sq = dx * dx + dy * dy + dyaw * dyaw;
|
||||
} else {
|
||||
dist_sq = dx * dx + dy * dy;
|
||||
}
|
||||
if (dist_sq < min_dist_sq) {
|
||||
min_dist_sq = dist_sq;
|
||||
min_s = s;
|
||||
}
|
||||
}
|
||||
|
||||
// The nearest path point to align to needs to be not in collision, else
|
||||
// let the obstacle critic take over in this region due to dynamic obstacles
|
||||
if (min_s != 0 && (*data.path_pts_valid)[min_s]) {
|
||||
summed_dist += sqrtf(min_dist_sq);
|
||||
}
|
||||
}
|
||||
|
||||
cost[t] = summed_dist / traj_pts_eval;
|
||||
}
|
||||
|
||||
data.costs += xt::pow(std::move(cost) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::PathAlignLegacyCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,109 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/path_angle_critic.hpp"
|
||||
|
||||
#include <math.h>
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void PathAngleCritic::initialize()
|
||||
{
|
||||
auto getParentParam = parameters_handler_->getParamGetter(parent_name_);
|
||||
float vx_min;
|
||||
getParentParam(vx_min, "vx_min", -0.35);
|
||||
if (fabs(vx_min) < 1e-6) { // zero
|
||||
reversing_allowed_ = false;
|
||||
} else if (vx_min < 0.0) { // reversing possible
|
||||
reversing_allowed_ = true;
|
||||
}
|
||||
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(offset_from_furthest_, "offset_from_furthest", 4);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 2.0);
|
||||
getParam(
|
||||
threshold_to_consider_,
|
||||
"threshold_to_consider", 0.5);
|
||||
getParam(
|
||||
max_angle_to_furthest_,
|
||||
"max_angle_to_furthest", 1.2);
|
||||
getParam(
|
||||
forward_preference_,
|
||||
"forward_preference", true);
|
||||
|
||||
if (!reversing_allowed_) {
|
||||
forward_preference_ = true;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"PathAngleCritic instantiated with %d power and %f weight. Reversing %s",
|
||||
power_, weight_, reversing_allowed_ ? "allowed." : "not allowed.");
|
||||
}
|
||||
|
||||
void PathAngleCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
utils::setPathFurthestPointIfNotSet(data);
|
||||
|
||||
auto offseted_idx = std::min(
|
||||
*data.furthest_reached_path_point + offset_from_furthest_, data.path.x.shape(0) - 1);
|
||||
|
||||
const float goal_x = xt::view(data.path.x, offseted_idx);
|
||||
const float goal_y = xt::view(data.path.y, offseted_idx);
|
||||
|
||||
if (utils::posePointAngle(
|
||||
data.state.pose.pose, goal_x, goal_y, forward_preference_) < max_angle_to_furthest_)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
auto yaws_between_points = xt::atan2(
|
||||
goal_y - data.trajectories.y,
|
||||
goal_x - data.trajectories.x);
|
||||
|
||||
auto yaws =
|
||||
xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points));
|
||||
|
||||
if (reversing_allowed_ && !forward_preference_) {
|
||||
const auto yaws_between_points_corrected = xt::where(
|
||||
yaws < M_PI_2, yaws_between_points, utils::normalize_angles(yaws_between_points + M_PI));
|
||||
const auto corrected_yaws = xt::abs(
|
||||
utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points_corrected));
|
||||
data.costs += xt::pow(xt::mean(corrected_yaws, {1}, immediate) * weight_, power_);
|
||||
} else {
|
||||
data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::PathAngleCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,79 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/path_follow_critic.hpp"
|
||||
|
||||
#include <xtensor/xmath.hpp>
|
||||
#include <xtensor/xsort.hpp>
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void PathFollowCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
|
||||
getParam(
|
||||
threshold_to_consider_,
|
||||
"threshold_to_consider", 1.4);
|
||||
getParam(offset_from_furthest_, "offset_from_furthest", 6);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 5.0);
|
||||
}
|
||||
|
||||
void PathFollowCritic::score(CriticData & data)
|
||||
{
|
||||
if (!enabled_ || data.path.x.shape(0) < 2 ||
|
||||
utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
utils::setPathFurthestPointIfNotSet(data);
|
||||
utils::setPathCostsIfNotSet(data, costmap_ros_);
|
||||
const size_t path_size = data.path.x.shape(0) - 1;
|
||||
|
||||
auto offseted_idx = std::min(
|
||||
*data.furthest_reached_path_point + offset_from_furthest_, path_size);
|
||||
|
||||
// Drive to the first valid path point, in case of dynamic obstacles on path
|
||||
// we want to drive past it, not through it
|
||||
bool valid = false;
|
||||
while (!valid && offseted_idx < path_size - 1) {
|
||||
valid = (*data.path_pts_valid)[offseted_idx];
|
||||
if (!valid) {
|
||||
offseted_idx++;
|
||||
}
|
||||
}
|
||||
|
||||
const auto path_x = data.path.x(offseted_idx);
|
||||
const auto path_y = data.path.y(offseted_idx);
|
||||
|
||||
const auto last_x = xt::view(data.trajectories.x, xt::all(), -1);
|
||||
const auto last_y = xt::view(data.trajectories.y, xt::all(), -1);
|
||||
|
||||
auto dists = xt::sqrt(
|
||||
xt::pow(last_x - path_x, 2) +
|
||||
xt::pow(last_y - path_y, 2));
|
||||
|
||||
data.costs += xt::pow(weight_ * std::move(dists), power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::PathFollowCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,55 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void PreferForwardCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 5.0);
|
||||
getParam(
|
||||
threshold_to_consider_,
|
||||
"threshold_to_consider", 0.5);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_, "PreferForwardCritic instantiated with %d power and %f weight.", power_, weight_);
|
||||
}
|
||||
|
||||
void PreferForwardCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
if (!enabled_ || utils::withinPositionGoalTolerance(
|
||||
threshold_to_consider_, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
auto backward_motion = xt::maximum(-data.state.vx, 0);
|
||||
data.costs += xt::pow(
|
||||
xt::sum(
|
||||
std::move(
|
||||
backward_motion) * data.model_dt, {1}, immediate) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::PreferForwardCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,50 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/twirling_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void TwirlingCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 10.0);
|
||||
|
||||
RCLCPP_INFO(
|
||||
logger_, "TwirlingCritic instantiated with %d power and %f weight.", power_, weight_);
|
||||
}
|
||||
|
||||
void TwirlingCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
if (!enabled_ ||
|
||||
utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.goal))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
const auto wz = xt::abs(data.state.wz);
|
||||
data.costs += xt::pow(xt::mean(wz, {1}, immediate) * weight_, power_);
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(
|
||||
mppi::critics::TwirlingCritic,
|
||||
mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,104 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
|
||||
|
||||
namespace mppi::critics
|
||||
{
|
||||
|
||||
void VelocityDeadbandCritic::initialize()
|
||||
{
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
|
||||
getParam(power_, "cost_power", 1);
|
||||
getParam(weight_, "cost_weight", 35.0);
|
||||
|
||||
// Recast double to float
|
||||
std::vector<double> deadband_velocities{0.0, 0.0, 0.0};
|
||||
getParam(deadband_velocities, "deadband_velocities", std::vector<double>{0.0, 0.0, 0.0});
|
||||
std::transform(
|
||||
deadband_velocities.begin(), deadband_velocities.end(), deadband_velocities_.begin(),
|
||||
[](double d) {return static_cast<float>(d);});
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
logger_, "VelocityDeadbandCritic instantiated with "
|
||||
<< power_ << " power, " << weight_ << " weight, deadband_velocity ["
|
||||
<< deadband_velocities_.at(0) << "," << deadband_velocities_.at(1) << ","
|
||||
<< deadband_velocities_.at(2) << "]");
|
||||
}
|
||||
|
||||
void VelocityDeadbandCritic::score(CriticData & data)
|
||||
{
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
if (!enabled_) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto & vx = data.state.vx;
|
||||
auto & wz = data.state.wz;
|
||||
|
||||
if (data.motion_model->isHolonomic()) {
|
||||
auto & vy = data.state.vy;
|
||||
if (power_ > 1u) {
|
||||
data.costs += xt::pow(
|
||||
xt::sum(
|
||||
std::move(
|
||||
xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) *
|
||||
data.model_dt,
|
||||
{1}, immediate) *
|
||||
weight_,
|
||||
power_);
|
||||
} else {
|
||||
data.costs += xt::sum(
|
||||
(std::move(
|
||||
xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(1)) - xt::fabs(vy), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) *
|
||||
data.model_dt,
|
||||
{1}, immediate) *
|
||||
weight_;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
if (power_ > 1u) {
|
||||
data.costs += xt::pow(
|
||||
xt::sum(
|
||||
std::move(
|
||||
xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0)) *
|
||||
data.model_dt,
|
||||
{1}, immediate) *
|
||||
weight_,
|
||||
power_);
|
||||
} else {
|
||||
data.costs += xt::sum(
|
||||
(std::move(
|
||||
xt::maximum(fabs(deadband_velocities_.at(0)) - xt::fabs(vx), 0) +
|
||||
xt::maximum(fabs(deadband_velocities_.at(2)) - xt::fabs(wz), 0))) *
|
||||
data.model_dt,
|
||||
{1}, immediate) *
|
||||
weight_;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
} // namespace mppi::critics
|
||||
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(mppi::critics::VelocityDeadbandCritic, mppi::critics::CriticFunction)
|
||||
@@ -0,0 +1,124 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/tools/noise_generator.hpp"
|
||||
|
||||
#include <memory>
|
||||
#include <mutex>
|
||||
#include <xtensor/xmath.hpp>
|
||||
#include <xtensor/xrandom.hpp>
|
||||
#include <xtensor/xnoalias.hpp>
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
void NoiseGenerator::initialize(
|
||||
mppi::models::OptimizerSettings & settings, bool is_holonomic,
|
||||
const std::string & name, ParametersHandler * param_handler)
|
||||
{
|
||||
settings_ = settings;
|
||||
is_holonomic_ = is_holonomic;
|
||||
active_ = true;
|
||||
|
||||
auto getParam = param_handler->getParamGetter(name);
|
||||
getParam(regenerate_noises_, "regenerate_noises", false);
|
||||
|
||||
if (regenerate_noises_) {
|
||||
noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
|
||||
} else {
|
||||
generateNoisedControls();
|
||||
}
|
||||
}
|
||||
|
||||
void NoiseGenerator::shutdown()
|
||||
{
|
||||
active_ = false;
|
||||
ready_ = true;
|
||||
noise_cond_.notify_all();
|
||||
if (noise_thread_.joinable()) {
|
||||
noise_thread_.join();
|
||||
}
|
||||
}
|
||||
|
||||
void NoiseGenerator::generateNextNoises()
|
||||
{
|
||||
// Trigger the thread to run in parallel to this iteration
|
||||
// to generate the next iteration's noises (if applicable).
|
||||
{
|
||||
std::unique_lock<std::mutex> guard(noise_lock_);
|
||||
ready_ = true;
|
||||
}
|
||||
noise_cond_.notify_all();
|
||||
}
|
||||
|
||||
void NoiseGenerator::setNoisedControls(
|
||||
models::State & state,
|
||||
const models::ControlSequence & control_sequence)
|
||||
{
|
||||
std::unique_lock<std::mutex> guard(noise_lock_);
|
||||
|
||||
xt::noalias(state.cvx) = control_sequence.vx + noises_vx_;
|
||||
xt::noalias(state.cvy) = control_sequence.vy + noises_vy_;
|
||||
xt::noalias(state.cwz) = control_sequence.wz + noises_wz_;
|
||||
}
|
||||
|
||||
void NoiseGenerator::reset(mppi::models::OptimizerSettings & settings, bool is_holonomic)
|
||||
{
|
||||
settings_ = settings;
|
||||
is_holonomic_ = is_holonomic;
|
||||
|
||||
// Recompute the noises on reset, initialization, and fallback
|
||||
{
|
||||
std::unique_lock<std::mutex> guard(noise_lock_);
|
||||
xt::noalias(noises_vx_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
|
||||
xt::noalias(noises_vy_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
|
||||
xt::noalias(noises_wz_) = xt::zeros<float>({settings_.batch_size, settings_.time_steps});
|
||||
ready_ = true;
|
||||
}
|
||||
|
||||
if (regenerate_noises_) {
|
||||
noise_cond_.notify_all();
|
||||
} else {
|
||||
generateNoisedControls();
|
||||
}
|
||||
}
|
||||
|
||||
void NoiseGenerator::noiseThread()
|
||||
{
|
||||
do {
|
||||
std::unique_lock<std::mutex> guard(noise_lock_);
|
||||
noise_cond_.wait(guard, [this]() {return ready_;});
|
||||
ready_ = false;
|
||||
generateNoisedControls();
|
||||
} while (active_);
|
||||
}
|
||||
|
||||
void NoiseGenerator::generateNoisedControls()
|
||||
{
|
||||
auto & s = settings_;
|
||||
|
||||
xt::noalias(noises_vx_) = xt::random::randn<float>(
|
||||
{s.batch_size, s.time_steps}, 0.0f,
|
||||
s.sampling_std.vx);
|
||||
xt::noalias(noises_wz_) = xt::random::randn<float>(
|
||||
{s.batch_size, s.time_steps}, 0.0f,
|
||||
s.sampling_std.wz);
|
||||
if (is_holonomic_) {
|
||||
xt::noalias(noises_vy_) = xt::random::randn<float>(
|
||||
{s.batch_size, s.time_steps}, 0.0f,
|
||||
s.sampling_std.vy);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,460 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
|
||||
#include <limits>
|
||||
#include <memory>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <cmath>
|
||||
#include <xtensor/xmath.hpp>
|
||||
#include <xtensor/xrandom.hpp>
|
||||
#include <xtensor/xnoalias.hpp>
|
||||
|
||||
#include "nav2_costmap_2d/costmap_filters/filter_values.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
using namespace xt::placeholders; // NOLINT
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
void Optimizer::initialize(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros,
|
||||
ParametersHandler * param_handler)
|
||||
{
|
||||
parent_ = parent;
|
||||
name_ = name;
|
||||
costmap_ros_ = costmap_ros;
|
||||
costmap_ = costmap_ros_->getCostmap();
|
||||
parameters_handler_ = param_handler;
|
||||
|
||||
auto node = parent_.lock();
|
||||
logger_ = node->get_logger();
|
||||
|
||||
getParams();
|
||||
|
||||
critic_manager_.on_configure(parent_, name_, costmap_ros_, parameters_handler_);
|
||||
noise_generator_.initialize(settings_, isHolonomic(), name_, parameters_handler_);
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
void Optimizer::shutdown()
|
||||
{
|
||||
noise_generator_.shutdown();
|
||||
}
|
||||
|
||||
void Optimizer::getParams()
|
||||
{
|
||||
std::string motion_model_name;
|
||||
|
||||
auto & s = settings_;
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
auto getParentParam = parameters_handler_->getParamGetter("");
|
||||
getParam(s.model_dt, "model_dt", 0.05f);
|
||||
getParam(s.time_steps, "time_steps", 56);
|
||||
getParam(s.batch_size, "batch_size", 1000);
|
||||
getParam(s.iteration_count, "iteration_count", 1);
|
||||
getParam(s.temperature, "temperature", 0.3f);
|
||||
getParam(s.gamma, "gamma", 0.015f);
|
||||
getParam(s.base_constraints.vx_max, "vx_max", 0.5);
|
||||
getParam(s.base_constraints.vx_min, "vx_min", -0.35);
|
||||
getParam(s.base_constraints.vy, "vy_max", 0.5);
|
||||
getParam(s.base_constraints.wz, "wz_max", 1.9);
|
||||
getParam(s.sampling_std.vx, "vx_std", 0.2);
|
||||
getParam(s.sampling_std.vy, "vy_std", 0.2);
|
||||
getParam(s.sampling_std.wz, "wz_std", 0.4);
|
||||
getParam(s.retry_attempt_limit, "retry_attempt_limit", 1);
|
||||
|
||||
getParam(motion_model_name, "motion_model", std::string("DiffDrive"));
|
||||
|
||||
s.constraints = s.base_constraints;
|
||||
setMotionModel(motion_model_name);
|
||||
parameters_handler_->addPostCallback([this]() {reset();});
|
||||
|
||||
double controller_frequency;
|
||||
getParentParam(controller_frequency, "controller_frequency", 0.0, ParameterType::Static);
|
||||
setOffset(controller_frequency);
|
||||
}
|
||||
|
||||
void Optimizer::setOffset(double controller_frequency)
|
||||
{
|
||||
const double controller_period = 1.0 / controller_frequency;
|
||||
constexpr double eps = 1e-6;
|
||||
|
||||
if ((controller_period + eps) < settings_.model_dt) {
|
||||
RCLCPP_WARN(
|
||||
logger_,
|
||||
"Controller period is less then model dt, consider setting it equal");
|
||||
} else if (abs(controller_period - settings_.model_dt) < eps) {
|
||||
RCLCPP_INFO(
|
||||
logger_,
|
||||
"Controller period is equal to model dt. Control sequence "
|
||||
"shifting is ON");
|
||||
settings_.shift_control_sequence = true;
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
"Controller period more then model dt, set it equal to model dt");
|
||||
}
|
||||
}
|
||||
|
||||
void Optimizer::reset()
|
||||
{
|
||||
state_.reset(settings_.batch_size, settings_.time_steps);
|
||||
control_sequence_.reset(settings_.time_steps);
|
||||
control_history_[0] = {0.0, 0.0, 0.0};
|
||||
control_history_[1] = {0.0, 0.0, 0.0};
|
||||
control_history_[2] = {0.0, 0.0, 0.0};
|
||||
control_history_[3] = {0.0, 0.0, 0.0};
|
||||
|
||||
settings_.constraints = settings_.base_constraints;
|
||||
|
||||
costs_ = xt::zeros<float>({settings_.batch_size});
|
||||
generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
|
||||
|
||||
noise_generator_.reset(settings_, isHolonomic());
|
||||
RCLCPP_INFO(logger_, "Optimizer reset");
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped Optimizer::evalControl(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
const nav_msgs::msg::Path & plan,
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav2_core::GoalChecker * goal_checker)
|
||||
{
|
||||
prepare(robot_pose, robot_speed, plan, goal, goal_checker);
|
||||
|
||||
do {
|
||||
optimize();
|
||||
} while (fallback(critics_data_.fail_flag));
|
||||
|
||||
utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
|
||||
auto control = getControlFromSequenceAsTwist(plan.header.stamp);
|
||||
|
||||
if (settings_.shift_control_sequence) {
|
||||
shiftControlSequence();
|
||||
}
|
||||
|
||||
return control;
|
||||
}
|
||||
|
||||
void Optimizer::optimize()
|
||||
{
|
||||
for (size_t i = 0; i < settings_.iteration_count; ++i) {
|
||||
generateNoisedTrajectories();
|
||||
critic_manager_.evalTrajectoriesScores(critics_data_);
|
||||
updateControlSequence();
|
||||
}
|
||||
}
|
||||
|
||||
bool Optimizer::fallback(bool fail)
|
||||
{
|
||||
static size_t counter = 0;
|
||||
|
||||
if (!fail) {
|
||||
counter = 0;
|
||||
return false;
|
||||
}
|
||||
|
||||
reset();
|
||||
|
||||
if (++counter > settings_.retry_attempt_limit) {
|
||||
counter = 0;
|
||||
throw std::runtime_error("Optimizer fail to compute path");
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void Optimizer::prepare(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
const nav_msgs::msg::Path & plan,
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav2_core::GoalChecker * goal_checker)
|
||||
{
|
||||
state_.pose = robot_pose;
|
||||
state_.speed = robot_speed;
|
||||
path_ = utils::toTensor(plan);
|
||||
goal_ = goal;
|
||||
|
||||
costs_.fill(0);
|
||||
|
||||
critics_data_.fail_flag = false;
|
||||
critics_data_.goal_checker = goal_checker;
|
||||
critics_data_.motion_model = motion_model_;
|
||||
critics_data_.furthest_reached_path_point.reset();
|
||||
critics_data_.path_pts_valid.reset();
|
||||
}
|
||||
|
||||
void Optimizer::shiftControlSequence()
|
||||
{
|
||||
using namespace xt::placeholders; // NOLINT
|
||||
control_sequence_.vx = xt::roll(control_sequence_.vx, -1);
|
||||
control_sequence_.wz = xt::roll(control_sequence_.wz, -1);
|
||||
|
||||
|
||||
xt::view(control_sequence_.vx, -1) =
|
||||
xt::view(control_sequence_.vx, -2);
|
||||
|
||||
xt::view(control_sequence_.wz, -1) =
|
||||
xt::view(control_sequence_.wz, -2);
|
||||
|
||||
|
||||
if (isHolonomic()) {
|
||||
control_sequence_.vy = xt::roll(control_sequence_.vy, -1);
|
||||
xt::view(control_sequence_.vy, -1) =
|
||||
xt::view(control_sequence_.vy, -2);
|
||||
}
|
||||
}
|
||||
|
||||
void Optimizer::generateNoisedTrajectories()
|
||||
{
|
||||
noise_generator_.setNoisedControls(state_, control_sequence_);
|
||||
noise_generator_.generateNextNoises();
|
||||
updateStateVelocities(state_);
|
||||
integrateStateVelocities(generated_trajectories_, state_);
|
||||
}
|
||||
|
||||
bool Optimizer::isHolonomic() const {return motion_model_->isHolonomic();}
|
||||
|
||||
void Optimizer::applyControlSequenceConstraints()
|
||||
{
|
||||
auto & s = settings_;
|
||||
|
||||
if (isHolonomic()) {
|
||||
control_sequence_.vy = xt::clip(control_sequence_.vy, -s.constraints.vy, s.constraints.vy);
|
||||
}
|
||||
|
||||
control_sequence_.vx = xt::clip(control_sequence_.vx, s.constraints.vx_min, s.constraints.vx_max);
|
||||
control_sequence_.wz = xt::clip(control_sequence_.wz, -s.constraints.wz, s.constraints.wz);
|
||||
|
||||
motion_model_->applyConstraints(control_sequence_);
|
||||
}
|
||||
|
||||
void Optimizer::updateStateVelocities(
|
||||
models::State & state) const
|
||||
{
|
||||
updateInitialStateVelocities(state);
|
||||
propagateStateVelocitiesFromInitials(state);
|
||||
}
|
||||
|
||||
void Optimizer::updateInitialStateVelocities(
|
||||
models::State & state) const
|
||||
{
|
||||
xt::noalias(xt::view(state.vx, xt::all(), 0)) = state.speed.linear.x;
|
||||
xt::noalias(xt::view(state.wz, xt::all(), 0)) = state.speed.angular.z;
|
||||
|
||||
if (isHolonomic()) {
|
||||
xt::noalias(xt::view(state.vy, xt::all(), 0)) = state.speed.linear.y;
|
||||
}
|
||||
}
|
||||
|
||||
void Optimizer::propagateStateVelocitiesFromInitials(
|
||||
models::State & state) const
|
||||
{
|
||||
motion_model_->predict(state);
|
||||
}
|
||||
|
||||
void Optimizer::integrateStateVelocities(
|
||||
xt::xtensor<float, 2> & trajectory,
|
||||
const xt::xtensor<float, 2> & sequence) const
|
||||
{
|
||||
float initial_yaw = tf2::getYaw(state_.pose.pose.orientation);
|
||||
|
||||
const auto vx = xt::view(sequence, xt::all(), 0);
|
||||
const auto vy = xt::view(sequence, xt::all(), 2);
|
||||
const auto wz = xt::view(sequence, xt::all(), 1);
|
||||
|
||||
auto traj_x = xt::view(trajectory, xt::all(), 0);
|
||||
auto traj_y = xt::view(trajectory, xt::all(), 1);
|
||||
auto traj_yaws = xt::view(trajectory, xt::all(), 2);
|
||||
|
||||
xt::noalias(traj_yaws) = xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw;
|
||||
|
||||
auto && yaw_cos = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
|
||||
auto && yaw_sin = xt::xtensor<float, 1>::from_shape(traj_yaws.shape());
|
||||
|
||||
const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _));
|
||||
|
||||
xt::noalias(xt::view(yaw_cos, 0)) = cosf(initial_yaw);
|
||||
xt::noalias(xt::view(yaw_sin, 0)) = sinf(initial_yaw);
|
||||
xt::noalias(xt::view(yaw_cos, xt::range(1, _))) = xt::cos(yaw_offseted);
|
||||
xt::noalias(xt::view(yaw_sin, xt::range(1, _))) = xt::sin(yaw_offseted);
|
||||
|
||||
auto && dx = xt::eval(vx * yaw_cos);
|
||||
auto && dy = xt::eval(vx * yaw_sin);
|
||||
|
||||
if (isHolonomic()) {
|
||||
dx = dx - vy * yaw_sin;
|
||||
dy = dy + vy * yaw_cos;
|
||||
}
|
||||
|
||||
xt::noalias(traj_x) = state_.pose.pose.position.x + xt::cumsum(dx * settings_.model_dt, 0);
|
||||
xt::noalias(traj_y) = state_.pose.pose.position.y + xt::cumsum(dy * settings_.model_dt, 0);
|
||||
}
|
||||
|
||||
void Optimizer::integrateStateVelocities(
|
||||
models::Trajectories & trajectories,
|
||||
const models::State & state) const
|
||||
{
|
||||
const float initial_yaw = tf2::getYaw(state.pose.pose.orientation);
|
||||
|
||||
xt::noalias(trajectories.yaws) =
|
||||
xt::cumsum(state.wz * settings_.model_dt, 1) + initial_yaw;
|
||||
|
||||
const auto yaws_cutted = xt::view(trajectories.yaws, xt::all(), xt::range(0, -1));
|
||||
|
||||
auto && yaw_cos = xt::xtensor<float, 2>::from_shape(trajectories.yaws.shape());
|
||||
auto && yaw_sin = xt::xtensor<float, 2>::from_shape(trajectories.yaws.shape());
|
||||
xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = cosf(initial_yaw);
|
||||
xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = sinf(initial_yaw);
|
||||
xt::noalias(xt::view(yaw_cos, xt::all(), xt::range(1, _))) = xt::cos(yaws_cutted);
|
||||
xt::noalias(xt::view(yaw_sin, xt::all(), xt::range(1, _))) = xt::sin(yaws_cutted);
|
||||
|
||||
auto && dx = xt::eval(state.vx * yaw_cos);
|
||||
auto && dy = xt::eval(state.vx * yaw_sin);
|
||||
|
||||
if (isHolonomic()) {
|
||||
dx = dx - state.vy * yaw_sin;
|
||||
dy = dy + state.vy * yaw_cos;
|
||||
}
|
||||
|
||||
xt::noalias(trajectories.x) = state.pose.pose.position.x +
|
||||
xt::cumsum(dx * settings_.model_dt, 1);
|
||||
xt::noalias(trajectories.y) = state.pose.pose.position.y +
|
||||
xt::cumsum(dy * settings_.model_dt, 1);
|
||||
}
|
||||
|
||||
xt::xtensor<float, 2> Optimizer::getOptimizedTrajectory()
|
||||
{
|
||||
auto && sequence =
|
||||
xt::xtensor<float, 2>::from_shape({settings_.time_steps, isHolonomic() ? 3u : 2u});
|
||||
auto && trajectories = xt::xtensor<float, 2>::from_shape({settings_.time_steps, 3});
|
||||
|
||||
xt::noalias(xt::view(sequence, xt::all(), 0)) = control_sequence_.vx;
|
||||
xt::noalias(xt::view(sequence, xt::all(), 1)) = control_sequence_.wz;
|
||||
|
||||
if (isHolonomic()) {
|
||||
xt::noalias(xt::view(sequence, xt::all(), 2)) = control_sequence_.vy;
|
||||
}
|
||||
|
||||
integrateStateVelocities(trajectories, sequence);
|
||||
return std::move(trajectories);
|
||||
}
|
||||
|
||||
void Optimizer::updateControlSequence()
|
||||
{
|
||||
auto & s = settings_;
|
||||
auto bounded_noises_vx = state_.cvx - control_sequence_.vx;
|
||||
auto bounded_noises_wz = state_.cwz - control_sequence_.wz;
|
||||
xt::noalias(costs_) +=
|
||||
s.gamma / powf(s.sampling_std.vx, 2) * xt::sum(
|
||||
xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate);
|
||||
xt::noalias(costs_) +=
|
||||
s.gamma / powf(s.sampling_std.wz, 2) * xt::sum(
|
||||
xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate);
|
||||
|
||||
if (isHolonomic()) {
|
||||
auto bounded_noises_vy = state_.cvy - control_sequence_.vy;
|
||||
xt::noalias(costs_) +=
|
||||
s.gamma / powf(s.sampling_std.vy, 2) * xt::sum(
|
||||
xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy,
|
||||
1, immediate);
|
||||
}
|
||||
|
||||
auto && costs_normalized = costs_ - xt::amin(costs_, immediate);
|
||||
auto && exponents = xt::eval(xt::exp(-1 / settings_.temperature * costs_normalized));
|
||||
auto && softmaxes = xt::eval(exponents / xt::sum(exponents, immediate));
|
||||
auto && softmaxes_extened = xt::eval(xt::view(softmaxes, xt::all(), xt::newaxis()));
|
||||
|
||||
xt::noalias(control_sequence_.vx) = xt::sum(state_.cvx * softmaxes_extened, 0, immediate);
|
||||
xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * softmaxes_extened, 0, immediate);
|
||||
if (isHolonomic()) {
|
||||
xt::noalias(control_sequence_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate);
|
||||
}
|
||||
|
||||
applyControlSequenceConstraints();
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
|
||||
const builtin_interfaces::msg::Time & stamp)
|
||||
{
|
||||
unsigned int offset = settings_.shift_control_sequence ? 1 : 0;
|
||||
|
||||
auto vx = control_sequence_.vx(offset);
|
||||
auto wz = control_sequence_.wz(offset);
|
||||
|
||||
if (isHolonomic()) {
|
||||
auto vy = control_sequence_.vy(offset);
|
||||
return utils::toTwistStamped(vx, vy, wz, stamp, costmap_ros_->getBaseFrameID());
|
||||
}
|
||||
|
||||
return utils::toTwistStamped(vx, wz, stamp, costmap_ros_->getBaseFrameID());
|
||||
}
|
||||
|
||||
void Optimizer::setMotionModel(const std::string & model)
|
||||
{
|
||||
if (model == "DiffDrive") {
|
||||
motion_model_ = std::make_shared<DiffDriveMotionModel>();
|
||||
} else if (model == "Omni") {
|
||||
motion_model_ = std::make_shared<OmniMotionModel>();
|
||||
} else if (model == "Ackermann") {
|
||||
motion_model_ = std::make_shared<AckermannMotionModel>(parameters_handler_, name_);
|
||||
} else {
|
||||
throw std::runtime_error(
|
||||
std::string(
|
||||
"Model " + model + " is not valid! Valid options are DiffDrive, Omni, "
|
||||
"or Ackermann"));
|
||||
}
|
||||
}
|
||||
|
||||
void Optimizer::setSpeedLimit(double speed_limit, bool percentage)
|
||||
{
|
||||
auto & s = settings_;
|
||||
if (speed_limit == nav2_costmap_2d::NO_SPEED_LIMIT) {
|
||||
s.constraints.vx_max = s.base_constraints.vx_max;
|
||||
s.constraints.vx_min = s.base_constraints.vx_min;
|
||||
s.constraints.vy = s.base_constraints.vy;
|
||||
s.constraints.wz = s.base_constraints.wz;
|
||||
} else {
|
||||
if (percentage) {
|
||||
// Speed limit is expressed in % from maximum speed of robot
|
||||
double ratio = speed_limit / 100.0;
|
||||
s.constraints.vx_max = s.base_constraints.vx_max * ratio;
|
||||
s.constraints.vx_min = s.base_constraints.vx_min * ratio;
|
||||
s.constraints.vy = s.base_constraints.vy * ratio;
|
||||
s.constraints.wz = s.base_constraints.wz * ratio;
|
||||
} else {
|
||||
// Speed limit is expressed in absolute value
|
||||
double ratio = speed_limit / s.base_constraints.vx_max;
|
||||
s.constraints.vx_max = s.base_constraints.vx_max * ratio;
|
||||
s.constraints.vx_min = s.base_constraints.vx_min * ratio;
|
||||
s.constraints.vy = s.base_constraints.vy * ratio;
|
||||
s.constraints.wz = s.base_constraints.wz * ratio;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
models::Trajectories & Optimizer::getGeneratedTrajectories()
|
||||
{
|
||||
return generated_trajectories_;
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,72 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
ParametersHandler::ParametersHandler(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
|
||||
{
|
||||
node_ = parent;
|
||||
auto node = node_.lock();
|
||||
node_name_ = node->get_name();
|
||||
logger_ = node->get_logger();
|
||||
}
|
||||
|
||||
void ParametersHandler::start()
|
||||
{
|
||||
auto node = node_.lock();
|
||||
on_set_param_handler_ = node->add_on_set_parameters_callback(
|
||||
std::bind(
|
||||
&ParametersHandler::dynamicParamsCallback, this,
|
||||
std::placeholders::_1));
|
||||
|
||||
auto get_param = getParamGetter(node_name_);
|
||||
get_param(verbose_, "verbose", false);
|
||||
}
|
||||
|
||||
rcl_interfaces::msg::SetParametersResult
|
||||
ParametersHandler::dynamicParamsCallback(
|
||||
std::vector<rclcpp::Parameter> parameters)
|
||||
{
|
||||
rcl_interfaces::msg::SetParametersResult result;
|
||||
std::lock_guard<std::mutex> lock(parameters_change_mutex_);
|
||||
|
||||
for (auto & pre_cb : pre_callbacks_) {
|
||||
pre_cb();
|
||||
}
|
||||
|
||||
for (auto & param : parameters) {
|
||||
const std::string & param_name = param.get_name();
|
||||
|
||||
if (auto callback = get_param_callbacks_.find(param_name);
|
||||
callback != get_param_callbacks_.end())
|
||||
{
|
||||
callback->second(param);
|
||||
} else {
|
||||
RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
for (auto & post_cb : post_callbacks_) {
|
||||
post_cb();
|
||||
}
|
||||
|
||||
result.successful = true;
|
||||
return result;
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,220 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
// Copyright (c) 2023 Dexory
|
||||
// Copyright (c) 2023 Open Navigation LLC
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "nav2_mppi_controller/tools/path_handler.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
void PathHandler::initialize(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap,
|
||||
std::shared_ptr<tf2_ros::Buffer> buffer, ParametersHandler * param_handler)
|
||||
{
|
||||
name_ = name;
|
||||
costmap_ = costmap;
|
||||
tf_buffer_ = buffer;
|
||||
auto node = parent.lock();
|
||||
logger_ = node->get_logger();
|
||||
parameters_handler_ = param_handler;
|
||||
|
||||
auto getParam = parameters_handler_->getParamGetter(name_);
|
||||
getParam(max_robot_pose_search_dist_, "max_robot_pose_search_dist", getMaxCostmapDist());
|
||||
getParam(prune_distance_, "prune_distance", 1.5);
|
||||
getParam(transform_tolerance_, "transform_tolerance", 0.1);
|
||||
getParam(enforce_path_inversion_, "enforce_path_inversion", false);
|
||||
if (enforce_path_inversion_) {
|
||||
getParam(inversion_xy_tolerance_, "inversion_xy_tolerance", 0.2);
|
||||
getParam(inversion_yaw_tolerance, "inversion_yaw_tolerance", 0.4);
|
||||
inversion_locale_ = 0u;
|
||||
}
|
||||
}
|
||||
|
||||
std::pair<nav_msgs::msg::Path, PathIterator>
|
||||
PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame(
|
||||
const geometry_msgs::msg::PoseStamped & global_pose)
|
||||
{
|
||||
using nav2_util::geometry_utils::euclidean_distance;
|
||||
|
||||
auto begin = global_plan_up_to_inversion_.poses.begin();
|
||||
|
||||
// Limit the search for the closest pose up to max_robot_pose_search_dist on the path
|
||||
auto closest_pose_upper_bound =
|
||||
nav2_util::geometry_utils::first_after_integrated_distance(
|
||||
global_plan_up_to_inversion_.poses.begin(), global_plan_up_to_inversion_.poses.end(),
|
||||
max_robot_pose_search_dist_);
|
||||
|
||||
// Find closest point to the robot
|
||||
auto closest_point = nav2_util::geometry_utils::min_by(
|
||||
begin, closest_pose_upper_bound,
|
||||
[&global_pose](const geometry_msgs::msg::PoseStamped & ps) {
|
||||
return euclidean_distance(global_pose, ps);
|
||||
});
|
||||
|
||||
nav_msgs::msg::Path transformed_plan;
|
||||
transformed_plan.header.frame_id = costmap_->getGlobalFrameID();
|
||||
transformed_plan.header.stamp = global_pose.header.stamp;
|
||||
|
||||
auto pruned_plan_end =
|
||||
nav2_util::geometry_utils::first_after_integrated_distance(
|
||||
closest_point, global_plan_up_to_inversion_.poses.end(), prune_distance_);
|
||||
|
||||
unsigned int mx, my;
|
||||
// Find the furthest relevent pose on the path to consider within costmap
|
||||
// bounds
|
||||
// Transforming it to the costmap frame in the same loop
|
||||
for (auto global_plan_pose = closest_point; global_plan_pose != pruned_plan_end;
|
||||
++global_plan_pose)
|
||||
{
|
||||
// Transform from global plan frame to costmap frame
|
||||
geometry_msgs::msg::PoseStamped costmap_plan_pose;
|
||||
global_plan_pose->header.stamp = global_pose.header.stamp;
|
||||
global_plan_pose->header.frame_id = global_plan_.header.frame_id;
|
||||
transformPose(costmap_->getGlobalFrameID(), *global_plan_pose, costmap_plan_pose);
|
||||
|
||||
// Check if pose is inside the costmap
|
||||
if (!costmap_->getCostmap()->worldToMap(
|
||||
costmap_plan_pose.pose.position.x, costmap_plan_pose.pose.position.y, mx, my))
|
||||
{
|
||||
return {transformed_plan, closest_point};
|
||||
}
|
||||
|
||||
// Filling the transformed plan to return with the transformed pose
|
||||
transformed_plan.poses.push_back(costmap_plan_pose);
|
||||
}
|
||||
|
||||
return {transformed_plan, closest_point};
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped PathHandler::transformToGlobalPlanFrame(
|
||||
const geometry_msgs::msg::PoseStamped & pose)
|
||||
{
|
||||
if (global_plan_up_to_inversion_.poses.empty()) {
|
||||
throw std::runtime_error("Received plan with zero length");
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped robot_pose;
|
||||
if (!transformPose(global_plan_up_to_inversion_.header.frame_id, pose, robot_pose)) {
|
||||
throw std::runtime_error(
|
||||
"Unable to transform robot pose into global plan's frame");
|
||||
}
|
||||
|
||||
return robot_pose;
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path PathHandler::transformPath(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose)
|
||||
{
|
||||
// Find relevent bounds of path to use
|
||||
geometry_msgs::msg::PoseStamped global_pose =
|
||||
transformToGlobalPlanFrame(robot_pose);
|
||||
auto [transformed_plan, lower_bound] = getGlobalPlanConsideringBoundsInCostmapFrame(global_pose);
|
||||
|
||||
prunePlan(global_plan_up_to_inversion_, lower_bound);
|
||||
|
||||
if (enforce_path_inversion_ && inversion_locale_ != 0u) {
|
||||
if (isWithinInversionTolerances(global_pose)) {
|
||||
prunePlan(global_plan_, global_plan_.poses.begin() + inversion_locale_);
|
||||
global_plan_up_to_inversion_ = global_plan_;
|
||||
inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
|
||||
}
|
||||
}
|
||||
|
||||
if (transformed_plan.poses.empty()) {
|
||||
throw std::runtime_error("Resulting plan has 0 poses in it.");
|
||||
}
|
||||
|
||||
return transformed_plan;
|
||||
}
|
||||
|
||||
bool PathHandler::transformPose(
|
||||
const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
|
||||
geometry_msgs::msg::PoseStamped & out_pose) const
|
||||
{
|
||||
if (in_pose.header.frame_id == frame) {
|
||||
out_pose = in_pose;
|
||||
return true;
|
||||
}
|
||||
|
||||
try {
|
||||
tf_buffer_->transform(
|
||||
in_pose, out_pose, frame,
|
||||
tf2::durationFromSec(transform_tolerance_));
|
||||
out_pose.header.frame_id = frame;
|
||||
return true;
|
||||
} catch (tf2::TransformException & ex) {
|
||||
RCLCPP_ERROR(logger_, "Exception in transformPose: %s", ex.what());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
double PathHandler::getMaxCostmapDist()
|
||||
{
|
||||
const auto & costmap = costmap_->getCostmap();
|
||||
return static_cast<double>(std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY())) *
|
||||
costmap->getResolution() * 0.50;
|
||||
}
|
||||
|
||||
void PathHandler::setPath(const nav_msgs::msg::Path & plan)
|
||||
{
|
||||
global_plan_ = plan;
|
||||
global_plan_up_to_inversion_ = global_plan_;
|
||||
if (enforce_path_inversion_) {
|
||||
inversion_locale_ = utils::removePosesAfterFirstInversion(global_plan_up_to_inversion_);
|
||||
}
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;}
|
||||
|
||||
void PathHandler::prunePlan(nav_msgs::msg::Path & plan, const PathIterator end)
|
||||
{
|
||||
plan.poses.erase(plan.poses.begin(), end);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped PathHandler::getTransformedGoal(
|
||||
const builtin_interfaces::msg::Time & stamp)
|
||||
{
|
||||
auto goal = global_plan_.poses.back();
|
||||
goal.header.frame_id = global_plan_.header.frame_id;
|
||||
goal.header.stamp = stamp;
|
||||
if (goal.header.frame_id.empty()) {
|
||||
throw std::runtime_error("Goal pose has an empty frame_id");
|
||||
}
|
||||
geometry_msgs::msg::PoseStamped transformed_goal;
|
||||
if (!transformPose(costmap_->getGlobalFrameID(), goal, transformed_goal)) {
|
||||
throw std::runtime_error("Unable to transform goal pose into costmap frame");
|
||||
}
|
||||
return transformed_goal;
|
||||
}
|
||||
|
||||
bool PathHandler::isWithinInversionTolerances(const geometry_msgs::msg::PoseStamped & robot_pose)
|
||||
{
|
||||
// Keep full path if we are within tolerance of the inversion pose
|
||||
const auto last_pose = global_plan_up_to_inversion_.poses.back();
|
||||
float distance = hypotf(
|
||||
robot_pose.pose.position.x - last_pose.pose.position.x,
|
||||
robot_pose.pose.position.y - last_pose.pose.position.y);
|
||||
|
||||
float angle_distance = angles::shortest_angular_distance(
|
||||
tf2::getYaw(robot_pose.pose.orientation),
|
||||
tf2::getYaw(last_pose.pose.orientation));
|
||||
|
||||
return distance <= inversion_xy_tolerance_ && fabs(angle_distance) <= inversion_yaw_tolerance;
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,130 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <memory>
|
||||
#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
|
||||
|
||||
namespace mppi
|
||||
{
|
||||
|
||||
void TrajectoryVisualizer::on_configure(
|
||||
rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
|
||||
const std::string & frame_id, ParametersHandler * parameters_handler)
|
||||
{
|
||||
auto node = parent.lock();
|
||||
logger_ = node->get_logger();
|
||||
frame_id_ = frame_id;
|
||||
trajectories_publisher_ =
|
||||
node->create_publisher<visualization_msgs::msg::MarkerArray>("/trajectories", 1);
|
||||
transformed_path_pub_ = node->create_publisher<nav_msgs::msg::Path>("transformed_global_plan", 1);
|
||||
parameters_handler_ = parameters_handler;
|
||||
|
||||
auto getParam = parameters_handler->getParamGetter(name + ".TrajectoryVisualizer");
|
||||
|
||||
getParam(trajectory_step_, "trajectory_step", 5);
|
||||
getParam(time_step_, "time_step", 3);
|
||||
|
||||
reset();
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::on_cleanup()
|
||||
{
|
||||
trajectories_publisher_.reset();
|
||||
transformed_path_pub_.reset();
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::on_activate()
|
||||
{
|
||||
trajectories_publisher_->on_activate();
|
||||
transformed_path_pub_->on_activate();
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::on_deactivate()
|
||||
{
|
||||
trajectories_publisher_->on_deactivate();
|
||||
transformed_path_pub_->on_deactivate();
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::add(
|
||||
const xt::xtensor<float, 2> & trajectory, const std::string & marker_namespace)
|
||||
{
|
||||
auto & size = trajectory.shape()[0];
|
||||
if (!size) {
|
||||
return;
|
||||
}
|
||||
|
||||
auto add_marker = [&](auto i) {
|
||||
float component = static_cast<float>(i) / static_cast<float>(size);
|
||||
|
||||
auto pose = utils::createPose(trajectory(i, 0), trajectory(i, 1), 0.06);
|
||||
auto scale =
|
||||
i != size - 1 ?
|
||||
utils::createScale(0.03, 0.03, 0.07) :
|
||||
utils::createScale(0.07, 0.07, 0.09);
|
||||
auto color = utils::createColor(0, component, component, 1);
|
||||
auto marker = utils::createMarker(
|
||||
marker_id_++, pose, scale, color, frame_id_, marker_namespace);
|
||||
points_->markers.push_back(marker);
|
||||
};
|
||||
|
||||
for (size_t i = 0; i < size; i++) {
|
||||
add_marker(i);
|
||||
}
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::add(
|
||||
const models::Trajectories & trajectories, const std::string & marker_namespace)
|
||||
{
|
||||
auto & shape = trajectories.x.shape();
|
||||
const float shape_1 = static_cast<float>(shape[1]);
|
||||
points_->markers.reserve(floor(shape[0] / trajectory_step_) * floor(shape[1] * time_step_));
|
||||
|
||||
for (size_t i = 0; i < shape[0]; i += trajectory_step_) {
|
||||
for (size_t j = 0; j < shape[1]; j += time_step_) {
|
||||
const float j_flt = static_cast<float>(j);
|
||||
float blue_component = 1.0f - j_flt / shape_1;
|
||||
float green_component = j_flt / shape_1;
|
||||
|
||||
auto pose = utils::createPose(trajectories.x(i, j), trajectories.y(i, j), 0.03);
|
||||
auto scale = utils::createScale(0.03, 0.03, 0.03);
|
||||
auto color = utils::createColor(0, green_component, blue_component, 1);
|
||||
auto marker = utils::createMarker(
|
||||
marker_id_++, pose, scale, color, frame_id_, marker_namespace);
|
||||
|
||||
points_->markers.push_back(marker);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::reset()
|
||||
{
|
||||
marker_id_ = 0;
|
||||
points_ = std::make_unique<visualization_msgs::msg::MarkerArray>();
|
||||
}
|
||||
|
||||
void TrajectoryVisualizer::visualize(const nav_msgs::msg::Path & plan)
|
||||
{
|
||||
if (trajectories_publisher_->get_subscription_count() > 0) {
|
||||
trajectories_publisher_->publish(std::move(points_));
|
||||
}
|
||||
|
||||
reset();
|
||||
|
||||
if (transformed_path_pub_->get_subscription_count() > 0) {
|
||||
auto plan_ptr = std::make_unique<nav_msgs::msg::Path>(plan);
|
||||
transformed_path_pub_->publish(std::move(plan_ptr));
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace mppi
|
||||
@@ -0,0 +1,40 @@
|
||||
set(TEST_NAMES
|
||||
optimizer_smoke_test
|
||||
controller_state_transition_test
|
||||
models_test
|
||||
noise_generator_test
|
||||
parameter_handler_test
|
||||
motion_model_tests
|
||||
trajectory_visualizer_tests
|
||||
utils_test
|
||||
path_handler_test
|
||||
critic_manager_test
|
||||
optimizer_unit_tests
|
||||
)
|
||||
|
||||
foreach(name IN LISTS TEST_NAMES)
|
||||
ament_add_gtest(${name}
|
||||
${name}.cpp
|
||||
)
|
||||
|
||||
ament_target_dependencies(${name}
|
||||
${dependencies_pkgs}
|
||||
)
|
||||
|
||||
target_link_libraries(${name}
|
||||
mppi_controller
|
||||
)
|
||||
|
||||
if(${TEST_DEBUG_INFO})
|
||||
target_compile_definitions(${name} PUBLIC -DTEST_DEBUG_INFO)
|
||||
endif()
|
||||
|
||||
endforeach()
|
||||
|
||||
# This is a special case requiring linking against the critics library
|
||||
ament_add_gtest(critics_tests critics_tests.cpp)
|
||||
ament_target_dependencies(critics_tests ${dependencies_pkgs})
|
||||
target_link_libraries(critics_tests mppi_controller mppi_critics)
|
||||
if(${TEST_DEBUG_INFO})
|
||||
target_compile_definitions(critics_tests PUBLIC -DTEST_DEBUG_INFO)
|
||||
endif()
|
||||
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/controller.hpp"
|
||||
|
||||
#include "utils/utils.hpp"
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
// Tests basic transition from configure->active->process->deactive->cleanup
|
||||
|
||||
TEST(ControllerStateTransitionTest, ControllerNotFail)
|
||||
{
|
||||
const bool visualize = true;
|
||||
TestCostmapSettings costmap_settings{};
|
||||
|
||||
// Node Options
|
||||
rclcpp::NodeOptions options;
|
||||
std::vector<rclcpp::Parameter> params;
|
||||
setUpControllerParams(visualize, params);
|
||||
options.parameter_overrides(params);
|
||||
|
||||
auto node = getDummyNode(options);
|
||||
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock());
|
||||
auto costmap_ros = getDummyCostmapRos(costmap_settings);
|
||||
costmap_ros->setRobotFootprint(getDummySquareFootprint(0.01));
|
||||
|
||||
auto controller = getDummyController(node, tf_buffer, costmap_ros);
|
||||
|
||||
TestPose start_pose = costmap_settings.getCenterPose();
|
||||
const double path_step = costmap_settings.resolution;
|
||||
TestPathSettings path_settings{start_pose, 8, path_step, path_step};
|
||||
|
||||
// evalControl args
|
||||
auto pose = getDummyPointStamped(node, start_pose);
|
||||
auto velocity = getDummyTwist();
|
||||
auto path = getIncrementalDummyPath(node, path_settings);
|
||||
path.header.frame_id = costmap_ros->getGlobalFrameID();
|
||||
pose.header.frame_id = costmap_ros->getGlobalFrameID();
|
||||
|
||||
controller->setPlan(path);
|
||||
|
||||
EXPECT_NO_THROW(controller->computeVelocityCommands(pose, velocity, {}));
|
||||
|
||||
controller->setSpeedLimit(0.5, true);
|
||||
controller->setSpeedLimit(0.5, false);
|
||||
controller->setSpeedLimit(1.0, true);
|
||||
controller->deactivate();
|
||||
controller->cleanup();
|
||||
}
|
||||
@@ -0,0 +1,139 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/critic_manager.hpp"
|
||||
|
||||
// Tests critic manager
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
using namespace mppi::critics; // NOLINT
|
||||
|
||||
class DummyCritic : public CriticFunction
|
||||
{
|
||||
public:
|
||||
virtual void initialize() {initialized_ = true;}
|
||||
virtual void score(CriticData & /*data*/) {scored_ = true;}
|
||||
bool initialized_{false}, scored_{false};
|
||||
};
|
||||
|
||||
class CriticManagerWrapper : public CriticManager
|
||||
{
|
||||
public:
|
||||
CriticManagerWrapper()
|
||||
: CriticManager() {}
|
||||
|
||||
virtual void loadCritics()
|
||||
{
|
||||
critics_.clear();
|
||||
auto instance = std::unique_ptr<critics::CriticFunction>(new DummyCritic);
|
||||
critics_.push_back(std::move(instance));
|
||||
critics_.back()->on_configure(
|
||||
parent_, name_, name_ + "." + "DummyCritic", costmap_ros_,
|
||||
parameters_handler_);
|
||||
}
|
||||
|
||||
std::string getFullNameWrapper(const std::string & name)
|
||||
{
|
||||
return getFullName(name);
|
||||
}
|
||||
|
||||
bool getDummyCriticInitialized()
|
||||
{
|
||||
return dynamic_cast<DummyCritic *>(critics_[0].get())->initialized_;
|
||||
}
|
||||
|
||||
bool getDummyCriticScored()
|
||||
{
|
||||
return dynamic_cast<DummyCritic *>(critics_[0].get())->scored_;
|
||||
}
|
||||
};
|
||||
|
||||
class CriticManagerWrapperEnum : public CriticManager
|
||||
{
|
||||
public:
|
||||
CriticManagerWrapperEnum()
|
||||
: CriticManager() {}
|
||||
|
||||
unsigned int getCriticNum()
|
||||
{
|
||||
return critics_.size();
|
||||
}
|
||||
};
|
||||
|
||||
TEST(CriticManagerTests, BasicCriticOperations)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
// Configuration should get parameters and initialize critic functions
|
||||
CriticManagerWrapper critic_manager;
|
||||
critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler);
|
||||
EXPECT_TRUE(critic_manager.getDummyCriticInitialized());
|
||||
|
||||
// Evaluation of critics should score them, but only if failure flag is not set
|
||||
models::State state;
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs;
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt};
|
||||
|
||||
data.fail_flag = true;
|
||||
EXPECT_FALSE(critic_manager.getDummyCriticScored());
|
||||
data.fail_flag = false;
|
||||
critic_manager.evalTrajectoriesScores(data);
|
||||
EXPECT_TRUE(critic_manager.getDummyCriticScored());
|
||||
|
||||
// This should get the full namespaced name of the critics
|
||||
EXPECT_EQ(critic_manager.getFullNameWrapper("name"), std::string("mppi::critics::name"));
|
||||
}
|
||||
|
||||
TEST(CriticManagerTests, CriticLoadingTest)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
node->declare_parameter(
|
||||
"critic_manager.critics",
|
||||
rclcpp::ParameterValue(std::vector<std::string>{"ConstraintCritic", "PreferForwardCritic"}));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State state;
|
||||
costmap_ros->on_configure(state);
|
||||
|
||||
// This should grab the critics parameter and load the 2 requested plugins
|
||||
CriticManagerWrapperEnum critic_manager;
|
||||
critic_manager.on_configure(node, "critic_manager", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic_manager.getCriticNum(), 2u);
|
||||
}
|
||||
@@ -0,0 +1,798 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/critics/constraint_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/goal_angle_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/goal_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/obstacles_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/cost_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/path_align_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/path_align_legacy_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/path_angle_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/path_follow_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/prefer_forward_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/twirling_critic.hpp"
|
||||
#include "nav2_mppi_controller/critics/velocity_deadband_critic.hpp"
|
||||
#include "nav2_core/exceptions.hpp"
|
||||
#include "utils_test.cpp" // NOLINT
|
||||
|
||||
// Tests the various critic plugin functions
|
||||
|
||||
// ROS lock used from utils_test.cpp
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
using namespace mppi::critics; // NOLINT
|
||||
using namespace mppi::utils; // NOLINT
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
TEST(CriticTests, ConstraintsCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly and that defaults are reasonable
|
||||
ConstraintCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
EXPECT_TRUE(critic.getMaxVelConstraint() > 0.0);
|
||||
EXPECT_TRUE(critic.getMinVelConstraint() < 0.0);
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide velocities in constraints, should not have any costs
|
||||
state.vx = 0.40 * xt::ones<float>({1000, 30});
|
||||
state.vy = xt::zeros<float>({1000, 30});
|
||||
state.wz = xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
|
||||
|
||||
// provide out of maximum velocity constraint
|
||||
auto last_batch_traj_in_full = xt::view(state.vx, -1, xt::all());
|
||||
last_batch_traj_in_full = 0.60 * xt::ones<float>({30});
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0);
|
||||
// 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
|
||||
EXPECT_NEAR(costs(999), 1.2, 0.01);
|
||||
costs = xt::zeros<float>({1000});
|
||||
|
||||
// provide out of minimum velocity constraint
|
||||
auto first_batch_traj_in_full = xt::view(state.vx, 1, xt::all());
|
||||
first_batch_traj_in_full = -0.45 * xt::ones<float>({30});
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0);
|
||||
// 4.0 weight * 0.1 model_dt * 0.1 error introduced * 30 timesteps = 1.2
|
||||
EXPECT_NEAR(costs(1), 1.2, 0.01);
|
||||
costs = xt::zeros<float>({1000});
|
||||
|
||||
// Now with ackermann, all in constraint so no costs to score
|
||||
state.vx = 0.40 * xt::ones<float>({1000, 30});
|
||||
state.wz = 1.5 * xt::ones<float>({1000, 30});
|
||||
data.motion_model = std::make_shared<AckermannMotionModel>(¶m_handler, node->get_name());
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
|
||||
|
||||
// Now violating the ackermann constraints
|
||||
state.wz = 2.5 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0);
|
||||
// 4.0 weight * 0.1 model_dt * (0.2 - 0.4/2.5) * 30 timesteps = 0.48
|
||||
EXPECT_NEAR(costs(1), 0.48, 0.01);
|
||||
}
|
||||
|
||||
TEST(CriticTests, ObstacleCriticMisalignedParams) {
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
auto getParam = param_handler.getParamGetter("critic");
|
||||
bool consider_footprint;
|
||||
getParam(consider_footprint, "consider_footprint", true);
|
||||
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
ObstaclesCritic critic;
|
||||
// Expect throw when settings mismatched
|
||||
EXPECT_THROW(
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler),
|
||||
nav2_core::PlannerException
|
||||
);
|
||||
}
|
||||
|
||||
TEST(CriticTests, ObstacleCriticAlignedParams) {
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
auto getParam = param_handler.getParamGetter("critic");
|
||||
bool consider_footprint;
|
||||
getParam(consider_footprint, "consider_footprint", false);
|
||||
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
ObstaclesCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
}
|
||||
|
||||
|
||||
TEST(CriticTests, CostCriticMisAlignedParams) {
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
auto getParam = param_handler.getParamGetter("critic");
|
||||
bool consider_footprint;
|
||||
getParam(consider_footprint, "consider_footprint", true);
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
CostCritic critic;
|
||||
// Expect throw when settings mismatched
|
||||
EXPECT_THROW(
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler),
|
||||
nav2_core::PlannerException
|
||||
);
|
||||
}
|
||||
|
||||
TEST(CriticTests, CostCriticAlignedParams) {
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
auto getParam = param_handler.getParamGetter("critic");
|
||||
bool consider_footprint;
|
||||
getParam(consider_footprint, "consider_footprint", false);
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
CostCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
}
|
||||
|
||||
TEST(CriticTests, GoalAngleCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
GoalAngleCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path too far from `threshold_to_consider` to consider
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 10.0;
|
||||
path.y(9) = 0.0;
|
||||
path.yaws(9) = 3.14;
|
||||
goal.position.x = 10.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
|
||||
|
||||
// Lets move it even closer, just to be sure it still doesn't trigger
|
||||
state.pose.pose.position.x = 9.2;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
|
||||
|
||||
// provide state pose and path below `threshold_to_consider` to consider
|
||||
state.pose.pose.position.x = 9.7;
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0);
|
||||
EXPECT_NEAR(costs(0), 9.42, 0.02); // (3.14 - 0.0) * 3.0 weight
|
||||
}
|
||||
|
||||
TEST(CriticTests, GoalCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
GoalCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing with all trajectories set to 0
|
||||
|
||||
// provide state poses and path far, should not trigger
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 10.0;
|
||||
path.y(9) = 0.0;
|
||||
goal.position.x = 10.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(costs(2), 0.0, 1e-6); // (0 * 5.0 weight
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); // Should all be 0 * 1000
|
||||
costs = xt::zeros<float>({1000});
|
||||
|
||||
// provide state pose and path close
|
||||
path.x(9) = 0.5;
|
||||
path.y(9) = 0.0;
|
||||
goal.position.x = 0.5;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(costs(2), 2.5, 1e-6); // (sqrt(10.0 * 10.0) * 5.0 weight
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 2500.0, 1e-6); // should be 2.5 * 1000
|
||||
}
|
||||
|
||||
TEST(CriticTests, PathAngleCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
PathAngleCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path close, within pose tolerance so won't do anything
|
||||
state.pose.pose.position.x = 0.0;
|
||||
state.pose.pose.position.y = 0.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path close but outside of tol. with less than PI/2 angular diff.
|
||||
path.x(9) = 0.95;
|
||||
goal.position.x = 0.95;
|
||||
data.furthest_reached_path_point = 2; // So it grabs the 2 + offset_from_furthest_ = 6th point
|
||||
path.x(6) = 1.0; // angle between path point and pose = 0 < max_angle_to_furthest_
|
||||
path.y(6) = 0.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path close but outside of tol. with more than PI/2 angular diff.
|
||||
path.x(6) = -1.0; // angle between path point and pose > max_angle_to_furthest_
|
||||
path.y(6) = 4.0;
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0.0);
|
||||
EXPECT_NEAR(costs(0), 3.6315, 1e-2); // atan2(4,-1) [1.81] * 2.0 weight
|
||||
}
|
||||
|
||||
TEST(CriticTests, PreferForwardCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
PreferForwardCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path far away, not within positional tolerances
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 10.0;
|
||||
goal.position.x = 10.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f);
|
||||
|
||||
// provide state pose and path close to trigger behavior but with all forward motion
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
state.vx = xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0f, 1e-6f);
|
||||
|
||||
// provide state pose and path close to trigger behavior but with all reverse motion
|
||||
state.vx = -1.0 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_GT(xt::sum(costs, immediate)(), 0.0f);
|
||||
EXPECT_NEAR(costs(0), 15.0f, 1e-3f); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length
|
||||
}
|
||||
|
||||
TEST(CriticTests, TwirlingCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
data.goal_checker = &goal_checker;
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
TwirlingCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path far away, not within positional tolerances
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 10.0;
|
||||
goal.position.x = 10.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path close to trigger behavior but with no angular variation
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
state.wz = xt::zeros<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// Provide nearby with some motion
|
||||
auto traj_view = xt::view(state.wz, 0, xt::all());
|
||||
traj_view = 10.0;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(costs(0), 100.0, 1e-6); // (mean(10.0) * 10.0 weight
|
||||
costs = xt::zeros<float>({1000});
|
||||
|
||||
// Now try again with some wiggling noise
|
||||
traj_view = xt::random::randn<float>({30}, 0.0, 0.5);
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(costs(0), 3.3, 4e-1); // (mean of noise with mu=0, sigma=0.5 * 10.0 weight
|
||||
}
|
||||
|
||||
TEST(CriticTests, PathFollowCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
data.goal_checker = &goal_checker;
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
PathFollowCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and goal close within positional tolerances
|
||||
state.pose.pose.position.x = 2.0;
|
||||
path.reset(6);
|
||||
path.x(5) = 1.8;
|
||||
goal.position.x = 1.8;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable
|
||||
// pose differential is (0, 0) and (0.15, 0)
|
||||
path.x(5) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 750.0, 1e-2); // 0.15 * 5 weight * 1000
|
||||
}
|
||||
|
||||
TEST(CriticTests, PathAlignCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
data.goal_checker = &goal_checker;
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
PathAlignCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path close within positional tolerances
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 0.85;
|
||||
goal.position.x = 0.85;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable
|
||||
// but data furthest point reached is 0 and offset default is 20, so returns
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// but with empty trajectories and paths, should still be zero
|
||||
*data.furthest_reached_path_point = 21;
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// and with a valid path to pass invalid path condition
|
||||
state.pose.pose.position.x = 0.0;
|
||||
data.path_pts_valid.reset(); // Recompute on new path
|
||||
path.reset(22);
|
||||
path.x(0) = 0;
|
||||
path.x(1) = 0.1;
|
||||
path.x(2) = 0.2;
|
||||
path.x(3) = 0.3;
|
||||
path.x(4) = 0.4;
|
||||
path.x(5) = 0.5;
|
||||
path.x(6) = 0.6;
|
||||
path.x(7) = 0.7;
|
||||
path.x(8) = 0.8;
|
||||
path.x(9) = 0.9;
|
||||
path.x(10) = 0.9;
|
||||
path.x(11) = 0.9;
|
||||
path.x(12) = 0.9;
|
||||
path.x(13) = 0.9;
|
||||
path.x(14) = 0.9;
|
||||
path.x(15) = 0.9;
|
||||
path.x(16) = 0.9;
|
||||
path.x(17) = 0.9;
|
||||
path.x(18) = 0.9;
|
||||
path.x(19) = 0.9;
|
||||
path.x(20) = 0.9;
|
||||
path.x(21) = 0.9;
|
||||
goal.position.x = 0.9;
|
||||
generated_trajectories.x = 0.66 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
// 0.66 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 6600.0, 1e-2);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// but path is blocked in collision
|
||||
auto * costmap = costmap_ros->getCostmap();
|
||||
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
|
||||
for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m
|
||||
for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m
|
||||
costmap->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
data.path_pts_valid.reset(); // Recompute on new path
|
||||
costs = xt::zeros<float>({1000});
|
||||
path.x = 1.5 * xt::ones<float>({22});
|
||||
path.y = 1.5 * xt::ones<float>({22});
|
||||
goal.position.x = 1.5;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
}
|
||||
|
||||
TEST(CriticTests, PathAlignLegacyCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
state.reset(1000, 30);
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
generated_trajectories.reset(1000, 30);
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<DiffDriveMotionModel>();
|
||||
TestGoalChecker goal_checker; // from utils_tests tolerance of 0.25 positionally
|
||||
data.goal_checker = &goal_checker;
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly
|
||||
PathAlignLegacyCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide state poses and path close within positional tolerances
|
||||
state.pose.pose.position.x = 1.0;
|
||||
path.reset(10);
|
||||
path.x(9) = 0.85;
|
||||
goal.position.x = 0.85;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable
|
||||
// but data furthest point reached is 0 and offset default is 20, so returns
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// but with empty trajectories and paths, should still be zero
|
||||
*data.furthest_reached_path_point = 21;
|
||||
path.x(9) = 0.15;
|
||||
goal.position.x = 0.15;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// and with a valid path to pass invalid path condition
|
||||
state.pose.pose.position.x = 0.0;
|
||||
data.path_pts_valid.reset(); // Recompute on new path
|
||||
path.reset(22);
|
||||
path.x(0) = 0;
|
||||
path.x(1) = 0.1;
|
||||
path.x(2) = 0.2;
|
||||
path.x(3) = 0.3;
|
||||
path.x(4) = 0.4;
|
||||
path.x(5) = 0.5;
|
||||
path.x(6) = 0.6;
|
||||
path.x(7) = 0.7;
|
||||
path.x(8) = 0.8;
|
||||
path.x(9) = 0.9;
|
||||
path.x(10) = 0.9;
|
||||
path.x(11) = 0.9;
|
||||
path.x(12) = 0.9;
|
||||
path.x(13) = 0.9;
|
||||
path.x(14) = 0.9;
|
||||
path.x(15) = 0.9;
|
||||
path.x(16) = 0.9;
|
||||
path.x(17) = 0.9;
|
||||
path.x(18) = 0.9;
|
||||
path.x(19) = 0.9;
|
||||
path.x(20) = 0.9;
|
||||
path.x(21) = 0.9;
|
||||
goal.position.x = 0.9;
|
||||
generated_trajectories.x = 0.66 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
// 0.04 * 1000 * 10 weight * 6 num pts eval / 6 normalization term
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 400.0, 1e-2);
|
||||
|
||||
// provide state pose and path far enough to enable, with data to pass condition
|
||||
// but path is blocked in collision
|
||||
auto * costmap = costmap_ros->getCostmap();
|
||||
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
|
||||
for (unsigned int i = 11; i <= 30; ++i) { // 1.1m-3m
|
||||
for (unsigned int j = 11; j <= 30; ++j) { // 1.1m-3m
|
||||
costmap->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
|
||||
data.path_pts_valid.reset(); // Recompute on new path
|
||||
costs = xt::zeros<float>({1000});
|
||||
path.x = 1.5 * xt::ones<float>({22});
|
||||
path.y = 1.5 * xt::ones<float>({22});
|
||||
goal.position.x = 1.5;
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6);
|
||||
}
|
||||
|
||||
TEST(CriticTests, VelocityDeadbandCritic)
|
||||
{
|
||||
// Standard preamble
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
auto getParam = param_handler.getParamGetter("critic");
|
||||
std::vector<double> deadband_velocities_;
|
||||
getParam(deadband_velocities_, "deadband_velocities", std::vector<double>{0.08, 0.08, 0.08});
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
|
||||
models::State state;
|
||||
models::ControlSequence control_sequence;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs = xt::zeros<float>({1000});
|
||||
float model_dt = 0.1;
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt,
|
||||
false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
data.motion_model = std::make_shared<OmniMotionModel>();
|
||||
|
||||
// Initialization testing
|
||||
|
||||
// Make sure initializes correctly and that defaults are reasonable
|
||||
VelocityDeadbandCritic critic;
|
||||
critic.on_configure(node, "mppi", "critic", costmap_ros, ¶m_handler);
|
||||
EXPECT_EQ(critic.getName(), "critic");
|
||||
|
||||
// Scoring testing
|
||||
|
||||
// provide velocities out of deadband bounds, should not have any costs
|
||||
state.vx = 0.80 * xt::ones<float>({1000, 30});
|
||||
state.vy = 0.60 * xt::ones<float>({1000, 30});
|
||||
state.wz = 0.80 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6);
|
||||
|
||||
// Test cost value
|
||||
state.vx = 0.01 * xt::ones<float>({1000, 30});
|
||||
state.vy = 0.02 * xt::ones<float>({1000, 30});
|
||||
state.wz = 0.021 * xt::ones<float>({1000, 30});
|
||||
critic.score(data);
|
||||
// 35.0 weight * 0.1 model_dt * (0.07 + 0.06 + 0.059) * 30 timesteps = 56.7
|
||||
EXPECT_NEAR(costs(1), 19.845, 0.01);
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
#include "nav2_mppi_controller/models/path.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/models/trajectories.hpp"
|
||||
|
||||
// Tests model classes with methods
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi::models; // NOLINT
|
||||
|
||||
TEST(ModelsTest, ControlSequenceTest)
|
||||
{
|
||||
// populate the object
|
||||
ControlSequence sequence;
|
||||
sequence.vx = xt::ones<float>({10});
|
||||
sequence.vy = xt::ones<float>({10});
|
||||
sequence.wz = xt::ones<float>({10});
|
||||
|
||||
// Show you can get contents
|
||||
EXPECT_EQ(sequence.vx(4), 1);
|
||||
EXPECT_EQ(sequence.vy(4), 1);
|
||||
EXPECT_EQ(sequence.wz(4), 1);
|
||||
|
||||
sequence.reset(20);
|
||||
|
||||
// Show contents are gone and new size
|
||||
EXPECT_EQ(sequence.vx(4), 0);
|
||||
EXPECT_EQ(sequence.vy(4), 0);
|
||||
EXPECT_EQ(sequence.wz(4), 0);
|
||||
EXPECT_EQ(sequence.vx.shape(0), 20u);
|
||||
EXPECT_EQ(sequence.vy.shape(0), 20u);
|
||||
EXPECT_EQ(sequence.wz.shape(0), 20u);
|
||||
}
|
||||
|
||||
TEST(ModelsTest, PathTest)
|
||||
{
|
||||
// populate the object
|
||||
Path path;
|
||||
path.x = xt::ones<float>({10});
|
||||
path.y = xt::ones<float>({10});
|
||||
path.yaws = xt::ones<float>({10});
|
||||
|
||||
// Show you can get contents
|
||||
EXPECT_EQ(path.x(4), 1);
|
||||
EXPECT_EQ(path.y(4), 1);
|
||||
EXPECT_EQ(path.yaws(4), 1);
|
||||
|
||||
path.reset(20);
|
||||
|
||||
// Show contents are gone and new size
|
||||
EXPECT_EQ(path.x(4), 0);
|
||||
EXPECT_EQ(path.y(4), 0);
|
||||
EXPECT_EQ(path.yaws(4), 0);
|
||||
EXPECT_EQ(path.x.shape(0), 20u);
|
||||
EXPECT_EQ(path.y.shape(0), 20u);
|
||||
EXPECT_EQ(path.yaws.shape(0), 20u);
|
||||
}
|
||||
|
||||
TEST(ModelsTest, StateTest)
|
||||
{
|
||||
// populate the object
|
||||
State state;
|
||||
state.vx = xt::ones<float>({10, 10});
|
||||
state.vy = xt::ones<float>({10, 10});
|
||||
state.wz = xt::ones<float>({10, 10});
|
||||
state.cvx = xt::ones<float>({10, 10});
|
||||
state.cvy = xt::ones<float>({10, 10});
|
||||
state.cwz = xt::ones<float>({10, 10});
|
||||
|
||||
// Show you can get contents
|
||||
EXPECT_EQ(state.cvx(4), 1);
|
||||
EXPECT_EQ(state.cvy(4), 1);
|
||||
EXPECT_EQ(state.cwz(4), 1);
|
||||
EXPECT_EQ(state.vx(4), 1);
|
||||
EXPECT_EQ(state.vy(4), 1);
|
||||
EXPECT_EQ(state.wz(4), 1);
|
||||
|
||||
state.reset(20, 40);
|
||||
|
||||
// Show contents are gone and new size
|
||||
EXPECT_EQ(state.cvx(4), 0);
|
||||
EXPECT_EQ(state.cvy(4), 0);
|
||||
EXPECT_EQ(state.cwz(4), 0);
|
||||
EXPECT_EQ(state.vx(4), 0);
|
||||
EXPECT_EQ(state.vy(4), 0);
|
||||
EXPECT_EQ(state.wz(4), 0);
|
||||
EXPECT_EQ(state.cvx.shape(0), 20u);
|
||||
EXPECT_EQ(state.cvy.shape(0), 20u);
|
||||
EXPECT_EQ(state.cwz.shape(0), 20u);
|
||||
EXPECT_EQ(state.cvx.shape(1), 40u);
|
||||
EXPECT_EQ(state.cvy.shape(1), 40u);
|
||||
EXPECT_EQ(state.cwz.shape(1), 40u);
|
||||
EXPECT_EQ(state.vx.shape(0), 20u);
|
||||
EXPECT_EQ(state.vy.shape(0), 20u);
|
||||
EXPECT_EQ(state.wz.shape(0), 20u);
|
||||
EXPECT_EQ(state.vx.shape(1), 40u);
|
||||
EXPECT_EQ(state.vy.shape(1), 40u);
|
||||
EXPECT_EQ(state.wz.shape(1), 40u);
|
||||
}
|
||||
|
||||
TEST(ModelsTest, TrajectoriesTest)
|
||||
{
|
||||
// populate the object
|
||||
Trajectories trajectories;
|
||||
trajectories.x = xt::ones<float>({10, 10});
|
||||
trajectories.y = xt::ones<float>({10, 10});
|
||||
trajectories.yaws = xt::ones<float>({10, 10});
|
||||
|
||||
// Show you can get contents
|
||||
EXPECT_EQ(trajectories.x(4), 1);
|
||||
EXPECT_EQ(trajectories.y(4), 1);
|
||||
EXPECT_EQ(trajectories.yaws(4), 1);
|
||||
|
||||
trajectories.reset(20, 40);
|
||||
|
||||
// Show contents are gone and new size
|
||||
EXPECT_EQ(trajectories.x(4), 0);
|
||||
EXPECT_EQ(trajectories.y(4), 0);
|
||||
EXPECT_EQ(trajectories.yaws(4), 0);
|
||||
EXPECT_EQ(trajectories.x.shape(0), 20u);
|
||||
EXPECT_EQ(trajectories.y.shape(0), 20u);
|
||||
EXPECT_EQ(trajectories.yaws.shape(0), 20u);
|
||||
EXPECT_EQ(trajectories.x.shape(1), 40u);
|
||||
EXPECT_EQ(trajectories.y.shape(1), 40u);
|
||||
EXPECT_EQ(trajectories.yaws.shape(1), 40u);
|
||||
}
|
||||
@@ -0,0 +1,257 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
|
||||
// Tests motion models
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
TEST(MotionModelTests, DiffDriveTest)
|
||||
{
|
||||
models::ControlSequence control_sequence;
|
||||
models::State state;
|
||||
int batches = 1000;
|
||||
int timesteps = 50;
|
||||
control_sequence.reset(timesteps); // populates with zeros
|
||||
state.reset(batches, timesteps); // populates with zeros
|
||||
std::unique_ptr<DiffDriveMotionModel> model =
|
||||
std::make_unique<DiffDriveMotionModel>();
|
||||
|
||||
// Check that predict properly populates the trajectory velocities with the control velocities
|
||||
state.cvx = 10 * xt::ones<float>({batches, timesteps});
|
||||
state.cvy = 5 * xt::ones<float>({batches, timesteps});
|
||||
state.cwz = 1 * xt::ones<float>({batches, timesteps});
|
||||
|
||||
// Manually set state index 0 from initial conditions which would be the speed of the robot
|
||||
xt::view(state.vx, xt::all(), 0) = 10;
|
||||
xt::view(state.wz, xt::all(), 0) = 1;
|
||||
|
||||
model->predict(state);
|
||||
|
||||
EXPECT_EQ(state.vx, state.cvx);
|
||||
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
|
||||
EXPECT_EQ(state.wz, state.cwz);
|
||||
|
||||
// Check that application of constraints are empty for Diff Drive
|
||||
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
|
||||
control_sequence.vx(i) = i * i * i;
|
||||
control_sequence.wz(i) = i * i * i;
|
||||
}
|
||||
|
||||
models::ControlSequence initial_control_sequence = control_sequence;
|
||||
model->applyConstraints(control_sequence);
|
||||
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
|
||||
EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy);
|
||||
EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz);
|
||||
|
||||
// Check that Diff Drive is properly non-holonomic
|
||||
EXPECT_EQ(model->isHolonomic(), false);
|
||||
|
||||
// Check it cleanly destructs
|
||||
model.reset();
|
||||
}
|
||||
|
||||
TEST(MotionModelTests, OmniTest)
|
||||
{
|
||||
models::ControlSequence control_sequence;
|
||||
models::State state;
|
||||
int batches = 1000;
|
||||
int timesteps = 50;
|
||||
control_sequence.reset(timesteps); // populates with zeros
|
||||
state.reset(batches, timesteps); // populates with zeros
|
||||
std::unique_ptr<OmniMotionModel> model =
|
||||
std::make_unique<OmniMotionModel>();
|
||||
|
||||
// Check that predict properly populates the trajectory velocities with the control velocities
|
||||
state.cvx = 10 * xt::ones<float>({batches, timesteps});
|
||||
state.cvy = 5 * xt::ones<float>({batches, timesteps});
|
||||
state.cwz = 1 * xt::ones<float>({batches, timesteps});
|
||||
|
||||
// Manually set state index 0 from initial conditions which would be the speed of the robot
|
||||
xt::view(state.vx, xt::all(), 0) = 10;
|
||||
xt::view(state.vy, xt::all(), 0) = 5;
|
||||
xt::view(state.wz, xt::all(), 0) = 1;
|
||||
|
||||
model->predict(state);
|
||||
|
||||
EXPECT_EQ(state.vx, state.cvx);
|
||||
EXPECT_EQ(state.vy, state.cvy); // holonomic
|
||||
EXPECT_EQ(state.wz, state.cwz);
|
||||
|
||||
// Check that application of constraints are empty for Omni Drive
|
||||
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
|
||||
control_sequence.vx(i) = i * i * i;
|
||||
control_sequence.vy(i) = i * i * i;
|
||||
control_sequence.wz(i) = i * i * i;
|
||||
}
|
||||
|
||||
models::ControlSequence initial_control_sequence = control_sequence;
|
||||
model->applyConstraints(control_sequence);
|
||||
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
|
||||
EXPECT_EQ(initial_control_sequence.vy, control_sequence.vy);
|
||||
EXPECT_EQ(initial_control_sequence.wz, control_sequence.wz);
|
||||
|
||||
// Check that Omni Drive is properly holonomic
|
||||
EXPECT_EQ(model->isHolonomic(), true);
|
||||
|
||||
// Check it cleanly destructs
|
||||
model.reset();
|
||||
}
|
||||
|
||||
TEST(MotionModelTests, AckermannTest)
|
||||
{
|
||||
models::ControlSequence control_sequence;
|
||||
models::State state;
|
||||
int batches = 1000;
|
||||
int timesteps = 50;
|
||||
control_sequence.reset(timesteps); // populates with zeros
|
||||
state.reset(batches, timesteps); // populates with zeros
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
ParametersHandler param_handler(node);
|
||||
std::unique_ptr<AckermannMotionModel> model =
|
||||
std::make_unique<AckermannMotionModel>(¶m_handler, node->get_name());
|
||||
|
||||
// Check that predict properly populates the trajectory velocities with the control velocities
|
||||
state.cvx = 10 * xt::ones<float>({batches, timesteps});
|
||||
state.cvy = 5 * xt::ones<float>({batches, timesteps});
|
||||
state.cwz = 1 * xt::ones<float>({batches, timesteps});
|
||||
|
||||
// Manually set state index 0 from initial conditions which would be the speed of the robot
|
||||
xt::view(state.vx, xt::all(), 0) = 10;
|
||||
xt::view(state.wz, xt::all(), 0) = 1;
|
||||
|
||||
model->predict(state);
|
||||
|
||||
EXPECT_EQ(state.vx, state.cvx);
|
||||
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
|
||||
EXPECT_EQ(state.wz, state.cwz);
|
||||
|
||||
// Check that application of constraints are non-empty for Ackermann Drive
|
||||
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
|
||||
control_sequence.vx(i) = i * i * i;
|
||||
control_sequence.wz(i) = i * i * i * i;
|
||||
}
|
||||
|
||||
models::ControlSequence initial_control_sequence = control_sequence;
|
||||
model->applyConstraints(control_sequence);
|
||||
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
|
||||
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
|
||||
EXPECT_NE(initial_control_sequence.wz, control_sequence.wz);
|
||||
for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) {
|
||||
EXPECT_GT(control_sequence.wz(i), 0.0);
|
||||
}
|
||||
|
||||
// Now, check the specifics of the minimum curvature constraint
|
||||
EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
|
||||
for (unsigned int i = 1; i != control_sequence.vx.shape(0); i++) {
|
||||
EXPECT_TRUE(fabs(control_sequence.vx(i)) / fabs(control_sequence.wz(i)) >= 0.2);
|
||||
}
|
||||
|
||||
// Check that Ackermann Drive is properly non-holonomic and parameterized
|
||||
EXPECT_EQ(model->isHolonomic(), false);
|
||||
|
||||
// Check it cleanly destructs
|
||||
model.reset();
|
||||
}
|
||||
|
||||
TEST(MotionModelTests, AckermannReversingTest)
|
||||
{
|
||||
models::ControlSequence control_sequence;
|
||||
models::ControlSequence control_sequence2;
|
||||
models::State state;
|
||||
int batches = 1000;
|
||||
int timesteps = 50;
|
||||
control_sequence.reset(timesteps); // populates with zeros
|
||||
control_sequence2.reset(timesteps); // populates with zeros
|
||||
state.reset(batches, timesteps); // populates with zeros
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
ParametersHandler param_handler(node);
|
||||
std::unique_ptr<AckermannMotionModel> model =
|
||||
std::make_unique<AckermannMotionModel>(¶m_handler, node->get_name());
|
||||
|
||||
// Check that predict properly populates the trajectory velocities with the control velocities
|
||||
state.cvx = 10 * xt::ones<float>({batches, timesteps});
|
||||
state.cvy = 5 * xt::ones<float>({batches, timesteps});
|
||||
state.cwz = 1 * xt::ones<float>({batches, timesteps});
|
||||
|
||||
// Manually set state index 0 from initial conditions which would be the speed of the robot
|
||||
xt::view(state.vx, xt::all(), 0) = 10;
|
||||
xt::view(state.wz, xt::all(), 0) = 1;
|
||||
|
||||
model->predict(state);
|
||||
|
||||
EXPECT_EQ(state.vx, state.cvx);
|
||||
EXPECT_EQ(state.vy, xt::zeros<float>({batches, timesteps})); // non-holonomic
|
||||
EXPECT_EQ(state.wz, state.cwz);
|
||||
|
||||
// Check that application of constraints are non-empty for Ackermann Drive
|
||||
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
|
||||
float idx = static_cast<float>(i);
|
||||
control_sequence.vx(i) = -idx * idx * idx; // now reversing
|
||||
control_sequence.wz(i) = idx * idx * idx * idx;
|
||||
}
|
||||
|
||||
models::ControlSequence initial_control_sequence = control_sequence;
|
||||
model->applyConstraints(control_sequence);
|
||||
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
|
||||
EXPECT_EQ(initial_control_sequence.vx, control_sequence.vx);
|
||||
EXPECT_NE(initial_control_sequence.wz, control_sequence.wz);
|
||||
for (unsigned int i = 1; i != control_sequence.wz.shape(0); i++) {
|
||||
EXPECT_GT(control_sequence.wz(i), 0.0);
|
||||
}
|
||||
|
||||
// Repeat with negative rotation direction
|
||||
for (unsigned int i = 0; i != control_sequence2.vx.shape(0); i++) {
|
||||
float idx = static_cast<float>(i);
|
||||
control_sequence2.vx(i) = -idx * idx * idx; // now reversing
|
||||
control_sequence2.wz(i) = -idx * idx * idx * idx;
|
||||
}
|
||||
|
||||
models::ControlSequence initial_control_sequence2 = control_sequence2;
|
||||
model->applyConstraints(control_sequence2);
|
||||
// VX equal since this doesn't change, the WZ is reduced if breaking the constraint
|
||||
EXPECT_EQ(initial_control_sequence2.vx, control_sequence2.vx);
|
||||
EXPECT_NE(initial_control_sequence2.wz, control_sequence2.wz);
|
||||
for (unsigned int i = 1; i != control_sequence2.wz.shape(0); i++) {
|
||||
EXPECT_LT(control_sequence2.wz(i), 0.0);
|
||||
}
|
||||
|
||||
// Now, check the specifics of the minimum curvature constraint
|
||||
EXPECT_NEAR(model->getMinTurningRadius(), 0.2, 1e-6);
|
||||
for (unsigned int i = 1; i != control_sequence2.vx.shape(0); i++) {
|
||||
EXPECT_TRUE(fabs(control_sequence2.vx(i)) / fabs(control_sequence2.wz(i)) >= 0.2);
|
||||
}
|
||||
|
||||
// Check that Ackermann Drive is properly non-holonomic and parameterized
|
||||
EXPECT_EQ(model->isHolonomic(), false);
|
||||
|
||||
// Check it cleanly destructs
|
||||
model.reset();
|
||||
}
|
||||
@@ -0,0 +1,131 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_lifecycle/lifecycle_node.hpp"
|
||||
#include "nav2_mppi_controller/tools/noise_generator.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/models/optimizer_settings.hpp"
|
||||
#include "nav2_mppi_controller/models/state.hpp"
|
||||
#include "nav2_mppi_controller/models/control_sequence.hpp"
|
||||
|
||||
// Tests noise generator object
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
TEST(NoiseGeneratorTest, NoiseGeneratorLifecycle)
|
||||
{
|
||||
// Tests shuts down internal thread cleanly
|
||||
NoiseGenerator generator;
|
||||
mppi::models::OptimizerSettings settings;
|
||||
settings.batch_size = 100;
|
||||
settings.time_steps = 25;
|
||||
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
|
||||
node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(false));
|
||||
ParametersHandler handler(node);
|
||||
|
||||
generator.initialize(settings, false, "test_name", &handler);
|
||||
generator.reset(settings, false);
|
||||
generator.shutdown();
|
||||
}
|
||||
|
||||
TEST(NoiseGeneratorTest, NoiseGeneratorMain)
|
||||
{
|
||||
// Tests shuts down internal thread cleanly
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("node");
|
||||
node->declare_parameter("test_name.regenerate_noises", rclcpp::ParameterValue(true));
|
||||
ParametersHandler handler(node);
|
||||
NoiseGenerator generator;
|
||||
mppi::models::OptimizerSettings settings;
|
||||
settings.batch_size = 100;
|
||||
settings.time_steps = 25;
|
||||
settings.sampling_std.vx = 0.1;
|
||||
settings.sampling_std.vy = 0.1;
|
||||
settings.sampling_std.wz = 0.1;
|
||||
|
||||
// Populate a potential control sequence
|
||||
mppi::models::ControlSequence control_sequence;
|
||||
control_sequence.reset(25);
|
||||
for (unsigned int i = 0; i != control_sequence.vx.shape(0); i++) {
|
||||
control_sequence.vx(i) = i;
|
||||
control_sequence.vy(i) = i;
|
||||
control_sequence.wz(i) = i;
|
||||
}
|
||||
|
||||
mppi::models::State state;
|
||||
state.reset(settings.batch_size, settings.time_steps);
|
||||
|
||||
// Request an update with no noise yet generated, should result in identical outputs
|
||||
generator.initialize(settings, false, "test_name", &handler);
|
||||
generator.reset(settings, false); // sets initial sizing and zeros out noises
|
||||
generator.setNoisedControls(state, control_sequence);
|
||||
EXPECT_EQ(state.cvx(0), 0);
|
||||
EXPECT_EQ(state.cvy(0), 0);
|
||||
EXPECT_EQ(state.cwz(0), 0);
|
||||
EXPECT_EQ(state.cvx(9), 9);
|
||||
EXPECT_EQ(state.cvy(9), 9);
|
||||
EXPECT_EQ(state.cwz(9), 9);
|
||||
|
||||
// Request an update with noise requested
|
||||
generator.generateNextNoises();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
generator.setNoisedControls(state, control_sequence);
|
||||
EXPECT_NE(state.cvx(0), 0);
|
||||
EXPECT_EQ(state.cvy(0), 0); // Not populated in non-holonomic
|
||||
EXPECT_NE(state.cwz(0), 0);
|
||||
EXPECT_NE(state.cvx(9), 9);
|
||||
EXPECT_EQ(state.cvy(9), 9); // Not populated in non-holonomic
|
||||
EXPECT_NE(state.cwz(9), 9);
|
||||
|
||||
EXPECT_NEAR(state.cvx(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cvy(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cwz(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cvx(9), 9, 0.3);
|
||||
EXPECT_NEAR(state.cvy(9), 9, 0.3);
|
||||
EXPECT_NEAR(state.cwz(9), 9, 0.3);
|
||||
|
||||
// Test holonomic setting
|
||||
generator.reset(settings, true); // Now holonomically
|
||||
generator.generateNextNoises();
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
generator.setNoisedControls(state, control_sequence);
|
||||
EXPECT_NE(state.cvx(0), 0);
|
||||
EXPECT_NE(state.cvy(0), 0); // Now populated in non-holonomic
|
||||
EXPECT_NE(state.cwz(0), 0);
|
||||
EXPECT_NE(state.cvx(9), 9);
|
||||
EXPECT_NE(state.cvy(9), 9); // Now populated in non-holonomic
|
||||
EXPECT_NE(state.cwz(9), 9);
|
||||
|
||||
EXPECT_NEAR(state.cvx(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cvy(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cwz(0), 0, 0.3);
|
||||
EXPECT_NEAR(state.cvx(9), 9, 0.3);
|
||||
EXPECT_NEAR(state.cvy(9), 9, 0.3);
|
||||
EXPECT_NEAR(state.cwz(9), 9, 0.3);
|
||||
|
||||
generator.shutdown();
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
|
||||
#include <nav2_costmap_2d/cost_values.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d.hpp>
|
||||
#include <nav2_costmap_2d/costmap_2d_ros.hpp>
|
||||
#include <nav2_core/goal_checker.hpp>
|
||||
|
||||
#include <xtensor/xarray.hpp>
|
||||
#include <xtensor/xio.hpp>
|
||||
#include <xtensor/xview.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
|
||||
#include "utils/utils.hpp"
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
// Smoke tests the optimizer
|
||||
|
||||
class OptimizerSuite : public ::testing::TestWithParam<std::tuple<std::string,
|
||||
std::vector<std::string>, bool>> {};
|
||||
|
||||
TEST_P(OptimizerSuite, OptimizerTest) {
|
||||
auto [motion_model, critics, consider_footprint] = GetParam();
|
||||
|
||||
int batch_size = 400;
|
||||
int time_steps = 15;
|
||||
unsigned int path_points = 50u;
|
||||
int iteration_count = 1;
|
||||
double lookahead_distance = 10.0;
|
||||
|
||||
TestCostmapSettings costmap_settings{};
|
||||
auto costmap_ros = getDummyCostmapRos(costmap_settings);
|
||||
auto costmap = costmap_ros->getCostmap();
|
||||
|
||||
TestPose start_pose = costmap_settings.getCenterPose();
|
||||
double path_step = costmap_settings.resolution;
|
||||
|
||||
TestPathSettings path_settings{start_pose, path_points, path_step, path_step};
|
||||
TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count,
|
||||
lookahead_distance, motion_model, consider_footprint};
|
||||
|
||||
unsigned int offset = 4;
|
||||
unsigned int obstacle_size = offset * 2;
|
||||
|
||||
unsigned char obstacle_cost = 250;
|
||||
|
||||
auto [obst_x, obst_y] = costmap_settings.getCenterIJ();
|
||||
|
||||
obst_x = obst_x - offset;
|
||||
obst_y = obst_y - offset;
|
||||
addObstacle(costmap, {obst_x, obst_y, obstacle_size, obstacle_cost});
|
||||
|
||||
printInfo(optimizer_settings, path_settings, critics);
|
||||
auto node = getDummyNode(optimizer_settings, critics);
|
||||
auto parameters_handler = std::make_unique<mppi::ParametersHandler>(node);
|
||||
auto optimizer = getDummyOptimizer(node, costmap_ros, parameters_handler.get());
|
||||
|
||||
// evalControl args
|
||||
auto pose = getDummyPointStamped(node, start_pose);
|
||||
auto velocity = getDummyTwist();
|
||||
auto path = getIncrementalDummyPath(node, path_settings);
|
||||
auto goal = path.poses.back().pose;
|
||||
nav2_core::GoalChecker * dummy_goal_checker{nullptr};
|
||||
|
||||
EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, goal, dummy_goal_checker));
|
||||
}
|
||||
|
||||
INSTANTIATE_TEST_SUITE_P(
|
||||
OptimizerTests,
|
||||
OptimizerSuite,
|
||||
::testing::Values(
|
||||
std::make_tuple(
|
||||
"Omni",
|
||||
std::vector<std::string>(
|
||||
{{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"},
|
||||
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
|
||||
true),
|
||||
std::make_tuple(
|
||||
"DiffDrive",
|
||||
std::vector<std::string>(
|
||||
{{"GoalCritic"}, {"GoalAngleCritic"}, {"CostCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
|
||||
true),
|
||||
std::make_tuple(
|
||||
"Ackermann",
|
||||
std::vector<std::string>(
|
||||
{{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
|
||||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}),
|
||||
true))
|
||||
);
|
||||
@@ -0,0 +1,639 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
|
||||
// Tests main optimizer functions
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
using namespace mppi::critics; // NOLINT
|
||||
using namespace mppi::utils; // NOLINT
|
||||
using xt::evaluation_strategy::immediate;
|
||||
|
||||
class OptimizerTester : public Optimizer
|
||||
{
|
||||
public:
|
||||
OptimizerTester()
|
||||
: Optimizer() {}
|
||||
|
||||
void testSetDiffModel()
|
||||
{
|
||||
EXPECT_EQ(motion_model_.get(), nullptr);
|
||||
EXPECT_NO_THROW(setMotionModel("DiffDrive"));
|
||||
EXPECT_NE(motion_model_.get(), nullptr);
|
||||
EXPECT_TRUE(dynamic_cast<DiffDriveMotionModel *>(motion_model_.get()));
|
||||
EXPECT_FALSE(isHolonomic());
|
||||
}
|
||||
|
||||
void testSetOmniModel()
|
||||
{
|
||||
EXPECT_EQ(motion_model_.get(), nullptr);
|
||||
EXPECT_NO_THROW(setMotionModel("Omni"));
|
||||
EXPECT_NE(motion_model_.get(), nullptr);
|
||||
EXPECT_TRUE(dynamic_cast<OmniMotionModel *>(motion_model_.get()));
|
||||
EXPECT_TRUE(isHolonomic());
|
||||
}
|
||||
|
||||
void testSetAckModel()
|
||||
{
|
||||
EXPECT_EQ(motion_model_.get(), nullptr);
|
||||
EXPECT_NO_THROW(setMotionModel("Ackermann"));
|
||||
EXPECT_NE(motion_model_.get(), nullptr);
|
||||
EXPECT_TRUE(dynamic_cast<AckermannMotionModel *>(motion_model_.get()));
|
||||
EXPECT_FALSE(isHolonomic());
|
||||
}
|
||||
|
||||
void testSetRandModel()
|
||||
{
|
||||
EXPECT_EQ(motion_model_.get(), nullptr);
|
||||
try {
|
||||
setMotionModel("Random");
|
||||
FAIL();
|
||||
} catch (...) {
|
||||
SUCCEED();
|
||||
}
|
||||
EXPECT_EQ(motion_model_.get(), nullptr);
|
||||
}
|
||||
|
||||
void resetMotionModel()
|
||||
{
|
||||
motion_model_.reset();
|
||||
}
|
||||
|
||||
void setOffsetWrapper(const double freq)
|
||||
{
|
||||
return setOffset(freq);
|
||||
}
|
||||
|
||||
bool getShiftControlSequence()
|
||||
{
|
||||
return settings_.shift_control_sequence;
|
||||
}
|
||||
|
||||
void fillOptimizerWithGarbage()
|
||||
{
|
||||
state_.vx = 0.43432 * xt::ones<float>({1000, 10});
|
||||
control_sequence_.vx = 342.0 * xt::ones<float>({30});
|
||||
control_history_[0] = {43, 5646, 32432};
|
||||
costs_ = 5.32 * xt::ones<float>({56453});
|
||||
generated_trajectories_.x = 432.234 * xt::ones<float>({7865, 1});
|
||||
}
|
||||
|
||||
void testReset()
|
||||
{
|
||||
reset();
|
||||
|
||||
EXPECT_EQ(state_.vx, xt::zeros<float>({1000, 50}));
|
||||
EXPECT_EQ(control_sequence_.vx, xt::zeros<float>({50}));
|
||||
EXPECT_EQ(control_history_[0].vx, 0.0);
|
||||
EXPECT_EQ(control_history_[0].vy, 0.0);
|
||||
EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6);
|
||||
EXPECT_EQ(generated_trajectories_.x, xt::zeros<float>({1000, 50}));
|
||||
}
|
||||
|
||||
bool fallbackWrapper(bool fail)
|
||||
{
|
||||
return fallback(fail);
|
||||
}
|
||||
|
||||
void testPrepare(
|
||||
const geometry_msgs::msg::PoseStamped & robot_pose,
|
||||
const geometry_msgs::msg::Twist & robot_speed,
|
||||
const nav_msgs::msg::Path & plan,
|
||||
const geometry_msgs::msg::Pose & goal,
|
||||
nav2_core::GoalChecker * goal_checker)
|
||||
{
|
||||
prepare(robot_pose, robot_speed, plan, goal, goal_checker);
|
||||
|
||||
EXPECT_EQ(critics_data_.goal_checker, nullptr);
|
||||
EXPECT_NEAR(xt::sum(costs_, immediate)(), 0, 1e-6); // should be reset
|
||||
EXPECT_FALSE(critics_data_.fail_flag); // should be reset
|
||||
EXPECT_FALSE(critics_data_.motion_model->isHolonomic()); // object is valid + diff drive
|
||||
EXPECT_FALSE(critics_data_.furthest_reached_path_point.has_value()); // val is not set
|
||||
EXPECT_FALSE(critics_data_.path_pts_valid.has_value()); // val is not set
|
||||
EXPECT_EQ(state_.pose.pose.position.x, 999);
|
||||
EXPECT_EQ(state_.speed.linear.y, 4.0);
|
||||
EXPECT_EQ(path_.x.shape(0), 17u);
|
||||
}
|
||||
|
||||
void shiftControlSequenceWrapper()
|
||||
{
|
||||
return shiftControlSequence();
|
||||
}
|
||||
|
||||
std::pair<double, double> getVelLimits()
|
||||
{
|
||||
auto & s = settings_;
|
||||
return {s.constraints.vx_min, s.constraints.vx_max};
|
||||
}
|
||||
|
||||
void applyControlSequenceConstraintsWrapper()
|
||||
{
|
||||
return applyControlSequenceConstraints();
|
||||
}
|
||||
|
||||
models::ControlSequence & grabControlSequence()
|
||||
{
|
||||
return control_sequence_;
|
||||
}
|
||||
|
||||
void testupdateStateVels()
|
||||
{
|
||||
// updateInitialStateVelocities
|
||||
models::State state;
|
||||
state.reset(1000, 50);
|
||||
state.speed.linear.x = 5.0;
|
||||
state.speed.linear.y = 1.0;
|
||||
state.speed.angular.z = 6.0;
|
||||
state.cvx = 0.75 * xt::ones<float>({1000, 50});
|
||||
state.cvy = 0.5 * xt::ones<float>({1000, 50});
|
||||
state.cwz = 0.1 * xt::ones<float>({1000, 50});
|
||||
updateInitialStateVelocities(state);
|
||||
EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
|
||||
EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
|
||||
EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
|
||||
|
||||
// propagateStateVelocitiesFromInitials
|
||||
propagateStateVelocitiesFromInitials(state);
|
||||
EXPECT_NEAR(state.vx(0, 0), 5.0, 1e-6);
|
||||
EXPECT_NEAR(state.vy(0, 0), 1.0, 1e-6);
|
||||
EXPECT_NEAR(state.wz(0, 0), 6.0, 1e-6);
|
||||
EXPECT_NEAR(state.vx(0, 1), 0.75, 1e-6);
|
||||
EXPECT_NEAR(state.vy(0, 1), 0.5, 1e-6);
|
||||
EXPECT_NEAR(state.wz(0, 1), 0.1, 1e-6);
|
||||
|
||||
// Putting them together: updateStateVelocities
|
||||
state.reset(1000, 50);
|
||||
state.speed.linear.x = -5.0;
|
||||
state.speed.linear.y = -1.0;
|
||||
state.speed.angular.z = -6.0;
|
||||
state.cvx = -0.75 * xt::ones<float>({1000, 50});
|
||||
state.cvy = -0.5 * xt::ones<float>({1000, 50});
|
||||
state.cwz = -0.1 * xt::ones<float>({1000, 50});
|
||||
updateStateVelocities(state);
|
||||
EXPECT_NEAR(state.vx(0, 0), -5.0, 1e-6);
|
||||
EXPECT_NEAR(state.vy(0, 0), -1.0, 1e-6);
|
||||
EXPECT_NEAR(state.wz(0, 0), -6.0, 1e-6);
|
||||
EXPECT_NEAR(state.vx(0, 1), -0.75, 1e-6);
|
||||
EXPECT_NEAR(state.vy(0, 1), -0.5, 1e-6);
|
||||
EXPECT_NEAR(state.wz(0, 1), -0.1, 1e-6);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::TwistStamped getControlFromSequenceAsTwistWrapper()
|
||||
{
|
||||
builtin_interfaces::msg::Time stamp;
|
||||
return getControlFromSequenceAsTwist(stamp);
|
||||
}
|
||||
|
||||
void integrateStateVelocitiesWrapper(
|
||||
models::Trajectories & traj,
|
||||
const models::State & state)
|
||||
{
|
||||
return integrateStateVelocities(traj, state);
|
||||
}
|
||||
};
|
||||
|
||||
TEST(OptimizerTests, BasicInitializedFunctions)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Should be empty of size batches x time steps
|
||||
// and tests getting set params: time_steps, batch_size, controller_frequency
|
||||
auto trajs = optimizer_tester.getGeneratedTrajectories();
|
||||
EXPECT_EQ(trajs.x.shape(0), 1000u);
|
||||
EXPECT_EQ(trajs.x.shape(1), 50u);
|
||||
EXPECT_EQ(trajs.x, xt::zeros<float>({1000, 50}));
|
||||
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
auto traj = optimizer_tester.getOptimizedTrajectory();
|
||||
EXPECT_EQ(traj(5, 0), 0.0); // x
|
||||
EXPECT_EQ(traj(5, 1), 0.0); // y
|
||||
EXPECT_EQ(traj(5, 2), 0.0); // yaw
|
||||
EXPECT_EQ(traj.shape(0), 50u);
|
||||
EXPECT_EQ(traj.shape(1), 3u);
|
||||
|
||||
optimizer_tester.reset();
|
||||
optimizer_tester.shutdown();
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, TestOptimizerMotionModels)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Diff Drive should be non-holonomic
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetDiffModel();
|
||||
|
||||
// Omni Drive should be holonomic
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
|
||||
// // Ackermann should be non-holonomic
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetAckModel();
|
||||
|
||||
// // Rand should fail
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetRandModel();
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, setOffsetTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1));
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test offsets are properly set based on relationship of model_dt and controller frequency
|
||||
// Also tests getting set model_dt parameter.
|
||||
EXPECT_THROW(optimizer_tester.setOffsetWrapper(1.0), std::runtime_error);
|
||||
EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(30.0));
|
||||
EXPECT_FALSE(optimizer_tester.getShiftControlSequence());
|
||||
EXPECT_NO_THROW(optimizer_tester.setOffsetWrapper(10.0));
|
||||
EXPECT_TRUE(optimizer_tester.getShiftControlSequence());
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, resetTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Tests resetting the full state of all the functions after filling with garbage
|
||||
optimizer_tester.fillOptimizerWithGarbage();
|
||||
optimizer_tester.testReset();
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, FallbackTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test fallback logic, also tests getting set param retry_attempt_limit
|
||||
// Because retry set to 2, it should attempt soft resets 2x before throwing exception
|
||||
// for hard reset
|
||||
EXPECT_FALSE(optimizer_tester.fallbackWrapper(false));
|
||||
EXPECT_TRUE(optimizer_tester.fallbackWrapper(true));
|
||||
EXPECT_TRUE(optimizer_tester.fallbackWrapper(true));
|
||||
EXPECT_THROW(optimizer_tester.fallbackWrapper(true), std::runtime_error);
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, PrepareTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test Prepare function to set the state of the robot pose/speed on new cycle
|
||||
// Populate the contents with things easily identifiable if correct
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = 999;
|
||||
geometry_msgs::msg::Twist speed;
|
||||
speed.linear.y = 4.0;
|
||||
nav_msgs::msg::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
path.poses.resize(17);
|
||||
|
||||
optimizer_tester.testPrepare(pose, speed, path, goal, nullptr);
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, shiftControlSequenceTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test shiftControlSequence by setting the 2nd value to something unique to neighbors
|
||||
auto & sequence = optimizer_tester.grabControlSequence();
|
||||
sequence.reset(100);
|
||||
sequence.vx(0) = 9999;
|
||||
sequence.vx(1) = 6;
|
||||
sequence.vx(2) = 888;
|
||||
sequence.vy(0) = 9999;
|
||||
sequence.vy(1) = 6;
|
||||
sequence.vy(2) = 888;
|
||||
sequence.wz(0) = 9999;
|
||||
sequence.wz(1) = 6;
|
||||
sequence.wz(2) = 888;
|
||||
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
optimizer_tester.shiftControlSequenceWrapper();
|
||||
|
||||
EXPECT_EQ(sequence.vx(0), 6);
|
||||
EXPECT_EQ(sequence.vy(0), 6);
|
||||
EXPECT_EQ(sequence.wz(0), 6);
|
||||
EXPECT_EQ(sequence.vx(1), 888);
|
||||
EXPECT_EQ(sequence.vy(1), 888);
|
||||
EXPECT_EQ(sequence.wz(1), 888);
|
||||
EXPECT_EQ(sequence.vx(2), 0);
|
||||
EXPECT_EQ(sequence.vy(2), 0);
|
||||
EXPECT_EQ(sequence.wz(2), 0);
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, SpeedLimitTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.retry_attempt_limit", rclcpp::ParameterValue(2));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test Speed limits API
|
||||
auto [v_min, v_max] = optimizer_tester.getVelLimits();
|
||||
EXPECT_EQ(v_max, 0.5f);
|
||||
EXPECT_EQ(v_min, -0.35f);
|
||||
optimizer_tester.setSpeedLimit(0, false);
|
||||
auto [v_min2, v_max2] = optimizer_tester.getVelLimits();
|
||||
EXPECT_EQ(v_max2, 0.5f);
|
||||
EXPECT_EQ(v_min2, -0.35f);
|
||||
optimizer_tester.setSpeedLimit(50.0, true);
|
||||
auto [v_min3, v_max3] = optimizer_tester.getVelLimits();
|
||||
EXPECT_NEAR(v_max3, 0.5 / 2.0, 1e-3);
|
||||
EXPECT_NEAR(v_min3, -0.35 / 2.0, 1e-3);
|
||||
optimizer_tester.setSpeedLimit(0, true);
|
||||
auto [v_min4, v_max4] = optimizer_tester.getVelLimits();
|
||||
EXPECT_EQ(v_max4, 0.5f);
|
||||
EXPECT_EQ(v_min4, -0.35f);
|
||||
optimizer_tester.setSpeedLimit(0.75, false);
|
||||
auto [v_min5, v_max5] = optimizer_tester.getVelLimits();
|
||||
EXPECT_NEAR(v_max5, 0.75, 1e-3);
|
||||
EXPECT_NEAR(v_min5, -0.5249, 1e-2);
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, applyControlSequenceConstraintsTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
|
||||
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
|
||||
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.75));
|
||||
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test constraints being applied to ensure feasibility of trajectories
|
||||
// Also tests param get of set vx/vy/wz min/maxes
|
||||
|
||||
// Set model to omni to consider holonomic vy elements
|
||||
// Ack is not tested here because `applyConstraints` is covered in detail
|
||||
// in motion_models_test.cpp
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
auto & sequence = optimizer_tester.grabControlSequence();
|
||||
|
||||
// Test boundary of limits
|
||||
sequence.vx = xt::ones<float>({50});
|
||||
sequence.vy = 0.75 * xt::ones<float>({50});
|
||||
sequence.wz = 2.0 * xt::ones<float>({50});
|
||||
optimizer_tester.applyControlSequenceConstraintsWrapper();
|
||||
EXPECT_EQ(sequence.vx, xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.vy, 0.75 * xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.wz, 2.0 * xt::ones<float>({50}));
|
||||
|
||||
// Test breaking limits sets to maximum
|
||||
sequence.vx = 5.0 * xt::ones<float>({50});
|
||||
sequence.vy = 5.0 * xt::ones<float>({50});
|
||||
sequence.wz = 5.0 * xt::ones<float>({50});
|
||||
optimizer_tester.applyControlSequenceConstraintsWrapper();
|
||||
EXPECT_EQ(sequence.vx, xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.vy, 0.75 * xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.wz, 2.0 * xt::ones<float>({50}));
|
||||
|
||||
// Test breaking limits sets to minimum
|
||||
sequence.vx = -5.0 * xt::ones<float>({50});
|
||||
sequence.vy = -5.0 * xt::ones<float>({50});
|
||||
sequence.wz = -5.0 * xt::ones<float>({50});
|
||||
optimizer_tester.applyControlSequenceConstraintsWrapper();
|
||||
EXPECT_EQ(sequence.vx, -1.0 * xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.vy, -0.75 * xt::ones<float>({50}));
|
||||
EXPECT_EQ(sequence.wz, -2.0 * xt::ones<float>({50}));
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, updateStateVelocitiesTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
|
||||
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
|
||||
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60));
|
||||
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test settings of the state to the initial robot speed to start rollout
|
||||
// Set model to omni to consider holonomic vy elements
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
optimizer_tester.testupdateStateVels();
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, getControlFromSequenceAsTwistTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
node->declare_parameter("mppic.vx_max", rclcpp::ParameterValue(1.0));
|
||||
node->declare_parameter("mppic.vx_min", rclcpp::ParameterValue(-1.0));
|
||||
node->declare_parameter("mppic.vy_max", rclcpp::ParameterValue(0.60));
|
||||
node->declare_parameter("mppic.wz_max", rclcpp::ParameterValue(2.0));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
|
||||
// Test conversion of control sequence into a Twist command to execute
|
||||
auto & sequence = optimizer_tester.grabControlSequence();
|
||||
sequence.vx = 0.25 * xt::ones<float>({10});
|
||||
sequence.vy = 0.5 * xt::ones<float>({10});
|
||||
sequence.wz = 0.1 * xt::ones<float>({10});
|
||||
|
||||
auto diff_t = optimizer_tester.getControlFromSequenceAsTwistWrapper();
|
||||
EXPECT_NEAR(diff_t.twist.linear.x, 0.25, 1e-6);
|
||||
EXPECT_NEAR(diff_t.twist.linear.y, 0.0, 1e-6); // Y should not be populated
|
||||
EXPECT_NEAR(diff_t.twist.angular.z, 0.1, 1e-6);
|
||||
|
||||
// Set model to omni to consider holonomic vy elements
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
auto omni_t = optimizer_tester.getControlFromSequenceAsTwistWrapper();
|
||||
EXPECT_NEAR(omni_t.twist.linear.x, 0.25, 1e-6);
|
||||
EXPECT_NEAR(omni_t.twist.linear.y, 0.5, 1e-6); // Now it should be
|
||||
EXPECT_NEAR(omni_t.twist.angular.z, 0.1, 1e-6);
|
||||
}
|
||||
|
||||
TEST(OptimizerTests, integrateStateVelocitiesTests)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
OptimizerTester optimizer_tester;
|
||||
node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0));
|
||||
node->declare_parameter("mppic.batch_size", rclcpp::ParameterValue(1000));
|
||||
node->declare_parameter("mppic.model_dt", rclcpp::ParameterValue(0.1));
|
||||
node->declare_parameter("mppic.time_steps", rclcpp::ParameterValue(50));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
optimizer_tester.initialize(node, "mppic", costmap_ros, ¶m_handler);
|
||||
optimizer_tester.resetMotionModel();
|
||||
optimizer_tester.testSetOmniModel();
|
||||
|
||||
// Test integration of velocities for trajectory rollout poses
|
||||
|
||||
// Give it a couple of easy const traj and check rollout, start from 0
|
||||
models::State state;
|
||||
state.reset(1000, 50);
|
||||
models::Trajectories traj;
|
||||
state.vx = 0.1 * xt::ones<float>({1000, 50});
|
||||
xt::view(state.vx, xt::all(), 0) = xt::zeros<float>({1000});
|
||||
state.vy = xt::zeros<float>({1000, 50});
|
||||
state.wz = xt::zeros<float>({1000, 50});
|
||||
|
||||
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
|
||||
EXPECT_EQ(traj.y, xt::zeros<float>({1000, 50}));
|
||||
EXPECT_EQ(traj.yaws, xt::zeros<float>({1000, 50}));
|
||||
for (unsigned int i = 0; i != traj.x.shape(1); i++) {
|
||||
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
|
||||
}
|
||||
|
||||
// Give it a bit of a more complex trajectory to crunch
|
||||
state.vy = 0.2 * xt::ones<float>({1000, 50});
|
||||
xt::view(state.vy, xt::all(), 0) = xt::zeros<float>({1000});
|
||||
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
|
||||
|
||||
EXPECT_EQ(traj.yaws, xt::zeros<float>({1000, 50}));
|
||||
for (unsigned int i = 0; i != traj.x.shape(1); i++) {
|
||||
EXPECT_NEAR(traj.x(1, i), i * 0.1 /*vel*/ * 0.1 /*dt*/, 1e-3);
|
||||
EXPECT_NEAR(traj.y(1, i), i * 0.2 /*vel*/ * 0.1 /*dt*/, 1e-3);
|
||||
}
|
||||
|
||||
// Lets add some angular motion to the mix
|
||||
state.vy = xt::zeros<float>({1000, 50});
|
||||
state.wz = 0.2 * xt::ones<float>({1000, 50});
|
||||
xt::view(state.wz, xt::all(), 0) = xt::zeros<float>({1000});
|
||||
optimizer_tester.integrateStateVelocitiesWrapper(traj, state);
|
||||
|
||||
float x = 0;
|
||||
float y = 0;
|
||||
for (unsigned int i = 1; i != traj.x.shape(1); i++) {
|
||||
std::cout << i << std::endl;
|
||||
x += (0.1 /*vx*/ * cos(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
|
||||
y += (0.1 /*vx*/ * sin(0.2 /*wz*/ * 0.1 /*model_dt*/ * (i - 1))) * 0.1 /*model_dt*/;
|
||||
|
||||
EXPECT_NEAR(traj.x(1, i), x, 1e-6);
|
||||
EXPECT_NEAR(traj.y(1, i), y, 1e-6);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,181 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
|
||||
// Tests parameter handler object
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
class ParametersHandlerWrapper : public ParametersHandler
|
||||
{
|
||||
public:
|
||||
ParametersHandlerWrapper() = default;
|
||||
|
||||
explicit ParametersHandlerWrapper(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & parent)
|
||||
: ParametersHandler(parent) {}
|
||||
|
||||
template<typename T>
|
||||
auto asWrapped(rclcpp::Parameter parameter)
|
||||
{
|
||||
return ParametersHandler::as<T>(parameter);
|
||||
}
|
||||
};
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
TEST(ParameterHandlerTest, asTypeConversionTest)
|
||||
{
|
||||
ParametersHandlerWrapper a;
|
||||
rclcpp::Parameter int_p("int_parameter", rclcpp::ParameterValue(1));
|
||||
rclcpp::Parameter double_p("double_parameter", rclcpp::ParameterValue(10.0));
|
||||
rclcpp::Parameter bool_p("bool_parameter", rclcpp::ParameterValue(false));
|
||||
rclcpp::Parameter string_p("string_parameter", rclcpp::ParameterValue(std::string("hello")));
|
||||
|
||||
rclcpp::Parameter intv_p("intv_parameter", rclcpp::ParameterValue(std::vector<int>{1}));
|
||||
rclcpp::Parameter doublev_p(
|
||||
"doublev_parameter", rclcpp::ParameterValue(std::vector<double>{10.0}));
|
||||
rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector<bool>{false}));
|
||||
rclcpp::Parameter stringv_p(
|
||||
"stringv_parameter", rclcpp::ParameterValue(std::vector<std::string>{std::string("hello")}));
|
||||
|
||||
EXPECT_EQ(a.asWrapped<int>(int_p), 1);
|
||||
EXPECT_EQ(a.asWrapped<double>(double_p), 10.0);
|
||||
EXPECT_EQ(a.asWrapped<bool>(bool_p), false);
|
||||
EXPECT_EQ(a.asWrapped<std::string>(string_p), std::string("hello"));
|
||||
|
||||
EXPECT_EQ(a.asWrapped<std::vector<int64_t>>(intv_p)[0], 1);
|
||||
EXPECT_EQ(a.asWrapped<std::vector<double>>(doublev_p)[0], 10.0);
|
||||
EXPECT_EQ(a.asWrapped<std::vector<bool>>(boolv_p)[0], false);
|
||||
EXPECT_EQ(a.asWrapped<std::vector<std::string>>(stringv_p)[0], std::string("hello"));
|
||||
}
|
||||
|
||||
TEST(ParameterHandlerTest, PrePostDynamicCallbackTest)
|
||||
{
|
||||
bool pre_triggered = false, post_triggered = false, dynamic_triggered = false;
|
||||
auto preCb = [&]() {
|
||||
if (post_triggered) {
|
||||
throw std::runtime_error("Post-callback triggered before pre-callback!");
|
||||
}
|
||||
pre_triggered = true;
|
||||
};
|
||||
|
||||
auto postCb = [&]() {
|
||||
if (!pre_triggered) {
|
||||
throw std::runtime_error("Pre-callback was not triggered before post-callback!");
|
||||
}
|
||||
post_triggered = true;
|
||||
};
|
||||
|
||||
auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) {
|
||||
dynamic_triggered = true;
|
||||
};
|
||||
|
||||
rclcpp::Parameter random_param("blah_blah", rclcpp::ParameterValue(true));
|
||||
rclcpp::Parameter random_param2("use_sim_time", rclcpp::ParameterValue(true));
|
||||
bool val = false;
|
||||
|
||||
ParametersHandlerWrapper a;
|
||||
a.addPreCallback(preCb);
|
||||
a.addPostCallback(postCb);
|
||||
a.addDynamicParamCallback("use_sim_time", dynamicCb);
|
||||
a.setDynamicParamCallback(val, "blah_blah");
|
||||
|
||||
// Dynamic callback should not trigger, wrong parameter, but val should be updated
|
||||
a.dynamicParamsCallback(std::vector<rclcpp::Parameter>{random_param});
|
||||
EXPECT_FALSE(dynamic_triggered);
|
||||
EXPECT_TRUE(pre_triggered);
|
||||
EXPECT_TRUE(post_triggered);
|
||||
EXPECT_TRUE(val);
|
||||
|
||||
// Now dynamic parameter bool should be updated, right param called!
|
||||
pre_triggered = false, post_triggered = false;
|
||||
a.dynamicParamsCallback(std::vector<rclcpp::Parameter>{random_param2});
|
||||
EXPECT_TRUE(dynamic_triggered);
|
||||
EXPECT_TRUE(pre_triggered);
|
||||
EXPECT_TRUE(post_triggered);
|
||||
}
|
||||
|
||||
TEST(ParameterHandlerTest, GetSystemParamsTest)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
node->declare_parameter("param1", rclcpp::ParameterValue(true));
|
||||
node->declare_parameter("ns.param2", rclcpp::ParameterValue(7));
|
||||
|
||||
// Get parameters in global namespace and in subnamespaces
|
||||
ParametersHandler handler(node);
|
||||
auto getParamer = handler.getParamGetter("");
|
||||
bool p1 = false;
|
||||
int p2 = 0;
|
||||
getParamer(p1, "param1", false);
|
||||
getParamer(p2, "ns.param2", 0);
|
||||
EXPECT_EQ(p1, true);
|
||||
EXPECT_EQ(p2, 7);
|
||||
|
||||
// Get parameters in subnamespaces using name semantics of getter
|
||||
auto getParamer2 = handler.getParamGetter("ns");
|
||||
p2 = 0;
|
||||
getParamer2(p2, "param2", 0);
|
||||
EXPECT_EQ(p2, 7);
|
||||
}
|
||||
|
||||
TEST(ParameterHandlerTest, DynamicAndStaticParametersTest)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7));
|
||||
node->declare_parameter("static_int", rclcpp::ParameterValue(7));
|
||||
ParametersHandlerWrapper handler(node);
|
||||
handler.start();
|
||||
|
||||
// Get parameters and check they have initial values
|
||||
auto getParamer = handler.getParamGetter("");
|
||||
int p1 = 0, p2 = 0;
|
||||
getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic);
|
||||
getParamer(p2, "static_int", 0, ParameterType::Static);
|
||||
EXPECT_EQ(p1, 7);
|
||||
EXPECT_EQ(p2, 7);
|
||||
|
||||
// Now change them both via dynamic parameters
|
||||
auto rec_param = std::make_shared<rclcpp::AsyncParametersClient>(
|
||||
node->get_node_base_interface(), node->get_node_topics_interface(),
|
||||
node->get_node_graph_interface(),
|
||||
node->get_node_services_interface());
|
||||
|
||||
auto results = rec_param->set_parameters_atomically(
|
||||
{rclcpp::Parameter("dynamic_int", 10),
|
||||
rclcpp::Parameter("static_int", 10)});
|
||||
|
||||
rclcpp::spin_until_future_complete(
|
||||
node->get_node_base_interface(),
|
||||
results);
|
||||
|
||||
// Now, only param1 should change, param 2 should be the same
|
||||
EXPECT_EQ(p1, 10);
|
||||
EXPECT_EQ(p2, 7);
|
||||
}
|
||||
@@ -0,0 +1,249 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/tools/path_handler.hpp"
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
// Tests path handling
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
class PathHandlerWrapper : public PathHandler
|
||||
{
|
||||
public:
|
||||
PathHandlerWrapper()
|
||||
: PathHandler() {}
|
||||
|
||||
void pruneGlobalPlanWrapper(nav_msgs::msg::Path & path, const PathIterator end)
|
||||
{
|
||||
return prunePlan(path, end);
|
||||
}
|
||||
|
||||
double getMaxCostmapDistWrapper()
|
||||
{
|
||||
return getMaxCostmapDist();
|
||||
}
|
||||
|
||||
std::pair<nav_msgs::msg::Path, PathIterator>
|
||||
getGlobalPlanConsideringBoundsInCostmapFrameWrapper(const geometry_msgs::msg::PoseStamped & pose)
|
||||
{
|
||||
return getGlobalPlanConsideringBoundsInCostmapFrame(pose);
|
||||
}
|
||||
|
||||
bool transformPoseWrapper(
|
||||
const std::string & frame, const geometry_msgs::msg::PoseStamped & in_pose,
|
||||
geometry_msgs::msg::PoseStamped & out_pose) const
|
||||
{
|
||||
return transformPose(frame, in_pose, out_pose);
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped transformToGlobalPlanFrameWrapper(
|
||||
const geometry_msgs::msg::PoseStamped & pose)
|
||||
{
|
||||
return transformToGlobalPlanFrame(pose);
|
||||
}
|
||||
|
||||
void setGlobalPlanUpToInversion(const nav_msgs::msg::Path & path)
|
||||
{
|
||||
global_plan_up_to_inversion_ = path;
|
||||
}
|
||||
|
||||
bool isWithinInversionTolerancesWrapper(const geometry_msgs::msg::PoseStamped & robot_pose)
|
||||
{
|
||||
return isWithinInversionTolerances(robot_pose);
|
||||
}
|
||||
|
||||
nav_msgs::msg::Path & getInvertedPath()
|
||||
{
|
||||
return global_plan_up_to_inversion_;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(PathHandlerTests, GetAndPrunePath)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
PathHandlerWrapper handler;
|
||||
|
||||
path.header.frame_id = "fkframe";
|
||||
path.poses.resize(11);
|
||||
|
||||
handler.setPath(path);
|
||||
auto & rtn_path = handler.getPath();
|
||||
EXPECT_EQ(path.header.frame_id, rtn_path.header.frame_id);
|
||||
EXPECT_EQ(path.poses.size(), rtn_path.poses.size());
|
||||
|
||||
PathIterator it = rtn_path.poses.begin() + 5;
|
||||
handler.pruneGlobalPlanWrapper(rtn_path, it);
|
||||
auto rtn2_path = handler.getPath();
|
||||
EXPECT_EQ(rtn2_path.poses.size(), 6u);
|
||||
}
|
||||
|
||||
TEST(PathHandlerTests, TestBounds)
|
||||
{
|
||||
PathHandlerWrapper handler;
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
auto results = costmap_ros->set_parameters_atomically(
|
||||
{rclcpp::Parameter("global_frame", "odom"),
|
||||
rclcpp::Parameter("robot_base_frame", "base_link")});
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State state;
|
||||
costmap_ros->on_configure(state);
|
||||
|
||||
// Test initialization and getting costmap basic metadata
|
||||
handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler);
|
||||
EXPECT_EQ(handler.getMaxCostmapDistWrapper(), 2.5);
|
||||
|
||||
// Set tf between map odom and base_link
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
|
||||
std::make_unique<tf2_ros::TransformBroadcaster>(node);
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.frame_id = "map";
|
||||
t.child_frame_id = "base_link";
|
||||
tf_broadcaster_->sendTransform(t);
|
||||
t.header.frame_id = "map";
|
||||
t.child_frame_id = "odom";
|
||||
tf_broadcaster_->sendTransform(t);
|
||||
|
||||
// Test getting the global plans within a bounds window
|
||||
nav_msgs::msg::Path path;
|
||||
path.header.frame_id = "map";
|
||||
path.poses.resize(100);
|
||||
for (unsigned int i = 0; i != path.poses.size(); i++) {
|
||||
path.poses[i].pose.position.x = i;
|
||||
path.poses[i].header.frame_id = "map";
|
||||
}
|
||||
geometry_msgs::msg::PoseStamped robot_pose;
|
||||
robot_pose.header.frame_id = "odom";
|
||||
robot_pose.pose.position.x = 25.0;
|
||||
|
||||
handler.setPath(path);
|
||||
auto [transformed_plan, closest] =
|
||||
handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
|
||||
auto & path_inverted = handler.getInvertedPath();
|
||||
EXPECT_EQ(closest - path_inverted.poses.begin(), 25);
|
||||
handler.pruneGlobalPlanWrapper(path_inverted, closest);
|
||||
auto & path_pruned = handler.getInvertedPath();
|
||||
EXPECT_EQ(path_pruned.poses.size(), 75u);
|
||||
}
|
||||
|
||||
TEST(PathHandlerTests, TestTransforms)
|
||||
{
|
||||
PathHandlerWrapper handler;
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9));
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
ParametersHandler param_handler(node);
|
||||
rclcpp_lifecycle::State state;
|
||||
costmap_ros->on_configure(state);
|
||||
|
||||
// Test basic transformations and path handling
|
||||
handler.initialize(node, "dummy", costmap_ros, costmap_ros->getTfBuffer(), ¶m_handler);
|
||||
|
||||
// Set tf between map odom and base_link
|
||||
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_ =
|
||||
std::make_unique<tf2_ros::TransformBroadcaster>(node);
|
||||
geometry_msgs::msg::TransformStamped t;
|
||||
t.header.frame_id = "map";
|
||||
t.child_frame_id = "base_link";
|
||||
tf_broadcaster_->sendTransform(t);
|
||||
t.header.frame_id = "map";
|
||||
t.child_frame_id = "odom";
|
||||
tf_broadcaster_->sendTransform(t);
|
||||
|
||||
nav_msgs::msg::Path path;
|
||||
path.header.frame_id = "map";
|
||||
path.poses.resize(100);
|
||||
for (unsigned int i = 0; i != path.poses.size(); i++) {
|
||||
path.poses[i].pose.position.x = i;
|
||||
path.poses[i].header.frame_id = "map";
|
||||
}
|
||||
|
||||
geometry_msgs::msg::PoseStamped robot_pose, output_pose;
|
||||
robot_pose.header.frame_id = "odom";
|
||||
robot_pose.pose.position.x = 2.5;
|
||||
|
||||
EXPECT_TRUE(handler.transformPoseWrapper("map", robot_pose, output_pose));
|
||||
EXPECT_EQ(output_pose.pose.position.x, 2.5);
|
||||
|
||||
EXPECT_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose), std::runtime_error);
|
||||
handler.setPath(path);
|
||||
EXPECT_NO_THROW(handler.transformToGlobalPlanFrameWrapper(robot_pose));
|
||||
|
||||
auto [path_out, closest] =
|
||||
handler.getGlobalPlanConsideringBoundsInCostmapFrameWrapper(robot_pose);
|
||||
|
||||
// Put it all together
|
||||
auto final_path = handler.transformPath(robot_pose);
|
||||
EXPECT_EQ(final_path.poses.size(), path_out.poses.size());
|
||||
}
|
||||
|
||||
TEST(PathHandlerTests, TestInversionToleranceChecks)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = static_cast<double>(i);
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
path.poses.back().pose.orientation.w = 1;
|
||||
|
||||
PathHandlerWrapper handler;
|
||||
handler.setGlobalPlanUpToInversion(path);
|
||||
|
||||
// Not near (0,0)
|
||||
geometry_msgs::msg::PoseStamped robot_pose;
|
||||
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
|
||||
// Exactly on top of it
|
||||
robot_pose.pose.position.x = 9;
|
||||
robot_pose.pose.orientation.w = 1.0;
|
||||
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
|
||||
// Laterally of it
|
||||
robot_pose.pose.position.y = 9;
|
||||
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
|
||||
// On top but off angled
|
||||
robot_pose.pose.position.y = 0;
|
||||
robot_pose.pose.orientation.z = 0.8509035;
|
||||
robot_pose.pose.orientation.w = 0.525322;
|
||||
EXPECT_FALSE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
|
||||
// On top but off angled within tolerances
|
||||
robot_pose.pose.position.y = 0;
|
||||
robot_pose.pose.orientation.w = 0.9961947;
|
||||
robot_pose.pose.orientation.z = 0.0871558;
|
||||
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
|
||||
// Offset spatially + off angled but both within tolerances
|
||||
robot_pose.pose.position.x = 9.10;
|
||||
EXPECT_TRUE(handler.isWithinInversionTolerancesWrapper(robot_pose));
|
||||
}
|
||||
@@ -0,0 +1,155 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/tools/trajectory_visualizer.hpp"
|
||||
|
||||
// Tests trajectory visualization
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
TEST(TrajectoryVisualizerTests, StateTransition)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto parameters_handler = std::make_unique<ParametersHandler>(node);
|
||||
|
||||
TrajectoryVisualizer vis;
|
||||
vis.on_configure(node, "my_name", "map", parameters_handler.get());
|
||||
vis.on_activate();
|
||||
vis.on_deactivate();
|
||||
vis.on_cleanup();
|
||||
}
|
||||
|
||||
TEST(TrajectoryVisualizerTests, VisPathRepub)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto parameters_handler = std::make_unique<ParametersHandler>(node);
|
||||
nav_msgs::msg::Path recieved_path;
|
||||
nav_msgs::msg::Path pub_path;
|
||||
pub_path.header.frame_id = "fake_frame";
|
||||
pub_path.poses.resize(5);
|
||||
|
||||
auto my_sub = node->create_subscription<nav_msgs::msg::Path>(
|
||||
"transformed_global_plan", 10,
|
||||
[&](const nav_msgs::msg::Path msg) {recieved_path = msg;});
|
||||
|
||||
TrajectoryVisualizer vis;
|
||||
vis.on_configure(node, "my_name", "map", parameters_handler.get());
|
||||
vis.on_activate();
|
||||
vis.visualize(pub_path);
|
||||
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
EXPECT_EQ(recieved_path.poses.size(), 5u);
|
||||
EXPECT_EQ(recieved_path.header.frame_id, "fake_frame");
|
||||
}
|
||||
|
||||
TEST(TrajectoryVisualizerTests, VisOptimalTrajectory)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto parameters_handler = std::make_unique<ParametersHandler>(node);
|
||||
|
||||
visualization_msgs::msg::MarkerArray recieved_msg;
|
||||
auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
|
||||
"/trajectories", 10,
|
||||
[&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;});
|
||||
|
||||
// optimal_trajectory empty, should fail to publish
|
||||
xt::xtensor<float, 2> optimal_trajectory;
|
||||
TrajectoryVisualizer vis;
|
||||
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
|
||||
vis.on_activate();
|
||||
vis.add(optimal_trajectory, "Optimal Trajectory");
|
||||
nav_msgs::msg::Path bogus_path;
|
||||
vis.visualize(bogus_path);
|
||||
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
EXPECT_EQ(recieved_msg.markers.size(), 0u);
|
||||
|
||||
// Now populated with content, should publish
|
||||
optimal_trajectory = xt::ones<float>({20, 2});
|
||||
vis.add(optimal_trajectory, "Optimal Trajectory");
|
||||
vis.visualize(bogus_path);
|
||||
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
|
||||
// Should have 20 trajectory points in the map frame
|
||||
EXPECT_EQ(recieved_msg.markers.size(), 20u);
|
||||
EXPECT_EQ(recieved_msg.markers[0].header.frame_id, "fkmap");
|
||||
|
||||
// Check IDs are properly populated
|
||||
EXPECT_EQ(recieved_msg.markers[0].id, 0);
|
||||
EXPECT_EQ(recieved_msg.markers[1].id, 1);
|
||||
EXPECT_EQ(recieved_msg.markers[10].id, 10);
|
||||
|
||||
// Check poses are correct
|
||||
EXPECT_EQ(recieved_msg.markers[0].pose.position.x, 1);
|
||||
EXPECT_EQ(recieved_msg.markers[0].pose.position.y, 1);
|
||||
EXPECT_EQ(recieved_msg.markers[0].pose.position.z, 0.06);
|
||||
|
||||
// Check that scales are rational
|
||||
EXPECT_EQ(recieved_msg.markers[0].scale.x, 0.03);
|
||||
EXPECT_EQ(recieved_msg.markers[0].scale.y, 0.03);
|
||||
EXPECT_EQ(recieved_msg.markers[0].scale.z, 0.07);
|
||||
|
||||
EXPECT_EQ(recieved_msg.markers[19].scale.x, 0.07);
|
||||
EXPECT_EQ(recieved_msg.markers[19].scale.y, 0.07);
|
||||
EXPECT_EQ(recieved_msg.markers[19].scale.z, 0.09);
|
||||
|
||||
// Check that the colors are rational
|
||||
for (unsigned int i = 0; i != recieved_msg.markers.size() - 1; i++) {
|
||||
EXPECT_LT(recieved_msg.markers[i].color.g, recieved_msg.markers[i + 1].color.g);
|
||||
EXPECT_LT(recieved_msg.markers[i].color.b, recieved_msg.markers[i + 1].color.b);
|
||||
EXPECT_EQ(recieved_msg.markers[i].color.r, recieved_msg.markers[i + 1].color.r);
|
||||
EXPECT_EQ(recieved_msg.markers[i].color.a, recieved_msg.markers[i + 1].color.a);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TrajectoryVisualizerTests, VisCandidateTrajectories)
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>("my_node");
|
||||
auto parameters_handler = std::make_unique<ParametersHandler>(node);
|
||||
|
||||
visualization_msgs::msg::MarkerArray recieved_msg;
|
||||
auto my_sub = node->create_subscription<visualization_msgs::msg::MarkerArray>(
|
||||
"/trajectories", 10,
|
||||
[&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;});
|
||||
|
||||
models::Trajectories candidate_trajectories;
|
||||
candidate_trajectories.x = xt::ones<float>({200, 12});
|
||||
candidate_trajectories.y = xt::ones<float>({200, 12});
|
||||
candidate_trajectories.yaws = xt::ones<float>({200, 12});
|
||||
|
||||
TrajectoryVisualizer vis;
|
||||
vis.on_configure(node, "my_name", "fkmap", parameters_handler.get());
|
||||
vis.on_activate();
|
||||
vis.add(candidate_trajectories, "Candidate Trajectories");
|
||||
nav_msgs::msg::Path bogus_path;
|
||||
vis.visualize(bogus_path);
|
||||
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
// 40 * 4, for 5 trajectory steps + 3 point steps
|
||||
EXPECT_EQ(recieved_msg.markers.size(), 160u);
|
||||
}
|
||||
@@ -0,0 +1,247 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
#include <geometry_msgs/msg/twist.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_lifecycle/lifecycle_node.hpp>
|
||||
|
||||
#include "nav2_mppi_controller/motion_models.hpp"
|
||||
#include "nav2_mppi_controller/optimizer.hpp"
|
||||
#include "nav2_mppi_controller/tools/parameters_handler.hpp"
|
||||
#include "nav2_mppi_controller/controller.hpp"
|
||||
|
||||
#include "models.hpp"
|
||||
|
||||
namespace detail
|
||||
{
|
||||
|
||||
template<typename TMessage, typename TNode>
|
||||
void setHeader(TMessage && msg, TNode node, std::string frame)
|
||||
{
|
||||
auto time = node->get_clock()->now();
|
||||
msg.header.frame_id = frame;
|
||||
msg.header.stamp = time;
|
||||
}
|
||||
|
||||
} // namespace detail
|
||||
|
||||
|
||||
/**
|
||||
* Adds some parameters for the optimizer to a special container.
|
||||
*
|
||||
* @param params_ container for optimizer's parameters.
|
||||
*/
|
||||
void setUpOptimizerParams(
|
||||
const TestOptimizerSettings & s,
|
||||
const std::vector<std::string> & critics,
|
||||
std::vector<rclcpp::Parameter> & params_, std::string node_name = std::string("dummy"))
|
||||
{
|
||||
constexpr double dummy_freq = 50.0;
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".iteration_count", s.iteration_count));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".batch_size", s.batch_size));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".time_steps", s.time_steps));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".lookahead_dist", s.lookahead_distance));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".motion_model", s.motion_model));
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".critics", critics));
|
||||
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
|
||||
}
|
||||
|
||||
void setUpControllerParams(
|
||||
bool visualize, std::vector<rclcpp::Parameter> & params_,
|
||||
std::string node_name = std::string("dummy"))
|
||||
{
|
||||
double dummy_freq = 50.0;
|
||||
params_.emplace_back(rclcpp::Parameter(node_name + ".visualize", visualize));
|
||||
params_.emplace_back(rclcpp::Parameter("controller_frequency", dummy_freq));
|
||||
}
|
||||
|
||||
rclcpp::NodeOptions getOptimizerOptions(
|
||||
TestOptimizerSettings s,
|
||||
const std::vector<std::string> & critics)
|
||||
{
|
||||
std::vector<rclcpp::Parameter> params;
|
||||
rclcpp::NodeOptions options;
|
||||
setUpOptimizerParams(s, critics, params);
|
||||
options.parameter_overrides(params);
|
||||
return options;
|
||||
}
|
||||
|
||||
geometry_msgs::msg::Point getDummyPoint(double x, double y)
|
||||
{
|
||||
geometry_msgs::msg::Point point;
|
||||
point.x = x;
|
||||
point.y = y;
|
||||
point.z = 0;
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos()
|
||||
{
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>("cost_map_node");
|
||||
costmap_ros->on_configure(rclcpp_lifecycle::State{});
|
||||
return costmap_ros;
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2D> getDummyCostmap(TestCostmapSettings s)
|
||||
{
|
||||
auto costmap = std::make_shared<nav2_costmap_2d::Costmap2D>(
|
||||
s.cells_x, s.cells_y, s.resolution, s.origin_x, s.origin_y, s.cost_map_default_value);
|
||||
|
||||
return costmap;
|
||||
}
|
||||
|
||||
std::vector<geometry_msgs::msg::Point> getDummySquareFootprint(double a)
|
||||
{
|
||||
return {getDummyPoint(a, a), getDummyPoint(-a, -a), getDummyPoint(a, -a), getDummyPoint(-a, a)};
|
||||
}
|
||||
|
||||
std::shared_ptr<nav2_costmap_2d::Costmap2DROS> getDummyCostmapRos(TestCostmapSettings s)
|
||||
{
|
||||
auto costmap_ros = getDummyCostmapRos();
|
||||
auto costmap_ptr = costmap_ros->getCostmap();
|
||||
auto costmap = getDummyCostmap(s);
|
||||
*(costmap_ptr) = *costmap;
|
||||
|
||||
costmap_ros->setRobotFootprint(getDummySquareFootprint(s.footprint_size));
|
||||
|
||||
return costmap_ros;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
|
||||
getDummyNode(
|
||||
TestOptimizerSettings s, std::vector<std::string> critics,
|
||||
std::string node_name = std::string("dummy"))
|
||||
{
|
||||
auto node =
|
||||
std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, getOptimizerOptions(s, critics));
|
||||
return node;
|
||||
}
|
||||
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode>
|
||||
getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("dummy"))
|
||||
{
|
||||
auto node = std::make_shared<rclcpp_lifecycle::LifecycleNode>(node_name, options);
|
||||
return node;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TParamHandler>
|
||||
std::shared_ptr<mppi::Optimizer> getDummyOptimizer(
|
||||
TNode node, TCostMap costmap_ros,
|
||||
TParamHandler * params_handler)
|
||||
{
|
||||
std::shared_ptr<mppi::Optimizer> optimizer = std::make_shared<mppi::Optimizer>();
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
optimizer->initialize(weak_ptr_node, node->get_name(), costmap_ros, params_handler);
|
||||
|
||||
return optimizer;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TFBuffer, typename TParamHandler>
|
||||
mppi::PathHandler getDummyPathHandler(
|
||||
TNode node, TCostMap costmap_ros, TFBuffer tf_buffer,
|
||||
TParamHandler * params_handler)
|
||||
{
|
||||
mppi::PathHandler path_handler;
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
path_handler.initialize(weak_ptr_node, node->get_name(), costmap_ros, tf_buffer, params_handler);
|
||||
|
||||
return path_handler;
|
||||
}
|
||||
|
||||
template<typename TNode, typename TCostMap, typename TFBuffer>
|
||||
std::shared_ptr<nav2_mppi_controller::MPPIController> getDummyController(
|
||||
TNode node, TFBuffer tf_buffer,
|
||||
TCostMap costmap_ros)
|
||||
{
|
||||
auto controller = std::make_shared<nav2_mppi_controller::MPPIController>();
|
||||
std::weak_ptr<rclcpp_lifecycle::LifecycleNode> weak_ptr_node{node};
|
||||
|
||||
controller->configure(weak_ptr_node, node->get_name(), tf_buffer, costmap_ros);
|
||||
controller->activate();
|
||||
return controller;
|
||||
}
|
||||
|
||||
auto getDummyTwist()
|
||||
{
|
||||
geometry_msgs::msg::Twist twist;
|
||||
return twist;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
geometry_msgs::msg::PoseStamped
|
||||
getDummyPointStamped(TNode & node, std::string frame = std::string("odom"))
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped point;
|
||||
detail::setHeader(point, node, frame);
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
geometry_msgs::msg::PoseStamped getDummyPointStamped(TNode & node, TestPose pose)
|
||||
{
|
||||
geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node);
|
||||
point.pose.position.x = pose.x;
|
||||
point.pose.position.y = pose.y;
|
||||
|
||||
return point;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
nav_msgs::msg::Path getDummyPath(TNode node, std::string frame = std::string("odom"))
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
detail::setHeader(path, node, frame);
|
||||
return path;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
auto getDummyPath(size_t points_count, TNode node)
|
||||
{
|
||||
auto path = getDummyPath(node);
|
||||
|
||||
for (size_t i = 0; i < points_count; i++) {
|
||||
path.poses.push_back(getDummyPointStamped(node));
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
|
||||
template<typename TNode>
|
||||
nav_msgs::msg::Path getIncrementalDummyPath(TNode node, TestPathSettings s)
|
||||
{
|
||||
auto path = getDummyPath(node);
|
||||
|
||||
for (size_t i = 0; i < s.poses_count; i++) {
|
||||
double x = s.start_pose.x + static_cast<double>(i) * s.step_x;
|
||||
double y = s.start_pose.y + static_cast<double>(i) * s.step_y;
|
||||
path.poses.push_back(getDummyPointStamped(node, TestPose{x, y}));
|
||||
}
|
||||
|
||||
return path;
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include <string>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
struct TestOptimizerSettings
|
||||
{
|
||||
int batch_size;
|
||||
int time_steps;
|
||||
int iteration_count;
|
||||
double lookahead_distance;
|
||||
std::string motion_model;
|
||||
bool consider_footprint;
|
||||
};
|
||||
|
||||
struct TestPose
|
||||
{
|
||||
double x;
|
||||
double y;
|
||||
};
|
||||
|
||||
struct TestCostmapSettings
|
||||
{
|
||||
const unsigned int cells_x = 40;
|
||||
const unsigned int cells_y = 40;
|
||||
const double origin_x = 0.0;
|
||||
const double origin_y = 0.0;
|
||||
const double resolution = 0.1;
|
||||
const unsigned char cost_map_default_value = 0;
|
||||
const double footprint_size = 0.15;
|
||||
|
||||
std::pair<unsigned int, unsigned int> getCenterIJ()
|
||||
{
|
||||
return {
|
||||
cells_x / 2,
|
||||
cells_y / 2};
|
||||
}
|
||||
|
||||
TestPose getCenterPose()
|
||||
{
|
||||
return {
|
||||
static_cast<double>(cells_x) * resolution / 2.0,
|
||||
static_cast<double>(cells_y) * resolution / 2.0};
|
||||
}
|
||||
};
|
||||
struct TestObstaclesSettings
|
||||
{
|
||||
unsigned int center_cells_x;
|
||||
unsigned int center_cells_y;
|
||||
unsigned int obstacle_size;
|
||||
unsigned char obstacle_cost;
|
||||
};
|
||||
|
||||
struct TestPathSettings
|
||||
{
|
||||
TestPose start_pose;
|
||||
unsigned int poses_count;
|
||||
double step_x;
|
||||
double step_y;
|
||||
};
|
||||
@@ -0,0 +1,248 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <string_view>
|
||||
#include <rclcpp/executors.hpp>
|
||||
|
||||
#include "tf2_ros/transform_broadcaster.h"
|
||||
|
||||
#include "nav2_costmap_2d/costmap_2d.hpp"
|
||||
#include "nav2_costmap_2d/costmap_2d_ros.hpp"
|
||||
|
||||
#include "models.hpp"
|
||||
#include "factory.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
template<typename TNode>
|
||||
void waitSome(const std::chrono::nanoseconds & duration, TNode & node)
|
||||
{
|
||||
rclcpp::Time start_time = node->now();
|
||||
while (rclcpp::ok() && node->now() - start_time <= rclcpp::Duration(duration)) {
|
||||
rclcpp::spin_some(node->get_node_base_interface());
|
||||
std::this_thread::sleep_for(3ms);
|
||||
}
|
||||
}
|
||||
|
||||
void sendTf(
|
||||
std::string_view source, std::string_view dest,
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster,
|
||||
std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, size_t n)
|
||||
{
|
||||
while (--n != 0u) {
|
||||
auto t = geometry_msgs::msg::TransformStamped();
|
||||
t.header.frame_id = source;
|
||||
t.child_frame_id = dest;
|
||||
|
||||
t.header.stamp = node->now() + rclcpp::Duration(3ms);
|
||||
t.transform.translation.x = 0.0;
|
||||
t.transform.translation.y = 0.0;
|
||||
t.transform.translation.z = 0.0;
|
||||
t.transform.rotation.x = 0.0;
|
||||
t.transform.rotation.y = 0.0;
|
||||
t.transform.rotation.z = 0.0;
|
||||
t.transform.rotation.w = 1.0;
|
||||
|
||||
tf_broadcaster->sendTransform(t);
|
||||
|
||||
// Allow tf_buffer_ to be filled by listener
|
||||
waitSome(10ms, node);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Print costmap to stdout.
|
||||
* @param costmap map to be printed.
|
||||
*/
|
||||
void printMap(const nav2_costmap_2d::Costmap2D & costmap)
|
||||
{
|
||||
for (unsigned int i = 0; i < costmap.getSizeInCellsY(); i++) {
|
||||
for (unsigned int j = 0; j < costmap.getSizeInCellsX(); j++) {
|
||||
printf("%4d", static_cast<int>(costmap.getCost(j, i)));
|
||||
}
|
||||
printf("\n\n");
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Print costmap with trajectory and goal point to stdout.
|
||||
* @param costmap map to be printed.
|
||||
* @param trajectory trajectory container (xt::tensor) to be printed.
|
||||
* @param goal_point goal point to be printed.
|
||||
*/
|
||||
template<typename TTrajectory>
|
||||
void printMapWithTrajectoryAndGoal(
|
||||
nav2_costmap_2d::Costmap2D & costmap, const TTrajectory & trajectory,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
const unsigned int trajectory_cost = 1;
|
||||
const unsigned int goal_cost = 2;
|
||||
|
||||
std::cout << "Costmap: \n trajectory = " << trajectory_cost << "\n goal = " << goal_cost
|
||||
<< "\n obsctacle = 255 \n";
|
||||
|
||||
// create new costmap
|
||||
nav2_costmap_2d::Costmap2D costmap2d(
|
||||
costmap.getSizeInCellsX(), costmap.getSizeInCellsY(), costmap.getResolution(),
|
||||
costmap.getOriginX(), costmap.getOriginY(), costmap.getDefaultValue());
|
||||
|
||||
// copy obstacles from original costmap
|
||||
costmap2d = costmap;
|
||||
|
||||
// add trajectory on map
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
for (size_t i = 0; i < trajectory.shape()[0]; ++i) {
|
||||
costmap2d.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
|
||||
costmap2d.setCost(point_mx, point_my, trajectory_cost);
|
||||
}
|
||||
|
||||
unsigned int goal_j{0};
|
||||
unsigned int goal_i{0};
|
||||
costmap2d.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
|
||||
std::cout << "Goal Position: " << goal_j << " " << goal_i << "\n";
|
||||
costmap2d.setCost(goal_j, goal_i, goal_cost);
|
||||
printMap(costmap2d);
|
||||
}
|
||||
|
||||
/**
|
||||
* Add a square obstacle to the costmap.
|
||||
* @param costmap map to be modified.
|
||||
* @param upper_left_corner_x obstacle upper left corner X coord (on the
|
||||
* costmap).
|
||||
* @param upper_left_corner_y obstacle upper left corner Y coord (on the
|
||||
* costmap).
|
||||
* @param size obstacle side size.
|
||||
* @param cost obstacle value on costmap.
|
||||
*/
|
||||
void addObstacle(
|
||||
nav2_costmap_2d::Costmap2D * costmap, unsigned int upper_left_corner_x,
|
||||
unsigned int upper_left_corner_y, unsigned int size, unsigned char cost)
|
||||
{
|
||||
for (unsigned int i = upper_left_corner_x; i < upper_left_corner_x + size; i++) {
|
||||
for (unsigned int j = upper_left_corner_y; j < upper_left_corner_y + size; j++) {
|
||||
costmap->setCost(i, j, cost);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void printInfo(
|
||||
TestOptimizerSettings os, TestPathSettings ps,
|
||||
const std::vector<std::string> & critics)
|
||||
{
|
||||
std::stringstream ss;
|
||||
for (auto str : critics) {
|
||||
ss << str << " ";
|
||||
}
|
||||
|
||||
std::cout << //
|
||||
"\n\n--------------------OPTIMIZER OPTIONS-----------------------------\n" <<
|
||||
"Critics: " << ss.str() << "\n" \
|
||||
"Motion model: " << os.motion_model << "\n"
|
||||
"Consider footprint: " << os.consider_footprint << "\n" <<
|
||||
"Iterations: " << os.iteration_count << "\n" <<
|
||||
"Batch size: " << os.batch_size << "\n" <<
|
||||
"Time steps: " << os.time_steps << "\n" <<
|
||||
"Path points: " << ps.poses_count << "\n" <<
|
||||
"\n-------------------------------------------------------------------\n\n";
|
||||
}
|
||||
|
||||
void addObstacle(nav2_costmap_2d::Costmap2D * costmap, TestObstaclesSettings s)
|
||||
{
|
||||
addObstacle(costmap, s.center_cells_x, s.center_cells_y, s.obstacle_size, s.obstacle_cost);
|
||||
}
|
||||
|
||||
/**
|
||||
* Check the trajectory for collisions with obstacles on the map.
|
||||
* @param trajectory trajectory container (xt::tensor) to be checked.
|
||||
* @param costmap costmap with obstacles
|
||||
* @return true - if the trajectory crosses an obstacle on the map, false - if
|
||||
* not
|
||||
*/
|
||||
template<typename TTrajectory>
|
||||
bool inCollision(const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap)
|
||||
{
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
|
||||
for (size_t i = 0; i < trajectory.shape(0); ++i) {
|
||||
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), point_mx, point_my);
|
||||
auto cost_ = costmap.getCost(point_mx, point_my);
|
||||
if (cost_ > nav2_costmap_2d::FREE_SPACE || cost_ == nav2_costmap_2d::NO_INFORMATION) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
unsigned char getCost(const nav2_costmap_2d::Costmap2D & costmap, double x, double y)
|
||||
{
|
||||
unsigned int point_mx = 0;
|
||||
unsigned int point_my = 0;
|
||||
|
||||
costmap.worldToMap(x, y, point_mx, point_my);
|
||||
return costmap.getCost(point_mx, point_my);
|
||||
}
|
||||
|
||||
template<typename TTrajectory>
|
||||
bool isGoalReached(
|
||||
const TTrajectory & trajectory, const nav2_costmap_2d::Costmap2D & costmap,
|
||||
const geometry_msgs::msg::PoseStamped & goal)
|
||||
{
|
||||
unsigned int trajectory_j = 0;
|
||||
unsigned int trajectory_i = 0;
|
||||
|
||||
unsigned int goal_j = 0;
|
||||
unsigned int goal_i = 0;
|
||||
costmap.worldToMap(goal.pose.position.x, goal.pose.position.y, goal_j, goal_i);
|
||||
|
||||
auto match = [](unsigned int i, unsigned int j, unsigned int i_dst, unsigned int j_dst) {
|
||||
if (i == i_dst && j == j_dst) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
|
||||
auto match_near = [&](unsigned int i, unsigned int j) {
|
||||
if (match(i, j, goal_i, goal_j) ||
|
||||
match(i, j, goal_i + 1, goal_j) ||
|
||||
match(i, j, goal_i - 1, goal_j) ||
|
||||
match(i, j, goal_i, goal_j + 1) ||
|
||||
match(i, j, goal_i, goal_j - 1) ||
|
||||
match(i, j, goal_i + 1, goal_j + 1) ||
|
||||
match(i, j, goal_i + 1, goal_j - 1) ||
|
||||
match(i, j, goal_i - 1, goal_j + 1) ||
|
||||
match(i, j, goal_i - 1, goal_j - 1))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
};
|
||||
// clang-format on
|
||||
|
||||
for (size_t i = 0; i < trajectory.shape(0); ++i) {
|
||||
costmap.worldToMap(trajectory(i, 0), trajectory(i, 1), trajectory_j, trajectory_i);
|
||||
if (match_near(trajectory_i, trajectory_j)) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
@@ -0,0 +1,445 @@
|
||||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov
|
||||
//
|
||||
// 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.
|
||||
|
||||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include <xtensor/xrandom.hpp>
|
||||
#include "gtest/gtest.h"
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "nav2_mppi_controller/tools/utils.hpp"
|
||||
#include "nav2_mppi_controller/models/path.hpp"
|
||||
|
||||
// Tests noise generator object
|
||||
|
||||
class RosLockGuard
|
||||
{
|
||||
public:
|
||||
RosLockGuard() {rclcpp::init(0, nullptr);}
|
||||
~RosLockGuard() {rclcpp::shutdown();}
|
||||
};
|
||||
RosLockGuard g_rclcpp;
|
||||
|
||||
using namespace mppi::utils; // NOLINT
|
||||
using namespace mppi; // NOLINT
|
||||
|
||||
class TestGoalChecker : public nav2_core::GoalChecker
|
||||
{
|
||||
public:
|
||||
TestGoalChecker() {}
|
||||
|
||||
virtual void initialize(
|
||||
const rclcpp_lifecycle::LifecycleNode::WeakPtr & /*parent*/,
|
||||
const std::string & /*plugin_name*/,
|
||||
const std::shared_ptr<nav2_costmap_2d::Costmap2DROS>/*costmap_ros*/) {}
|
||||
|
||||
virtual void reset() {}
|
||||
|
||||
virtual bool isGoalReached(
|
||||
const geometry_msgs::msg::Pose & /*query_pose*/,
|
||||
const geometry_msgs::msg::Pose & /*goal_pose*/,
|
||||
const geometry_msgs::msg::Twist & /*velocity*/) {return false;}
|
||||
|
||||
virtual bool getTolerances(
|
||||
geometry_msgs::msg::Pose & pose_tolerance,
|
||||
geometry_msgs::msg::Twist & /*vel_tolerance*/)
|
||||
{
|
||||
pose_tolerance.position.x = 0.25;
|
||||
pose_tolerance.position.y = 0.25;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
TEST(UtilsTests, MarkerPopulationUtils)
|
||||
{
|
||||
auto pose = createPose(1.0, 2.0, 3.0);
|
||||
EXPECT_EQ(pose.position.x, 1.0);
|
||||
EXPECT_EQ(pose.position.y, 2.0);
|
||||
EXPECT_EQ(pose.position.z, 3.0);
|
||||
EXPECT_EQ(pose.orientation.w, 1.0);
|
||||
|
||||
auto scale = createScale(1.0, 2.0, 3.0);
|
||||
EXPECT_EQ(scale.x, 1.0);
|
||||
EXPECT_EQ(scale.y, 2.0);
|
||||
EXPECT_EQ(scale.z, 3.0);
|
||||
|
||||
auto color = createColor(1.0, 2.0, 3.0, 0.0);
|
||||
EXPECT_EQ(color.r, 1.0);
|
||||
EXPECT_EQ(color.g, 2.0);
|
||||
EXPECT_EQ(color.b, 3.0);
|
||||
EXPECT_EQ(color.a, 0.0);
|
||||
|
||||
auto marker = createMarker(999, pose, scale, color, "map", "ns");
|
||||
EXPECT_EQ(marker.header.frame_id, "map");
|
||||
EXPECT_EQ(marker.id, 999);
|
||||
EXPECT_EQ(marker.pose, pose);
|
||||
EXPECT_EQ(marker.scale, scale);
|
||||
EXPECT_EQ(marker.color, color);
|
||||
EXPECT_EQ(marker.ns, "ns");
|
||||
}
|
||||
|
||||
TEST(UtilsTests, ConversionTests)
|
||||
{
|
||||
geometry_msgs::msg::TwistStamped output;
|
||||
builtin_interfaces::msg::Time time;
|
||||
|
||||
// Check population is correct
|
||||
output = toTwistStamped(0.5, 0.3, time, "map");
|
||||
EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
|
||||
EXPECT_NEAR(output.twist.linear.y, 0.0, 1e-6);
|
||||
EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
|
||||
EXPECT_EQ(output.header.frame_id, "map");
|
||||
EXPECT_EQ(output.header.stamp, time);
|
||||
|
||||
output = toTwistStamped(0.5, 0.4, 0.3, time, "map");
|
||||
EXPECT_NEAR(output.twist.linear.x, 0.5, 1e-6);
|
||||
EXPECT_NEAR(output.twist.linear.y, 0.4, 1e-6);
|
||||
EXPECT_NEAR(output.twist.angular.z, 0.3, 1e-6);
|
||||
EXPECT_EQ(output.header.frame_id, "map");
|
||||
EXPECT_EQ(output.header.stamp, time);
|
||||
|
||||
nav_msgs::msg::Path path;
|
||||
path.poses.resize(5);
|
||||
path.poses[2].pose.position.x = 5;
|
||||
path.poses[2].pose.position.y = 50;
|
||||
models::Path path_t = toTensor(path);
|
||||
|
||||
// Check population is correct
|
||||
EXPECT_EQ(path_t.x.shape(0), 5u);
|
||||
EXPECT_EQ(path_t.y.shape(0), 5u);
|
||||
EXPECT_EQ(path_t.yaws.shape(0), 5u);
|
||||
EXPECT_EQ(path_t.x(2), 5);
|
||||
EXPECT_EQ(path_t.y(2), 50);
|
||||
EXPECT_NEAR(path_t.yaws(2), 0.0, 1e-6);
|
||||
}
|
||||
|
||||
TEST(UtilsTests, WithTolTests)
|
||||
{
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position.x = 10.0;
|
||||
pose.position.y = 1.0;
|
||||
|
||||
nav2_core::GoalChecker * goal_checker = new TestGoalChecker;
|
||||
|
||||
nav_msgs::msg::Path path;
|
||||
path.poses.resize(2);
|
||||
geometry_msgs::msg::Pose & goal = path.poses.back().pose;
|
||||
|
||||
// Create CriticData with state and goal initialized
|
||||
models::State state;
|
||||
state.pose.pose = pose;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path_critic;
|
||||
xt::xtensor<float, 1> costs;
|
||||
float model_dt;
|
||||
CriticData data = {
|
||||
state, generated_trajectories, path_critic, goal,
|
||||
costs, model_dt, false, nullptr, nullptr, std::nullopt, std::nullopt};
|
||||
|
||||
// Test not in tolerance
|
||||
goal.position.x = 0.0;
|
||||
goal.position.y = 0.0;
|
||||
EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
|
||||
EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, goal));
|
||||
|
||||
// Test in tolerance
|
||||
goal.position.x = 9.8;
|
||||
goal.position.y = 0.95;
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
|
||||
|
||||
goal.position.x = 10.0;
|
||||
goal.position.y = 0.76;
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
|
||||
|
||||
goal.position.x = 9.76;
|
||||
goal.position.y = 1.0;
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, goal));
|
||||
EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, goal));
|
||||
|
||||
delete goal_checker;
|
||||
goal_checker = nullptr;
|
||||
EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, goal));
|
||||
}
|
||||
|
||||
TEST(UtilsTests, AnglesTests)
|
||||
{
|
||||
// Test angle normalization by creating insane angles
|
||||
xt::xtensor<float, 1> angles, zero_angles;
|
||||
angles = xt::ones<float>({100});
|
||||
for (unsigned int i = 0; i != angles.shape(0); i++) {
|
||||
angles(i) = i * i;
|
||||
if (i % 2 == 0) {
|
||||
angles(i) *= -1;
|
||||
}
|
||||
}
|
||||
|
||||
auto norm_ang = normalize_angles(angles);
|
||||
for (unsigned int i = 0; i != norm_ang.shape(0); i++) {
|
||||
EXPECT_TRUE((norm_ang(i) >= -M_PI) && (norm_ang(i) <= M_PI));
|
||||
}
|
||||
|
||||
// Test shortest angular distance
|
||||
zero_angles = xt::zeros<float>({100});
|
||||
auto ang_dist = shortest_angular_distance(angles, zero_angles);
|
||||
for (unsigned int i = 0; i != ang_dist.shape(0); i++) {
|
||||
EXPECT_TRUE((ang_dist(i) >= -M_PI) && (ang_dist(i) <= M_PI));
|
||||
}
|
||||
|
||||
// Test point-pose angle
|
||||
geometry_msgs::msg::Pose pose;
|
||||
pose.position.x = 0.0;
|
||||
pose.position.y = 0.0;
|
||||
pose.orientation.w = 1.0;
|
||||
double point_x = 1.0, point_y = 0.0;
|
||||
bool forward_preference = true;
|
||||
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
|
||||
forward_preference = false;
|
||||
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
|
||||
point_x = -1.0;
|
||||
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), 0.0, 1e-6);
|
||||
forward_preference = true;
|
||||
EXPECT_NEAR(posePointAngle(pose, point_x, point_y, forward_preference), M_PI, 1e-6);
|
||||
}
|
||||
|
||||
TEST(UtilsTests, FurthestAndClosestReachedPoint)
|
||||
{
|
||||
models::State state;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs;
|
||||
float model_dt = 0.1;
|
||||
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
|
||||
// Attempt to set furthest point if notionally set, should not change
|
||||
data.furthest_reached_path_point = 99999;
|
||||
setPathFurthestPointIfNotSet(data);
|
||||
EXPECT_EQ(data.furthest_reached_path_point, 99999);
|
||||
|
||||
// Attempt to set if not set already with no other information, should fail
|
||||
CriticData data2 =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
setPathFurthestPointIfNotSet(data2);
|
||||
EXPECT_EQ(data2.furthest_reached_path_point, 0);
|
||||
|
||||
// Test the actual computation of the path point reached
|
||||
generated_trajectories.x = xt::ones<float>({100, 2});
|
||||
generated_trajectories.y = xt::zeros<float>({100, 2});
|
||||
generated_trajectories.yaws = xt::zeros<float>({100, 2});
|
||||
|
||||
nav_msgs::msg::Path plan;
|
||||
plan.poses.resize(10);
|
||||
for (unsigned int i = 0; i != plan.poses.size(); i++) {
|
||||
plan.poses[i].pose.position.x = 0.2 * i;
|
||||
plan.poses[i].pose.position.y = 0.0;
|
||||
}
|
||||
path = toTensor(plan);
|
||||
|
||||
CriticData data3 =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
EXPECT_EQ(findPathFurthestReachedPoint(data3), 5u);
|
||||
EXPECT_EQ(findPathTrajectoryInitialPoint(data3), 5u);
|
||||
}
|
||||
|
||||
TEST(UtilsTests, findPathCosts)
|
||||
{
|
||||
models::State state;
|
||||
models::Trajectories generated_trajectories;
|
||||
models::Path path;
|
||||
geometry_msgs::msg::Pose goal;
|
||||
xt::xtensor<float, 1> costs;
|
||||
float model_dt = 0.1;
|
||||
|
||||
CriticData data =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
|
||||
// Test not set if already set, should not change
|
||||
data.path_pts_valid = std::vector<bool>(10, false);
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
(*data.path_pts_valid)[i] = false;
|
||||
}
|
||||
EXPECT_TRUE(data.path_pts_valid);
|
||||
setPathCostsIfNotSet(data, nullptr);
|
||||
EXPECT_EQ(data.path_pts_valid->size(), 10u);
|
||||
|
||||
CriticData data3 =
|
||||
{state, generated_trajectories, path, goal, costs, model_dt, false, nullptr, nullptr,
|
||||
std::nullopt, std::nullopt}; /// Caution, keep references
|
||||
|
||||
auto costmap_ros = std::make_shared<nav2_costmap_2d::Costmap2DROS>(
|
||||
"dummy_costmap", "", "dummy_costmap");
|
||||
rclcpp_lifecycle::State lstate;
|
||||
costmap_ros->on_configure(lstate);
|
||||
auto * costmap = costmap_ros->getCostmap();
|
||||
// island in the middle of lethal cost to cross. Costmap defaults to size 5x5 @ 10cm resolution
|
||||
for (unsigned int i = 10; i <= 30; ++i) { // 1m-3m
|
||||
for (unsigned int j = 10; j <= 30; ++j) { // 1m-3m
|
||||
costmap->setCost(i, j, 254);
|
||||
}
|
||||
}
|
||||
for (unsigned int i = 40; i <= 45; ++i) { // 4m-4.5m
|
||||
for (unsigned int j = 45; j <= 45; ++j) { // 4m-4.5m
|
||||
costmap->setCost(i, j, 253);
|
||||
}
|
||||
}
|
||||
|
||||
path.reset(50);
|
||||
path.x(1) = 999999999; // OFF COSTMAP
|
||||
path.y(1) = 999999999;
|
||||
path.x(10) = 1.5; // IN LETHAL
|
||||
path.y(10) = 1.5;
|
||||
path.x(20) = 4.2; // IN INFLATED
|
||||
path.y(20) = 4.2;
|
||||
|
||||
// This should be evaluated and have real outputs now
|
||||
setPathCostsIfNotSet(data3, costmap_ros);
|
||||
EXPECT_TRUE(data3.path_pts_valid.has_value());
|
||||
for (unsigned int i = 0; i != path.x.shape(0) - 1; i++) {
|
||||
if (i == 1 || i == 10) {
|
||||
EXPECT_FALSE((*data3.path_pts_valid)[i]);
|
||||
} else {
|
||||
EXPECT_TRUE((*data3.path_pts_valid)[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(UtilsTests, SmootherTest)
|
||||
{
|
||||
models::ControlSequence noisey_sequence, sequence_init;
|
||||
noisey_sequence.vx = 0.2 * xt::ones<float>({30});
|
||||
noisey_sequence.vy = 0.0 * xt::ones<float>({30});
|
||||
noisey_sequence.wz = 0.3 * xt::ones<float>({30});
|
||||
|
||||
// Make the sequence noisy
|
||||
auto noises = xt::random::randn<float>({30}, 0.0, 0.2);
|
||||
noisey_sequence.vx += noises;
|
||||
noisey_sequence.vy += noises;
|
||||
noisey_sequence.wz += noises;
|
||||
sequence_init = noisey_sequence;
|
||||
|
||||
std::array<mppi::models::Control, 4> history, history_init;
|
||||
history[3].vx = 0.1;
|
||||
history[3].vy = 0.0;
|
||||
history[3].wz = 0.3;
|
||||
history[2].vx = 0.1;
|
||||
history[2].vy = 0.0;
|
||||
history[2].wz = 0.3;
|
||||
history[1].vx = 0.1;
|
||||
history[1].vy = 0.0;
|
||||
history[1].wz = 0.3;
|
||||
history[0].vx = 0.0;
|
||||
history[0].vy = 0.0;
|
||||
history[0].wz = 0.0;
|
||||
history_init = history;
|
||||
|
||||
models::OptimizerSettings settings;
|
||||
settings.shift_control_sequence = false; // so result stores 0th value in history
|
||||
|
||||
savitskyGolayFilter(noisey_sequence, history, settings);
|
||||
|
||||
// Check history is propogated backward
|
||||
EXPECT_NEAR(history_init[3].vx, history[2].vx, 0.02);
|
||||
EXPECT_NEAR(history_init[3].vy, history[2].vy, 0.02);
|
||||
EXPECT_NEAR(history_init[3].wz, history[2].wz, 0.02);
|
||||
|
||||
// Check history element is updated for first command
|
||||
EXPECT_NEAR(history[3].vx, 0.2, 0.05);
|
||||
EXPECT_NEAR(history[3].vy, 0.0, 0.035);
|
||||
EXPECT_NEAR(history[3].wz, 0.23, 0.02);
|
||||
|
||||
// Check that path is smoother
|
||||
float smoothed_val{0}, original_val{0};
|
||||
for (unsigned int i = 0; i != noisey_sequence.vx.shape(0); i++) {
|
||||
smoothed_val += fabs(noisey_sequence.vx(i) - 0.2);
|
||||
smoothed_val += fabs(noisey_sequence.vy(i) - 0.0);
|
||||
smoothed_val += fabs(noisey_sequence.wz(i) - 0.3);
|
||||
|
||||
original_val += fabs(sequence_init.vx(i) - 0.2);
|
||||
original_val += fabs(sequence_init.vy(i) - 0.0);
|
||||
original_val += fabs(sequence_init.wz(i) - 0.3);
|
||||
}
|
||||
|
||||
EXPECT_LT(smoothed_val, original_val);
|
||||
}
|
||||
|
||||
TEST(UtilsTests, FindPathInversionTest)
|
||||
{
|
||||
// Straight path, no inversions to be found
|
||||
nav_msgs::msg::Path path;
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
EXPECT_EQ(utils::findFirstPathInversion(path), 10u);
|
||||
|
||||
// To short to process
|
||||
path.poses.erase(path.poses.begin(), path.poses.begin() + 7);
|
||||
EXPECT_EQ(utils::findFirstPathInversion(path), 3u);
|
||||
|
||||
// Has inversion at index 10, so should return 11 for the first point afterwards
|
||||
// 0 1 2 3 4 5 6 7 8 9 10 **9** 8 7 6 5 4 3 2 1
|
||||
path.poses.clear();
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = 10 - i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
EXPECT_EQ(utils::findFirstPathInversion(path), 11u);
|
||||
}
|
||||
|
||||
TEST(UtilsTests, RemovePosesAfterPathInversionTest)
|
||||
{
|
||||
nav_msgs::msg::Path path;
|
||||
// straight path
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
|
||||
|
||||
// try empty path
|
||||
path.poses.clear();
|
||||
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 0u);
|
||||
|
||||
// cusping path
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
for (unsigned int i = 0; i != 10; i++) {
|
||||
geometry_msgs::msg::PoseStamped pose;
|
||||
pose.pose.position.x = 10 - i;
|
||||
path.poses.push_back(pose);
|
||||
}
|
||||
EXPECT_EQ(utils::removePosesAfterFirstInversion(path), 11u);
|
||||
// Check to see if removed
|
||||
EXPECT_EQ(path.poses.size(), 11u);
|
||||
EXPECT_EQ(path.poses.back().pose.position.x, 10);
|
||||
}
|
||||
Reference in New Issue
Block a user