improve base calibration (#190)

* add rollout calibration using linear movements
* parameterize the calibration_steps

README.md

@@ -12,9 +12,9 @@ configuration YAML in the case of camera intrinsics.
 
 Two additional ROS nodes are used for mobile-base related parameter tuning:
 
- * _base_calibration_node_ - can determine scaling factors for gyro and track
-   width parameters by rotating the robot in place and tracking the actual
-   rotation based on the laser scanner view of a wall.
+ * _base_calibration_node_ - can determine scaling factors for wheel diameter,
+   track width and gyro gain by moving and rotating the robot while tracking
+   the actual movement based on the laser scanner view of a wall.
  * _magnetometer_calibration_ - can be used to do hard iron calibration
    of a magnetometer.
 
@@ -437,21 +437,28 @@ free.
 ## The _base_calibration_node_
 
 To run the _base_calibration_node_ node, you need a somewhat open space with a large
-(~3 meters wide) wall that you can point the robot at. The robot should be
-pointed at the wall and it will then spin around at several different speeds.
-On each rotation it will stop and capture the laser data. Afterwards, the
-node uses the angle of the wall as measured by the laser scanner to determine
-how far the robot has actually rotated versus the measurements from the gyro
-and odometry. We then compute scalar corrections for both the gyro and the
-odometry.
-
-Node parameters:
-
- * <code>/base_controller/track_width</code> - this is the default track width.
- * <code>/imu/gyro/scale</code> - this is the initial gyro scale.
- * <code>~min_angle/~max_angle</code> how much of the laser scan to use when
+(~3 meters wide) wall that you can point the robot at.
+
+Starting with the `0.10` release of `robot_calibration`, the actual movements the
+robot does can be programmed via the `calibration_steps` parameter:
+
+ * <code>calibration_steps</code> - should be a list of string names of calibration
+   steps to run.
+ * <code>step_name/type</code> - should be either `spin` or `rollout`.
+ * <code>step_name/velocity</code> - velocity to move the robot. This will be
+   interpreted as angular velocity for `spin` steps and linear velocity for `rollout`
+   steps.
+ * <code>step_name/rotations</code> - only valid for `spin` steps. Number of
+   rotations to complete at given `velocity`.
+ * <code>step_name/distance</code> - only valid for `rollout` steps. Distance
+   in meters, to rollout the robot.
+
+Additional parameters:
+
+ * <code>accel_limit</code> - acceleration limit for `spin` steps (radians/second^2).
+ * <code>linear_accel_limit</code> - acceleration limit for `rollout` steps (meters/second^2).
+ * <code>min_angle/max_angle</code> how much of the laser scan to use when
    measuring the wall angle (radians).
- * <code>~accel_limit</code> - acceleration limit for rotation (radians/second^2).
 
 Node topics:
 
@@ -464,10 +471,22 @@ Node topics:
  * <code>/cmd_vel</code> - the node publishes rotation commands to this topic, unless
    manual mode is enabled. Message type is <code>geometry_msgs/Twist</code>.
 
-The output of the node is a new scale for the gyro and the odometry. The application
-of these values is largely dependent on the drivers being used for the robot. For
-robots using _ros_control_ or _robot_control_ there is a track_width parameter
-typically supplied as a ROS parameter in your launch file.
+The output of the node is a series of scalars to apply (where a value of 1.0 means
+there is currently no error):
+
+```
+[base_calibration_node-1] track_width_scale: 0.986743
+[base_calibration_node-1] imu_scale: 0.984465
+[base_calibration_node-1] rollout_scale: 0.981911
+```
+
+The application of these values is largely dependent on the drivers being used
+for the robot. For robots using _ros_control_ or _robot_control_ there is a
+`track_width` parameter typically supplied as a ROS parameter in your launch file.
+
+Note: in ROS 1, these scalars were pre-multiplied by the existing `track_width`
+or `imu_scale` - however, with the lack of a unified parameter server in ROS 2,
+this is no longer done.
 
 ## The _magnetometer_calibration_ node
 

robot_calibration/include/robot_calibration/optimization/base_calibration.hpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022 Michael Ferguson
+ * Copyright (C) 2022-2024 Michael Ferguson
  * Copyright (C) 2015 Fetch Robotics Inc.
  * Copyright (C) 2014 Unbounded Robotics Inc.
  *
@@ -56,13 +56,16 @@ class BaseCalibration : public rclcpp::Node
   /** @brief Spin and record imu, odom, scan. */
   bool spin(double velocity, int rotations, bool verbose = false);
 
+  /** @brief Move forward/backward and record odom and scan */
+  bool rollout(double velocity, double distance, bool verbose = false);
+
 private:
   void odometryCallback(const nav_msgs::msg::Odometry::ConstSharedPtr& odom);
   void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr& imu);
   void laserCallback(const sensor_msgs::msg::LaserScan::ConstSharedPtr& scan);
 
