Multiplied trajectory cost in the end instead of multiplying it in every iteration (#30)

Author: CihatAltiparmak

frenet_trajectory_planner/include/frenet_trajectory_planner/costs/lateral_distance_cost.hpp

@@ -32,10 +32,10 @@ double LateralDistanceCost::cost(
   double trajectory_cost = 0;
 
   for (auto frenet_state : frenet_trajectory) {
-    trajectory_cost += std::abs(frenet_state[3] * K_distance_);
+    trajectory_cost += std::abs(frenet_state[3]);
   }
 
-  return trajectory_cost;
+  return K_distance_ * trajectory_cost;
 }
 
 }

frenet_trajectory_planner/include/frenet_trajectory_planner/costs/longtitutal_velocity_cost.hpp

@@ -35,10 +35,10 @@ double LongtitutalVelocityCost::cost(
   double trajectory_cost = 0;
 
   for (auto frenet_state : frenet_trajectory) {
-    trajectory_cost += std::abs(frenet_state[1] - desired_velocity_) * K_longtitutal_velocity_;
+    trajectory_cost += std::abs(frenet_state[1] - desired_velocity_);
   }
 
-  return trajectory_cost;
+  return K_longtitutal_velocity_ * trajectory_cost;
 }
 
 }

nav2_frenet_ilqr_controller/src/costs/lateral_distance_cost.cpp

@@ -34,10 +34,10 @@ double LateralDistanceCost::cost(
   double trajectory_cost = 0;
 
   for (auto frenet_state : frenet_trajectory) {
-    trajectory_cost += std::abs(frenet_state[3] * K_lateral_distance_);
+    trajectory_cost += std::abs(frenet_state[3]);
   }
 
-  return trajectory_cost;
+  return K_lateral_distance_ * trajectory_cost;
 }
 
 }

nav2_frenet_ilqr_controller/src/costs/longtitutal_velocity_cost.cpp

@@ -40,10 +40,10 @@ double LongtitutalVelocityCost::cost(
   double trajectory_cost = 0;
 
   for (auto frenet_state : frenet_trajectory) {
-    trajectory_cost += std::abs(frenet_state[1] - desired_velocity_) * K_longtitutal_velocity_;
+    trajectory_cost += std::abs(frenet_state[1] - desired_velocity_);
   }
 
-  return trajectory_cost;
+  return K_longtitutal_velocity_ * trajectory_cost;
 }
 
 }