Fixed bug with rotational_recovery

rotational_recovery/src/rotational_recovery.cpp

@@ -260,20 +260,27 @@ namespace rotational_recovery
     // 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 = 1.0;
+    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
     geometry_msgs::Twist twist;
-    twist = base_frame_twist_;
-    twist.angular.z = angular_speed_steer_;
-    double max_duration = angle / angular_speed_steer_;
+    if (angle<0.0) {
+      twist.angular.z = -1.0 *angular_speed_steer_;
+    }
+    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, angular_speed_steer_);
+    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_recovery/include/twist_recovery/twist_recovery.h

@@ -83,6 +83,7 @@ namespace twist_recovery
 
       double duration_;
       double linear_speed_limit_;
+      double linear_speed_steer_;
       double angular_speed_limit_;
       double linear_acceleration_limit_;
       double angular_acceleration_limit_;

twist_recovery/src/twist_recovery.cpp

@@ -44,7 +44,14 @@ namespace twist_recovery
   // Constructor
   TwistRecovery::TwistRecovery () :
   global_costmap_(NULL), local_costmap_(NULL), tf_(NULL), initialized_(false)
-  {}
+  {
+    base_frame_twist_.linear.x = 0.0;
+    base_frame_twist_.linear.y = 0.0;
+    base_frame_twist_.linear.z = 0.0;
+    base_frame_twist_.angular.x = 0.0;
+    base_frame_twist_.angular.y = 0.0;
+    base_frame_twist_.angular.z = 0.0;
+  }
 
   // Destructor
   TwistRecovery::~TwistRecovery ()
@@ -84,6 +91,7 @@ namespace twist_recovery
 
     private_nh.param("duration", duration_, 3.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_, 1.0);
     private_nh.param("linear_acceleration_limit", linear_acceleration_limit_, 4.0);
     private_nh.param("angular_acceleration_limit", angular_acceleration_limit_, 3.2);
@@ -201,21 +209,23 @@ namespace twist_recovery
     // 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", base_frame_twist_.linear.x,
-    base_frame_twist_.linear.y, base_frame_twist_.angular.z, max_duration);
+    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(base_frame_twist_, max_duration-t));
+      pub_.publish(scaleGivenAccelerationLimits(twist, max_duration-t));
       r.sleep();
     }
 
-    base_frame_twist_.linear.x = 0.0;
+    twist.linear.x = 0.0;
     ROS_INFO_NAMED ("top", "Applying STOP for %.2f s", stop_time);
     for (double t=0; t<stop_time; t+=1/controller_frequency_) {
-      pub_.publish(scaleGivenAccelerationLimits(base_frame_twist_, stop_time-t));
+      pub_.publish(scaleGivenAccelerationLimits(twist, stop_time-t));
       r.sleep();
     }
   }