-  /** @brief Send a rotational velocity command. **/
-  void sendVelocityCommand(double vel);
+  /** @brief Send a velocity command. **/
+  void sendVelocityCommand(double linear, double angular);
 
   /** @brief Reset the odom/imu counters. */
   void resetInternal();
@@ -74,7 +77,8 @@ class BaseCalibration : public rclcpp::Node
   rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr scan_subscriber_;
 
   rclcpp::Time last_odom_stamp_;
-  double odom_angle_;
+  // These are actually "accumulated" values since last reset
+  double odom_angle_, odom_dist_;
 
   rclcpp::Time last_imu_stamp_;
   double imu_angle_;
@@ -83,13 +87,16 @@ class BaseCalibration : public rclcpp::Node
   double scan_angle_, scan_r2_, scan_dist_, r2_tolerance_;
 
   double min_angle_, max_angle_;
-  double accel_limit_;
+  double accel_limit_, linear_accel_limit_;
   double align_velocity_, align_gain_, align_tolerance_;
 
   std::vector<double> scan_;
   std::vector<double> imu_;
   std::vector<double> odom_;
 
+  std::vector<double> rollout_odom_;
+  std::vector<double> rollout_scan_;
+
   std::recursive_mutex data_mutex_;
   bool ready_;
 };

robot_calibration/src/base_calibration.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2022 Michael Ferguson
+ * Copyright (C) 2022-2024 Michael Ferguson
  * Copyright (C) 2015 Fetch Robotics Inc.
  * Copyright (C) 2014 Unbounded Robotics Inc.
  *
@@ -42,6 +42,7 @@ BaseCalibration::BaseCalibration()
 
   // How fast to accelerate
   accel_limit_ = this->declare_parameter<double>("accel_limit", 2.0);
+  linear_accel_limit_ = this->declare_parameter<double>("linear_accel_limit", 0.5);
   // Maximum velocity to command base during alignment
   align_velocity_ = this->declare_parameter<double>("align_velocity", 0.2);
   // Gain to turn alignment error into velocity
@@ -70,6 +71,9 @@ void BaseCalibration::clearMessages()
   scan_.clear();
   odom_.clear();
   imu_.clear();
+
+  rollout_odom_.clear();
+  rollout_scan_.clear();
 }
 
 std::string BaseCalibration::print()
@@ -81,27 +85,45 @@ std::string BaseCalibration::print()
 
 std::string BaseCalibration::printCalibrationData()
 {
-  double odom, imu;
-  odom = this->declare_parameter<double>("base_controller/track_width", 0.37476);
-  imu = this->declare_parameter<double>("imu_gyro_scale", 0.001221729);
+  // Output calibrated values
+  std::stringstream ss;
+
+  // Compute track width and gyro scale
+  if (!scan_.empty())
+  {
+    double odom_scale = 0.0;
+    double imu_scale = 0.0;
+
+    // Get sum
+    for (size_t i = 0; i < scan_.size(); ++i)
+    {
+      odom_scale += (scan_[i] - odom_[i]) / odom_[i];
+      imu_scale += (scan_[i] - imu_[i]) / imu_[i];
+    }
+    // Divide sum by size
+    odom_scale /= scan_.size();
+    imu_scale /= scan_.size();
 
-  // Scaling to be computed
-  double odom_scale = 0.0;
-  double imu_scale = 0.0;
+    ss << "track_width_scale: " << (1.0 + odom_scale) << std::endl;
+    ss << "imu_scale: " << (1.0 + imu_scale) << std::endl;
+  }
 
-  // Get sum
-  for (size_t i = 0; i < scan_.size(); ++i)
+  // Compute rollout scale
+  if (!rollout_odom_.empty())
   {
-    odom_scale += (scan_[i] - odom_[i]) / odom_[i];
-    imu_scale += (scan_[i] - imu_[i]) / imu_[i];
+    double scale = 0.0;
+
+    // Get sums
+    for (size_t i = 0; i < rollout_odom_.size(); ++i)
+    {
+      scale += (rollout_scan_[i] - rollout_odom_[i]) / rollout_odom_[i];
+    }
+    // Divide sum by size
+    scale /= rollout_odom_.size();
+
+    ss << "rollout_scale: " << (1.0 + scale) << std::endl;
   }
-  // Divide sum by size
-  odom_scale /= scan_.size();
-  imu_scale /= scan_.size();
-  // Output odom/imu values
-  std::stringstream ss;
-  ss << "odom: " << odom * (1.0 + odom_scale) << std::endl;
-  ss << "imu: " << imu * (1.0 + imu_scale) << std::endl;
+
   return ss.str();
 }
 
@@ -125,8 +147,8 @@ bool BaseCalibration::align(double angle, bool verbose)
     }
 
     // Send command
