From faaf8243ef31890a9e5dbc5fbe43c3eed0aead10 Mon Sep 17 00:00:00 2001 From: Michael Ferguson Date: Tue, 10 Dec 2024 20:13:26 -0500 Subject: [PATCH] improve base calibration (#190) * add rollout calibration using linear movements * parameterize the calibration_steps --- README.md | 61 ++++++--- .../optimization/base_calibration.hpp | 17 ++- robot_calibration/src/base_calibration.cpp | 125 ++++++++++++++---- .../src/nodes/base_calibration.cpp | 47 +++++-- 4 files changed, 186 insertions(+), 64 deletions(-) diff --git a/README.md b/README.md index 3b9106c4..d9e19031 100644 --- a/README.md +++ b/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: - - * /base_controller/track_width - this is the default track width. - * /imu/gyro/scale - this is the initial gyro scale. - * ~min_angle/~max_angle 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: + + * calibration_steps - should be a list of string names of calibration + steps to run. + * step_name/type - should be either `spin` or `rollout`. + * step_name/velocity - velocity to move the robot. This will be + interpreted as angular velocity for `spin` steps and linear velocity for `rollout` + steps. + * step_name/rotations - only valid for `spin` steps. Number of + rotations to complete at given `velocity`. + * step_name/distance - only valid for `rollout` steps. Distance + in meters, to rollout the robot. + +Additional parameters: + + * accel_limit - acceleration limit for `spin` steps (radians/second^2). + * linear_accel_limit - acceleration limit for `rollout` steps (meters/second^2). + * min_angle/max_angle how much of the laser scan to use when measuring the wall angle (radians). - * ~accel_limit - acceleration limit for rotation (radians/second^2). Node topics: @@ -464,10 +471,22 @@ Node topics: * /cmd_vel - the node publishes rotation commands to this topic, unless manual mode is enabled. Message type is geometry_msgs/Twist. -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 diff --git a/robot_calibration/include/robot_calibration/optimization/base_calibration.hpp b/robot_calibration/include/robot_calibration/optimization/base_calibration.hpp index 1f35b48c..e34c4d4c 100644 --- a/robot_calibration/include/robot_calibration/optimization/base_calibration.hpp +++ b/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::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 scan_; std::vector imu_; std::vector odom_; + std::vector rollout_odom_; + std::vector rollout_scan_; + std::recursive_mutex data_mutex_; bool ready_; }; diff --git a/robot_calibration/src/base_calibration.cpp b/robot_calibration/src/base_calibration.cpp index ce03c551..490919ec 100644 --- a/robot_calibration/src/base_calibration.cpp +++ b/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("accel_limit", 2.0); + linear_accel_limit_ = this->declare_parameter("linear_accel_limit", 0.5); // Maximum velocity to command base during alignment align_velocity_ = this->declare_parameter("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("base_controller/track_width", 0.37476); - imu = this->declare_parameter("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 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 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 diff --git a/robot_calibration/src/nodes/base_calibration.cpp b/robot_calibration/src/nodes/base_calibration.cpp index 677f50ad..6e8efefc 100644 --- a/robot_calibration/src/nodes/base_calibration.cpp +++ b/robot_calibration/src/nodes/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. * @@ -32,15 +32,44 @@ int main(int argc, char** argv) bool verbose = b->declare_parameter("verbose", false); - // Rotate at several different speeds - b->spin(0.5, 1, verbose); - b->spin(1.5, 1, verbose); - b->spin(3.0, 2, verbose); - b->spin(-0.5, 1, verbose); - b->spin(-1.5, 1, verbose); - b->spin(-3.0, 2, verbose); + // Load parameters + auto calibration_steps = b->declare_parameter>( + "calibration_steps", std::vector()); - // TODO: drive towards wall, to calibrate rollout + if (calibration_steps.empty()) + { + RCLCPP_WARN(b->get_logger(), "No calibration_steps defined, using defaults"); + // Rotate at several different speeds + b->spin(0.5, 1, verbose); + b->spin(1.5, 1, verbose); + b->spin(3.0, 2, verbose); + b->spin(-0.5, 1, verbose); + b->spin(-1.5, 1, verbose); + b->spin(-3.0, 2, verbose); + } + else + { + for (auto step : calibration_steps) + { + std::string step_type = b->declare_parameter(step + ".type", ""); + if (step_type == "spin") + { + double velocity = b->declare_parameter(step + ".velocity", 1.0); + int rotations = b->declare_parameter(step + ".rotations", 1); + b->spin(velocity, rotations, verbose); + } + else if (step_type == "rollout") + { + double velocity = b->declare_parameter(step + ".velocity", 1.0); + double distance = b->declare_parameter(step + ".distance", 0.0); + b->rollout(velocity, distance, verbose); + } + else + { + RCLCPP_ERROR(b->get_logger(), "Unrecognized step type: %s", step_type.c_str()); + } + } + } // Output yaml file {