update

include/mppic/models/optimizer_settings.hpp

@@ -2,7 +2,7 @@
 #pragma once
 
 #include "mppic/models/constraints.hpp"
-#include "optional"
+#include <cstddef>
 
 namespace mppi::models
 {
@@ -19,9 +19,6 @@ struct OptimizerSettings
   unsigned int iteration_count{0};
   bool shift_control_sequence{false};
   size_t retry_attempt_limit{0};
-
-
-  std::optional<AckermannConstraints> ackermann_constraints;
 };
 
 }  // namespace mppi::models

include/mppic/motion_model.hpp

@@ -29,44 +29,58 @@ inline bool isHolonomic(MotionModelType type) {
   return false;
 }
 
-class IModelConstraints {
+class IMotionModel {
 public:
-  virtual ~IModelConstraints() = default;
-  virtual void applyConstraints(const models::State & state) = 0;
+  virtual ~IMotionModel() = default;
+
+  virtual MotionModelType getMotionModelType() = 0;
+
+  virtual void applyConstraints(models::State & /*state*/) {};
+  bool isHolonomic() { return ::mppi::isHolonomic(getMotionModelType());};
 };
 
-class AckermannConstraints final : public IModelConstraints {
+class AckermannMotionModel final : public IMotionModel {
 public:
-  AckermannConstraints(ParametersHandler * param_handler) {
+  explicit AckermannMotionModel(ParametersHandler * param_handler) {
 
   auto getParam = param_handler->getParamGetter("AckermannConstraints");
   getParam(min_turning_r_, "min_turning_r", 0.2);
 }
 
-void applyConstraints(const models::State & state) override {
+MotionModelType getMotionModelType() override {
+    return MotionModelType::Ackermann;
+};
+
+void applyConstraints(models::State & state) override {
   auto v = state.getVelocitiesVX();
   auto w = state.getVelocitiesWZ();
 
-  auto mask = v / w > min_turning_r_; 
-  auto w_masked = xt::masked_view(w, mask);
-  w_masked = xt::sign(v) /  min_turning_r_;
+  auto view = xt::masked_view(w, v / w > min_turning_r_);
+  view = xt::sign(v) /  min_turning_r_;
 }
 
 private:
   double min_turning_r_{0};
 };
 
-inline std::unique_ptr<IModelConstraints> 
-getModelConstraintsUniquePtr(ParametersHandler * param_handler, MotionModelType type) {
-    switch (type) {
-    case MotionModelType::Omni:
-    case MotionModelType::DiffDrive:
-      return{};
-    case MotionModelType::Ackermann:
-      return std::make_unique<AckermannConstraints>(param_handler);
-    }
+class OmniMotionModel final : public IMotionModel {
+public:
+  OmniMotionModel() = default;
 
-    return {};
+MotionModelType getMotionModelType() override {
+    return MotionModelType::Omni;
 }
 
+};
+
+class DiffDriveMotionModel final : public IMotionModel {
+public:
+  DiffDriveMotionModel() = default;
+
+MotionModelType getMotionModelType() override {
+    return MotionModelType::DiffDrive;
+}
+
+};
+
 }  // namespace mppi::models

include/mppic/optimizer.hpp

@@ -124,8 +124,8 @@ class Optimizer
   nav2_costmap_2d::Costmap2D * costmap_;
   std::string name_;
 
-  MotionModelType motion_model_type_;
-  std::unique_ptr<IModelConstraints> model_constraints_;
+  std::function<prediction_model_t> prediction_model_;
+  std::unique_ptr<IMotionModel> motion_model_;
 
   ParametersHandler * parameters_handler_;
   CriticManager critic_manager_;
@@ -134,7 +134,6 @@ class Optimizer
 
   models::State state_;
   models::ControlSequence control_sequence_;
-  std::function<prediction_model_t> prediction_model_;
 
   xt::xtensor<double, 3> generated_trajectories_;
   xt::xtensor<double, 2> plan_;

src/optimizer.cpp

@@ -208,7 +208,7 @@ void Optimizer::generateNoisedControls()
   }
 }
 
-bool Optimizer::isHolonomic() const {return ::mppi::isHolonomic(motion_model_type_);}
+bool Optimizer::isHolonomic() const {return motion_model_->isHolonomic();}
 
 void Optimizer::applyControlConstraints()
 {
@@ -221,9 +221,7 @@ void Optimizer::applyControlConstraints()
     vy = xt::clip(vy, -s.constraints.vy, s.constraints.vy);
   }
 
-  if (model_constraints_) {
-    model_constraints_->applyConstraints(state_);
-  }
+  motion_model_->applyConstraints(state_);
 
   vx = xt::clip(vx, -s.constraints.vx, s.constraints.vx);
   wz = xt::clip(wz, -s.constraints.wz, s.constraints.wz);
@@ -340,12 +338,12 @@ geometry_msgs::msg::TwistStamped Optimizer::getControlFromSequenceAsTwist(
 
 void Optimizer::setMotionModel(const std::string & model)
 {
-  if (model == "DiffDrive") {
-    motion_model_type_ = MotionModelType::DiffDrive;
+if (model == "DiffDrive") {
+    motion_model_ = std::make_unique<DiffDriveMotionModel>();
   } else if (model == "Omni") {
-    motion_model_type_ = MotionModelType::Omni;
+    motion_model_ = std::make_unique<OmniMotionModel>();
   } else if (model == "Ackermann") {
-    motion_model_type_ = MotionModelType::Ackermann;
+    motion_model_ = std::make_unique<AckermannMotionModel>(parameters_handler_);
   } else {
     throw std::runtime_error(
             std::string(
@@ -354,7 +352,6 @@ void Optimizer::setMotionModel(const std::string & model)
               model.c_str()));
   }
 
-  model_constraints_ = getModelConstraintsUniquePtr(parameters_handler_, motion_model_type_);
   state_.idx.setLayout(isHolonomic());
   control_sequence_.idx.setLayout(isHolonomic());
 }

test/optimizer_test.cpp

@@ -15,7 +15,7 @@
 
 #include "mppic/optimizer.hpp"
 #include "mppic/parameters_handler.hpp"
-#include "mppic/motion_models.hpp"
+#include "mppic/motion_model.hpp"
 
 #include "utils/utils.hpp"
 
@@ -127,12 +127,3 @@ TEST(MPPIOptimizer, OptimizerTestOmniCircle)
   EXPECT_TRUE(!inCollision(trajectory, *costmap));
   EXPECT_TRUE(isGoalReached(trajectory, *costmap, goal_point));
 }
-
-TEST(MPPIOptimizer, AckermannException)
-{
-  mppi::AckermannMotionModel model;
-  xt::xtensor<double, 2> in;
-  mppi::models::StateIdxes idx;
-  EXPECT_FALSE(model.isHolonomic());
-  EXPECT_THROW(model.predict(in, idx), std::runtime_error);
-}

test/utils/factory.hpp

@@ -14,7 +14,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/lifecycle_node.hpp>
 
-#include "mppic/motion_models.hpp"
+#include "mppic/motion_model.hpp"
 #include "mppic/parameters_handler.hpp"
 #include "mppic/optimizer.hpp"
 #include "mppic/controller.hpp"