-    double velocity = std::min(std::max(-error * align_gain_, -align_velocity_), align_velocity_);
-    sendVelocityCommand(velocity);
+    double velocity = std::min(std::max(error * align_gain_, -align_velocity_), align_velocity_);
+    sendVelocityCommand(0.0, velocity);
 
     // Sleep a moment
     rclcpp::sleep_for(std::chrono::milliseconds(20));
@@ -138,13 +160,13 @@ bool BaseCalibration::align(double angle, bool verbose)
     // Exit if shutting down
     if (!rclcpp::ok())
     {
-      sendVelocityCommand(0.0);
+      sendVelocityCommand(0.0, 0.0);
       return false;
     }
   }
 
   // Done - stop the robot
-  sendVelocityCommand(0.0);
+  sendVelocityCommand(0.0, 0.0);
   std::cout << "...done" << std::endl;
   rclcpp::sleep_for(std::chrono::milliseconds(250));
 
@@ -169,20 +191,20 @@ bool BaseCalibration::spin(double velocity, int rotations, bool verbose)
     {
       std::cout << scan_angle_ << " " << odom_angle_ << " " << imu_angle_ << std::endl;
     }
-    sendVelocityCommand(velocity);
+    sendVelocityCommand(0.0, velocity);
     rclcpp::sleep_for(std::chrono::milliseconds(20));
     rclcpp::spin_some(this->shared_from_this());
 
     // Exit if shutting down
     if (!rclcpp::ok())
     {
-      sendVelocityCommand(0.0);
+      sendVelocityCommand(0.0, 0.0);
       return false;
     }
   }
 
   // Stop the robot
-  sendVelocityCommand(0.0);
+  sendVelocityCommand(0.0, 0.0);
   std::cout << "...done" << std::endl;
 
   // Wait to stop
@@ -203,12 +225,56 @@ bool BaseCalibration::spin(double velocity, int rotations, bool verbose)
   return true;
 }
 
+bool BaseCalibration::rollout(double velocity, double distance, bool verbose)
+{
+  // Align straight at the wall
+  align(0.0, verbose);
+  resetInternal();
+  std::cout << "rollout..." << std::endl;
+
+  double scan_start = scan_dist_;
+
+  // Need to account for de-acceleration time (v^2/2a)
+  double d = fabs(distance) - (0.5 * velocity * velocity / linear_accel_limit_);
+
+  while (fabs(odom_dist_) < d)
+  {
+    if (verbose)
+    {
+      std::cout << odom_dist_ << " " << scan_dist_ << std::endl;
+    }
+    sendVelocityCommand(velocity, 0.0);
+    rclcpp::sleep_for(std::chrono::milliseconds(20));
+    rclcpp::spin_some(this->shared_from_this());
+
+    // Exit if shutting down
+    if (!rclcpp::ok())
+    {
+      sendVelocityCommand(0.0, 0.0);
+      return false;
+    }
+  }
+
+  // Stop the robot
+  sendVelocityCommand(0.0, 0.0);
+  std::cout << "...done" << std::endl;
+
+  // Wait to stop
+  rclcpp::sleep_for(std::chrono::seconds(1));
+
+  // Save measurements
+  rollout_odom_.push_back(odom_dist_);
+  rollout_scan_.push_back(scan_start - scan_dist_);
+  return true;
+}
+
 void BaseCalibration::odometryCallback(const nav_msgs::msg::Odometry::ConstSharedPtr& odom)
 {
   std::lock_guard<std::recursive_mutex> lock(data_mutex_);
 
   double dt = rclcpp::Time(odom->header.stamp).seconds() - last_odom_stamp_.seconds();
   odom_angle_ += odom->twist.twist.angular.z * dt;
+  odom_dist_ += odom->twist.twist.linear.x * dt;
 
   last_odom_stamp_ = odom->header.stamp;
 }
@@ -294,23 +360,24 @@ void BaseCalibration::laserCallback(const sensor_msgs::msg::LaserScan::ConstShar
   }
 
   scan_dist_ = mean_y;
-  scan_angle_ = atan2((n*xy-x*y)/(n*xx-x*x), 1.0);
+  scan_angle_ = atan2((n * xy - x * y)/(n * xx - x * x), 1.0);
   scan_r2_ = fabs(xy)/(xx * yy);
   last_scan_stamp_ = scan->header.stamp;
   ready_ = true;
 }
 
-void BaseCalibration::sendVelocityCommand(double vel)
+void BaseCalibration::sendVelocityCommand(double linear, double angular)
 {
   geometry_msgs::msg::Twist twist;
-  twist.angular.z = vel;
+  twist.linear.x = linear;
+  twist.angular.z = angular;
   cmd_pub_->publish(twist);
 }
 
 void BaseCalibration::resetInternal()
 {
   std::lock_guard<std::recursive_mutex> lock(data_mutex_);
-  odom_angle_ = imu_angle_ = scan_angle_ = scan_r2_ = 0.0;
+  odom_angle_ = odom_dist_ = imu_angle_ = scan_angle_ = scan_r2_ = 0.0;
 }
 
 }  // namespace robot_calibration