Skip to content
Snippets Groups Projects
Commit ee2dfda1 authored by José Enrique Domínguez Vidal's avatar José Enrique Domínguez Vidal
Browse files

stop not only with LETHAL cost but also with INCRIBED_INFLATED cost

parent 09192fa9
No related branches found
No related tags found
No related merge requests found
......@@ -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;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment