diff --git a/rotational_recovery/include/rotational_recovery/rotational_recovery.h b/rotational_recovery/include/rotational_recovery/rotational_recovery.h
index a2b0feceeeef2bef9bc137c1a0db13cd5f0a9497..15f16640d9a51d53eeb13839a55f7aef1d0abc54 100644
--- a/rotational_recovery/include/rotational_recovery/rotational_recovery.h
+++ b/rotational_recovery/include/rotational_recovery/rotational_recovery.h
@@ -31,6 +31,8 @@ namespace rotational_recovery
private:
+ void escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost);
+ void moveRobot(const geometry_msgs::Twist& twist, const double max_duration);
geometry_msgs::Pose2D getCurrentLocalPose () const;
geometry_msgs::Twist scaleGivenAccelerationLimits (const geometry_msgs::Twist& twist, const double time_remaining) const;
double normalizedPoseCost (const geometry_msgs::Pose2D& pose) const;
diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp
index 23aab7d28f739b070bb4262434dd65b91f24d530..96bbaabb342ab1198f9bf6f4dbe8595bfa8f810b 100644
--- a/rotational_recovery/src/rotational_recovery.cpp
+++ b/rotational_recovery/src/rotational_recovery.cpp
@@ -259,35 +259,346 @@ namespace rotational_recovery
}
+ // Apply the given twist during the given time
+ void RotationalRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration)
+ {
+ ros::Rate r(controller_frequency_);
+ ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
+ twist.linear.y, twist.angular.z, max_duration);
+
+ // We will now apply this twist open-loop for d seconds (scaled so we can
+ // guarantee stopping at the end)
+ for (double t=0; t<max_duration; t+=1/controller_frequency_) {
+ pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
+ r.sleep();
+ }
+ return;
+ }
+
+ // Apply algorithm to escape from a lethal obstacle
+ 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;
+ double cost_left_1, cost_left_2, cost_left_3 = 0;
+ double cost_right_1, cost_right_2, cost_right_3 = 0;
+ double t = 0.0;
+ int count_cost = 0;
+
+ // Simulate cost moving 1 global_cmap's cell in each direction
+ t = 0.2;
+ twist.linear.x = 1.0;
+ cost_front_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ 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;
+ cost_front_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ 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;
+ cost_front_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ cost_right_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+
+ ROS_INFO_NAMED ("top", "Original cost: %.2f", cost);
+ ROS_INFO_NAMED ("top", "1st costs: %.2f, %.2f, %.2f, %.2f", cost_front_1, cost_back_1, cost_left_1, cost_right_1);
+ ROS_INFO_NAMED ("top", "2nd costs: %.2f, %.2f, %.2f, %.2f", cost_front_2, cost_back_2, cost_left_2, cost_right_2);
+ ROS_INFO_NAMED ("top", "3rd costs: %.2f, %.2f, %.2f, %.2f", cost_front_3, cost_back_3, cost_left_3, cost_right_3);
+
+ if (cost_front_1 >= cost) count_cost++;
+ if (cost_back_1 >= cost) count_cost++;
+ if (cost_left_1 >= cost) count_cost++;
+ if (cost_right_1 >= cost) count_cost++;
+
+ switch(count_cost){
+ case 0: // If we are executing this function, there must be at least 1 lethal cost so this is Impossible
+ return;
+
+ case 1: // Moving just 1 cell arround and obtaining just 1 lethal cost means that our current cost is not lethal. Impossible
+ return;
+
+ case 2: // Two lethal costs mean a little edge
+ // Technically it is impossible that both front and back movements imply a lethal cost as well as both left and right
+ // So the question is to know towards which direction move the robot and towards which side turn it.
+ // 1st movement: move away
+ if (cost_front_1 >= cost) { // Lethal cost in the front, move backwards
+ if (cost_back_2 >= cost) { // Lethal cost too near the back, move just a bit
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem at the back, move further
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else { // Lethal cost at the back, move forwards
+ if (cost_front_2 >= cost) { // Lethal cost too near the front, move just a bit
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem, move further
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ // 2nd movement: turn
+ if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = -0.5;
+ double max_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, max_duration);
+ }
+ else { // Lethal cost on the right, move towards left
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.5;
+ double max_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, max_duration);
+ }
+ return;
+
+ case 3: // Three lethal cost mean a wall or a big edge
+ // There are two possibilities:
+ // 1. The obstacle is at the robot's front/back so it can move backwards/forwards without problems
+ // 2. The obstacle is at one side so it's necessary to know how long the robot must move forward or
+ // backward to create a gap big enough to turn to the other side
+ if (cost_back_1 < cost) { // Case 1.backwards
+ if (cost_back_2 >= cost) { // Lethal cost too near the back, move just a bit
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem at the back, move further
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else if (cost_front_1 < cost) { // Case 1.forwards
+ if (cost_front_2 >= cost) { // Lethal cost too near the front, move just a bit
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem, move further
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else { // Case 2
+ if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right
+ double min_time = 0.2;
+ double turn_duration = 0.0;
+ bool move_for = true;
+ 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){
+ twist.linear.x = 1.0;
+ 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.0;
+ double cost_forw_left = normalizedPoseCost(forw_left);
+ if (cost_forw_left < cost) {
+ move_for = true;
+ break;
+ }
+
+ twist.linear.x = -1.0;
+ 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.0;
+ double cost_backw_left = normalizedPoseCost(backw_left);
+
+ if (cost_backw_left < cost) {
+ move_for = false;
+ break;
+ }
+ min_time = min_time + 0.2;
+ }
+ // 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);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, 5.0*min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = -0.5;
+ turn_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
+
+ } // End lethal cost on the left
+
+ else { // Lethal cost on the right, move towards left
+ double min_time = 0.2;
+ double turn_duration = 0.0;
+ bool move_for = true;
+ 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){
+ twist.linear.x = 1.0;
+ 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.0;
+ double cost_forw_left = normalizedPoseCost(forw_left);
+ if (cost_forw_left < cost) {
+ move_for = true;
+ break;
+ }
+
+ twist.linear.x = -1.0;
+ 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.0;
+ double cost_backw_left = normalizedPoseCost(backw_left);
+
+ if (cost_backw_left < cost) {
+ move_for = false;
+ break;
+ }
+ min_time = min_time + 0.2;
+ }
+ // 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);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, 5.0*min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = 0.5;
+ turn_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
+
+ }
+ }
+
+ return;
+
+ case 4: // Completely surrounded by lethal cost. Robot probably damaged. Don't do anything.
+ return;
+
+ default:
+ return;
+
+ }
+ return;
+
+ }
+
+
void RotationalRecovery::runBehavior ()
{
ROS_ASSERT (initialized_);
- // We need to calculate the angle that gives us the best clearance
- geometry_msgs::Pose2D current_pose = getCurrentLocalPose();
- ros::Rate r(controller_frequency_);
double stop_time = 0.5;
- ROS_WARN_NAMED ("top", "--- Applying Rotational recovery ---");
- double angle = rotationalAngle(current_pose);
-
- // We calculate the twist to move the robot towards that angle
+ ros::Rate r(controller_frequency_);
geometry_msgs::Twist twist;
- if (angle<0.0) {
- twist.angular.z = -1.0 *angular_speed_steer_;
+
+ geometry_msgs::Pose2D current_pose = getCurrentLocalPose();
+ double cost = normalizedPoseCost(current_pose); // Cost of the original positon
+
+ // We need to calculate the angle that gives us the best clearance
+ if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
+ escapeFromLethalCost(current_pose, twist, cost);
}
else {
- twist.angular.z = angular_speed_steer_;
- }
- double max_duration = fabs(angle / angular_speed_steer_);
+ ROS_WARN_NAMED ("top", "--- Applying Rotational recovery ---");
+ double angle = rotationalAngle(current_pose);
- // We apply this twist open-loop for d seconds (scaled so we can guarantee
- // stopping at the end)
- ROS_INFO_NAMED ("top", "Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z);
- for (double t=0; t<max_duration; t+=1/controller_frequency_) {
- if(t<0.05){
- ROS_DEBUG_NAMED ("top", "twist.linear.x=%.2f, .y=%.2f, .z=%.2f, .angular.x=%.2f, .y=%.2f, .z=%.2f", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z);
+ // We calculate the twist to move the robot towards that angle
+ if (angle<0.0) {
+ twist.angular.z = -1.0 *angular_speed_steer_;
}
- pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
- r.sleep();
+ else {
+ twist.angular.z = angular_speed_steer_;
+ }
+ double max_duration = fabs(angle / angular_speed_steer_);
+
+ // We apply this twist open-loop for d seconds (scaled so we can guarantee
+ // stopping at the end)
+ ROS_INFO_NAMED ("top", "Applying %.2f rad turn [turning %.2f s at %.2f rad/s]", angle, max_duration, twist.angular.z);
+ for (double t=0; t<max_duration; t+=1/controller_frequency_) {
+ if(t<0.05){
+ ROS_DEBUG_NAMED ("top", "twist.linear.x=%.2f, .y=%.2f, .z=%.2f, .angular.x=%.2f, .y=%.2f, .z=%.2f", twist.linear.x, twist.linear.y, twist.linear.z, twist.angular.x, twist.angular.y, twist.angular.z);
+ }
+ pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
+ r.sleep();
+ }
+
}
twist.angular.z = 0.0;
@@ -296,6 +607,7 @@ namespace rotational_recovery
pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t));
r.sleep();
}
+
}
} // namespace rotational_recovery
diff --git a/twist_recovery/include/twist_recovery/twist_recovery.h b/twist_recovery/include/twist_recovery/twist_recovery.h
index 75925452086a720f0de09f5ce5e4ceecc848420a..4f56f72669e5a7ed810bc400006ab873883d9ab5 100644
--- a/twist_recovery/include/twist_recovery/twist_recovery.h
+++ b/twist_recovery/include/twist_recovery/twist_recovery.h
@@ -61,6 +61,8 @@ namespace twist_recovery
private:
+ void escapeFromLethalCost(const geometry_msgs::Pose2D& current_position, geometry_msgs::Twist& twist, const double cost);
+ void moveRobot(const geometry_msgs::Twist& twist, const double max_duration);
geometry_msgs::Pose2D getCurrentLocalPose () const;
geometry_msgs::Twist scaleGivenAccelerationLimits (const geometry_msgs::Twist& twist, const double time_remaining) const;
double nonincreasingCostInterval (const geometry_msgs::Pose2D& current, const geometry_msgs::Twist& twist, const double min_time) const;
diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp
index be56b0472a2f9915bd1b7e057de51cb25bee8963..1991a6e30685428f81a9cd5938a6e075c083f112 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_, 0.3);
+ private_nh.param("linear_speed_limit", linear_speed_limit_, 1.0);
private_nh.param("linear_speed_steer", linear_speed_steer_, -0.2);
private_nh.param("angular_speed_limit", angular_speed_limit_, 1.0);
private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
@@ -148,6 +148,7 @@ namespace twist_recovery
}
double cost = normalizedPoseCost(current_position); // Cost of the original positon
double cost_after_min_time = normalizedPoseCost(forwardSimulate(current_position, twist, min_time)); // Cost after the first k seconds we allow lethal cost
+
if (cost_after_min_time > cost) {
ROS_WARN_STREAM_NAMED("cost", "Cost after time allowing lethal cost [" << cost_after_min_time <<
"] is greater than original cost [" << cost << "]");
@@ -204,18 +205,9 @@ namespace twist_recovery
return pose;
}
- void TwistRecovery::runBehavior ()
+ // Apply the given twist during the given time
+ void TwistRecovery::moveRobot(const geometry_msgs::Twist& twist, const double max_duration)
{
- ROS_ASSERT (initialized_);
- double stop_time = 0.5;
-
- // Threre is a maximum duration this recovery can run defined in duration_.
- // However, there may be an obstacle so it is necessary to figure out how
- // long we can safely run the behavior.
- const geometry_msgs::Pose2D& current_pose = getCurrentLocalPose();
- const double max_duration = nonincreasingCostInterval(current_pose, base_frame_twist_);
- geometry_msgs::Twist twist;
- twist.linear.x = -0.2;
ros::Rate r(controller_frequency_);
ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
twist.linear.y, twist.angular.z, max_duration);
@@ -226,6 +218,324 @@ namespace twist_recovery
pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
r.sleep();
}
+ return;
+ }
+
+ // Apply algorithm to escape from a lethal obstacle
+ void TwistRecovery::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;
+ double cost_left_1, cost_left_2, cost_left_3 = 0;
+ double cost_right_1, cost_right_2, cost_right_3 = 0;
+ double t = 0.0;
+ int count_cost = 0;
+
+ // Simulate cost moving 1 global_cmap's cell in each direction
+ t = 0.2;
+ twist.linear.x = 1.0;
+ cost_front_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_1 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ 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;
+ cost_front_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_2 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ 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;
+ cost_front_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.x = -1.0;
+ cost_back_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 1.0;
+ cost_left_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+ twist.linear.y = -1.0;
+ cost_right_3 = normalizedPoseCost(forwardSimulate(current_position, twist, t));
+
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+
+ ROS_INFO_NAMED ("top", "Original cost: %.2f", cost);
+ ROS_INFO_NAMED ("top", "1st costs: %.2f, %.2f, %.2f, %.2f", cost_front_1, cost_back_1, cost_left_1, cost_right_1);
+ ROS_INFO_NAMED ("top", "2nd costs: %.2f, %.2f, %.2f, %.2f", cost_front_2, cost_back_2, cost_left_2, cost_right_2);
+ ROS_INFO_NAMED ("top", "3rd costs: %.2f, %.2f, %.2f, %.2f", cost_front_3, cost_back_3, cost_left_3, cost_right_3);
+
+ if (cost_front_1 >= cost) count_cost++;
+ if (cost_back_1 >= cost) count_cost++;
+ if (cost_left_1 >= cost) count_cost++;
+ if (cost_right_1 >= cost) count_cost++;
+
+ switch(count_cost){
+ case 0: // If we are executing this function, there must be at least 1 lethal cost so this is Impossible
+ return;
+
+ case 1: // Moving just 1 cell arround and obtaining just 1 lethal cost means that our current cost is not lethal. Impossible
+ return;
+
+ case 2: // Two lethal costs mean a little edge
+ // Technically it is impossible that both front and back movements imply a lethal cost as well as both left and right
+ // So the question is to know towards which direction move the robot and towards which side turn it.
+ // 1st movement: move away
+ if (cost_front_1 >= cost) { // Lethal cost in the front, move backwards
+ if (cost_back_2 >= cost) { // Lethal cost too near the back, move just a bit
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem at the back, move further
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else { // Lethal cost at the back, move forwards
+ if (cost_front_2 >= cost) { // Lethal cost too near the front, move just a bit
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem, move further
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ // 2nd movement: turn
+ if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = -0.5;
+ double max_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, max_duration);
+ }
+ else { // Lethal cost on the right, move towards left
+ twist.linear.x = 0.0;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.5;
+ double max_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, max_duration);
+ }
+ return;
+
+ case 3: // Three lethal cost mean a wall or a big edge
+ // There are two possibilities:
+ // 1. The obstacle is at the robot's front/back so it can move backwards/forwards without problems
+ // 2. The obstacle is at one side so it's necessary to know how long the robot must move forward or
+ // backward to create a gap big enough to turn to the other side
+ if (cost_back_1 < cost) { // Case 1.backwards
+ if (cost_back_2 >= cost) { // Lethal cost too near the back, move just a bit
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem at the back, move further
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else if (cost_front_1 < cost) { // Case 1.forwards
+ if (cost_front_2 >= cost) { // Lethal cost too near the front, move just a bit
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 1.0;
+ moveRobot(twist, max_duration);
+ }
+ else { // No problem, move further
+ twist.linear.x = 0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ double max_duration = 2.0;
+ moveRobot(twist, max_duration);
+ }
+ }
+ else { // Case 2
+ if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right
+ double min_time = 0.2;
+ double turn_duration = 0.0;
+ bool move_for = true;
+ 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){
+ twist.linear.x = 1.0;
+ 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.0;
+ double cost_forw_left = normalizedPoseCost(forw_left);
+ if (cost_forw_left < cost) {
+ move_for = true;
+ break;
+ }
+
+ twist.linear.x = -1.0;
+ 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.0;
+ double cost_backw_left = normalizedPoseCost(backw_left);
+
+ if (cost_backw_left < cost) {
+ move_for = false;
+ break;
+ }
+ min_time = min_time + 0.2;
+ }
+ // 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);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, 5.0*min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = -0.5;
+ turn_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
+
+ } // End lethal cost on the left
+
+ else { // Lethal cost on the right, move towards left
+ double min_time = 0.2;
+ double turn_duration = 0.0;
+ bool move_for = true;
+ 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){
+ twist.linear.x = 1.0;
+ 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.0;
+ double cost_forw_left = normalizedPoseCost(forw_left);
+ if (cost_forw_left < cost) {
+ move_for = true;
+ break;
+ }
+
+ twist.linear.x = -1.0;
+ 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.0;
+ double cost_backw_left = normalizedPoseCost(backw_left);
+
+ if (cost_backw_left < cost) {
+ move_for = false;
+ break;
+ }
+ min_time = min_time + 0.2;
+ }
+ // 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);
+ }
+ else {
+ twist.linear.x = -0.2;
+ twist.linear.y = 0.0;
+ twist.angular.z = 0.0;
+ moveRobot(twist, 5.0*min_time);
+ }
+ twist.linear.x = 0.0;
+ twist.angular.z = 0.5;
+ turn_duration = 1.5708/0.5; // Turn pi/2 rad
+ moveRobot(twist, turn_duration);
+
+ }
+ }
+
+ return;
+
+ case 4: // Completely surrounded by lethal cost. Robot probably damaged. Don't do anything.
+ return;
+
+ default:
+ return;
+
+ }
+ return;
+
+ }
+
+ void TwistRecovery::runBehavior ()
+ {
+ ROS_ASSERT (initialized_);
+ double stop_time = 0.5;
+ ros::Rate r(controller_frequency_);
+
+ // Threre is a maximum duration this recovery can run defined in duration_.
+ // However, there may be an obstacle so it is necessary to figure out how
+ // long we can safely run the behavior.
+ geometry_msgs::Twist twist;
+ const geometry_msgs::Pose2D& current_pose = getCurrentLocalPose();
+ double cost = normalizedPoseCost(current_pose); // Cost of the original positon
+
+ if (cost >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE){
+ escapeFromLethalCost(current_pose, twist, cost);
+ }
+ else {
+ const double max_duration = nonincreasingCostInterval(current_pose, base_frame_twist_);
+ twist.linear.x = -0.2;
+ ROS_WARN_NAMED ("top", "Applying Twist recovery (%.2f, %.2f, %.2f) for %.2f seconds", twist.linear.x,
+ twist.linear.y, twist.angular.z, max_duration);
+
+ // We will now apply this twist open-loop for d seconds (scaled so we can
+ // guarantee stopping at the end)
+ for (double t=0; t<max_duration; t+=1/controller_frequency_) {
+ pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
+ r.sleep();
+ }
+
+ }
twist.linear.x = 0.0;
ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time);
@@ -233,6 +543,7 @@ namespace twist_recovery
pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t));
r.sleep();
}
+
}
} // namespace twist_recovery