Worked trajectory costs out in a way that they are mean of total cost

nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp

@@ -37,7 +37,7 @@ double LateralDistanceCost::cost(
     trajectory_cost += std::abs(frenet_state[3]);
   }
 
-  return K_lateral_distance_ * trajectory_cost;
+  return K_lateral_distance_ * trajectory_cost / frenet_trajectory.size();
 }
 
 }

nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp

@@ -43,7 +43,7 @@ double LongtitutalVelocityCost::cost(
     trajectory_cost += std::abs(frenet_state[1] - desired_velocity_);
   }
 
-  return K_longtitutal_velocity_ * trajectory_cost;
+  return K_longtitutal_velocity_ * trajectory_cost / frenet_trajectory.size();
 }
 
 }

nav2_frenet_ilqr_controller/src/costs/obstacle_cost.cpp

@@ -45,7 +45,7 @@ double ObstacleCost::cost(
     trajectory_cost += point_cost;
   }
 
-  return K_obstacle_ * trajectory_cost;
+  return K_obstacle_ * trajectory_cost / cartesian_trajectory.size();
 }
 
 }