diff --git a/rotational_recovery/src/rotational_recovery.cpp b/rotational_recovery/src/rotational_recovery.cpp index a2864c10467dac2a99df8494a1fef819843e6ff3..edc43487f29d9ca04ce9cb2399bb5a35ef290084 100644 --- a/rotational_recovery/src/rotational_recovery.cpp +++ b/rotational_recovery/src/rotational_recovery.cpp @@ -341,6 +341,7 @@ namespace rotational_recovery return; case 2: // Two lethal costs mean a little edge + ROS_WARN_NAMED ("top", "RotationalRecovery (lethal cost algorithm): case with 2 sides with lethal cost"); // 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 @@ -399,6 +400,7 @@ namespace rotational_recovery // 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 + ROS_WARN_NAMED ("top", "RotationalRecovery (lethal cost algorithm): case with 3 sides with lethal cost including front"); 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; @@ -415,6 +417,7 @@ namespace rotational_recovery } } else if (cost_front_1 < cost) { // Case 1.forwards + ROS_WARN_NAMED ("top", "RotationalRecovery (lethal cost algorithm): case with 3 sides with lethal cost including back"); 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; @@ -431,6 +434,7 @@ namespace rotational_recovery } } else { // Case 2 + ROS_WARN_NAMED ("top", "RotationalRecovery (lethal cost algorithm): case with 3 sides with lethal cost including front AND back"); if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right double min_time = 1.0; double turn_duration = 0.0; diff --git a/twist_recovery/src/twist_recovery.cpp b/twist_recovery/src/twist_recovery.cpp index cb4e0340e80fb31b06b480eb1e09d406512781d3..21659d0723cf7c0058b655aa83871bea2bf847ce 100644 --- a/twist_recovery/src/twist_recovery.cpp +++ b/twist_recovery/src/twist_recovery.cpp @@ -292,6 +292,7 @@ namespace twist_recovery return; case 2: // Two lethal costs mean a little edge + ROS_WARN_NAMED ("top", "TwistRecovery (lethal cost algorithm): case with 2 sides with lethal cost"); // 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 @@ -350,6 +351,7 @@ namespace twist_recovery // 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 + ROS_WARN_NAMED ("top", "TwistRecovery (lethal cost algorithm): case with 3 sides with lethal cost including front"); 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; @@ -366,6 +368,7 @@ namespace twist_recovery } } else if (cost_front_1 < cost) { // Case 1.forwards + ROS_WARN_NAMED ("top", "TwistRecovery (lethal cost algorithm): case with 3 sides with lethal cost including back"); 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; @@ -382,6 +385,7 @@ namespace twist_recovery } } else { // Case 2 + ROS_WARN_NAMED ("top", "TwistRecovery (lethal cost algorithm): case with 3 sides with lethal cost including front AND back"); if (cost_left_1 >= cost) { // Lethal cost on the left, move towards right double min_time = 1.0; double turn_duration = 0.0;