From 4b9bd8638de2cdd5bde363c84ecd4760105ec00e Mon Sep 17 00:00:00 2001
From: "mergify[bot]" <37929162+mergify[bot]@users.noreply.github.com>
Date: Wed, 22 Mar 2023 08:41:54 -0700
Subject: [PATCH] ackermann motion model bug (#3498) (#3501)

Prevent cost to be modified twice.

(cherry picked from commit 482017ce02ec08eadd3a23440e619b0e506cb9c5)

Co-authored-by: HAIDAR OBEID <31267966+ObeidHaidar@users.noreply.github.com>
---
 nav2_mppi_controller/src/critics/constraint_critic.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/nav2_mppi_controller/src/critics/constraint_critic.cpp b/nav2_mppi_controller/src/critics/constraint_critic.cpp
index 886c0e4b05b..fc26f89d118 100644
--- a/nav2_mppi_controller/src/critics/constraint_critic.cpp
+++ b/nav2_mppi_controller/src/critics/constraint_critic.cpp
@@ -65,6 +65,7 @@ void ConstraintCritic::score(CriticData & data)
         std::move(out_of_min_bounds_motion) +
         std::move(out_of_turning_rad_motion)) *
         data.model_dt, {1}, immediate) * weight_, power_);
+    return;
   }
 
   data.costs += xt::pow(