diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index 0266ffa70638dc90ec53f1c6a198340304ce6db6..6696e88a5966895f3f1cf9230238182daeefe770 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 3a8009afac4fc5d0040bca126ba440d7593a7ecd..87b98e45c01f07248e2a0c8e1d0717404ed3737c 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;