From ee2dfda100de44fee797a5c9b631c9611c5cbbda Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?=
<jdominguez@iri.upc.edu>
Date: Fri, 29 Nov 2019 12:18:44 +0100
Subject: [PATCH] stop not only with LETHAL cost but also with
INCRIBED_INFLATED cost
---
.../src/stepback_and_steerturn_recovery.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
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 aad16ad..4682123 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;
--
GitLab