core/traj-opt-problem : have replaceStageCircular() return ejected knot

+ bring behaviour closer to FIFO-style queue

Author: ManifoldFR

include/aligator/core/traj-opt-problem.hpp

@@ -210,9 +210,11 @@ template <typename _Scalar> struct TrajOptProblemTpl {
                           std::size_t num_threads = 1,
                           bool compute_second_order = true) const;
 
-  /// @brief Pop out the first StageModel and replace by the supplied one;
-  /// updates the supplied problem data (TrajOptDataTpl) object.
-  void replaceStageCircular(const xyz::polymorphic<StageModel> &model);
+  /// @brief Advance each stage model by one slot after inserting a new stage,
+  /// pop out the first model.
+  /// @returns The first StageModel.
+  xyz::polymorphic<StageModel>
+  replaceStageCircular(const xyz::polymorphic<StageModel> &model);
 
   bool checkIntegrity() const;
 

include/aligator/core/traj-opt-problem.hxx

@@ -164,11 +164,13 @@ inline std::size_t TrajOptProblemTpl<Scalar>::numSteps() const {
 }
 
 template <typename Scalar>
-void TrajOptProblemTpl<Scalar>::replaceStageCircular(
-    const xyz::polymorphic<StageModel> &model) {
+auto TrajOptProblemTpl<Scalar>::replaceStageCircular(
+    const xyz::polymorphic<StageModel> &model) -> xyz::polymorphic<StageModel> {
   addStage(model);
   rotate_vec_left(stages_);
+  auto last = std::move(stages_.back());
   stages_.pop_back();
+  return last;
 }
 
 template <typename Scalar>