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