[WIP] Improve testability to explore velocity command generation for an Ackermann Steered Polaris

.gitignore

@@ -54,3 +54,20 @@ cmake-build-debug/
 
 # doxygen docs
 doc/html/
+
+#Vim
+# Vim Swap
+[._]*.s[a-v][a-z]
+[._]*.sw[a-p]
+[._]s[a-rt-v][a-z]
+[._]ss[a-gi-z]
+[._]sw[a-p]
+
+# Vim Persistent undo
+[._]*.un~
+
+# Vim Session
+Session.vim
+
+# Vim Temporary
+.netrwhist

nav2_mppi_controller/benchmark/controller_benchmark.cpp

@@ -46,13 +46,13 @@ void prepareAndRunBenchmark(
   bool consider_footprint, std::string motion_model,
   std::vector<std::string> critics, benchmark::State & state)
 {
+  double controller_frequency = 50.0;
   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);
@@ -62,8 +62,9 @@ void prepareAndRunBenchmark(
   double path_step = costmap_settings.resolution;
 
   TestPathSettings path_settings{start_pose, path_points, path_step, path_step};
+  TestControllerSettings controller_settings{controller_frequency, visualize};
   TestOptimizerSettings optimizer_settings{batch_size, time_steps, iteration_count,
-    lookahead_distance, motion_model, consider_footprint};
+    motion_model, consider_footprint};
 
   unsigned int offset = 4;
   unsigned int obstacle_size = offset * 2;
@@ -80,7 +81,7 @@ void prepareAndRunBenchmark(
 
   rclcpp::NodeOptions options;
   std::vector<rclcpp::Parameter> params;
-  setUpControllerParams(visualize, params);
+  setUpControllerParams(controller_settings, params);
   setUpOptimizerParams(optimizer_settings, critics, params);
   options.parameter_overrides(params);
   auto node = getDummyNode(options);

nav2_mppi_controller/benchmark/optimizer_benchmark.cpp

@@ -53,7 +53,6 @@ void prepareAndRunBenchmark(
   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);
@@ -64,7 +63,7 @@ void prepareAndRunBenchmark(
 
   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};
+    motion_model, consider_footprint};
 
   unsigned int offset = 4;
   unsigned int obstacle_size = offset * 2;

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -120,6 +120,8 @@ class MPPIController : public nav2_core::Controller
   TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
+  bool visualize_generated_trajectories_;
+  bool visualize_optimal_trajectory_;
 };
 
 }  // namespace nav2_mppi_controller

nav2_mppi_controller/include/nav2_mppi_controller/critic_manager.hpp

@@ -48,7 +48,6 @@ class CriticManager
     */
   CriticManager() = default;
 
-
   /**
     * @brief Virtual Destructor for mppi::CriticManager
     */
@@ -65,6 +64,11 @@ class CriticManager
     rclcpp_lifecycle::LifecycleNode::WeakPtr parent, const std::string & name,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS>, ParametersHandler *);
 
+  /**
+    * @brief reinitialise critic plugins to load new params
+    */
+  virtual void reInitializeCritics();
+
   /**
     * @brief Score trajectories by the set of loaded critic functions
     * @param CriticData Struct of necessary information to pass to the critic functions

nav2_mppi_controller/include/nav2_mppi_controller/models/control_sequence.hpp

@@ -16,6 +16,7 @@
 #define NAV2_MPPI_CONTROLLER__MODELS__CONTROL_SEQUENCE_HPP_
 
 #include <xtensor/xtensor.hpp>
+#include <xtensor/xnoalias.hpp>
 
 namespace mppi::models
 {
@@ -27,6 +28,13 @@ namespace mppi::models
 struct Control
 {
   float vx, vy, wz;
+
+  void reset()
+  {
+    vx = 0.0f;
+    vy = 0.0f;
+    wz = 0.0f;
+  }
 };
 
 /**
@@ -41,9 +49,9 @@ struct ControlSequence
 
   void reset(unsigned int time_steps)
   {
-    vx = xt::zeros<float>({time_steps});
-    vy = xt::zeros<float>({time_steps});
-    wz = xt::zeros<float>({time_steps});
+    xt::noalias(vx) = xt::zeros<float>({time_steps});
+    xt::noalias(vy) = xt::zeros<float>({time_steps});
+    xt::noalias(wz) = xt::zeros<float>({time_steps});
   }
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/models/path.hpp

@@ -16,6 +16,7 @@
 #define NAV2_MPPI_CONTROLLER__MODELS__PATH_HPP_
 
 #include <xtensor/xtensor.hpp>
+#include <xtensor/xnoalias.hpp>
 
 namespace mppi::models
 {
@@ -35,9 +36,9 @@ struct Path
     */
   void reset(unsigned int size)
   {
-    x = xt::zeros<float>({size});
-    y = xt::zeros<float>({size});
-    yaws = xt::zeros<float>({size});
+    xt::noalias(x) = xt::zeros<float>({size});
+    xt::noalias(y) = xt::zeros<float>({size});
+    xt::noalias(yaws) = xt::zeros<float>({size});
   }
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/models/state.hpp

