From 1d4c3f42cf2f56180fce1a747a73740c78d61f2c Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?=
 <jdominguez@iri.upc.edu>
Date: Tue, 3 Dec 2019 17:03:38 +0100
Subject: [PATCH] Avoiding obstacle collition

---
 rotational_recovery/src/rotational_recovery.cpp | 2 +-
 twist_recovery/src/twist_recovery.cpp           | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index c2aacee..23aab7d 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 f2888d2..be56b04 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;
-- 
GitLab