diff --git a/.circleci/config.yml b/.circleci/config.yml index ede7b367e9..6ed07d76e9 100644 --- a/.circleci/config.yml +++ b/.circleci/config.yml @@ -33,12 +33,12 @@ _commands: - restore_cache: name: Restore Cache << parameters.key >> keys: - - "<< parameters.key >>-v12\ + - "<< parameters.key >>-v13\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ -{{ checksum \"<< parameters.workspace >>/lockfile.txt\" }}" - - "<< parameters.key >>-v12\ + - "<< parameters.key >>-v13\ -{{ arch }}\ -main\ -\ @@ -58,7 +58,7 @@ _commands: steps: - save_cache: name: Save Cache << parameters.key >> - key: "<< parameters.key >>-v12\ + key: "<< parameters.key >>-v13\ -{{ arch }}\ -{{ .Branch }}\ -{{ .Environment.CIRCLE_PR_NUMBER }}\ @@ -453,6 +453,7 @@ executors: release_exec: docker: - image: ghcr.io/ros-planning/navigation2:main + resource_class: large working_directory: /opt/overlay_ws environment: <<: *common_environment diff --git a/.circleci/defaults.yaml b/.circleci/defaults.yaml index e0d946f78e..e1d62a04d2 100644 --- a/.circleci/defaults.yaml +++ b/.circleci/defaults.yaml @@ -6,7 +6,7 @@ _common: &common "build": <<: *common "executor": "parallel" - "parallel-workers": 2 + "parallel-workers": 4 "symlink-install": true "test": <<: *common diff --git a/codecov.yml b/codecov.yml index b30da12897..03ba906278 100644 --- a/codecov.yml +++ b/codecov.yml @@ -7,3 +7,7 @@ ignore: - "*/test/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests - "**/test_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp - "**/*_tests.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp + - "*/**/benchmark/*" # ignore package test directories, e.g. nav2_dwb_controller/costmap_queue/tests + - "*/benchmark/**/*" # ignore package test directories, e.g. nav2_costmap_2d/tests + - "**/benchmark_*.*" # ignore files starting with test_ e.g. nav2_map_server/test/test_constants.cpp + - "**/*_benchmark.*" # ignore files ending with _tests e.g. nav2_voxel_grid/test/voxel_grid_tests.cpp diff --git a/nav2_mppi_controller/CMakeLists.txt b/nav2_mppi_controller/CMakeLists.txt new file mode 100644 index 0000000000..2b496d3d58 --- /dev/null +++ b/nav2_mppi_controller/CMakeLists.txt @@ -0,0 +1,93 @@ +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) + + +find_package(ament_cmake REQUIRED) +find_package(xtensor REQUIRED) + +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() +add_compile_options(-O3 -mavx2 -mfma -finline-limit=1000000 -ffp-contract=fast -ffast-math) + +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/goal_critic.cpp + src/critics/goal_angle_critic.cpp + src/critics/path_align_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 +) + +set(libraries mppi_controller mppi_critics) + +foreach(lib IN LISTS libraries) + target_compile_options(${lib} PUBLIC -fconcepts) + target_include_directories(${lib} PUBLIC include ${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() diff --git a/nav2_mppi_controller/LICENSE.md b/nav2_mppi_controller/LICENSE.md new file mode 100644 index 0000000000..da24c0064f --- /dev/null +++ b/nav2_mppi_controller/LICENSE.md @@ -0,0 +1,22 @@ +MIT License + +Copyright (c) 2021-2022 Fast Sense Studio +Copyright (c) 2022-2023 Samsung Research America + +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. diff --git a/nav2_mppi_controller/README.md b/nav2_mppi_controller/README.md new file mode 100644 index 0000000000..cef31357dc --- /dev/null +++ b/nav2_mppi_controller/README.md @@ -0,0 +1,266 @@ +# Model Predictive Path Integral Controller + +![](media/demo.gif) + +## 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 + +## 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 standart deviation for VX | + | vy_std | double | Default 0.2. Sampling standart deviation for VY | + | wz_std | double | Default 0.4. Sampling standart 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 considiration 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. | +#### 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. | + +#### 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.40. 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.0. Distance between robot and goal above which goal cost starts being considered | + + +#### Obstacles Critic + | 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 criticial 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. + +#### 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.4. 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 presense of dynamic objects in the scene. | + + +#### 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.4. 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 | + +#### 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 0.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.4. 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. | + +### 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 + AckermannConstrains: + min_turning_r: 0.2 + critics: ["ConstraintCritic", "ObstaclesCritic", "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.0 + GoalAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 3.0 + threshold_to_consider: 0.4 + PreferForwardCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + threshold_to_consider: 0.4 + 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 + PathAlignCritic: + enabled: true + cost_power: 1 + cost_weight: 14.0 + max_path_occupancy_ratio: 0.05 + trajectory_point_step: 3 + threshold_to_consider: 0.40 + offset_from_furthest: 20 + PathFollowCritic: + enabled: true + cost_power: 1 + cost_weight: 5.0 + offset_from_furthest: 5 + threshold_to_consider: 0.6 + PathAngleCritic: + enabled: true + cost_power: 1 + cost_weight: 2.0 + offset_from_furthest: 4 + threshold_to_consider: 0.40 + max_angle_to_furthest: 1.0 + # 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. Visualizating 2000 batches @ 56 points at 30 hz is _alot_. + +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 proprtional 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. + +### 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 artifically 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. + +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 maneuvors 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 satesfaction, 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 duing 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. diff --git a/nav2_mppi_controller/benchmark/CMakeLists.txt b/nav2_mppi_controller/benchmark/CMakeLists.txt new file mode 100644 index 0000000000..97db9f4185 --- /dev/null +++ b/nav2_mppi_controller/benchmark/CMakeLists.txt @@ -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() diff --git a/nav2_mppi_controller/benchmark/controller_benchmark.cpp b/nav2_mppi_controller/benchmark/controller_benchmark.cpp new file mode 100644 index 0000000000..9a2cabb4dc --- /dev/null +++ b/nav2_mppi_controller/benchmark/controller_benchmark.cpp @@ -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 +#include + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#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 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 params; + setUpControllerParams(visualize, params); + setUpOptimizerParams(optimizer_settings, critics, params); + options.parameter_overrides(params); + auto node = getDummyNode(options); + + auto tf_buffer = std::make_shared(node->get_clock()); + tf_buffer->setUsingDedicatedThread(true); // One-thread broadcasting-listening model + + auto broadcaster = + std::make_shared(node); + auto tf_listener = std::make_shared(*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 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 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 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 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 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 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 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 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 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 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 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(); diff --git a/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp new file mode 100644 index 0000000000..a7216bc80d --- /dev/null +++ b/nav2_mppi_controller/benchmark/optimizer_benchmark.cpp @@ -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 +#include + +#include "gtest/gtest.h" +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#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 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(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 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 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 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 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 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 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 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 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 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 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 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(); diff --git a/nav2_mppi_controller/critics.xml b/nav2_mppi_controller/critics.xml new file mode 100644 index 0000000000..669944f2fe --- /dev/null +++ b/nav2_mppi_controller/critics.xml @@ -0,0 +1,41 @@ + + + + + mppi critic for obstacle avoidance + + + + mppi critic for driving towards the goal + + + + mppi critic for achieving the goal heading angle + + + + mppi critic for aligning to path + + + + mppi critic for tracking the path in the correct heading + + + + mppi critic for driving towards the goal that is furthest among trajectories nearest path points + + + + + + + + mppi critic for preventing twirling behavior when using omnidirectional models + + + + mppi critic for incentivizing moving within kinematic and dynamic bounds + + + + diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp new file mode 100644 index 0000000000..82b850bc46 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp @@ -0,0 +1,127 @@ +// 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 +#include + +#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 tf, + const std::shared_ptr 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() override; + + /** + * @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::Logger logger_{rclcpp::get_logger("MPPIController")}; + std::shared_ptr costmap_ros_; + std::shared_ptr tf_buffer_; + + std::unique_ptr parameters_handler_; + Optimizer optimizer_; + PathHandler path_handler_; + TrajectoryVisualizer trajectory_visualizer_; + + bool visualize_; +}; + +} // namespace nav2_mppi_controller + +#endif // NAV2_MPPI_CONTROLLER__CONTROLLER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp new file mode 100644 index 0000000000..31965eb1cf --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_data.hpp @@ -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__CRITIC_DATA_HPP_ +#define NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ + +#include +#include +#include + +#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, path, costs, and + * important parameters to share + */ +struct CriticData +{ + const models::State & state; + const models::Trajectories & trajectories; + const models::Path & path; + + xt::xtensor & costs; + float & model_dt; + + bool fail_flag; + nav2_core::GoalChecker * goal_checker; + std::shared_ptr motion_model; + std::optional> path_pts_valid; + std::optional furthest_reached_path_point; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__CRITIC_DATA_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.hpp new file mode 100644 index 0000000000..88d277c14e --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_function.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 +#include + +#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 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 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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp new file mode 100644 index 0000000000..d8eeb87a3a --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp @@ -0,0 +1,98 @@ +// 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 +#include +#include +#include +#include + +#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 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, 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 costmap_ros_; + std::string name_; + + ParametersHandler * parameters_handler_; + std::vector critic_names_; + std::unique_ptr> loader_; + std::vector> critics_; + + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__CRITIC_MANAGER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/constraint_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/constraint_critic.hpp new file mode 100644 index 0000000000..ef53434189 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/constraint_critic.hpp @@ -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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp new file mode 100644 index 0000000000..08ec354cf3 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_angle_critic.hpp @@ -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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp new file mode 100644 index 0000000000..62d6bd1042 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/goal_critic.hpp @@ -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__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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp new file mode 100644 index 0000000000..a026b0fb2a --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/obstacles_critic.hpp @@ -0,0 +1,102 @@ +// 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 +#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 + */ + 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 + */ + 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 + */ + 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 + */ + double findCircumscribedCost(std::shared_ptr costmap); + +protected: + nav2_costmap_2d::FootprintCollisionChecker + collision_checker_{nullptr}; + + bool consider_footprint_{true}; + double collision_cost_{0}; + float inflation_scale_factor_{0}, inflation_radius_{0}; + + float possibly_inscribed_cost_; + float collision_margin_distance_; + float near_goal_distance_; + + unsigned int power_{0}; + float repulsion_weight_, critical_weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__OBSTACLES_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp new file mode 100644 index 0000000000..49995b9e3a --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_align_critic.hpp @@ -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. + +#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 presense 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}; + unsigned int power_{0}; + float weight_{0}; +}; + +} // namespace mppi::critics + +#endif // NAV2_MPPI_CONTROLLER__CRITICS__PATH_ALIGN_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp new file mode 100644 index 0000000000..92130dceb3 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_angle_critic.hpp @@ -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. + +#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: + double max_angle_to_furthest_{0}; + 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_ANGLE_CRITIC_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.hpp new file mode 100644 index 0000000000..12317c7b61 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/path_follow_critic.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__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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp new file mode 100644 index 0000000000..e17ad235e5 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/prefer_forward_critic.hpp @@ -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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp new file mode 100644 index 0000000000..3e316a567e --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/critics/twirling_critic.hpp @@ -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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.hpp new file mode 100644 index 0000000000..b7f9b6f3cc --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/constraints.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 +{ + double vx_max; + double vx_min; + double vy; + double wz; +}; + +/** + * @struct mppi::models::SamplingStd + * @brief Noise parameters for sampling trajectories + */ +struct SamplingStd +{ + double vx; + double vy; + double wz; +}; + +} // namespace mppi::models + +#endif // NAV2_MPPI_CONTROLLER__MODELS__CONSTRAINTS_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp new file mode 100644 index 0000000000..7a96ad1c94 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp @@ -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 + +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 vx; + xt::xtensor vy; + xt::xtensor wz; + + void reset(unsigned int time_steps) + { + vx = xt::zeros({time_steps}); + vy = xt::zeros({time_steps}); + wz = xt::zeros({time_steps}); + } +}; + +} // namespace mppi::models + +#endif // NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp new file mode 100644 index 0000000000..61d7da74ec --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/optimizer_settings.hpp @@ -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 +#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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp new file mode 100644 index 0000000000..241a6928ba --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/path.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 + +namespace mppi::models +{ + +/** + * @struct mppi::models::Path + * @brief Path represented as a tensor + */ +struct Path +{ + xt::xtensor x; + xt::xtensor y; + xt::xtensor yaws; + + /** + * @brief Reset path data + */ + void reset(unsigned int size) + { + x = xt::zeros({size}); + y = xt::zeros({size}); + yaws = xt::zeros({size}); + } +}; + +} // namespace mppi::models + +#endif // NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp new file mode 100644 index 0000000000..7dd19eaff3 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/state.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 + +#include +#include + +namespace mppi::models +{ + +/** + * @struct mppi::models::State + * @brief State information: velocities, controls, poses, speed + */ +struct State +{ + xt::xtensor vx; + xt::xtensor vy; + xt::xtensor wz; + + xt::xtensor cvx; + xt::xtensor cvy; + xt::xtensor 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({batch_size, time_steps}); + vy = xt::zeros({batch_size, time_steps}); + wz = xt::zeros({batch_size, time_steps}); + + cvx = xt::zeros({batch_size, time_steps}); + cvy = xt::zeros({batch_size, time_steps}); + cwz = xt::zeros({batch_size, time_steps}); + } +}; +} // namespace mppi::models + +#endif // NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp new file mode 100644 index 0000000000..fa2c018120 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.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 +#include + +namespace mppi::models +{ + +/** + * @class mppi::models::Trajectories + * @brief Candidate Trajectories + */ +struct Trajectories +{ + xt::xtensor x; + xt::xtensor y; + xt::xtensor yaws; + + /** + * @brief Reset state data + */ + void reset(unsigned int batch_size, unsigned int time_steps) + { + x = xt::zeros({batch_size, time_steps}); + y = xt::zeros({batch_size, time_steps}); + yaws = xt::zeros({batch_size, time_steps}); + } +}; + +} // namespace mppi::models + +#endif // NAV2_MPPI_CONTROLLER__MODELS__TRAJECTORIES_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.hpp new file mode 100644 index 0000000000..af7c0bb612 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/motion_models.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 + +#include "nav2_mppi_controller/models/control_sequence.hpp" +#include "nav2_mppi_controller/models/state.hpp" +#include +#include +#include +#include + +#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) + { + auto getParam = param_handler->getParamGetter("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) * 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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp new file mode 100644 index 0000000000..e1a518ae84 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp @@ -0,0 +1,265 @@ +// 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 +#include + +#include +#include + +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "nav2_core/goal_checker.hpp" +#include "nav2_core/controller_exceptions.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 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, + 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 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, 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 & trajectories, + const xt::xtensor & 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 costmap_ros_; + nav2_costmap_2d::Costmap2D * costmap_; + std::string name_; + + std::shared_ptr motion_model_; + + ParametersHandler * parameters_handler_; + CriticManager critic_manager_; + NoiseGenerator noise_generator_; + + models::OptimizerSettings settings_; + + models::State state_; + models::ControlSequence control_sequence_; + std::array control_history_; + models::Trajectories generated_trajectories_; + models::Path path_; + xt::xtensor costs_; + + CriticData critics_data_ = + {state_, generated_trajectories_, path_, 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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp new file mode 100644 index 0000000000..6eb1b36384 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp @@ -0,0 +1,107 @@ +// 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 +#include +#include +#include +#include + +#include +#include + +#include "nav2_mppi_controller/models/optimizer_settings.hpp" +#include +#include + +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 + */ + void initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic); + + /** + * @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 noises_vx_; + xt::xtensor noises_vy_; + xt::xtensor 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}; +}; + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__NOISE_GENERATOR_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp new file mode 100644 index 0000000000..1667fa6d79 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.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__TOOLS__PARAMETERS_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PARAMETERS_HANDLER_HPP_ + +#include +#include +#include +#include +#include +#include + +#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 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 + void addPostCallback(T && callback); + + /** + * @brief Set a callback to process before parameter changes + * @param callback Callback function + */ + template + void addPreCallback(T && callback); + + /** + * @brief Set a parameter to a dynamic parameter callback + * @param setting Parameter + * @param name Name of parameter + */ + template + 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 + 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 + 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 + 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 + 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> + get_param_callbacks_; + + std::vector> pre_callbacks_; + std::vector> 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 +void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback) +{ + get_param_callbacks_[name] = callback; +} + +template +void ParametersHandler::addPostCallback(T && callback) +{ + post_callbacks_.push_back(callback); +} + +template +void ParametersHandler::addPreCallback(T && callback) +{ + pre_callbacks_.push_back(callback); +} + +template +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(setting, name, node); + + if (param_type == ParameterType::Dynamic) { + setDynamicParamCallback(setting, name); + } +} + +template +void ParametersHandler::setParam( + SettingT & setting, const std::string & name, NodeT node) const +{ + ParamT param_in{}; + node->get_parameter(name, param_in); + setting = static_cast(param_in); +} + +template +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(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 +auto ParametersHandler::as(const rclcpp::Parameter & parameter) +{ + if constexpr (std::is_same_v) { + return parameter.as_bool(); + } else if constexpr (std::is_integral_v) { + return parameter.as_int(); + } else if constexpr (std::is_floating_point_v) { + return parameter.as_double(); + } else if constexpr (std::is_same_v) { + return parameter.as_string(); + } else if constexpr (std::is_same_v>) { + return parameter.as_integer_array(); + } else if constexpr (std::is_same_v>) { + return parameter.as_double_array(); + } else if constexpr (std::is_same_v>) { + return parameter.as_string_array(); + } else if constexpr (std::is_same_v>) { + return parameter.as_bool_array(); + } +} + +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PARAMETERS_HANDLER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp new file mode 100644 index 0000000000..c66d32257e --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/path_handler.hpp @@ -0,0 +1,146 @@ +// 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__PATH_HANDLER_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ + +#include +#include +#include +#include + +#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_core/controller_exceptions.hpp" + +#include "nav2_mppi_controller/tools/parameters_handler.hpp" + +namespace mppi +{ + +using PathIterator = std::vector::iterator; +using PathRange = std::pair; + +/** + * @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, + std::shared_ptr, 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); + +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 getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose); + + /** + * @brief Prune global path to only interesting portions + * @param end Final path iterator + */ + void pruneGlobalPlan(const PathIterator end); + + std::string name_; + std::shared_ptr costmap_; + std::shared_ptr tf_buffer_; + ParametersHandler * parameters_handler_; + + nav_msgs::msg::Path global_plan_; + rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")}; + + double max_robot_pose_search_dist_{0}; + double prune_distance_{0}; + double transform_tolerance_{0}; +}; +} // namespace mppi + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__PATH_HANDLER_HPP_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp new file mode 100644 index 0000000000..ba4dde4ca2 --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/trajectory_visualizer.hpp @@ -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 +#include +#include + +#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 & trajectory); + + /** + * @brief Add candidate trajectories to visualize + * @param trajectories Candidate trajectories + */ + void add(const models::Trajectories & trajectories); + + /** + * @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> + trajectories_publisher_; + std::shared_ptr> transformed_path_pub_; + + std::unique_ptr 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_ diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp new file mode 100644 index 0000000000..127bc318ae --- /dev/null +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/utils.hpp @@ -0,0 +1,523 @@ +// 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__UTILS_HPP_ +#define NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#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) +{ + using visualization_msgs::msg::Marker; + Marker marker; + marker.header.frame_id = frame_id; + marker.header.stamp = rclcpp::Time(0, 0); + marker.ns = "MarkerNS"; + 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 path Path to retreive goal pose from + * @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 models::Path & path) +{ + const auto goal_idx = path.x.shape(0) - 1; + const auto goal_x = path.x(goal_idx); + const auto goal_y = path.y(goal_idx); + + 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_x; + auto dy = robot.position.y - goal_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 path Path to retreive goal pose from + * @return bool If robot is within tolerance to the goal + */ +inline bool withinPositionGoalTolerance( + float pose_tolerance, + const geometry_msgs::msg::Pose & robot, + const models::Path & path) +{ + const auto goal_idx = path.x.shape(0) - 1; + const auto goal_x = path.x(goal_idx); + const auto goal_y = path.y(goal_idx); + + const auto pose_tolerance_sq = pose_tolerance * pose_tolerance; + + auto dx = robot.position.x - goal_x; + auto dy = robot.position.y - goal_y; + + auto dist_sq = dx * dx + dy * dy; + + 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 +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 +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; + double min_distance_by_path = std::numeric_limits::max(); + + for (size_t i = 0; i < dists.shape(0); i++) { + size_t min_id_by_path = 0; + for (size_t j = 0; j < dists.shape(1); j++) { + if (dists(i, j) < min_distance_by_path) { + min_distance_by_path = dists(i, j); + 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; + + double min_distance_by_path = std::numeric_limits::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 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(path_segments_count - 1, 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 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 + * @return Angle between two points + */ +inline double posePointAngle(const geometry_msgs::msg::Pose & pose, double point_x, double point_y) +{ + double pose_x = pose.position.x; + double pose_y = pose.position.y; + double pose_yaw = tf2::getYaw(pose.orientation); + + double yaw = atan2(point_y - pose_y, point_x - pose_x); + return abs(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 & control_history, + const models::OptimizerSettings & settings) +{ + // Savitzky-Golay Quadratic, 5-point Coefficients + xt::xarray filter = {-3.0, 12.0, 17.0, 12.0, -3.0}; + filter /= 35.0; + + const unsigned int num_sequences = control_sequence.vx.shape(0); + + // Too short to smooth meaningfully + if (num_sequences < 10) { + return; + } + + auto applyFilter = [&](const xt::xarray & data) -> float { + return xt::sum(data * filter, {0}, immediate)(); + }; + + auto applyFilterOverAxis = + [&](xt::xtensor & sequence, const float hist_0, const float hist_1) -> void + { + unsigned int idx = 0; + sequence(idx) = applyFilter( + { + hist_0, + hist_1, + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + + idx++; + sequence(idx) = applyFilter( + { + hist_1, + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + + for (idx = 2; idx != num_sequences - 3; idx++) { + sequence(idx) = applyFilter( + { + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 2)}); + } + + idx++; + sequence(idx) = applyFilter( + { + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx + 1), + sequence(idx + 1)}); + + idx++; + sequence(idx) = applyFilter( + { + sequence(idx - 2), + sequence(idx - 1), + sequence(idx), + sequence(idx), + sequence(idx)}); + }; + + // Filter trajectories + applyFilterOverAxis(control_sequence.vx, control_history[0].vx, control_history[1].vx); + applyFilterOverAxis(control_sequence.vy, control_history[0].vy, control_history[1].vy); + applyFilterOverAxis(control_sequence.wz, control_history[0].wz, control_history[1].wz); + + // Update control history + unsigned int offset = settings.shift_control_sequence ? 1 : 0; + control_history[0] = control_history[1]; + control_history[1] = { + control_sequence.vx(offset), + control_sequence.vy(offset), + control_sequence.wz(offset)}; +} + +} // namespace mppi::utils + +#endif // NAV2_MPPI_CONTROLLER__TOOLS__UTILS_HPP_ diff --git a/nav2_mppi_controller/media/demo.gif b/nav2_mppi_controller/media/demo.gif new file mode 100644 index 0000000000..68031ef515 Binary files /dev/null and b/nav2_mppi_controller/media/demo.gif differ diff --git a/nav2_mppi_controller/mppic.xml b/nav2_mppi_controller/mppic.xml new file mode 100644 index 0000000000..021a3be8eb --- /dev/null +++ b/nav2_mppi_controller/mppic.xml @@ -0,0 +1,7 @@ + + + + MPPI Controller for Nav2 + + + diff --git a/nav2_mppi_controller/package.xml b/nav2_mppi_controller/package.xml new file mode 100644 index 0000000000..27c4e8b6dc --- /dev/null +++ b/nav2_mppi_controller/package.xml @@ -0,0 +1,42 @@ + + + + nav2_mppi_controller + 0.2.1 + nav2_mppi_controller + Aleksei Budyakov + Steve Macenski + MIT + + ament_cmake + ament_cmake_ros + + rclcpp + nav2_common + nav2_core + nav2_util + nav2_costmap_2d + geometry_msgs + visualization_msgs + nav2_msgs + pluginlib + tf2_geometry_msgs + tf2 + tf2_eigen + tf2_ros + std_msgs + xtensor + libomp-dev + benchmark + xsimd + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + ament_cmake + + + + + diff --git a/nav2_mppi_controller/src/controller.cpp b/nav2_mppi_controller/src/controller.cpp new file mode 100644 index 0000000000..0cf5cf2259 --- /dev/null +++ b/nav2_mppi_controller/src/controller.cpp @@ -0,0 +1,125 @@ +// 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 +#include +#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 tf, + const std::shared_ptr costmap_ros) +{ + parent_ = parent; + costmap_ros_ = costmap_ros; + tf_buffer_ = tf; + name_ = name; + parameters_handler_ = std::make_unique(parent); + + auto node = parent_.lock(); + // Get high-level controller parameters + auto getParam = parameters_handler_->getParamGetter(name_); + getParam(visualize_, "visualize", false); + + // 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 + + std::lock_guard lock(*parameters_handler_->getLock()); + nav_msgs::msg::Path transformed_plan = path_handler_.transformPath(robot_pose); + + geometry_msgs::msg::TwistStamped cmd = + optimizer_.evalControl(robot_pose, robot_speed, transformed_plan, goal_checker); + +#ifdef BENCHMARK_TESTING + auto end = std::chrono::system_clock::now(); + auto duration = std::chrono::duration_cast(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()); + trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory()); + 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) diff --git a/nav2_mppi_controller/src/critic_manager.cpp b/nav2_mppi_controller/src/critic_manager.cpp new file mode 100644 index 0000000000..2a7a77a234 --- /dev/null +++ b/nav2_mppi_controller/src/critic_manager.cpp @@ -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 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{}, ParameterType::Static); +} + +void CriticManager::loadCritics() +{ + if (!loader_) { + loader_ = std::make_unique>( + "nav2_mppi_controller", "mppi::critics::CriticFunction"); + } + + critics_.clear(); + for (auto name : critic_names_) { + std::string fullname = getFullName(name); + auto instance = std::unique_ptr( + 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 diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp new file mode 100644 index 0000000000..886c0e4b05 --- /dev/null +++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp @@ -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, vy_min; + getParentParam(vx_max, "vx_max", 0.5); + getParentParam(vy_max, "vy_max", 0.0); + getParentParam(vx_min, "vx_min", -0.35); + getParentParam(vy_min, "vy_min", 0.0); + + const float min_sgn = vx_min > 0.0 ? 1.0 : -1.0; + max_vel_ = std::sqrt(vx_max * vx_max + vy_max * vy_max); + min_vel_ = min_sgn * std::sqrt(vx_min * vx_min + vy_min * vy_min); +} + +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(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_); + } + + 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_EXPORT_CLASS(mppi::critics::ConstraintCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/goal_angle_critic.cpp b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp new file mode 100644 index 0000000000..c196871327 --- /dev/null +++ b/nav2_mppi_controller/src/critics/goal_angle_critic.cpp @@ -0,0 +1,62 @@ +// 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.40); + + 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_) { + return; + } + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.path)) + { + 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_EXPORT_CLASS( + mppi::critics::GoalAngleCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/goal_critic.cpp b/nav2_mppi_controller/src/critics/goal_critic.cpp new file mode 100644 index 0000000000..859a9c3f5c --- /dev/null +++ b/nav2_mppi_controller/src/critics/goal_critic.cpp @@ -0,0 +1,64 @@ +// 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_critic.hpp" + +namespace mppi::critics +{ + +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.0); + + RCLCPP_INFO( + logger_, "GoalCritic instantiated with %d power and %f weight.", + power_, weight_); +} + +void GoalCritic::score(CriticData & data) +{ + if (!enabled_) { + return; + } + + if (!utils::withinPositionGoalTolerance( + threshold_to_consider_, data.state.pose.pose, data.path)) + { + return; + } + + const auto goal_idx = data.path.x.shape(0) - 1; + + const auto goal_x = data.path.x(goal_idx); + const auto goal_y = data.path.y(goal_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 - goal_x, 2) + + xt::pow(last_y - goal_y, 2)); + + data.costs += xt::pow(std::move(dists) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS(mppi::critics::GoalCritic, mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/obstacles_critic.cpp b/nav2_mppi_controller/src/critics/obstacles_critic.cpp new file mode 100644 index 0000000000..f45ce72116 --- /dev/null +++ b/nav2_mppi_controller/src/critics/obstacles_critic.cpp @@ -0,0 +1,201 @@ +// 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 +#include "nav2_mppi_controller/critics/obstacles_critic.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_); + 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"); +} + +double ObstaclesCritic::findCircumscribedCost( + std::shared_ptr costmap) +{ + double result = -1.0; + bool inflation_layer_found = false; + // 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(*layer); + if (!inflation_layer) { + continue; + } + + inflation_layer_found = true; + const double circum_radius = costmap->getLayeredCostmap()->getCircumscribedRadius(); + const double resolution = costmap->getCostmap()->getResolution(); + result = inflation_layer->computeCost(circum_radius / resolution); + inflation_scale_factor_ = static_cast(inflation_layer->getCostScalingFactor()); + inflation_radius_ = static_cast(inflation_layer->getInflationRadius()); + } + + if (!inflation_layer_found) { + RCLCPP_WARN( + rclcpp::get_logger("computeCircumscribedCost"), + "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!"); + } + + return result; +} + +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 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.path)) { + near_goal = true; + } + + auto && raw_cost = xt::xtensor::from_shape({data.costs.shape(0)}); + raw_cost.fill(0.0); + auto && repulsive_cost = xt::xtensor::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; + float traj_cost = 0.0; + 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) {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 || inflation_scale_factor_ == 0) { + 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] = static_cast(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(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; + collision_checker_.worldToMap(x, y, x_i, y_i); + cost = collision_checker_.pointCost(x_i, y_i); + + if (consider_footprint_ && cost >= possibly_inscribed_cost_) { + cost = static_cast(collision_checker_.footprintCostAtPose( + x, y, theta, costmap_ros_->getRobotFootprint())); + collision_cost.using_footprint = true; + } + + return collision_cost; +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::ObstaclesCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/path_align_critic.cpp b/nav2_mppi_controller/src/critics/path_align_critic.cpp new file mode 100644 index 0000000000..b95b96a04a --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_align_critic.cpp @@ -0,0 +1,125 @@ +// 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_critic.hpp" + +#include +#include + +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.40f); + + 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.path)) + { + 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(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 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 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::from_shape({data.costs.shape(0)}); + + if (path_segments_count < 1) { + return; + } + + for (size_t t = 0; t < batch_size; ++t) { + float summed_dist = 0; + for (size_t p = trajectory_point_step_; p < time_steps; p += trajectory_point_step_) { + double min_dist_sq = std::numeric_limits::max(); + size_t 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> P; + float dx = P_x(s) - T_x(t, p); + float dy = P_y(s) - T_y(t, p); + float 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 += std::sqrt(min_dist_sq); + } + } + + cost[t] = summed_dist / traj_pts_eval; + } + + data.costs += xt::pow(std::move(cost) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAlignCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/path_angle_critic.cpp b/nav2_mppi_controller/src/critics/path_angle_critic.cpp new file mode 100644 index 0000000000..a588bbe291 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_angle_critic.cpp @@ -0,0 +1,80 @@ +// 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_angle_critic.hpp" + +#include + +namespace mppi::critics +{ + +void PathAngleCritic::initialize() +{ + 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.40f); + getParam( + max_angle_to_furthest_, + "max_angle_to_furthest", 1.2); + + + RCLCPP_INFO( + logger_, + "PathAngleCritic instantiated with %d power and %f weight.", + power_, weight_); +} + +void PathAngleCritic::score(CriticData & data) +{ + using xt::evaluation_strategy::immediate; + if (!enabled_) { + return; + } + + if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + 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) < max_angle_to_furthest_) { + return; + } + + const auto yaws_between_points = xt::atan2( + goal_y - data.trajectories.y, + goal_x - data.trajectories.x); + const auto yaws = + xt::abs(utils::shortest_angular_distance(data.trajectories.yaws, yaws_between_points)); + + data.costs += xt::pow(xt::mean(yaws, {1}, immediate) * weight_, power_); +} + +} // namespace mppi::critics + +#include + +PLUGINLIB_EXPORT_CLASS( + mppi::critics::PathAngleCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/path_follow_critic.cpp b/nav2_mppi_controller/src/critics/path_follow_critic.cpp new file mode 100644 index 0000000000..aa63327e90 --- /dev/null +++ b/nav2_mppi_controller/src/critics/path_follow_critic.cpp @@ -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 +#include + +namespace mppi::critics +{ + +void PathFollowCritic::initialize() +{ + auto getParam = parameters_handler_->getParamGetter(name_); + + getParam( + threshold_to_consider_, + "threshold_to_consider", 0.40f); + 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_ || + utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) + { + 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_EXPORT_CLASS( + mppi::critics::PathFollowCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp new file mode 100644 index 0000000000..5cea014bbc --- /dev/null +++ b/nav2_mppi_controller/src/critics/prefer_forward_critic.cpp @@ -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/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.40f); + + 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_) { + return; + } + + if (utils::withinPositionGoalTolerance(threshold_to_consider_, data.state.pose.pose, data.path)) { + 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_EXPORT_CLASS( + mppi::critics::PreferForwardCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/critics/twirling_critic.cpp b/nav2_mppi_controller/src/critics/twirling_critic.cpp new file mode 100644 index 0000000000..28eb71b48b --- /dev/null +++ b/nav2_mppi_controller/src/critics/twirling_critic.cpp @@ -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. + +#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_) { + return; + } + + if (utils::withinPositionGoalTolerance(data.goal_checker, data.state.pose.pose, data.path)) { + 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_EXPORT_CLASS( + mppi::critics::TwirlingCritic, + mppi::critics::CriticFunction) diff --git a/nav2_mppi_controller/src/noise_generator.cpp b/nav2_mppi_controller/src/noise_generator.cpp new file mode 100644 index 0000000000..60173789f0 --- /dev/null +++ b/nav2_mppi_controller/src/noise_generator.cpp @@ -0,0 +1,109 @@ +// 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 +#include +#include +#include +#include + +namespace mppi +{ + +void NoiseGenerator::initialize(mppi::models::OptimizerSettings & settings, bool is_holonomic) +{ + settings_ = settings; + is_holonomic_ = is_holonomic; + active_ = true; + noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this)); +} + +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. + { + std::unique_lock guard(noise_lock_); + ready_ = true; + } + noise_cond_.notify_all(); +} + +void NoiseGenerator::setNoisedControls( + models::State & state, + const models::ControlSequence & control_sequence) +{ + std::unique_lock 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 guard(noise_lock_); + xt::noalias(noises_vx_) = xt::zeros({settings_.batch_size, settings_.time_steps}); + xt::noalias(noises_vy_) = xt::zeros({settings_.batch_size, settings_.time_steps}); + xt::noalias(noises_wz_) = xt::zeros({settings_.batch_size, settings_.time_steps}); + ready_ = true; + } + noise_cond_.notify_all(); +} + +void NoiseGenerator::noiseThread() +{ + do { + std::unique_lock 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( + {s.batch_size, s.time_steps}, 0.0, + s.sampling_std.vx); + xt::noalias(noises_wz_) = xt::random::randn( + {s.batch_size, s.time_steps}, 0.0, + s.sampling_std.wz); + if (is_holonomic_) { + xt::noalias(noises_vy_) = xt::random::randn( + {s.batch_size, s.time_steps}, 0.0, + s.sampling_std.vy); + } +} + +} // namespace mppi diff --git a/nav2_mppi_controller/src/optimizer.cpp b/nav2_mppi_controller/src/optimizer.cpp new file mode 100644 index 0000000000..4adf555220 --- /dev/null +++ b/nav2_mppi_controller/src/optimizer.cpp @@ -0,0 +1,449 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include "nav2_core/controller_exceptions.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 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()); + + 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 nav2_core::ControllerException( + "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}; + + costs_ = xt::zeros({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, nav2_core::GoalChecker * goal_checker) +{ + prepare(robot_pose, robot_speed, plan, 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 nav2_core::NoValidControl("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, nav2_core::GoalChecker * goal_checker) +{ + state_.pose = robot_pose; + state_.speed = robot_speed; + path_ = utils::toTensor(plan); + 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 & trajectory, + const xt::xtensor & sequence) const +{ + double 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) = + utils::normalize_angles(xt::cumsum(wz * settings_.model_dt, 0) + initial_yaw); + + auto && yaw_cos = xt::xtensor::from_shape(traj_yaws.shape()); + auto && yaw_sin = xt::xtensor::from_shape(traj_yaws.shape()); + + const auto yaw_offseted = xt::view(traj_yaws, xt::range(1, _)); + + xt::noalias(xt::view(yaw_cos, 0)) = std::cos(initial_yaw); + xt::noalias(xt::view(yaw_sin, 0)) = std::sin(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 double initial_yaw = tf2::getYaw(state.pose.pose.orientation); + + xt::noalias(trajectories.yaws) = + utils::normalize_angles(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::from_shape(trajectories.yaws.shape()); + auto && yaw_sin = xt::xtensor::from_shape(trajectories.yaws.shape()); + xt::noalias(xt::view(yaw_cos, xt::all(), 0)) = std::cos(initial_yaw); + xt::noalias(xt::view(yaw_sin, xt::all(), 0)) = std::sin(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 Optimizer::getOptimizedTrajectory() +{ + auto && sequence = + xt::xtensor::from_shape({settings_.time_steps, isHolonomic() ? 3u : 2u}); + auto && trajectories = xt::xtensor::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 / std::pow(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 / std::pow(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 / std::pow(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_.vy) = xt::sum(state_.cvy * softmaxes_extened, 0, immediate); + xt::noalias(control_sequence_.wz) = xt::sum(state_.cwz * 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(); + } else if (model == "Omni") { + motion_model_ = std::make_shared(); + } else if (model == "Ackermann") { + motion_model_ = std::make_shared(parameters_handler_); + } else { + throw nav2_core::ControllerException( + 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 diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp new file mode 100644 index 0000000000..fd284b60d4 --- /dev/null +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -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 parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + std::lock_guard 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 diff --git a/nav2_mppi_controller/src/path_handler.cpp b/nav2_mppi_controller/src/path_handler.cpp new file mode 100644 index 0000000000..83ec415e17 --- /dev/null +++ b/nav2_mppi_controller/src/path_handler.cpp @@ -0,0 +1,167 @@ +// 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/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 costmap, + std::shared_ptr 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); +} + +std::pair +PathHandler::getGlobalPlanConsideringBoundsInCostmapFrame( + const geometry_msgs::msg::PoseStamped & global_pose) +{ + using nav2_util::geometry_utils::euclidean_distance; + + auto begin = global_plan_.poses.begin(); + auto end = global_plan_.poses.end(); + + auto closest_pose_upper_bound = + nav2_util::geometry_utils::first_after_integrated_distance( + global_plan_.poses.begin(), global_plan_.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; + + 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 != end; ++global_plan_pose) { + // Distance relative to robot pose check + auto distance = euclidean_distance(global_pose, *global_plan_pose); + if (distance >= prune_distance_) { + return {transformed_plan, closest_point}; + } + + // Transform from global plan frame to costmap frame + geometry_msgs::msg::PoseStamped costmap_plan_pose; + global_plan_pose->header.stamp = global_pose.header.stamp; + 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_.poses.empty()) { + throw nav2_core::InvalidPath("Received plan with zero length"); + } + + geometry_msgs::msg::PoseStamped robot_pose; + if (!transformPose(global_plan_.header.frame_id, pose, robot_pose)) { + throw nav2_core::ControllerTFError( + "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); + + pruneGlobalPlan(lower_bound); + + if (transformed_plan.poses.empty()) { + throw nav2_core::InvalidPath("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 std::max(costmap->getSizeInCellsX(), costmap->getSizeInCellsY()) * + costmap->getResolution() / 2.0; +} + +void PathHandler::setPath(const nav_msgs::msg::Path & plan) +{ + global_plan_ = plan; +} + +nav_msgs::msg::Path & PathHandler::getPath() {return global_plan_;} + +void PathHandler::pruneGlobalPlan(const PathIterator end) +{ + global_plan_.poses.erase(global_plan_.poses.begin(), end); +} + +} // namespace mppi diff --git a/nav2_mppi_controller/src/trajectory_visualizer.cpp b/nav2_mppi_controller/src/trajectory_visualizer.cpp new file mode 100644 index 0000000000..320a5856d3 --- /dev/null +++ b/nav2_mppi_controller/src/trajectory_visualizer.cpp @@ -0,0 +1,127 @@ +// 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 +#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("/trajectories", 1); + transformed_path_pub_ = node->create_publisher("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 & trajectory) +{ + auto & size = trajectory.shape()[0]; + if (!size) { + return; + } + + auto add_marker = [&](auto i) { + float component = static_cast(i) / static_cast(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_); + points_->markers.push_back(marker); + }; + + for (size_t i = 0; i < size; i++) { + add_marker(i); + } +} + +void TrajectoryVisualizer::add( + const models::Trajectories & trajectories) +{ + auto & shape = trajectories.x.shape(); + const float shape_1 = static_cast(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(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_); + + points_->markers.push_back(marker); + } + } +} + +void TrajectoryVisualizer::reset() +{ + marker_id_ = 0; + points_ = std::make_unique(); +} + +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(plan); + transformed_path_pub_->publish(std::move(plan_ptr)); + } +} + +} // namespace mppi diff --git a/nav2_mppi_controller/test/CMakeLists.txt b/nav2_mppi_controller/test/CMakeLists.txt new file mode 100644 index 0000000000..6305d4e31a --- /dev/null +++ b/nav2_mppi_controller/test/CMakeLists.txt @@ -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() diff --git a/nav2_mppi_controller/test/controller_state_transition_test.cpp b/nav2_mppi_controller/test/controller_state_transition_test.cpp new file mode 100644 index 0000000000..e003859c62 --- /dev/null +++ b/nav2_mppi_controller/test/controller_state_transition_test.cpp @@ -0,0 +1,73 @@ +// 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 +#include +#include + +#include +#include + +#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 params; + setUpControllerParams(visualize, params); + options.parameter_overrides(params); + + auto node = getDummyNode(options); + auto tf_buffer = std::make_shared(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); + + 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(); +} diff --git a/nav2_mppi_controller/test/critic_manager_test.cpp b/nav2_mppi_controller/test/critic_manager_test.cpp new file mode 100644 index 0000000000..0ed1fc811f --- /dev/null +++ b/nav2_mppi_controller/test/critic_manager_test.cpp @@ -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 +#include + +#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(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(critics_[0].get())->initialized_; + } + + bool getDummyCriticScored() + { + return dynamic_cast(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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs; + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, 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("my_node"); + node->declare_parameter( + "critic_manager.critics", + rclcpp::ParameterValue(std::vector{"ConstraintCritic", "PreferForwardCritic"})); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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); +} diff --git a/nav2_mppi_controller/test/critics_tests.cpp b/nav2_mppi_controller/test/critics_tests.cpp new file mode 100644 index 0000000000..d574cd769e --- /dev/null +++ b/nav2_mppi_controller/test/critics_tests.cpp @@ -0,0 +1,535 @@ +// 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 +#include + +#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/path_align_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 "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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + + // 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({1000, 30}); + state.vy = xt::zeros({1000, 30}); + state.wz = xt::ones({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({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({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({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({1000}); + + // Now with ackermann, all in constraint so no costs to score + state.vx = 0.40 * xt::ones({1000, 30}); + state.wz = 1.5 * xt::ones({1000, 30}); + data.motion_model = std::make_shared(¶m_handler); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0, 1e-6); + + // Now violating the ackermann constraints + state.wz = 2.5 * xt::ones({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, GoalAngleCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + + // 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; + 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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + + // 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; + 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({1000}); + + // provide state pose and path close + path.x(9) = 0.5; + path.y(9) = 0.0; + 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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + 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; + 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; + 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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + 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; + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close to trigger behavior but with all forward motion + path.x(9) = 0.15; + state.vx = xt::ones({1000, 30}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); + + // provide state pose and path close to trigger behavior but with all reverse motion + state.vx = -1.0 * xt::ones({1000, 30}); + critic.score(data); + EXPECT_GT(xt::sum(costs, immediate)(), 0.0); + EXPECT_NEAR(costs(0), 15.0, 1e-6); // 1.0 * 0.1 model_dt * 5.0 weight * 30 length +} + +TEST(CriticTests, TwirlingCritic) +{ + // Standard preamble + auto node = std::make_shared("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + 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; + 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; + state.wz = xt::zeros({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({1000}); + + // Now try again with some wiggling noise + traj_view = xt::random::randn({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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + 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 path close within positional tolerances + state.pose.pose.position.x = 1.0; + path.reset(6); + path.x(5) = 0.85; + 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; + 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("my_node"); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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; + xt::xtensor costs = xt::zeros({1000}); + float model_dt = 0.1; + CriticData data = + {state, generated_trajectories, path, costs, model_dt, false, nullptr, nullptr, std::nullopt, + std::nullopt}; + data.motion_model = std::make_shared(); + 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; + 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; + 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; + 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; + generated_trajectories.x = 0.66 * xt::ones({1000, 30}); + critic.score(data); + // 0.04 * 1000 * 10 weight * 4 num pts eval / 4 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({1000}); + path.x = 1.5 * xt::ones({22}); + path.y = 1.5 * xt::ones({22}); + critic.score(data); + EXPECT_NEAR(xt::sum(costs, immediate)(), 0.0, 1e-6); +} diff --git a/nav2_mppi_controller/test/models_test.cpp b/nav2_mppi_controller/test/models_test.cpp new file mode 100644 index 0000000000..12936d5875 --- /dev/null +++ b/nav2_mppi_controller/test/models_test.cpp @@ -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({10}); + sequence.vy = xt::ones({10}); + sequence.wz = xt::ones({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({10}); + path.y = xt::ones({10}); + path.yaws = xt::ones({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({10, 10}); + state.vy = xt::ones({10, 10}); + state.wz = xt::ones({10, 10}); + state.cvx = xt::ones({10, 10}); + state.cvy = xt::ones({10, 10}); + state.cwz = xt::ones({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({10, 10}); + trajectories.y = xt::ones({10, 10}); + trajectories.yaws = xt::ones({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); +} diff --git a/nav2_mppi_controller/test/motion_model_tests.cpp b/nav2_mppi_controller/test/motion_model_tests.cpp new file mode 100644 index 0000000000..8b130e311d --- /dev/null +++ b/nav2_mppi_controller/test/motion_model_tests.cpp @@ -0,0 +1,179 @@ +// 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 +#include + +#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 model = + std::make_unique(); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({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({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 model = + std::make_unique(); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({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("my_node"); + ParametersHandler param_handler(node); + std::unique_ptr model = + std::make_unique(¶m_handler); + + // Check that predict properly populates the trajectory velocities with the control velocities + state.cvx = 10 * xt::ones({batches, timesteps}); + state.cvy = 5 * xt::ones({batches, timesteps}); + state.cwz = 1 * xt::ones({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({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; + } + + 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); + + // 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(); +} diff --git a/nav2_mppi_controller/test/noise_generator_test.cpp b/nav2_mppi_controller/test/noise_generator_test.cpp new file mode 100644 index 0000000000..3788f2b8a3 --- /dev/null +++ b/nav2_mppi_controller/test/noise_generator_test.cpp @@ -0,0 +1,121 @@ +// 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 +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" +#include "nav2_mppi_controller/tools/noise_generator.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; + + generator.initialize(settings, false); + generator.shutdown(); +} + +TEST(NoiseGeneratorTest, NoiseGeneratorMain) +{ + // Tests shuts down internal thread cleanly + 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); + 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(); +} diff --git a/nav2_mppi_controller/test/optimizer_smoke_test.cpp b/nav2_mppi_controller/test/optimizer_smoke_test.cpp new file mode 100644 index 0000000000..d694496395 --- /dev/null +++ b/nav2_mppi_controller/test/optimizer_smoke_test.cpp @@ -0,0 +1,115 @@ +// 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 +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +#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, 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(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}; + + EXPECT_NO_THROW(optimizer->evalControl(pose, velocity, path, dummy_goal_checker)); +} + +INSTANTIATE_TEST_SUITE_P( + OptimizerTests, + OptimizerSuite, + ::testing::Values( + std::make_tuple( + "Omni", + std::vector( + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, {"PathAlignCritic"}, + {"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), + true), + std::make_tuple( + "DiffDrive", + std::vector( + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), + true), + std::make_tuple( + "Ackermann", + std::vector( + {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, + {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}), + true)) +); diff --git a/nav2_mppi_controller/test/optimizer_unit_tests.cpp b/nav2_mppi_controller/test/optimizer_unit_tests.cpp new file mode 100644 index 0000000000..f27471d28a --- /dev/null +++ b/nav2_mppi_controller/test/optimizer_unit_tests.cpp @@ -0,0 +1,636 @@ +// 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 +#include + +#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(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(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(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({1000, 10}); + control_sequence_.vx = 342.0 * xt::ones({30}); + control_history_[0] = {43, 5646, 32432}; + costs_ = 5.32 * xt::ones({56453}); + generated_trajectories_.x = 432.234 * xt::ones({7865, 1}); + } + + void testReset() + { + reset(); + + EXPECT_EQ(state_.vx, xt::zeros({1000, 50})); + EXPECT_EQ(control_sequence_.vx, xt::zeros({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({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, nav2_core::GoalChecker * goal_checker) + { + prepare(robot_pose, robot_speed, plan, 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 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({1000, 50}); + state.cvy = 0.5 * xt::ones({1000, 50}); + state.cwz = 0.1 * xt::ones({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({1000, 50}); + state.cvy = -0.5 * xt::ones({1000, 50}); + state.cwz = -0.1 * xt::ones({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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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({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("my_node"); + OptimizerTester optimizer_tester; + node->declare_parameter("controller_frequency", rclcpp::ParameterValue(30.0)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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; + path.poses.resize(17); + + optimizer_tester.testPrepare(pose, speed, path, nullptr); +} + +TEST(OptimizerTests, shiftControlSequenceTests) +{ + auto node = std::make_shared("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( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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.5); + EXPECT_EQ(v_min, -0.35); + optimizer_tester.setSpeedLimit(0, false); + auto [v_min2, v_max2] = optimizer_tester.getVelLimits(); + EXPECT_EQ(v_max2, 0.5); + EXPECT_EQ(v_min2, -0.35); + 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.5); + EXPECT_EQ(v_min4, -0.35); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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({50}); + sequence.vy = 0.75 * xt::ones({50}); + sequence.wz = 2.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, xt::ones({50})); + EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + + // Test breaking limits sets to maximum + sequence.vx = 5.0 * xt::ones({50}); + sequence.vy = 5.0 * xt::ones({50}); + sequence.wz = 5.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, xt::ones({50})); + EXPECT_EQ(sequence.vy, 0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, 2.0 * xt::ones({50})); + + // Test breaking limits sets to minimum + sequence.vx = -5.0 * xt::ones({50}); + sequence.vy = -5.0 * xt::ones({50}); + sequence.wz = -5.0 * xt::ones({50}); + optimizer_tester.applyControlSequenceConstraintsWrapper(); + EXPECT_EQ(sequence.vx, -1.0 * xt::ones({50})); + EXPECT_EQ(sequence.vy, -0.75 * xt::ones({50})); + EXPECT_EQ(sequence.wz, -2.0 * xt::ones({50})); +} + +TEST(OptimizerTests, updateStateVelocitiesTests) +{ + auto node = std::make_shared("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( + "dummy_costmap", "", "dummy_costmap", true); + 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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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({10}); + sequence.vy = 0.5 * xt::ones({10}); + sequence.wz = 0.1 * xt::ones({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("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( + "dummy_costmap", "", "dummy_costmap", true); + 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({1000, 50}); + xt::view(state.vx, xt::all(), 0) = xt::zeros({1000}); + state.vy = xt::zeros({1000, 50}); + state.wz = xt::zeros({1000, 50}); + + optimizer_tester.integrateStateVelocitiesWrapper(traj, state); + EXPECT_EQ(traj.y, xt::zeros({1000, 50})); + EXPECT_EQ(traj.yaws, xt::zeros({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({1000, 50}); + xt::view(state.vy, xt::all(), 0) = xt::zeros({1000}); + optimizer_tester.integrateStateVelocitiesWrapper(traj, state); + + EXPECT_EQ(traj.yaws, xt::zeros({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({1000, 50}); + state.wz = 0.2 * xt::ones({1000, 50}); + xt::view(state.wz, xt::all(), 0) = xt::zeros({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); + } +} diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp new file mode 100644 index 0000000000..bcc3208e78 --- /dev/null +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -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 +#include + +#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 + auto asWrapped(rclcpp::Parameter parameter) + { + return ParametersHandler::as(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{1})); + rclcpp::Parameter doublev_p( + "doublev_parameter", rclcpp::ParameterValue(std::vector{10.0})); + rclcpp::Parameter boolv_p("boolv_parameter", rclcpp::ParameterValue(std::vector{false})); + rclcpp::Parameter stringv_p( + "stringv_parameter", rclcpp::ParameterValue(std::vector{std::string("hello")})); + + EXPECT_EQ(a.asWrapped(int_p), 1); + EXPECT_EQ(a.asWrapped(double_p), 10.0); + EXPECT_EQ(a.asWrapped(bool_p), false); + EXPECT_EQ(a.asWrapped(string_p), std::string("hello")); + + EXPECT_EQ(a.asWrapped>(intv_p)[0], 1); + EXPECT_EQ(a.asWrapped>(doublev_p)[0], 10.0); + EXPECT_EQ(a.asWrapped>(boolv_p)[0], false); + EXPECT_EQ(a.asWrapped>(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{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{random_param2}); + EXPECT_TRUE(dynamic_triggered); + EXPECT_TRUE(pre_triggered); + EXPECT_TRUE(post_triggered); +} + +TEST(ParameterHandlerTest, GetSystemParamsTest) +{ + auto node = std::make_shared("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("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( + 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); +} diff --git a/nav2_mppi_controller/test/path_handler_test.cpp b/nav2_mppi_controller/test/path_handler_test.cpp new file mode 100644 index 0000000000..3eb737ed36 --- /dev/null +++ b/nav2_mppi_controller/test/path_handler_test.cpp @@ -0,0 +1,191 @@ +// 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 +#include + +#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(const PathIterator end) + { + return pruneGlobalPlan(end); + } + + double getMaxCostmapDistWrapper() + { + return getMaxCostmapDist(); + } + + std::pair + 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); + } +}; + +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(it); + auto rtn2_path = handler.getPath(); + EXPECT_EQ(rtn2_path.poses.size(), 6u); +} + +TEST(PathHandlerTests, TestBounds) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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 tf_broadcaster_ = + std::make_unique(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_in = handler.getPath(); + EXPECT_EQ(closest - path_in.poses.begin(), 25); + handler.pruneGlobalPlanWrapper(closest); + auto & path_pruned = handler.getPath(); + EXPECT_EQ(path_pruned.poses.size(), 75u); +} + +TEST(PathHandlerTests, TestTransforms) +{ + PathHandlerWrapper handler; + auto node = std::make_shared("my_node"); + node->declare_parameter("dummy.max_robot_pose_search_dist", rclcpp::ParameterValue(99999.9)); + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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 tf_broadcaster_ = + std::make_unique(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()); +} diff --git a/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp new file mode 100644 index 0000000000..7417856cae --- /dev/null +++ b/nav2_mppi_controller/test/trajectory_visualizer_tests.cpp @@ -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 +#include + +#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("my_node"); + auto parameters_handler = std::make_unique(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("my_node"); + auto parameters_handler = std::make_unique(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( + "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("my_node"); + auto parameters_handler = std::make_unique(node); + + visualization_msgs::msg::MarkerArray recieved_msg; + auto my_sub = node->create_subscription( + "/trajectories", 10, + [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + + // optimal_trajectory empty, should fail to publish + xt::xtensor optimal_trajectory; + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(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({20, 2}); + vis.add(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("my_node"); + auto parameters_handler = std::make_unique(node); + + visualization_msgs::msg::MarkerArray recieved_msg; + auto my_sub = node->create_subscription( + "/trajectories", 10, + [&](const visualization_msgs::msg::MarkerArray msg) {recieved_msg = msg;}); + + models::Trajectories candidate_trajectories; + candidate_trajectories.x = xt::ones({200, 12}); + candidate_trajectories.y = xt::ones({200, 12}); + candidate_trajectories.yaws = xt::ones({200, 12}); + + TrajectoryVisualizer vis; + vis.on_configure(node, "my_name", "fkmap", parameters_handler.get()); + vis.on_activate(); + vis.add(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); +} diff --git a/nav2_mppi_controller/test/utils/factory.hpp b/nav2_mppi_controller/test/utils/factory.hpp new file mode 100644 index 0000000000..2940465184 --- /dev/null +++ b/nav2_mppi_controller/test/utils/factory.hpp @@ -0,0 +1,237 @@ +// 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 +#include +#include + +#include "nav2_costmap_2d/costmap_2d.hpp" +#include "nav2_costmap_2d/costmap_2d_ros.hpp" + +#include +#include +#include +#include +#include + +#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 +{ +auto setHeader(auto && msg, auto 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 & critics, + std::vector & 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 & 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 & critics) +{ + std::vector 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 getDummyCostmapRos() +{ + auto costmap_ros = std::make_shared("cost_map_node"); + costmap_ros->on_configure(rclcpp_lifecycle::State{}); + return costmap_ros; +} + +std::shared_ptr getDummyCostmap(TestCostmapSettings s) +{ + auto costmap = std::make_shared( + s.cells_x, s.cells_y, s.resolution, s.origin_x, s.origin_y, s.cost_map_default_value); + + return costmap; +} + +std::vector getDummySquareFootprint(double a) +{ + return {getDummyPoint(a, a), getDummyPoint(-a, -a), getDummyPoint(a, -a), getDummyPoint(-a, a)}; +} + +std::shared_ptr 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 +getDummyNode( + TestOptimizerSettings s, std::vector critics, + std::string node_name = std::string("dummy")) +{ + auto node = + std::make_shared(node_name, getOptimizerOptions(s, critics)); + return node; +} + +std::shared_ptr +getDummyNode(rclcpp::NodeOptions options, std::string node_name = std::string("dummy")) +{ + auto node = std::make_shared(node_name, options); + return node; +} + +std::shared_ptr getDummyOptimizer( + auto node, auto costmap_ros, + auto * params_handler) +{ + std::shared_ptr optimizer = std::make_shared(); + std::weak_ptr weak_ptr_node{node}; + + optimizer->initialize(weak_ptr_node, node->get_name(), costmap_ros, params_handler); + + return optimizer; +} + +mppi::PathHandler getDummyPathHandler( + auto node, auto costmap_ros, auto tf_buffer, + auto * params_handler) +{ + mppi::PathHandler path_handler; + std::weak_ptr weak_ptr_node{node}; + + path_handler.initialize(weak_ptr_node, node->get_name(), costmap_ros, tf_buffer, params_handler); + + return path_handler; +} + +std::shared_ptr getDummyController( + auto node, auto tf_buffer, + auto costmap_ros) +{ + auto controller = std::make_shared(); + std::weak_ptr 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; +} + +geometry_msgs::msg::PoseStamped +getDummyPointStamped(auto & node, std::string frame = std::string("odom")) +{ + geometry_msgs::msg::PoseStamped point; + detail::setHeader(point, node, frame); + + return point; +} + +geometry_msgs::msg::PoseStamped getDummyPointStamped(auto & node, TestPose pose) +{ + geometry_msgs::msg::PoseStamped point = getDummyPointStamped(node); + point.pose.position.x = pose.x; + point.pose.position.y = pose.y; + + return point; +} + +nav_msgs::msg::Path getDummyPath(auto node, std::string frame = std::string("odom")) +{ + nav_msgs::msg::Path path; + detail::setHeader(path, node, frame); + return path; +} + +auto getDummyPath(size_t points_count, auto node) +{ + auto path = getDummyPath(node); + + for (size_t i = 0; i < points_count; i++) { + path.poses.push_back(getDummyPointStamped(node)); + } + + return path; +} + +nav_msgs::msg::Path getIncrementalDummyPath(auto 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(i) * s.step_x; + double y = s.start_pose.y + static_cast(i) * s.step_y; + path.poses.push_back(getDummyPointStamped(node, TestPose{x, y})); + } + + return path; +} diff --git a/nav2_mppi_controller/test/utils/models.hpp b/nav2_mppi_controller/test/utils/models.hpp new file mode 100644 index 0000000000..fa819efe77 --- /dev/null +++ b/nav2_mppi_controller/test/utils/models.hpp @@ -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 +#include +#include +#include + +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 getCenterIJ() + { + return { + cells_x / 2, + cells_y / 2}; + } + + TestPose getCenterPose() + { + return { + static_cast(cells_x) * resolution / 2.0, + static_cast(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; +}; diff --git a/nav2_mppi_controller/test/utils/utils.hpp b/nav2_mppi_controller/test/utils/utils.hpp new file mode 100644 index 0000000000..4c59089ef8 --- /dev/null +++ b/nav2_mppi_controller/test/utils/utils.hpp @@ -0,0 +1,244 @@ +// 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 +#include +#include +#include +#include +#include + +#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 + +void waitSome(const std::chrono::nanoseconds & duration, auto & 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 tf_broadcaster, + std::shared_ptr 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(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. + */ +void printMapWithTrajectoryAndGoal( + nav2_costmap_2d::Costmap2D & costmap, const auto & 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 & 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 + */ +bool inCollision(const auto & 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); +} + +bool isGoalReached( + const auto & 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; +} diff --git a/nav2_mppi_controller/test/utils_test.cpp b/nav2_mppi_controller/test/utils_test.cpp new file mode 100644 index 0000000000..bd5a010448 --- /dev/null +++ b/nav2_mppi_controller/test/utils_test.cpp @@ -0,0 +1,357 @@ +// 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 +#include + +#include +#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/*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"); + 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); +} + +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; + + // Test not in tolerance + nav_msgs::msg::Path path; + path.poses.resize(2); + path.poses[1].pose.position.x = 0.0; + path.poses[1].pose.position.y = 0.0; + models::Path path_t = toTensor(path); + EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, path_t)); + EXPECT_FALSE(withinPositionGoalTolerance(0.25, pose, path_t)); + + // Test in tolerance + path.poses[1].pose.position.x = 9.8; + path.poses[1].pose.position.y = 0.95; + path_t = toTensor(path); + EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + + path.poses[1].pose.position.x = 10.0; + path.poses[1].pose.position.y = 0.76; + path_t = toTensor(path); + EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + + path.poses[1].pose.position.x = 9.76; + path.poses[1].pose.position.y = 1.0; + path_t = toTensor(path); + EXPECT_TRUE(withinPositionGoalTolerance(goal_checker, pose, path_t)); + EXPECT_TRUE(withinPositionGoalTolerance(0.25, pose, path_t)); + + delete goal_checker; + goal_checker = nullptr; + EXPECT_FALSE(withinPositionGoalTolerance(goal_checker, pose, path_t)); +} + +TEST(UtilsTests, AnglesTests) +{ + // Test angle normalization by creating insane angles + xt::xtensor angles, zero_angles; + angles = xt::ones({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({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; + EXPECT_NEAR(posePointAngle(pose, point_x, point_y), 0.0, 1e-6); +} + +TEST(UtilsTests, FurthestAndClosestReachedPoint) +{ + models::State state; + models::Trajectories generated_trajectories; + models::Path path; + xt::xtensor costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, 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, 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({100, 2}); + generated_trajectories.y = xt::zeros({100, 2}); + generated_trajectories.yaws = xt::zeros({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, 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; + xt::xtensor costs; + float model_dt = 0.1; + + CriticData data = + {state, generated_trajectories, path, 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(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, costs, model_dt, false, nullptr, nullptr, + std::nullopt, std::nullopt}; /// Caution, keep references + + auto costmap_ros = std::make_shared( + "dummy_costmap", "", "dummy_costmap", true); + 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({30}); + noisey_sequence.vy = 0.0 * xt::ones({30}); + noisey_sequence.wz = 0.3 * xt::ones({30}); + + // Make the sequence noisey + auto noises = xt::random::randn({30}, 0.0, 0.2); + noisey_sequence.vx += noises; + noisey_sequence.vy += noises; + noisey_sequence.wz += noises; + sequence_init = noisey_sequence; + + std::array history, history_init; + 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[1].vx, history[0].vx, 0.02); + EXPECT_NEAR(history_init[1].vy, history[0].vy, 0.02); + EXPECT_NEAR(history_init[1].wz, history[0].wz, 0.02); + + // Check history element is updated for first command + EXPECT_NEAR(history[1].vx, 0.2, 0.05); + EXPECT_NEAR(history[1].vy, 0.0, 0.02); + EXPECT_NEAR(history[1].wz, 0.23, 0.02); + + // Check that path is smoother + float smoothed_val, original_val; + 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); +} diff --git a/navigation2/package.xml b/navigation2/package.xml index 8475920c57..94a3593024 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -25,6 +25,7 @@ nav2_lifecycle_manager nav2_map_server nav2_msgs + nav2_mppi_controller nav2_navfn_planner nav2_planner nav2_behaviors