Updated values for calculate maneuvers in twist_recovery

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_);