diff --git a/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp b/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
index aad16adf7608736c1e57fabdf31343fc5990bfa6..4682123a4c6e275761954f3ca39881aa0be0d2cd 100644
--- a/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
+++ b/stepback_and_steerturn_recovery/src/stepback_and_steerturn_recovery.cpp
@@ -193,7 +193,7 @@ geometry_msgs::Pose2D StepBackAndSteerTurnRecovery::getPoseToObstacle (const geo
       next_cost = normalizedPoseCost(current_tmp);
       ROS_DEBUG_NAMED ("top", "finish Cost");
     //if (next_cost > cost) {
-    if (/*next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE ||*/ next_cost >= costmap_2d::LETHAL_OBSTACLE) {
+    if (next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || next_cost >= costmap_2d::LETHAL_OBSTACLE) {
       ROS_DEBUG_STREAM_NAMED ("cost", "Cost at " << t << " and pose " << forwardSimulate(current, twist, t)
                               << " is " << next_cost << " which is greater than previous cost " << cost);
       break;