-
Notifications
You must be signed in to change notification settings - Fork 1.3k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Tony Najjar
committed
Mar 2, 2023
1 parent
9b91662
commit ecc0c34
Showing
69 changed files
with
9,130 additions
and
0 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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. |
Large diffs are not rendered by default.
Oops, something went wrong.
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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() |
238 changes: 238 additions & 0 deletions
238
nav2_mppi_controller/benchmark/controller_benchmark.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,238 @@ | ||
// Copyright (c) 2022 Samsung Research America, @artofnothingness Alexey Budyakov | ||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
// You may obtain a copy of the License at | ||
// | ||
// http://www.apache.org/licenses/LICENSE-2.0 | ||
// | ||
// Unless required by applicable law or agreed to in writing, software | ||
// distributed under the License is distributed on an "AS IS" BASIS, | ||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
// See the License for the specific language governing permissions and | ||
// limitations under the License. | ||
|
||
#include <benchmark/benchmark.h> | ||
#include <string> | ||
|
||
#include <geometry_msgs/msg/pose_stamped.hpp> | ||
#include <geometry_msgs/msg/twist.hpp> | ||
#include <nav_msgs/msg/path.hpp> | ||
|
||
#include <nav2_costmap_2d/cost_values.hpp> | ||
#include <nav2_costmap_2d/costmap_2d.hpp> | ||
#include <nav2_costmap_2d/costmap_2d_ros.hpp> | ||
#include <nav2_core/goal_checker.hpp> | ||
|
||
#include <xtensor/xarray.hpp> | ||
#include <xtensor/xio.hpp> | ||
#include <xtensor/xview.hpp> | ||
|
||
#include "nav2_mppi_controller/motion_models.hpp" | ||
#include "nav2_mppi_controller/controller.hpp" | ||
|
||
#include "utils.hpp" | ||
|
||
class RosLockGuard | ||
{ | ||
public: | ||
RosLockGuard() {rclcpp::init(0, nullptr);} | ||
~RosLockGuard() {rclcpp::shutdown();} | ||
}; | ||
|
||
RosLockGuard g_rclcpp; | ||
|
||
void prepareAndRunBenchmark( | ||
bool consider_footprint, std::string motion_model, | ||
std::vector<std::string> critics, benchmark::State & state) | ||
{ | ||
bool visualize = false; | ||
|
||
int batch_size = 300; | ||
int time_steps = 12; | ||
unsigned int path_points = 50u; | ||
int iteration_count = 2; | ||
double lookahead_distance = 10.0; | ||
|
||
TestCostmapSettings costmap_settings{}; | ||
auto costmap_ros = getDummyCostmapRos(costmap_settings); | ||
auto costmap = costmap_ros->getCostmap(); | ||
|
||
TestPose start_pose = costmap_settings.getCenterPose(); | ||
double path_step = costmap_settings.resolution; | ||
|
||
TestPathSettings path_settings{start_pose, path_points, path_step, path_step}; | ||
TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count, | ||
lookahead_distance, motion_model, consider_footprint}; | ||
|
||
unsigned int offset = 4; | ||
unsigned int obstacle_size = offset * 2; | ||
|
||
unsigned char obstacle_cost = 250; | ||
|
||
auto [obst_x, obst_y] = costmap_settings.getCenterIJ(); | ||
|
||
obst_x = obst_x - offset; | ||
obst_y = obst_y - offset; | ||
addObstacle(costmap, {obst_x, obst_y, obstacle_size, obstacle_cost}); | ||
|
||
printInfo(optimizer_settings, path_settings, critics); | ||
|
||
rclcpp::NodeOptions options; | ||
std::vector<rclcpp::Parameter> params; | ||
setUpControllerParams(visualize, params); | ||
setUpOptimizerParams(optimizer_settings, critics, params); | ||
options.parameter_overrides(params); | ||
auto node = getDummyNode(options); | ||
|
||
auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock()); | ||
tf_buffer->setUsingDedicatedThread(true); // One-thread broadcasting-listening model | ||
|
||
auto broadcaster = | ||
std::make_shared<tf2_ros::TransformBroadcaster>(node); | ||
auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer); | ||
|
||
auto map_odom_broadcaster = std::async( | ||
std::launch::async, sendTf, "map", "odom", broadcaster, node, | ||
20); | ||
|
||
auto odom_base_link_broadcaster = std::async( | ||
std::launch::async, sendTf, "odom", "base_link", broadcaster, node, | ||
20); | ||
|
||
auto controller = getDummyController(node, tf_buffer, costmap_ros); | ||
|
||
// evalControl args | ||
auto pose = getDummyPointStamped(node, start_pose); | ||
auto velocity = getDummyTwist(); | ||
auto path = getIncrementalDummyPath(node, path_settings); | ||
|
||
controller->setPlan(path); | ||
|
||
nav2_core::GoalChecker * dummy_goal_checker{nullptr}; | ||
|
||
for (auto _ : state) { | ||
controller->computeVelocityCommands(pose, velocity, dummy_goal_checker); | ||
} | ||
map_odom_broadcaster.wait(); | ||
odom_base_link_broadcaster.wait(); | ||
} | ||
|
||
static void BM_DiffDrivePointFootprint(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "DiffDrive"; | ||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, | ||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_DiffDrive(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "DiffDrive"; | ||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, | ||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
|
||
static void BM_Omni(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Omni"; | ||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, | ||
{"TwirlingCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_Ackermann(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"}, | ||
{"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_GoalCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"GoalCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_GoalAngleCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"GoalAngleCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_ObstaclesCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"ObstaclesCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_ObstaclesCriticPointFootprint(benchmark::State & state) | ||
{ | ||
bool consider_footprint = false; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"ObstaclesCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_TwilringCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"TwirlingCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_PathFollowCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"PathFollowCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
static void BM_PathAngleCritic(benchmark::State & state) | ||
{ | ||
bool consider_footprint = true; | ||
std::string motion_model = "Ackermann"; | ||
std::vector<std::string> critics = {{"PathAngleCritic"}}; | ||
|
||
prepareAndRunBenchmark(consider_footprint, motion_model, critics, state); | ||
} | ||
|
||
BENCHMARK(BM_DiffDrivePointFootprint)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_DiffDrive)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_Omni)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_Ackermann)->Unit(benchmark::kMillisecond); | ||
|
||
BENCHMARK(BM_GoalCritic)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_GoalAngleCritic)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_PathAngleCritic)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_PathFollowCritic)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_ObstaclesCritic)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_ObstaclesCriticPointFootprint)->Unit(benchmark::kMillisecond); | ||
BENCHMARK(BM_TwilringCritic)->Unit(benchmark::kMillisecond); | ||
|
||
BENCHMARK_MAIN(); |
Oops, something went wrong.