@@ -16,6 +16,7 @@
 #define NAV2_MPPI_CONTROLLER__MODELS__STATE_HPP_
 
 #include <xtensor/xtensor.hpp>
+#include <xtensor/xnoalias.hpp>
 
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/twist.hpp>
@@ -45,13 +46,33 @@ struct State
     */
   void reset(unsigned int batch_size, unsigned int time_steps)
   {
-    vx = xt::zeros<float>({batch_size, time_steps});
-    vy = xt::zeros<float>({batch_size, time_steps});
-    wz = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(vx) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(vy) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(wz) = xt::zeros<float>({batch_size, time_steps});
 
-    cvx = xt::zeros<float>({batch_size, time_steps});
-    cvy = xt::zeros<float>({batch_size, time_steps});
-    cwz = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(cvx) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(cvy) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(cwz) = xt::zeros<float>({batch_size, time_steps});
+
+    pose.header.stamp.sec = 0;
+    pose.header.stamp.nanosec = 0;
+    pose.header.frame_id = "";
+
+    pose.pose.position.x = 0.0;
+    pose.pose.position.y = 0.0;
+    pose.pose.position.z = 0.0;
+
+    pose.pose.orientation.x = 0.0;
+    pose.pose.orientation.y = 0.0;
+    pose.pose.orientation.z = 0.0;
+    pose.pose.orientation.w = 1.0;
+
+    speed.linear.x = 0.0;
+    speed.linear.y = 0.0;
+    speed.linear.z = 0.0;
+    speed.angular.x = 0.0;
+    speed.angular.y = 0.0;
+    speed.angular.z = 0.0;
   }
 };
 }  // namespace mppi::models

nav2_mppi_controller/include/nav2_mppi_controller/models/trajectories.hpp

@@ -17,6 +17,7 @@
 
 #include <xtensor/xtensor.hpp>
 #include <xtensor/xview.hpp>
+#include <xtensor/xnoalias.hpp>
 
 namespace mppi::models
 {
@@ -36,9 +37,9 @@ struct Trajectories
     */
   void reset(unsigned int batch_size, unsigned int time_steps)
   {
-    x = xt::zeros<float>({batch_size, time_steps});
-    y = xt::zeros<float>({batch_size, time_steps});
-    yaws = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(x) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(y) = xt::zeros<float>({batch_size, time_steps});
+    xt::noalias(yaws) = xt::zeros<float>({batch_size, time_steps});
   }
 };
 

nav2_mppi_controller/include/nav2_mppi_controller/tools/noise_generator.hpp

@@ -105,6 +105,8 @@ class NoiseGenerator
   std::condition_variable noise_cond_;
   std::mutex noise_lock_;
   bool active_{false}, ready_{false}, regenerate_noises_{false};
+  bool dump_noises_{false};
+  int noise_seed_{0};
 };
 
 }  // namespace mppi

nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp

@@ -205,7 +205,15 @@ void ParametersHandler::getParam(
   setParam<ParamT>(setting, name, node);
 
   if (param_type == ParameterType::Dynamic) {
+    if (verbose_) {
+      RCLCPP_INFO(node->get_logger(), "setDynamicParamCallback for %s", name.c_str());
+    }
     setDynamicParamCallback(setting, name);
+  } else {
+    if (verbose_) {
+      RCLCPP_DEBUG(
+        node->get_logger(), "Static so no setDynamicParamCallback for %s", name.c_str());
+    }
   }
 }
 

nav2_mppi_controller/src/controller.cpp

@@ -37,6 +37,8 @@ void MPPIController::configure(
   // Get high-level controller parameters
   auto getParam = parameters_handler_->getParamGetter(name_);
   getParam(visualize_, "visualize", false);
+  getParam(visualize_generated_trajectories_, "visualize_generated_trajectories", true);
+  getParam(visualize_optimal_trajectory_, "visualize_optimal_trajectory", true);
 
   // Configure composed objects
   optimizer_.initialize(parent_, name_, costmap_ros_, parameters_handler_.get());
@@ -107,8 +109,12 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
 
 void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
 {
-  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
+  if (visualize_generated_trajectories_) {
+    trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  }
+  if (visualize_optimal_trajectory_) {
+    trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
+  }
   trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 

nav2_mppi_controller/src/critic_manager.cpp

@@ -59,6 +59,13 @@ void CriticManager::loadCritics()
   }
 }
 
