Remove vel pointer

Author: RBT22

nav2_behaviors/include/nav2_behaviors/plugins/drive_on_heading.hpp

@@ -137,7 +137,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
     if (acceleration_limit_ <= 0.0 && deceleration_limit_ <= 0.0) {
       cmd_vel->twist.linear.x = command_speed_;
     } else {
-      double current_speed = last_vel_ ? *last_vel_ : 0.0;
+      double current_speed = last_vel_ == std::numeric_limits<double>::max() ? 0.0 : last_vel_;
       auto remaining_distance = std::fabs(command_x_) - distance;
       double min_feasible_speed = -std::numeric_limits<double>::infinity();
       double max_feasible_speed = std::numeric_limits<double>::infinity();
@@ -150,7 +150,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       cmd_vel->twist.linear.x = std::clamp(command_speed_, min_feasible_speed, max_feasible_speed);
 
       // Check if we need to slow down to avoid overshooting
-      bool forward = command_speed_ > 0.0 ? true : false;
+      bool forward = command_speed_ > 0.0;
       if (forward && deceleration_limit_ > 0.0) {
         double max_vel_to_stop = std::sqrt(2.0 * deceleration_limit_ * remaining_distance);
         if (max_vel_to_stop < cmd_vel->twist.linear.x) {
@@ -175,7 +175,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
       return ResultStatus{Status::FAILED, ActionT::Result::COLLISION_AHEAD};
     }
 
-    last_vel_ = std::make_shared<double>(cmd_vel->twist.linear.x);
+    last_vel_ = cmd_vel->twist.linear.x;
 
     this->vel_pub_->publish(std::move(cmd_vel));
 
@@ -188,9 +188,9 @@ class DriveOnHeading : public TimedBehavior<ActionT>
    */
   CostmapInfoType getResourceInfo() override {return CostmapInfoType::LOCAL;}
 
-  void onCleanup() override {last_vel_.reset();}
+  void onCleanup() override {last_vel_ = std::numeric_limits<double>::max();}
 
-  void onActionCompletion() override {last_vel_.reset();}
+  void onActionCompletion() override {last_vel_ = std::numeric_limits<double>::max();}
 
 protected:
   /**
@@ -271,7 +271,7 @@ class DriveOnHeading : public TimedBehavior<ActionT>
   double simulate_ahead_time_;
   double acceleration_limit_;
   double deceleration_limit_;
-  std::shared_ptr<double> last_vel_;
+  double last_vel_ = std::numeric_limits<double>::max();
 };
 
 }  // namespace nav2_behaviors