remove ABI breaking changes

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()

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.

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()

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 <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();