fixed few review comments

Signed-off-by: Ayush1285 <[email protected]>

nav2_mppi_controller/benchmark/optimizer_benchmark.cpp

@@ -116,7 +116,8 @@ static void BM_Omni(benchmark::State & state)
 {
   bool consider_footprint = true;
   std::string motion_model = "Omni";
-  std::vector<std::string> critics = {};
+  std::vector<std::string> critics = {{"GoalCritic"}, {"GoalAngleCritic"}, {"ObstaclesCritic"},
+    {"PathAngleCritic"}, {"PathFollowCritic"}, {"PreferForwardCritic"}};
 
   prepareAndRunBenchmark(consider_footprint, motion_model, critics, state);
 }

nav2_mppi_controller/include/nav2_mppi_controller/controller.hpp

@@ -20,7 +20,7 @@
 
 #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/tools/trajectory_visualizer.hpp"
 #include "nav2_mppi_controller/models/constraints.hpp"
 #include "nav2_mppi_controller/tools/utils.hpp"
 
@@ -117,7 +117,7 @@ class MPPIController : public nav2_core::Controller
   std::unique_ptr<ParametersHandler> parameters_handler_;
   Optimizer optimizer_;
   PathHandler path_handler_;
-  //TrajectoryVisualizer trajectory_visualizer_;
+  TrajectoryVisualizer trajectory_visualizer_;
 
   bool visualize_;
 };

nav2_mppi_controller/src/controller.cpp

@@ -41,31 +41,31 @@ void MPPIController::configure(
   // 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());
+  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();
+  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();
+  trajectory_visualizer_.on_activate();
   parameters_handler_->start();
   RCLCPP_INFO(logger_, "Activated MPPI Controller: %s", name_.c_str());
 }
 
 void MPPIController::deactivate()
 {
-  //trajectory_visualizer_.on_deactivate();
+  trajectory_visualizer_.on_deactivate();
   RCLCPP_INFO(logger_, "Deactivated MPPI Controller: %s", name_.c_str());
 }
 
@@ -105,11 +105,11 @@ geometry_msgs::msg::TwistStamped MPPIController::computeVelocityCommands(
   return cmd;
 }
 
-void MPPIController::visualize(nav_msgs::msg::Path /*transformed_plan*/)
+void MPPIController::visualize(nav_msgs::msg::Path transformed_plan)
 {
-  //trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
-  //trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
-  //trajectory_visualizer_.visualize(std::move(transformed_plan));
+  trajectory_visualizer_.add(optimizer_.getGeneratedTrajectories(), "Candidate Trajectories");
+  trajectory_visualizer_.add(optimizer_.getOptimizedTrajectory(), "Optimal Trajectory");
+  trajectory_visualizer_.visualize(std::move(transformed_plan));
 }
 
 void MPPIController::setPlan(const nav_msgs::msg::Path & path)

nav2_mppi_controller/src/optimizer.cpp

