From fadcda8965cd574cd8972f6db1ce591a1ea92d9f Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Jos=C3=A9=20Enrique=20Dom=C3=ADnguez?=
<jdominguez@iri.upc.edu>
Date: Thu, 28 May 2020 10:58:49 +0200
Subject: [PATCH] Allowed movement reduced in escapeFromLethalCost()
---
.../src/rotational_recovery.cpp | 82 +++++++++++--------
twist_recovery/src/twist_recovery.cpp | 82 +++++++++++--------
2 files changed, 92 insertions(+), 72 deletions(-)
diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index 0266ffa..6696e88 100644
--- a/rotational_recovery/src/rotational_recovery.cpp
+++ b/rotational_recovery/src/rotational_recovery.cpp
@@ -441,11 +441,12 @@ namespace rotational_recovery
double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
+ bool found = false;
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
- while(true){
+ while(min_time <= 5.0){ // Only movements until 1.0 m allowed
twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
@@ -455,6 +456,7 @@ namespace rotational_recovery
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
move_for = true;
+ found = true;
break;
}
@@ -468,27 +470,30 @@ namespace rotational_recovery
if (cost_backw_left < cost) {
move_for = false;
+ found = true;
break;
}
min_time = min_time + 1.0;
}
- // At this point we know the time necessary to allow the robot to rotate
- if (move_for) {
- twist.linear.x = 0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
- }
- else {
- twist.linear.x = -0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
+ if (found) {
+ // At this point we know the time necessary to allow the robot to rotate
+ if (move_for) {
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = -angular_speed_limit_;
+ turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
}
- twist.linear.x = 0.0;
- twist.angular.z = -angular_speed_limit_;
- turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
- moveRobot(twist, turn_duration);
} // End lethal cost on the left
@@ -496,11 +501,12 @@ namespace rotational_recovery
double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
+ bool found = false;
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
- while(true){
+ while(min_time <= 5.0){ // Only movements until 1.0 m allowed
twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
@@ -510,6 +516,7 @@ namespace rotational_recovery
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
move_for = true;
+ found = true;
break;
}
@@ -523,30 +530,33 @@ namespace rotational_recovery
if (cost_backw_left < cost) {
move_for = false;
+ found = true;
break;
}
min_time = min_time + 1.0;
}
- // At this point we know the time necessary to allow the robot to rotate
- if (move_for) {
- twist.linear.x = 0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
- }
- else {
- twist.linear.x = -0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
+ if (found) {
+ // At this point we know the time necessary to allow the robot to rotate
+ if (move_for) {
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = angular_speed_limit_;
+ turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
}
- twist.linear.x = 0.0;
- twist.angular.z = angular_speed_limit_;
- turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
- moveRobot(twist, turn_duration);
- }
- }
+ } // End lethal cost on the right
+ } // End of case 3.2
return;
diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp
index 3a8009a..87b98e4 100644
--- a/twist_recovery/src/twist_recovery.cpp
+++ b/twist_recovery/src/twist_recovery.cpp
@@ -387,11 +387,12 @@ namespace twist_recovery
double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
+ bool found = false;
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
- while(true){
+ while(min_time <= 5.0){ // Only movements until 1.0 m allowed
twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
@@ -401,6 +402,7 @@ namespace twist_recovery
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
move_for = true;
+ found = true;
break;
}
@@ -414,27 +416,30 @@ namespace twist_recovery
if (cost_backw_left < cost) {
move_for = false;
+ found = true;
break;
}
min_time = min_time + 1.0;
}
- // At this point we know the time necessary to allow the robot to rotate
- if (move_for) {
- twist.linear.x = 0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
- }
- else {
- twist.linear.x = -0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
+ if (found) {
+ // At this point we know the time necessary to allow the robot to rotate
+ if (move_for) {
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = -angular_speed_limit_;
+ turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
}
- twist.linear.x = 0.0;
- twist.angular.z = -angular_speed_limit_;
- turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
- moveRobot(twist, turn_duration);
} // End lethal cost on the left
@@ -442,11 +447,12 @@ namespace twist_recovery
double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
+ bool found = false;
twist.linear.x = 0.0;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
- while(true){
+ while(min_time <= 5.0){ // Only movements until 1.0 m allowed
twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
@@ -456,6 +462,7 @@ namespace twist_recovery
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
move_for = true;
+ found = true;
break;
}
@@ -469,30 +476,33 @@ namespace twist_recovery
if (cost_backw_left < cost) {
move_for = false;
+ found = true;
break;
}
min_time = min_time + 1.0;
}
- // At this point we know the time necessary to allow the robot to rotate
- if (move_for) {
- twist.linear.x = 0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
- }
- else {
- twist.linear.x = -0.2;
- twist.linear.y = 0.0;
- twist.angular.z = 0.0;
- moveRobot(twist, min_time);
+ if (found) {
+ // At this point we know the time necessary to allow the robot to rotate
+ if (move_for) {
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = angular_speed_limit_;
+ turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
}
- twist.linear.x = 0.0;
- twist.angular.z = angular_speed_limit_;
- turn_duration = 1.5708/(angular_speed_limit_); // Turn pi/2 rad
- moveRobot(twist, turn_duration);
- }
- }
+ } // End lethal cost on the right
+ } // End of case 3.2
return;
--
GitLab