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
{