From dc38d7f44e046ba32c906e888a3468f850a381c1 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Wed, 20 Jan 2016 21:04:13 -0500 Subject: [PATCH] Create linear/quadratic meas cov model classes Both model also support a wheel resolution param, which is 0.0 by default. --- diff_drive_controller/CMakeLists.txt | 8 +- .../cfg/DiffDriveController.cfg | 2 + .../diff_drive_controller.h | 9 +- .../linear_meas_covariance_model.h | 93 ++++++++++++ .../meas_covariance_model.h | 143 ++++++++++++++++++ .../include/diff_drive_controller/odometry.h | 37 ++--- .../quadratic_meas_covariance_model.h | 114 ++++++++++++++ .../src/diff_drive_controller.cpp | 36 ++++- .../src/linear_meas_covariance_model.cpp | 72 +++++++++ diff_drive_controller/src/odometry.cpp | 33 ++-- .../src/quadratic_meas_covariance_model.cpp | 76 ++++++++++ diff_drive_controller/test/odometry_test.cpp | 10 +- 12 files changed, 574 insertions(+), 59 deletions(-) create mode 100644 diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h create mode 100644 diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h create mode 100644 diff_drive_controller/include/diff_drive_controller/quadratic_meas_covariance_model.h create mode 100644 diff_drive_controller/src/linear_meas_covariance_model.cpp create mode 100644 diff_drive_controller/src/quadratic_meas_covariance_model.cpp diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 252f922cf..326f3955c 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -49,12 +49,16 @@ include_directories( include ${catkin_INCLUDE_DIRS} include ${Boost_INCLUDE_DIRS} include ${EIGEN_INCLUDE_DIRS} - include ${sophus_INCLUDE_DIRS} include ${CERES_INCLUDE_DIRS}) roslint_cpp() -add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) +add_library(${PROJECT_NAME} + src/diff_drive_controller.cpp + src/linear_meas_covariance_model.cpp + src/quadratic_meas_covariance_model.cpp + src/odometry.cpp + src/speed_limiter.cpp) # Note that the entry for ${Ceres_LIBRARIES} was removed as we only used headers from that package target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${PROJECT_NAME}_gencfg) diff --git a/diff_drive_controller/cfg/DiffDriveController.cfg b/diff_drive_controller/cfg/DiffDriveController.cfg index 9d1501832..ee615672d 100755 --- a/diff_drive_controller/cfg/DiffDriveController.cfg +++ b/diff_drive_controller/cfg/DiffDriveController.cfg @@ -16,6 +16,8 @@ gen.add("right_wheel_radius_multiplier", double_t, 0, "Right wheel radius multi gen.add("k_l", double_t, 0, "Covariance model k_l multiplier.", 1.0, 0.0, 10.0) gen.add("k_r", double_t, 0, "Covariance model k_r multiplier.", 1.0, 0.0, 10.0) +gen.add("wheel_resolution", double_t, 0, "Wheel position resolution [rad] (used in the Covariance model).", 0.0, 0.0, 10.0) + gen.add("publish_cmd_vel_limited", bool_t, 0, "Publish limited velocity command.", False) gen.add("publish_state", bool_t, 0, "Publish joint trajectory controller state.", False) diff --git a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h index c69f2368d..0a0bcb44e 100644 --- a/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h +++ b/diff_drive_controller/include/diff_drive_controller/diff_drive_controller.h @@ -205,6 +205,8 @@ namespace diff_drive_controller double k_l; double k_r; + double wheel_resolution; + bool publish_state; bool publish_cmd_vel_limited; @@ -216,8 +218,9 @@ namespace diff_drive_controller , wheel_separation_multiplier(1.0) , left_wheel_radius_multiplier(1.0) , right_wheel_radius_multiplier(1.0) - , k_l(1.0) - , k_r(1.0) + , k_l(0.1) + , k_r(0.1) + , wheel_resolution(0.0001) , publish_state(false) , publish_cmd_vel_limited(false) , control_frequency_desired(0.0) @@ -241,6 +244,8 @@ namespace diff_drive_controller double k_l_; double k_r_; + double wheel_resolution_; // [rad] + /// Timeout to consider cmd_vel commands old: double cmd_vel_timeout_; diff --git a/diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h b/diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h new file mode 100644 index 000000000..7eeafacf9 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/linear_meas_covariance_model.h @@ -0,0 +1,93 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Clearpath Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef LINEAR_MEAS_COVARIANCE_MODEL_H_ +#define LINEAR_MEAS_COVARIANCE_MODEL_H_ + +#include + +namespace diff_drive_controller +{ + + /** + * \brief Linear Meas(urement) Covariance Model + * + * The odometry covariance is computed according with the model presented in: + * + * [Siegwart, 2004]: + * Roland Siegwart, Illah R. Nourbakhsh + * Introduction to Autonomous Mobile Robots + * 1st Edition, 2004 + * + * Section: + * 5.2.4 'An error model for odometric position estimation' (pp. 186-191) + * + * Although the twist covariance doesn't appear explicitly, the implementation + * here is based on the same covariance model used for the pose covariance. + * + * The model also includes the wheel resolution, as a constant additive + * diagonal covariance, that can be easily disabled by using a zero + * (ideal/perfect) wheel resolution. + */ + class LinearMeasCovarianceModel : public MeasCovarianceModel + { + public: + /** + * \brief Constructor + * \param[in] wheel_resolution Wheel resolution [rad] + */ + LinearMeasCovarianceModel(const double wheel_resolution = 0.0); + + /** + * \brief Destructor + */ + virtual ~LinearMeasCovarianceModel() + {} + + /** + * \brief Integrates w/o computing the jacobians + * \param [in] dp_l Left wheel position increment [rad] + * \param [in] dp_r Right wheel position increment [rad] + * \return Meas(urement) covariance + */ + const MeasCovariance& compute(const double dp_l, const double dp_r); + }; + +} // namespace diff_drive_controller + +#endif /* LINEAR_MEAS_COVARIANCE_MODEL_H_ */ diff --git a/diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h b/diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h new file mode 100644 index 000000000..136419f1d --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/meas_covariance_model.h @@ -0,0 +1,143 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Clearpath Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef MEAS_COVARIANCE_MODEL_H_ +#define MEAS_COVARIANCE_MODEL_H_ + +#include + +namespace diff_drive_controller +{ + + /** + * \brief Meas(urement) Covariance Model + */ + class MeasCovarianceModel + { + public: + /// Meas(urement) covariance type: + typedef Eigen::Matrix2d MeasCovariance; + + /** + * \brief Constructor + * \param[in] wheel_resolution Wheel resolution [rad] + */ + MeasCovarianceModel(const double wheel_resolution = 0.0) + : wheel_resolution_(wheel_resolution) + {} + + virtual ~MeasCovarianceModel() + {} + + /** + * \brief Integrates w/o computing the jacobians + * \param [in] dp_l Left wheel position increment [rad] + * \param [in] dp_r Right wheel position increment [rad] + * \return Meas(urement) covariance + */ + virtual const MeasCovariance& compute(const double dp_l, const double dp_r) = 0; + + /** + * \brief Left wheel covariance gain getter + * \return Left wheel covariance gain + */ + double getKl() const + { + return k_l_; + } + + /** + * \brief Right wheel covariance gain getter + * \return Right wheel covariance gain + */ + double getKr() const + { + return k_r_; + } + + /** + * \brief Wheel resolution getter + * \return Wheel resolution [rad] + */ + double getWheelResolution() const + { + return wheel_resolution_; + } + + /** + * \brief Left wheel covariance gain setter + * \param[in] k_l Left wheel covariance gain + */ + void setKl(const double k_l) + { + k_l_ = k_l; + } + + /** + * \brief Right wheel covariance gain setter + * \param[in] k_r Right wheel covariance gain + */ + void setKr(const double k_r) + { + k_r_ = k_r; + } + + /** + * \brief Wheel resolution setter + * \param[in] wheel_resolution Wheel resolution [rad] + */ + void setWheelResolution(const double wheel_resolution) + { + wheel_resolution_ = wheel_resolution; + } + + protected: + /// Meas(urement) covariance: + MeasCovariance meas_covariance_; + + /// Meas(urement) Covariance Model gains: + double k_l_; + double k_r_; + + /// Wheel resolution [rad]: + double wheel_resolution_; + }; + +} // namespace diff_drive_controller + +#endif /* MEAS_COVARIANCE_MODEL_H_ */ diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 739f3ff8c..2c2134a83 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -50,6 +50,8 @@ #include #include +#include + #include namespace diff_drive_controller @@ -59,19 +61,6 @@ namespace diff_drive_controller /** * \brief The Odometry class handles odometry readings * (2D pose and velocity with related timestamp) - * - * The odometry covariance is computed according with the model presented in: - * - * [Siegwart, 2004]: - * Roland Siegwart, Illah R. Nourbakhsh - * Introduction to Autonomous Mobile Robots - * 1st Edition, 2004 - * - * Section: - * 5.2.4 'An error model for odometric position estimation' (pp. 186-191) - * - * Although the twist covariance doesn't appear explicitly, the implementation - * here is based on the same covariance model used for the pose covariance. */ class Odometry { @@ -85,7 +74,7 @@ namespace diff_drive_controller typedef Covariance PoseCovariance; typedef Covariance TwistCovariance; - typedef Eigen::Matrix2d MeasCovariance; + typedef MeasCovarianceModel::MeasCovariance MeasCovariance; EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -259,8 +248,11 @@ namespace diff_drive_controller * \brief Sets the Measurement Covariance Model parameters: k_l and k_r * \param[in] k_l Left wheel velocity multiplier * \param[in] k_r Right wheel velocity multiplier + * \param[in] wheel_resolution Wheel resolution [rad] (assumed the same for + * both wheels */ - void setMeasCovarianceParams(const double k_l, const double k_r); + void setMeasCovarianceParams(const double k_l, const double k_r, + const double wheel_resolution); /** * \brief Velocity rolling window size setter @@ -295,13 +287,6 @@ namespace diff_drive_controller */ void updateIncrementalPose(const double dp_l, const double dp_r); - /** - * \brief Update the measurement covariance - * \param[in] dp_l Left wheel position increment [rad] - * \param[in] dp_r Right wheel position increment [rad] - */ - void updateMeasCovariance(const double dp_l, const double dp_r); - /** * \brief Reset linear and angular accumulators */ @@ -339,18 +324,14 @@ namespace diff_drive_controller TwistCovariance twist_covariance_; TwistCovariance minimum_twist_covariance_; - /// Measurement covariance: - MeasCovariance meas_covariance_; + /// Meas(urement) Covariance Model: + boost::shared_ptr meas_covariance_model_; /// Wheel kinematic parameters [m]: double wheel_separation_; double left_wheel_radius_; double right_wheel_radius_; - /// Measurement Covariance Model parameters: - double k_l_; - double k_r_; - /// Previous wheel position/state [rad]: double left_position_previous_; double right_position_previous_; diff --git a/diff_drive_controller/include/diff_drive_controller/quadratic_meas_covariance_model.h b/diff_drive_controller/include/diff_drive_controller/quadratic_meas_covariance_model.h new file mode 100644 index 000000000..0e9a51942 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/quadratic_meas_covariance_model.h @@ -0,0 +1,114 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Clearpath Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#ifndef QUADRATIC_MEAS_COVARIANCE_MODEL_H_ +#define QUADRATIC_MEAS_COVARIANCE_MODEL_H_ + +#include + +namespace diff_drive_controller +{ + + /** + * \brief Quadratic Meas(urement) Covariance Model + * + * The odometry covariance is computed using a similar model to the one + * presented in: + * + * [Siegwart, 2004]: + * Roland Siegwart, Illah R. Nourbakhsh + * Introduction to Autonomous Mobile Robots + * 1st Edition, 2004 + * + * Section: + * 5.2.4 'An error model for odometric position estimation' (pp. 186-191) + * + * Although the twist covariance doesn't appear explicitly, the implementation + * here is based on the same covariance model used for the pose covariance. + * + * Instead of using the std::abs of the wheel position increment dp, this + * model uses the squared (quadratic) value of dp, so the proportional term + * (multiplied by k_*_) does NOT depend on the control period dt (see + * explanation below). + * + * This is important because it makes the covariance model independent of + * the control period dt. Since dp = v * dt, the variance actually gives: + * + * dp^2 = (v * dt)^2 = v^2 * dt^2 + * + * where dt^2 gets cancelled when the covariance propagation for the twist + * is applied, i.e. the 1/dt jacobians pre- and post-multiply this + * covariance model, so the control perdio dt cancels out: + * + * (1/dt) * dt^2 * (1/dt) = dt^2 / dt^2 = 1 + * + * The model also includes the wheel resolution, as a constant additive + * diagonal covariance, that can be easily disabled by using a zero + * (ideal/perfect) wheel resolution. + * + * If the wheel resolution (DC offset) term is used, the covariance model + * would again vary with the control period dt because the wheel + * resolution is constant and does NOT include/depend on dt. + */ + class QuadraticMeasCovarianceModel : public MeasCovarianceModel + { + public: + /** + * \brief Constructor + * \param[in] wheel_resolution Wheel resolution [rad] + */ + QuadraticMeasCovarianceModel(const double wheel_resolution = 0.0); + + /** + * \brief Destructor + */ + virtual ~QuadraticMeasCovarianceModel() + {} + + /** + * \brief Integrates w/o computing the jacobians + * \param [in] dp_l Left wheel position increment [rad] + * \param [in] dp_r Right wheel position increment [rad] + * \return Meas(urement) covariance + */ + const MeasCovariance& compute(const double dp_l, const double dp_r); + }; + +} // namespace diff_drive_controller + +#endif /* QUADRATIC_MEAS_COVARIANCE_MODEL_H_ */ diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index a9ebf4202..9e9ead77a 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -132,8 +132,9 @@ namespace diff_drive_controller , wheel_separation_multiplier_(1.0) , left_wheel_radius_multiplier_(1.0) , right_wheel_radius_multiplier_(1.0) - , k_l_(1.0) - , k_r_(1.0) + , k_l_(0.1) + , k_r_(0.1) + , wheel_resolution_(0.0001) , cmd_vel_timeout_(0.5) , base_frame_id_("base_link") , enable_odom_tf_(true) @@ -234,6 +235,21 @@ namespace diff_drive_controller "Taking absolute value: " << k_r_ << "."); } + controller_nh.param("wheel_resolution", wheel_resolution_, wheel_resolution_); + + if (wheel_resolution_ < 0.0) + { + wheel_resolution_ = std::abs(k_l_); + ROS_ERROR_STREAM_NAMED(name_, + "Wheel resolution must be positive! " + "Taking absolute value: " << wheel_resolution_ << "."); + } + else if (wheel_resolution_ == 0.0) + { + ROS_WARN_STREAM_NAMED(name_, + "Wheel resolution is 0, but it's recommended to use a value > 0!"); + } + int velocity_rolling_window_size = 10; controller_nh.param("velocity_rolling_window_size", velocity_rolling_window_size, velocity_rolling_window_size); ROS_INFO_STREAM_NAMED(name_, "Velocity rolling window size of " @@ -326,10 +342,11 @@ namespace diff_drive_controller << ", left wheel radius " << wrl << ", right wheel radius " << wrr); - odometry_.setMeasCovarianceParams(k_l_, k_r_); + odometry_.setMeasCovarianceParams(k_l_, k_r_, wheel_resolution_); ROS_INFO_STREAM_NAMED(name_, "Measurement Covariance Model params : k_l " << k_l_ - << ", k_r " << k_r_); + << ", k_r " << k_r_ + << ", wheel resolution [rad] " << wheel_resolution_); dynamic_params_struct_.pose_from_joint_position = pose_from_joint_position_; dynamic_params_struct_.twist_from_joint_position = twist_from_joint_position_; @@ -342,6 +359,8 @@ namespace diff_drive_controller dynamic_params_struct_.k_l = k_l_; dynamic_params_struct_.k_r = k_r_; + dynamic_params_struct_.wheel_resolution = wheel_resolution_; + dynamic_params_struct_.publish_state = publish_state_; dynamic_params_struct_.publish_cmd_vel_limited = publish_cmd_vel_limited_; @@ -506,6 +525,8 @@ namespace diff_drive_controller k_l_ = dynamic_params.k_l; k_r_ = dynamic_params.k_r; + wheel_resolution_ = dynamic_params.wheel_resolution; + publish_state_ = dynamic_params.publish_state; publish_cmd_vel_limited_ = dynamic_params.publish_cmd_vel_limited; @@ -519,7 +540,7 @@ namespace diff_drive_controller // Set the odometry parameters: odometry_.setWheelParams(ws, wrl, wrr); - odometry_.setMeasCovarianceParams(k_l_, k_r_); + odometry_.setMeasCovarianceParams(k_l_, k_r_, wheel_resolution_); // COMPUTE AND PUBLISH ODOMETRY // Read wheel joint positions and velocities: @@ -982,6 +1003,8 @@ namespace diff_drive_controller dynamic_params_struct_.k_l = config.k_l; dynamic_params_struct_.k_r = config.k_r; + dynamic_params_struct_.wheel_resolution = config.wheel_resolution; + dynamic_params_struct_.publish_state = config.publish_state; dynamic_params_struct_.publish_cmd_vel_limited = config.publish_cmd_vel_limited; @@ -1000,7 +1023,8 @@ namespace diff_drive_controller ROS_DEBUG_STREAM_NAMED(name_, "Reconfigured Measurement Covariance Model params. " << "k_l: " << dynamic_params_struct_.k_l << ", " - << "k_r: " << dynamic_params_struct_.k_r); + << "k_r: " << dynamic_params_struct_.k_r << ", " + << "wheel resolution: " << dynamic_params_struct_.wheel_resolution); ROS_DEBUG_STREAM_NAMED(name_, "Reconfigured Debug Publishers params. " diff --git a/diff_drive_controller/src/linear_meas_covariance_model.cpp b/diff_drive_controller/src/linear_meas_covariance_model.cpp new file mode 100644 index 000000000..62aa65ebe --- /dev/null +++ b/diff_drive_controller/src/linear_meas_covariance_model.cpp @@ -0,0 +1,72 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Clearpath Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#include + +namespace diff_drive_controller +{ + +LinearMeasCovarianceModel::LinearMeasCovarianceModel( + const double wheel_resolution) + : MeasCovarianceModel(wheel_resolution) +{ + meas_covariance_.setZero(); +} + +const MeasCovarianceModel::MeasCovariance& +LinearMeasCovarianceModel::compute(const double dp_l, const double dp_r) +{ + /// Measurement (wheel position increment) covaraince model, proportional + /// to the wheel position increment abs(olute) value for each wheel: + const double dp_var_l = k_l_ * std::abs(dp_l); + const double dp_var_r = k_r_ * std::abs(dp_r); + + /// Wheel resolution covariance, which is like a DC offset equal to half of + /// the resolution, which is the theoretical average error: + const double dp_var_avg = 0.5 * wheel_resolution_; + + /// @todo This can be extended to support lateral slippage + /// k_s_ * [see/find Olson notes] + + /// Set covariance matrix (diagonal): + meas_covariance_.diagonal() << dp_var_l + dp_var_avg, + dp_var_r + dp_var_avg; +} + +} // namespace diff_drive_controller + diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index b6bc6d284..15ebf0fc6 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -43,6 +43,9 @@ #include +#include +#include + #include #include #include @@ -71,22 +74,19 @@ namespace diff_drive_controller , wheel_separation_(0.0) , left_wheel_radius_(0.0) , right_wheel_radius_(0.0) - , k_l_(1.0) - , k_r_(1.0) , left_position_previous_(0.0) , right_position_previous_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) , v_x_acc_(RollingWindow::window_size = velocity_rolling_window_size) , v_y_acc_(RollingWindow::window_size = velocity_rolling_window_size) , v_yaw_acc_(RollingWindow::window_size = velocity_rolling_window_size) + , meas_covariance_model_(new LinearMeasCovarianceModel()) , integrate_fun_( new AutoDiffIntegrateFunction( new DirectKinematicsIntegrateFunctor( new ExactIntegrateFunctor))) { - meas_covariance_.setZero(); - minimum_twist_covariance_.setIdentity(); minimum_twist_covariance_ *= DEFAULT_MINIMUM_TWIST_COVARIANCE; @@ -159,11 +159,11 @@ namespace diff_drive_controller (*integrate_fun_)(x_, y_, heading_, dp_l, dp_r, J_pose, J_meas); /// Update Measurement Covariance with the wheel joint position increments: - updateMeasCovariance(dp_l, dp_r); + MeasCovariance meas_covariance = meas_covariance_model_->compute(dp_l, dp_r); /// Update pose covariance: pose_covariance_ = J_pose * pose_covariance_ * J_pose.transpose() + - J_meas * meas_covariance_ * J_meas.transpose(); + J_meas * meas_covariance * J_meas.transpose(); /// Update incremental pose: // @todo in principle there's no need to use v_l, v_r at all, but this @@ -184,12 +184,12 @@ namespace diff_drive_controller (*integrate_fun_)(d_x_, d_y_, d_yaw_, dp_l, dp_r, J_pose, J_meas); /// Update Measurement Covariance with the wheel joint position increments: - updateMeasCovariance(dp_l, dp_r); + MeasCovariance meas_covariance = meas_covariance_model_->compute(dp_l, dp_r); /// Update incremental pose covariance: incremental_pose_covariance_ = J_pose * incremental_pose_covariance_ * J_pose.transpose() + - J_meas * meas_covariance_ * J_meas.transpose(); + J_meas * meas_covariance * J_meas.transpose(); } bool Odometry::updateTwist(const ros::Time& time) @@ -231,15 +231,6 @@ namespace diff_drive_controller return true; } - void Odometry::updateMeasCovariance(double v_l, double v_r) - { - /// Compute Measurement Covariance Model: - // @todo This can be extended to support lateral slippage - // k_s_ * [see/find Olson notes] - meas_covariance_.diagonal() << k_l_ * std::abs(v_l), - k_r_ * std::abs(v_r); - } - void Odometry::setWheelParams(const double wheel_separation, const double left_wheel_radius, const double right_wheel_radius) { @@ -259,10 +250,12 @@ namespace diff_drive_controller resetAccumulators(); } - void Odometry::setMeasCovarianceParams(const double k_l, const double k_r) + void Odometry::setMeasCovarianceParams(const double k_l, const double k_r, + const double wheel_resolution) { - k_l_ = k_l; - k_r_ = k_r; + meas_covariance_model_->setKl(k_l); + meas_covariance_model_->setKr(k_r); + meas_covariance_model_->setWheelResolution(wheel_resolution); } void Odometry::resetAccumulators() diff --git a/diff_drive_controller/src/quadratic_meas_covariance_model.cpp b/diff_drive_controller/src/quadratic_meas_covariance_model.cpp new file mode 100644 index 000000000..20e282edb --- /dev/null +++ b/diff_drive_controller/src/quadratic_meas_covariance_model.cpp @@ -0,0 +1,76 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, Clearpath Robotics, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the PAL Robotics nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* + * Author: Enrique Fernández + */ + +#include + +namespace diff_drive_controller +{ + +QuadraticMeasCovarianceModel::QuadraticMeasCovarianceModel( + const double wheel_resolution) + : MeasCovarianceModel(wheel_resolution) +{ + meas_covariance_.setZero(); +} + +const MeasCovarianceModel::MeasCovariance& +QuadraticMeasCovarianceModel::compute(const double dp_l, const double dp_r) +{ + /// Measurement (wheel position increment) covaraince model, proportional + /// to the wheel position increment squared (quadratic) for each wheel: + const double dp_std_l = k_l_ * dp_l; + const double dp_std_r = k_r_ * dp_r; + + const double dp_var_l = dp_std_l * dp_std_l; + const double dp_var_r = dp_std_r * dp_std_r; + + /// Wheel resolution covariance, which is like a DC offset equal to half of + /// the resolution, which is the theoretical average error: + const double dp_std_avg = 0.5 * wheel_resolution_; + const double dp_var_avg = dp_std_avg * dp_std_avg; + + /// @todo This can be extended to support lateral slippage + /// k_s_ * [see/find Olson notes] + + /// Set covariance matrix (diagonal): + meas_covariance_.diagonal() << dp_var_l + dp_var_avg, + dp_var_r + dp_var_avg; +} + +} // namespace diff_drive_controller + diff --git a/diff_drive_controller/test/odometry_test.cpp b/diff_drive_controller/test/odometry_test.cpp index a84260837..49dc922d6 100644 --- a/diff_drive_controller/test/odometry_test.cpp +++ b/diff_drive_controller/test/odometry_test.cpp @@ -46,6 +46,10 @@ const double RIGHT_WHEEL_RADIUS = LEFT_WHEEL_RADIUS; const double K_L = 0.01; const double K_R = K_L; +// Instead of a more realistic wheel resolution like the one below, we can also +// use 0.0 to avoid minor errors (see EPS_ constants below), but even with 0.0 +// we have them: +const double WHEEL_RESOLUTION = 2.0 * M_PI / (25.0 * 1024); const size_t CONTROL_STEPS = 100; const double CONTROL_PERIOD = 0.02; // [s] @@ -62,7 +66,7 @@ static void setupOdometry(diff_drive_controller::Odometry& odometry) odometry.setWheelParams(WHEEL_SEPARATION, LEFT_WHEEL_RADIUS, RIGHT_WHEEL_RADIUS); - odometry.setMeasCovarianceParams(K_L, K_R); + odometry.setMeasCovarianceParams(K_L, K_R, WHEEL_RESOLUTION); } static void moveOdometry(diff_drive_controller::Odometry& odometry, @@ -168,6 +172,10 @@ TEST(OdometryTest, testIntegrateMotionNoMoveFromInitial) diff_drive_controller::Odometry odometry(1); setupOdometry(odometry); + // Set wheel resolution to 0.0, because when it's != 0.0 the covariance + // always grows a little on every single step, even if the robot doesn't move: + odometry.setMeasCovarianceParams(K_L, K_R, 0.0); + // Save initial/current pose and twist state and covariance: const double x_0 = odometry.getX(); const double y_0 = odometry.getY();