ackermann motion model bug (ros-navigation#3498)

ObeidHaidar · web-flow · commit 482017ce02ec · 2023-03-22T08:40:11.000-07:00
Prevent cost to be modified twice.
diff --git 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(

Original file line number	Diff line number	Diff line change
`@@ -65,6 +65,7 @@ void ConstraintCritic::score(CriticData & data)`
`65`	`65`	`std::move(out_of_min_bounds_motion) +`
`66`	`66`	`std::move(out_of_turning_rad_motion)) *`
`67`	`67`	`data.model_dt, {1}, immediate) * weight_, power_);`
	`68`	`+ return;`
`68`	`69`	`}`
`69`	`70`
`70`	`71`	`data.costs += xt::pow(`