+void CriticManager::reInitializeCritics()
+{
+  for (auto & critic : critics_) {
+    critic->initialize();
+  }
+}
+
 std::string CriticManager::getFullName(const std::string & name)
 {
   return "mppi::critics::" + name;

nav2_mppi_controller/src/noise_generator.cpp

@@ -16,9 +16,13 @@
 
 #include <memory>
 #include <mutex>
+#include <fstream>
+#include <iostream>
 #include <xtensor/xmath.hpp>
 #include <xtensor/xrandom.hpp>
 #include <xtensor/xnoalias.hpp>
+#include <xtensor/xcsv.hpp>
+
 
 namespace mppi
 {
@@ -33,6 +37,12 @@ void NoiseGenerator::initialize(
 
   auto getParam = param_handler->getParamGetter(name);
   getParam(regenerate_noises_, "regenerate_noises", false);
+  getParam(dump_noises_, "dump_noises", true);
+  getParam(noise_seed_, "noise_seed", 0);
+
+  if (noise_seed_ != 0) {
+    xt::random::seed(noise_seed_);
+  }
 
   if (regenerate_noises_) {
     noise_thread_ = std::thread(std::bind(&NoiseGenerator::noiseThread, this));
@@ -119,6 +129,14 @@ void NoiseGenerator::generateNoisedControls()
       {s.batch_size, s.time_steps}, 0.0f,
       s.sampling_std.vy);
   }
+
+  if (dump_noises_) {
+    std::string fn("/tmp/mppi_noises_");
+    std::ofstream f(fn + "vx_" + std::to_string(s.sampling_std.vx) + ".csv",
+      std::ios::out | std::ios::trunc);
+    xt::dump_csv(f, noises_vx_);
+    dump_noises_ = false;
+  }
 }
 
 }  // namespace mppi

nav2_mppi_controller/src/optimizer.cpp

@@ -118,17 +118,16 @@ void Optimizer::reset()
 {
   state_.reset(settings_.batch_size, settings_.time_steps);
   control_sequence_.reset(settings_.time_steps);
-  control_history_[0] = {0.0f, 0.0f, 0.0f};
-  control_history_[1] = {0.0f, 0.0f, 0.0f};
-  control_history_[2] = {0.0f, 0.0f, 0.0f};
-  control_history_[3] = {0.0f, 0.0f, 0.0f};
-
+  for (auto & ch : control_history_) {
+    ch.reset();
+  }
   settings_.constraints = settings_.base_constraints;
 
-  costs_ = xt::zeros<float>({settings_.batch_size});
+  xt::noalias(costs_) = xt::zeros<float>({settings_.batch_size});
   generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
 
   noise_generator_.reset(settings_, isHolonomic());
+  critic_manager_.reInitializeCritics();
   RCLCPP_INFO(logger_, "Optimizer reset");
 }
 
@@ -196,6 +195,9 @@ void Optimizer::prepare(
   path_ = utils::toTensor(plan);
   costs_.fill(0.0f);
 
+  // FIXME: Should we be resetting generated_trajectories_ here???
+  // generated_trajectories_.reset(settings_.batch_size, settings_.time_steps);
+
   critics_data_.fail_flag = false;
   critics_data_.goal_checker = goal_checker;
   critics_data_.motion_model = motion_model_;
@@ -353,21 +355,20 @@ void Optimizer::updateControlSequence()
 {
   const bool is_holo = isHolonomic();
   auto & s = settings_;
-  auto bounded_noises_vx = state_.cvx - control_sequence_.vx;
-  auto bounded_noises_wz = state_.cwz - control_sequence_.wz;
   xt::noalias(costs_) +=
-    s.gamma / powf(s.sampling_std.vx, 2) * xt::sum(
-    xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) * bounded_noises_vx, 1, immediate);
+    s.gamma / powf(s.sampling_std.vx, 2) *
+    xt::sum(xt::view(control_sequence_.vx, xt::newaxis(), xt::all()) *
+    (state_.cvx - control_sequence_.vx), 1, immediate);
   xt::noalias(costs_) +=
-    s.gamma / powf(s.sampling_std.wz, 2) * xt::sum(
-    xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) * bounded_noises_wz, 1, immediate);
+    s.gamma / powf(s.sampling_std.wz, 2) *
+    xt::sum(xt::view(control_sequence_.wz, xt::newaxis(), xt::all()) *
+    (state_.cwz - control_sequence_.wz), 1, immediate);
 
   if (is_holo) {
-    auto bounded_noises_vy = state_.cvy - control_sequence_.vy;
     xt::noalias(costs_) +=
       s.gamma / powf(s.sampling_std.vy, 2) * xt::sum(
-      xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) * bounded_noises_vy,
-      1, immediate);
+      xt::view(control_sequence_.vy, xt::newaxis(), xt::all()) *
+      (state_.cvy - control_sequence_.vy), 1, immediate);
   }
 
   auto && costs_normalized = costs_ - xt::amin(costs_, immediate);