fix(cws): Trigger warning only for occupied cells

Ashuh · Feb 5, 2022 · 6c81281 · 6c81281
1 parent ca7989b
commit 6c81281
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/collision_warning_system/src/collision_warning_system.cpp b/collision_warning_system/src/collision_warning_system.cpp
@@ -12,6 +12,7 @@
 #include <vector>
 
 #include "costmap_generator/collision_checker.h"
+#include "costmap_generator/costmap_value.h"
 #include "f1tenth_utils/tf2_wrapper.h"
 
 CollisionWarningSystem::CollisionWarningSystem()
@@ -61,7 +62,8 @@ void CollisionWarningSystem::timerCallback(const ros::TimerEvent& timer_event)
 
     for (int i = 0; i < projected_trajectory.poses.size(); ++i)
     {
-      if (collision_checker_->checkCollision(projected_trajectory.poses.at(i)))
+      if (collision_checker_->checkCollision(projected_trajectory.poses.at(i)) ==
+          static_cast<int>(CostmapValue::OCCUPIED))
       {
         ttc.data = i * delta_t_;
         break;