diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp
index c437a8abf58a665f45e2b0334425a587492940be..ddf0dafcc84e54fba4ecda215ee06286a76b09a1 100644
--- a/twist_recovery/src/twist_recovery.cpp
+++ b/twist_recovery/src/twist_recovery.cpp
@@ -92,7 +92,7 @@ namespace twist_recovery
private_nh.param("angular_z", base_frame_twist_.angular.z, 0.0);
private_nh.param("duration", duration_, 3.0);
- private_nh.param("linear_speed_limit", linear_speed_limit_, 1.0);
+ private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
private_nh.param("linear_speed_steer", linear_speed_steer_, -0.2);
private_nh.param("angular_speed_limit", angular_speed_limit_, 0.2);
private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
@@ -109,9 +109,9 @@ namespace twist_recovery
geometry_msgs::Twist scaleTwist (const geometry_msgs::Twist& twist, const double scale)
{
geometry_msgs::Twist t;
- t.linear.x = twist.linear.x * scale;
- t.linear.y = twist.linear.y * scale;
- t.angular.z = twist.angular.z * scale;
+ t.linear.x = twist.linear.x / scale;
+ t.linear.y = twist.linear.y / scale;
+ t.angular.z = twist.angular.z / scale;
return t;
}
@@ -222,7 +222,7 @@ namespace twist_recovery
}
// Apply algorithm to escape from a lethal obstacle
- void TwistRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost)
+ void RotationalRecovery::escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost)
{
double cost_front_1, cost_front_2, cost_front_3 = 0;
double cost_back_1, cost_back_2, cost_back_3 = 0;
@@ -232,42 +232,42 @@ namespace twist_recovery
int count_cost = 0;
// Simulate cost moving 1 global_cmap's cell in each direction
- t = 0.2;
- twist.linear.x = 1.0;
+ t = 1.0;
+ twist.linear.x = 0.2;
cost_front_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.x = -1.0;
+ twist.linear.x = -0.2;
cost_back_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
+ twist.linear.y = 0.2;
cost_left_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.y = -1.0;
+ twist.linear.y = -0.2;
cost_right_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
// Simulate cost moving 2 global_cmap's cell in each direction
- t = 0.4;
- twist.linear.x = 1.0;
+ t = 2.0;
+ twist.linear.x = 0.2;
cost_front_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.x = -1.0;
+ twist.linear.x = -0.2;
cost_back_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
+ twist.linear.y = 0.2;
cost_left_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.y = -1.0;
+ twist.linear.y = -0.2;
cost_right_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
// Simulate cost moving 3 global_cmap's cell in each direction
- t = 0.6;
- twist.linear.x = 1.0;
+ t = 3.0;
+ twist.linear.x = 0.2;
cost_front_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.x = -1.0;
+ twist.linear.x = -0.2;
cost_back_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
+ twist.linear.y = 0.2;
cost_left_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
- twist.linear.y = -1.0;
+ twist.linear.y = -0.2;
cost_right_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
twist.linear.x = 0.0;
@@ -383,7 +383,7 @@ namespace twist_recovery
}
else { // Case 2
if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right
- double min_time = 0.2;
+ double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
twist.linear.x = 0.0;
@@ -391,11 +391,11 @@ namespace twist_recovery
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
while(true){
- twist.linear.x = 1.0;
+ twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
- geometry_msgs::Pose2D forw_left = forwardSimulate(forw, twist, 0.2);
+ twist.linear.y = 0.2;
+ geometry_msgs::Pose2D forw_left = forwardSimulate(forw, twist, 1.0);
twist.linear.y = 0.0;
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
@@ -403,11 +403,11 @@ namespace twist_recovery
break;
}
- twist.linear.x = -1.0;
+ twist.linear.x = -0.2;
geometry_msgs::Pose2D backw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
- geometry_msgs::Pose2D backw_left = forwardSimulate(backw, twist, 0.2);
+ twist.linear.y = 0.2;
+ geometry_msgs::Pose2D backw_left = forwardSimulate(backw, twist, 1.0);
twist.linear.y = 0.0;
double cost_backw_left = normalizedPoseCost(backw_left);
@@ -415,20 +415,20 @@ namespace twist_recovery
move_for = false;
break;
}
- min_time = min_time + 0.2;
+ 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, 5.0*min_time);
+ moveRobot(twist, min_time);
}
else {
twist.linear.x = -0.2;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
- moveRobot(twist, 5.0*min_time);
+ moveRobot(twist, min_time);
}
twist.linear.x = 0.0;
twist.angular.z = -angular_speed_limit_;
@@ -438,7 +438,7 @@ namespace twist_recovery
} // End lethal cost on the left
else { // Lethal cost on the right, move towards left
- double min_time = 0.2;
+ double min_time = 1.0;
double turn_duration = 0.0;
bool move_for = true;
twist.linear.x = 0.0;
@@ -446,11 +446,11 @@ namespace twist_recovery
twist.angular.z = 0.0;
// Iterate until some movement (forward or backwards) allows the robot to rotate
while(true){
- twist.linear.x = 1.0;
+ twist.linear.x = 0.2;
geometry_msgs::Pose2D forw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
- geometry_msgs::Pose2D forw_left = forwardSimulate(forw, twist, 0.2);
+ twist.linear.y = 0.2;
+ geometry_msgs::Pose2D forw_left = forwardSimulate(forw, twist, 1.0);
twist.linear.y = 0.0;
double cost_forw_left = normalizedPoseCost(forw_left);
if (cost_forw_left < cost) {
@@ -458,11 +458,11 @@ namespace twist_recovery
break;
}
- twist.linear.x = -1.0;
+ twist.linear.x = -0.2;
geometry_msgs::Pose2D backw = forwardSimulate(current_position, twist, min_time);
twist.linear.x = 0.0;
- twist.linear.y = 1.0;
- geometry_msgs::Pose2D backw_left = forwardSimulate(backw, twist, 0.2);
+ twist.linear.y = 0.2;
+ geometry_msgs::Pose2D backw_left = forwardSimulate(backw, twist, 1.0);
twist.linear.y = 0.0;
double cost_backw_left = normalizedPoseCost(backw_left);
@@ -470,20 +470,20 @@ namespace twist_recovery
move_for = false;
break;
}
- min_time = min_time + 0.2;
+ 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, 5.0*min_time);
+ moveRobot(twist, min_time);
}
else {
twist.linear.x = -0.2;
twist.linear.y = 0.0;
twist.angular.z = 0.0;
- moveRobot(twist, 5.0*min_time);
+ moveRobot(twist, min_time);
}
twist.linear.x = 0.0;
twist.angular.z = angular_speed_limit_;
@@ -506,6 +506,7 @@ namespace twist_recovery
}
+
void TwistRecovery::runBehavior ()
{
ROS_ASSERT (initialized_);