@@ -169,7 +169,6 @@ geometry_msgs::msg::TwistStamped Optimizer::evalControl(
 void Optimizer::optimize()
 {
   for (size_t i = 0; i < settings_.iteration_count; ++i) {
-
     generateNoisedTrajectories();
     critic_manager_.evalTrajectoriesScores(critics_data_);
     updateControlSequence();
@@ -233,32 +232,28 @@ void Optimizer::generateNoisedTrajectories()
 void Optimizer::applyControlSequenceConstraints()
 {
   auto & s = settings_;
-  float & vx_min = s.constraints.vx_min;
-  float & vx_max = s.constraints.vx_max;
-  float & vy = s.constraints.vy;
-  float & wz = s.constraints.wz;
-
-  float && max_delta_vx = s.model_dt * s.constraints.ax_max;
-  float && min_delta_vx = s.model_dt * s.constraints.ax_min;
-  float && max_delta_vy = s.model_dt * s.constraints.ay_max;
-  float && max_delta_wz = s.model_dt * s.constraints.az_max;
-  float vx_last = std::min(vx_max, std::max(control_sequence_.vx(0), vx_min));
-  float vy_last = std::min(vy, std::max(control_sequence_.vy(0), -vy));
-  float wz_last = std::min(wz, std::max(control_sequence_.wz(0), -wz));
+
+  float max_delta_vx = s.model_dt * s.constraints.ax_max;
+  float min_delta_vx = s.model_dt * s.constraints.ax_min;
+  float max_delta_vy = s.model_dt * s.constraints.ay_max;
+  float max_delta_wz = s.model_dt * s.constraints.az_max;
+  float vx_last = std::min(s.constraints.vx_max, std::max(control_sequence_.vx(0), s.constraints.vx_min));
+  float vy_last = std::min(s.constraints.vy, std::max(control_sequence_.vy(0), -s.constraints.vy));
+  float wz_last = std::min(s.constraints.wz, std::max(control_sequence_.wz(0), -s.constraints.wz));
   for (unsigned int i = 1; i != control_sequence_.vx.size(); i++) {
     float & vx_curr = control_sequence_.vx(i);
-    vx_curr = std::min(vx_max, std::max(vx_curr, vx_min));
+    vx_curr = std::min(s.constraints.vx_max, std::max(vx_curr, s.constraints.vx_min));
     vx_curr = std::min(vx_last + max_delta_vx, std::max(vx_curr, vx_last + min_delta_vx));
     vx_last = vx_curr;
 
     float & wz_curr = control_sequence_.wz(i);
-    wz_curr = std::min(wz, std::max(wz_curr, -wz));
+    wz_curr = std::min(s.constraints.wz, std::max(wz_curr, -s.constraints.wz));
     wz_curr = std::min(wz_last + max_delta_wz, std::max(wz_curr, wz_last - max_delta_wz));
     wz_last = wz_curr;
 
     if (isHolonomic()) {
       float & vy_curr = control_sequence_.vy(i);
-      vy_curr = std::min(vy, std::max(vy_curr, -vy));
+      vy_curr = std::min(s.constraints.vy, std::max(vy_curr, -s.constraints.vy));
       vy_curr = std::min(vy_last + max_delta_vy, std::max(vy_curr, vy_last - max_delta_vy));
       vy_last = vy_curr;
     }
@@ -292,11 +287,10 @@ void Optimizer::propagateStateVelocitiesFromInitials(
 }
 
 void Optimizer::integrateStateVelocities(
-  Eigen::ArrayXXf & trajectory,
+  Eigen::Array<float, -1, 3> & trajectory,
   const Eigen::ArrayXXf & sequence) const
 {
   float initial_yaw = static_cast<float>(tf2::getYaw(state_.pose.pose.orientation));
-  const float & dt_time = settings_.model_dt;
 
   const auto vx = sequence.col(0);
   const auto wz = sequence.col(1);
@@ -305,19 +299,19 @@ void Optimizer::integrateStateVelocities(
   auto traj_y = trajectory.col(1);
   auto traj_yaws = trajectory.col(2);
 
-  unsigned int n_size = traj_yaws.size();
+  size_t n_size = traj_yaws.size();
 
-  traj_yaws(0) = wz(0) * dt_time + initial_yaw;
+  traj_yaws(0) = wz(0) * settings_.model_dt + initial_yaw;
   float last_yaw = traj_yaws(0);
-  for(unsigned int i = 1; i != n_size; i++)
+  for(size_t i = 1; i != n_size; i++)
   {
     float & curr_yaw = traj_yaws(i);
-    curr_yaw = last_yaw + wz(i) * dt_time;
+    curr_yaw = last_yaw + wz(i) * settings_.model_dt;
     last_yaw = curr_yaw;
   }
 
-  auto yaw_cos = (utils::rollColumns(traj_yaws, -1).cos()).eval();
-  auto yaw_sin = (utils::rollColumns(traj_yaws, -1).sin()).eval();
+  Eigen::ArrayXf yaw_cos = utils::rollColumns(traj_yaws, -1).cos();
+  Eigen::ArrayXf yaw_sin = utils::rollColumns(traj_yaws, -1).sin();
   yaw_cos(0) = cosf(initial_yaw);
   yaw_sin(0) = sinf(initial_yaw);
 
@@ -330,16 +324,16 @@ void Optimizer::integrateStateVelocities(
     dy = (dy + vy * yaw_cos).eval();
   }
 
-  traj_x(0) = state_.pose.pose.position.x + dx(0) * dt_time;
-  traj_y(0) = state_.pose.pose.position.y + dy(0) * dt_time;
+  traj_x(0) = state_.pose.pose.position.x + dx(0) * settings_.model_dt;
+  traj_y(0) = state_.pose.pose.position.y + dy(0) * settings_.model_dt;
   float last_x = traj_x(0);
   float last_y = traj_y(0);
   for(unsigned int i = 1; i != n_size; i++)
   {
     float & curr_x = traj_x(i);
     float & curr_y = traj_y(i);
-    curr_x = last_x + dx(i) * dt_time;
-    curr_y = last_y + dy(i) * dt_time;
+    curr_x = last_x + dx(i) * settings_.model_dt;
+    curr_y = last_y + dy(i) * settings_.model_dt;
     last_x = curr_x;
     last_y = curr_y;
   }
@@ -351,12 +345,11 @@ void Optimizer::integrateStateVelocities(
 {
   const float initial_yaw = static_cast<float>(tf2::getYaw(state.pose.pose.orientation));
   const unsigned int n_cols = trajectories.yaws.cols();
-  const float & dt_time = settings_.model_dt;
 
-  trajectories.yaws.col(0) = state.wz.col(0) * dt_time + initial_yaw;
+  trajectories.yaws.col(0) = state.wz.col(0) * settings_.model_dt + initial_yaw;
   for(unsigned int i = 1; i != n_cols; i++)
   {
-    trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * dt_time;
+    trajectories.yaws.col(i) = trajectories.yaws.col(i - 1) + state.wz.col(i) * settings_.model_dt;
   }
 
   auto yaw_cos = (utils::rollColumns(trajectories.yaws, -1).cos()).eval();
@@ -373,20 +366,20 @@ void Optimizer::integrateStateVelocities(
     dy = dy + state.vy * yaw_cos;
   }
 
-  trajectories.x.col(0) = dx.col(0) * dt_time + state.pose.pose.position.x;
-  trajectories.y.col(0) = dy.col(0) * dt_time + state.pose.pose.position.y;
+  trajectories.x.col(0) = dx.col(0) * settings_.model_dt + state.pose.pose.position.x;
+  trajectories.y.col(0) = dy.col(0) * settings_.model_dt + state.pose.pose.position.y;
   for(unsigned int i = 1; i != n_cols; i++)
   {
-    trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * dt_time;
-    trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * dt_time;
+    trajectories.x.col(i) = trajectories.x.col(i - 1) + dx.col(i) * settings_.model_dt;
+    trajectories.y.col(i) = trajectories.y.col(i - 1) + dy.col(i) * settings_.model_dt;
   }
 }
 
 Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
 {
   const bool is_holo = isHolonomic();
-  auto && sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
-  auto && trajectories = Eigen::ArrayXXf(settings_.time_steps, 3);
+  Eigen::ArrayXXf sequence = Eigen::ArrayXXf(settings_.time_steps, is_holo ? 3 : 2);
+  Eigen::Array<float, -1, 3> trajectories = Eigen::Array<float, -1, 3>(settings_.time_steps, 3);
 
   sequence.col(0) = control_sequence_.vx;
   sequence.col(1) = control_sequence_.wz;
@@ -396,7 +389,7 @@ Eigen::ArrayXXf Optimizer::getOptimizedTrajectory()
   }
 
   integrateStateVelocities(trajectories, sequence);
-  return std::move(trajectories);
+  return trajectories;
 }
 
 void Optimizer::updateControlSequence()