add feature to align to arbitrary angle

robot_calibration/include/robot_calibration/calibration/base_calibration.h

@@ -44,8 +44,12 @@ class BaseCalibration
   /** @brief Print out the calibration data. */
   std::string printCalibrationData();
 
-  /** @brief Align to the wall. */
-  bool align(bool verbose = false);
+  /**
+   * @brief Align to the wall.
+   * @param Angle angle to align to wall.
+   * @param verbose Should the console output be stupidly verbose?
+   */
+  bool align(double angle, bool verbose = false);
 
   /** @brief Spin and record imu, odom, scan. */
   bool spin(double velocity, int rotations, bool verbose = false);

robot_calibration/src/base_calibration.cpp

@@ -100,7 +100,7 @@ std::string BaseCalibration::printCalibrationData()
   return ss.str();
 }
 
-bool BaseCalibration::align(bool verbose)
+bool BaseCalibration::align(double angle, bool verbose)
 {
   while (!ready_)
   {
@@ -109,22 +109,28 @@ bool BaseCalibration::align(bool verbose)
 
   std::cout << "aligning..." << std::endl;
 
+  double error = scan_angle_ - angle;
+
   double velocity = 0.2;
-  if (scan_angle_ < 0)
+  if (error < 0)
   {
     velocity = -0.2;
   }
 
-  while (fabs(scan_angle_) > 0.2 || (scan_r2_ < 0.1))
+  while (fabs(error) > 0.2 || (scan_r2_ < 0.1))
   {
     if (verbose)
     {
       std::cout << scan_r2_ << " " << scan_angle_ << std::endl;
     }
     sendVelocityCommand(velocity);
     ros::Duration(0.02).sleep();
+
+    // Update error before comparing again
+    error = scan_angle_ - angle;
   }
 
+  // Done - stop the robot
   sendVelocityCommand(0.0);
   std::cout << "...done" << std::endl;
   ros::Duration(0.25).sleep();
@@ -136,7 +142,8 @@ bool BaseCalibration::spin(double velocity, int rotations, bool verbose)
 {
   double scan_start = scan_angle_;
 
-  align();
+  // Align straight at the wall
+  align(0.0, verbose);
   resetInternal();
   std::cout << "spin..." << std::endl;