Fix dynamic param SmacPlannerLattice (#5478)

* Fix SmacPlannerLattice dynamic parameter early exit

Signed-off-by: Tony Najjar <[email protected]>

* remove comment

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Tony Najjar <[email protected]>

Author: tonynajjar

nav2_smac_planner/src/node_lattice.cpp

@@ -50,23 +50,21 @@ void LatticeMotionTable::initMotionModel(
   unsigned int & size_x_in,
   SearchInfo & search_info)
 {
-  size_x = size_x_in;
-
-  if (current_lattice_filepath == search_info.lattice_filepath) {
-    return;
-  }
-
   size_x = size_x_in;
   change_penalty = search_info.change_penalty;
   non_straight_penalty = search_info.non_straight_penalty;
   cost_penalty = search_info.cost_penalty;
   reverse_penalty = search_info.reverse_penalty;
   travel_distance_reward = 1.0f - search_info.retrospective_penalty;
-  current_lattice_filepath = search_info.lattice_filepath;
   allow_reverse_expansion = search_info.allow_reverse_expansion;
   rotation_penalty = search_info.rotation_penalty;
   min_turning_radius = search_info.minimum_turning_radius;
 
+  if (current_lattice_filepath == search_info.lattice_filepath) {
+    return;
+  }
+  current_lattice_filepath = search_info.lattice_filepath;
+
   // Get the metadata about this minimum control set
   lattice_metadata = getLatticeMetadata(current_lattice_filepath);
   std::ifstream latticeFile(current_lattice_filepath);