diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index c2aacee1aa8121d0bc0c61c61b4bb414a62eeaf1..23aab7d28f739b070bb4262434dd65b91f24d530 100644 --- a/rotational_recovery/src/rotational_recovery.cpp +++ b/rotational_recovery/src/rotational_recovery.cpp @@ -171,7 +171,7 @@ namespace rotational_recovery } //if (next_cost > cost) { - if (t>0.7 && (next_cost == costmap_2d::INSCRIBED_INFLATED_OBSTACLE || next_cost == costmap_2d::LETHAL_OBSTACLE)) { + if (t>0.4 && (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; diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp index f2888d21b774d8b7d9436765aa934b2e6268f8fc..be56b0472a2f9915bd1b7e057de51cb25bee8963 100644 --- a/twist_recovery/src/twist_recovery.cpp +++ b/twist_recovery/src/twist_recovery.cpp @@ -157,7 +157,7 @@ namespace twist_recovery cost = cost_after_min_time; for (t=min_time+simulation_inc_; t<=duration_; t+=simulation_inc_) { const double next_cost = normalizedPoseCost(forwardSimulate(current_position, twist, t)); - if (next_cost > cost) { + if ((next_cost > cost) || next_cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE) { ROS_DEBUG_STREAM_NAMED ("cost", "Cost after " << t << "seconds and pose " << forwardSimulate(current_position, twist, t) << " [" << next_cost << "] is greater than previous cost [" << cost << "]"); break;