consider assigning negative TTC cost to trajectories that will cause collision in the first prediction step

[rayvburn]

Conceptually - we should not assign a purely negative cost as we are not certain about the state in the next step.

A preliminary version was prepared as shown below:

diff --git a/src/ttc_cost_function.cpp b/src/ttc_cost_function.cpp
index 12422a6..b5fff1d 100644
--- a/src/ttc_cost_function.cpp
+++ b/src/ttc_cost_function.cpp
@@ -78,6 +78,12 @@ double TTCCostFunction::scoreTrajectory(base_local_planner::Trajectory& traj) {
                        dist_min_static,
                        dist_min_dynamic)
                ) {
+                       // check if collision occurs at the first prediction
+                       if (timestamp == 0.0) {
+                               return -13.0;
+                       }
+
                        // we'll score and stop here
                        // save data for a specific trajectory
                        robot_stat_obj_v_.push_back(state_prediction_static);