clean up calculate cost function

Author: StarrryNight

src/software/ai/navigator/trajectory/trajectory_planner.cpp

@@ -162,31 +162,3 @@ TrajectoryPathWithCost TrajectoryPlanner::getTrajectoryWithCost(
     return traj_with_cost;
 }
 
-double TrajectoryPlanner::calculateCost(
-    const TrajectoryPathWithCost &traj_with_cost) const
-{
-    double total_cost = traj_with_cost.traj_path.getTotalTime();
-
-    // Add a large cost if the trajectory collides with an obstacle
-    // Note that this ignores collisions that may be in at the
-    // start of the trajectory as those are unavoidable by all trajectories.
-    if (traj_with_cost.colliding_obstacle != nullptr)
-    {
-        total_cost += 6.0;
-    }
-
-    // The closer the collision is to the destination, the lower its cost will be
-    Point first_collision_position =
-        traj_with_cost.traj_path.getPosition(traj_with_cost.first_collision_time_s);
-    Point destination = traj_with_cost.traj_path.getDestination();
-    total_cost += (first_collision_position - destination).length();
-
-    total_cost += std::max(
-        0.0, (MAX_FUTURE_COLLISION_CHECK_SEC - traj_with_cost.first_collision_time_s));
-
-    total_cost += 3 * traj_with_cost.collision_duration_front_s;
-
-    total_cost += 1 * traj_with_cost.collision_duration_back_s;
-
-    return total_cost;
-}

src/software/ai/navigator/trajectory/trajectory_planner.h

@@ -35,14 +35,6 @@ class TrajectoryPlanner
         const std::optional<Point> &prev_sub_destination = std::nullopt);
 
    private:
-    /**
-     * Calculate the cost of the given trajectory path with cost
-     *
-     * @param traj_with_cost A complete trajectory path with cost
-     * @return The cost of the trajectory
-     */
-    double calculateCost(const TrajectoryPathWithCost &traj_with_cost) const;
-
     /**
      * Get a single trajectory with cost that goes directly from the start to the
      * destination.