diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index 1a118464eab858af51ac85b414e2b9c09066c227..5d9fb88bf129415ba709d1724628e06eaefc78d6 100644 --- a/rotational_recovery/src/rotational_recovery.cpp +++ b/rotational_recovery/src/rotational_recovery.cpp @@ -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_;