Skip to content
Snippets Groups Projects
Commit 357a0043 authored by José Enrique Domínguez Vidal's avatar José Enrique Domínguez Vidal
Browse files

Updated values for calculate maneuvers in case of lethal cost

parent 699c3e10
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
rotational_recovery/src/rotational_recovery.cpp
+ 39
− 39
View file @ 357a0043
......@@ -67,8 +67,8 @@ namespace rotational_recovery
private_nh.param("controller_frequency", controller_frequency_, 20.0);
private_nh.param("simulation_inc", simulation_inc_, 1/controller_frequency_);
private_nh.param("linear_speed_limit", linear_speed_limit_, 1.0);
private_nh.param("angular_speed_steer", angular_speed_steer_, 0.5);
private_nh.param("linear_speed_limit", linear_speed_limit_, 0.3);
private_nh.param("angular_speed_steer", angular_speed_steer_, 0.2);
private_nh.param("duration", duration_, 3.0);
......@@ -214,8 +214,8 @@ namespace rotational_recovery
ROS_DEBUG_NAMED ("top", "theta pre-update: %.2f", original_pose.theta);
rotated_pose.theta = original_pose.theta + angle;
ROS_DEBUG_NAMED ("top", "theta post-update: %.2f", rotated_pose.theta);
twist.linear.x = linear_speed_limit_ * cos(rotated_pose.theta);
twist.linear.y = linear_speed_limit_ * sin(rotated_pose.theta);
twist.linear.x = 1.0 * cos(rotated_pose.theta);
twist.linear.y = 1.0 * sin(rotated_pose.theta);
geometry_msgs::Pose2D pose_to_obstacle = getPoseToObstacle(rotated_pose, twist);
double dist_to_obstacle = getDistBetweenTwoPoints(rotated_pose, pose_to_obstacle);
......@@ -286,42 +286,42 @@ namespace rotational_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;
......@@ -437,7 +437,7 @@ namespace rotational_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;
......@@ -445,11 +445,11 @@ namespace rotational_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) {
......@@ -457,11 +457,11 @@ namespace rotational_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);
......@@ -469,20 +469,20 @@ namespace rotational_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_;
......@@ -492,7 +492,7 @@ namespace rotational_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;
......@@ -500,11 +500,11 @@ namespace rotational_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) {
......@@ -512,11 +512,11 @@ namespace rotational_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);
......@@ -524,20 +524,20 @@ namespace rotational_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_;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment