Storing last command velocity

Signed-off-by: Chanh Hoang <chanhhoang999x@gmail.com>

Author: chanhhoang99

nav2_mppi_controller/include/nav2_mppi_controller/optimizer.hpp

@@ -288,6 +288,8 @@ class Optimizer
     std::nullopt, std::nullopt};  /// Caution, keep references
 
   rclcpp::Logger logger_{rclcpp::get_logger("MPPIController")};
+
+  geometry_msgs::msg::Twist last_command_vel_{};
 };
 
 }  // namespace mppi

nav2_mppi_controller/src/optimizer.cpp

@@ -172,6 +172,12 @@ void Optimizer::reset(bool reset_dynamic_speed_limits)
     parent_, name_ + ".TrajectoryValidator",
     costmap_ros_, parameters_handler_, tf_buffer_, settings_);
 
+  if (settings_.open_loop) {
+    last_command_vel_.linear.x  = 0.0;
+    last_command_vel_.angular.z = 0.0;
+    last_command_vel_.linear.y  = 0.0;
+  }
+  
   RCLCPP_INFO(logger_, "Optimizer reset");
 }
 
@@ -214,6 +220,12 @@ std::tuple<geometry_msgs::msg::TwistStamped, Eigen::ArrayXXf> Optimizer::evalCon
   utils::savitskyGolayFilter(control_sequence_, control_history_, settings_);
   auto control = getControlFromSequenceAsTwist(plan.header.stamp);
 
+  if (settings_.open_loop) {
+    last_command_vel_.linear.x = control.twist.linear.x;
+    last_command_vel_.angular.z = control.twist.angular.z;
+    last_command_vel_.linear.y = control.twist.linear.y;
+  }
+
   if (settings_.shift_control_sequence) {
     shiftControlSequence();
   }
@@ -352,16 +364,16 @@ void Optimizer::updateInitialStateVelocities(models::State & state) const
 {
   const bool open = settings_.open_loop;
 
-  const float vx0 = open ? control_sequence_.vx(0) :
+  const float vx0 = open ? last_command_vel_.linear.x :
     static_cast<float>(state.speed.linear.x);
-  const float wz0 = open ? control_sequence_.wz(0) :
+  const float wz0 = open ? last_command_vel_.angular.z :
     static_cast<float>(state.speed.angular.z);
 
   state.vx.col(0) = vx0;
   state.wz.col(0) = wz0;
 
   if (isHolonomic()) {
-    const float vy0 = open ? control_sequence_.vy(0) :
+    const float vy0 = open ? last_command_vel_.linear.y :
       static_cast<float>(state.speed.linear.y);
     state.vy.col(0) = vy0;
   }