From ab0fc0f6db8d21cc47f20122bbf49eb89f774bb3 Mon Sep 17 00:00:00 2001 From: Eric Tappan Date: Fri, 18 Dec 2015 12:31:58 -0500 Subject: [PATCH 1/3] Added estimate_velocity_from_position param (default true) --- .../src/diff_drive_controller.cpp | 34 +++++++++++++++- diff_drive_controller/src/odometry.cpp | 40 +++++++++++++++++++ .../test/diff_drive_raw_velocity.test | 18 +++++++++ 3 files changed, 91 insertions(+), 1 deletion(-) create mode 100644 diff_drive_controller/test/diff_drive_raw_velocity.test diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 97265ef20..737aeaa30 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -109,6 +109,7 @@ namespace diff_drive_controller{ DiffDriveController::DiffDriveController() : open_loop_(false) + , estimate_velocity_from_position_(true) , command_struct_() , wheel_separation_(0.0) , wheel_radius_(0.0) @@ -162,6 +163,9 @@ namespace diff_drive_controller{ publish_period_ = ros::Duration(1.0 / publish_rate); controller_nh.param("open_loop", open_loop_, open_loop_); + controller_nh.param("estimate_velocity_from_position", estimate_velocity_from_position_, estimate_velocity_from_position_); + ROS_INFO_STREAM_COND_NAMED(estimate_velocity_from_position_, name_, "Velocity will be estimated from position."); + ROS_INFO_STREAM_COND_NAMED(!estimate_velocity_from_position_, name_, "Velocity will not be estimated from position."); controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_); ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " @@ -265,8 +269,13 @@ namespace diff_drive_controller{ } else { + bool trust_velocity = !estimate_velocity_from_position_; + bool never_trusted = estimate_velocity_from_position_; + bool found_nan = false; double left_pos = 0.0; double right_pos = 0.0; + double left_vel = 0.0; + double right_vel = 0.0; for (size_t i = 0; i < wheel_joints_size_; ++i) { const double lp = left_wheel_joints_[i].getPosition(); @@ -276,12 +285,35 @@ namespace diff_drive_controller{ left_pos += lp; right_pos += rp; + + if (!trust_velocity) + continue; + const double lv = left_wheel_joints_[i].getVelocity(); + const double rv = right_wheel_joints_[i].getVelocity(); + if (std::isnan(lv) || std::isnan(rv)) + { + trust_velocity = false; + found_nan = true; + continue; + } + + left_vel += lv; + right_vel += rv; } left_pos /= wheel_joints_size_; right_pos /= wheel_joints_size_; // Estimate linear and angular velocity using joint information - odometry_.update(left_pos, right_pos, time); + if (!trust_velocity) + { + odometry_.update(left_pos, right_pos, time); + } + else + { + left_vel /= wheel_joints_size_; + right_vel /= wheel_joints_size_; + odometry_.update(left_pos, right_pos, left_vel, right_vel, time); + } } // Publish odometry message diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 003572d6e..dc81beb9b 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -110,6 +110,46 @@ namespace diff_drive_controller return true; } + bool Odometry::update(double left_pos, double right_pos, + double left_vel, double right_vel, + const ros::Time &time) + { + /// Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * wheel_radius_; + const double right_wheel_cur_pos = right_pos * wheel_radius_; + + /// Get current wheel joint velocities: + const double dt = (time - timestamp_).toSec(); + const double left_wheel_est_vel = left_vel * wheel_radius_ * dt; + const double right_wheel_est_vel = right_vel * wheel_radius_ * dt; + + /// Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; + const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + } + void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) { /// Save last linear and angular velocity: diff --git a/diff_drive_controller/test/diff_drive_raw_velocity.test b/diff_drive_controller/test/diff_drive_raw_velocity.test new file mode 100644 index 000000000..1fc21dd97 --- /dev/null +++ b/diff_drive_controller/test/diff_drive_raw_velocity.test @@ -0,0 +1,18 @@ + + + + + + diffbot_controller: + estimate_velocity_from_position: false + + + + + + + + From 2b56c57999c37a741ff80376cf0086e40fd11e1c Mon Sep 17 00:00:00 2001 From: Eric Tappan Date: Fri, 18 Dec 2015 12:31:58 -0500 Subject: [PATCH 2/3] Added estimate_velocity_from_position param (default true) --- .../diff_drive_controller.h | 1 + .../include/diff_drive_controller/odometry.h | 12 ++++++ .../src/diff_drive_controller.cpp | 34 +++++++++++++++- diff_drive_controller/src/odometry.cpp | 40 +++++++++++++++++++ .../test/diff_drive_raw_velocity.test | 18 +++++++++ .../test/diff_drive_raw_velocity_invalid.test | 18 +++++++++ 6 files changed, 122 insertions(+), 1 deletion(-) create mode 100644 diff_drive_controller/test/diff_drive_raw_velocity.test create mode 100644 diff_drive_controller/test/diff_drive_raw_velocity_invalid.test 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 9447e22fb..36338da32 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 @@ -124,6 +124,7 @@ namespace diff_drive_controller{ boost::shared_ptr > odom_pub_; boost::shared_ptr > tf_odom_pub_; Odometry odometry_; + bool estimate_velocity_from_position_; /// Wheel separation, wrt the midpoint of the wheel width: double wheel_separation_; diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 82397857d..c00cc702f 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -86,6 +86,18 @@ namespace diff_drive_controller */ bool update(double left_pos, double right_pos, const ros::Time &time); + /** + * \brief Updates the odometry class with latest wheels position and velocity + * \param left_pos Left wheel position [rad] + * \param right_pos Right wheel position [rad] + * \param left_vel Left wheel velocity [rad/s] + * \param right_vel Right wheel velocity [rad/s] + * \param time Current time + * \return true if the odometry is actually updated + */ + bool update(double left_pos, double right_pos, double left_vel, + double right_vel, const ros::Time &time); + /** * \brief Updates the odometry class with latest velocity command * \param linear Linear velocity [m/s] diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 97265ef20..737aeaa30 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -109,6 +109,7 @@ namespace diff_drive_controller{ DiffDriveController::DiffDriveController() : open_loop_(false) + , estimate_velocity_from_position_(true) , command_struct_() , wheel_separation_(0.0) , wheel_radius_(0.0) @@ -162,6 +163,9 @@ namespace diff_drive_controller{ publish_period_ = ros::Duration(1.0 / publish_rate); controller_nh.param("open_loop", open_loop_, open_loop_); + controller_nh.param("estimate_velocity_from_position", estimate_velocity_from_position_, estimate_velocity_from_position_); + ROS_INFO_STREAM_COND_NAMED(estimate_velocity_from_position_, name_, "Velocity will be estimated from position."); + ROS_INFO_STREAM_COND_NAMED(!estimate_velocity_from_position_, name_, "Velocity will not be estimated from position."); controller_nh.param("wheel_separation_multiplier", wheel_separation_multiplier_, wheel_separation_multiplier_); ROS_INFO_STREAM_NAMED(name_, "Wheel separation will be multiplied by " @@ -265,8 +269,13 @@ namespace diff_drive_controller{ } else { + bool trust_velocity = !estimate_velocity_from_position_; + bool never_trusted = estimate_velocity_from_position_; + bool found_nan = false; double left_pos = 0.0; double right_pos = 0.0; + double left_vel = 0.0; + double right_vel = 0.0; for (size_t i = 0; i < wheel_joints_size_; ++i) { const double lp = left_wheel_joints_[i].getPosition(); @@ -276,12 +285,35 @@ namespace diff_drive_controller{ left_pos += lp; right_pos += rp; + + if (!trust_velocity) + continue; + const double lv = left_wheel_joints_[i].getVelocity(); + const double rv = right_wheel_joints_[i].getVelocity(); + if (std::isnan(lv) || std::isnan(rv)) + { + trust_velocity = false; + found_nan = true; + continue; + } + + left_vel += lv; + right_vel += rv; } left_pos /= wheel_joints_size_; right_pos /= wheel_joints_size_; // Estimate linear and angular velocity using joint information - odometry_.update(left_pos, right_pos, time); + if (!trust_velocity) + { + odometry_.update(left_pos, right_pos, time); + } + else + { + left_vel /= wheel_joints_size_; + right_vel /= wheel_joints_size_; + odometry_.update(left_pos, right_pos, left_vel, right_vel, time); + } } // Publish odometry message diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 003572d6e..dc81beb9b 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -110,6 +110,46 @@ namespace diff_drive_controller return true; } + bool Odometry::update(double left_pos, double right_pos, + double left_vel, double right_vel, + const ros::Time &time) + { + /// Get current wheel joint positions: + const double left_wheel_cur_pos = left_pos * wheel_radius_; + const double right_wheel_cur_pos = right_pos * wheel_radius_; + + /// Get current wheel joint velocities: + const double dt = (time - timestamp_).toSec(); + const double left_wheel_est_vel = left_vel * wheel_radius_ * dt; + const double right_wheel_est_vel = right_vel * wheel_radius_ * dt; + + /// Update old position with current: + left_wheel_old_pos_ = left_wheel_cur_pos; + right_wheel_old_pos_ = right_wheel_cur_pos; + + /// Compute linear and angular diff: + const double linear = (right_wheel_est_vel + left_wheel_est_vel) * 0.5 ; + const double angular = (right_wheel_est_vel - left_wheel_est_vel) / wheel_separation_; + + /// Integrate odometry: + integrate_fun_(linear, angular); + + /// We cannot estimate the speed with very small time intervals: + if (dt < 0.0001) + return false; // Interval too small to integrate with + + timestamp_ = time; + + /// Estimate speeds using a rolling mean to filter them out: + linear_acc_(linear/dt); + angular_acc_(angular/dt); + + linear_ = bacc::rolling_mean(linear_acc_); + angular_ = bacc::rolling_mean(angular_acc_); + + return true; + } + void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) { /// Save last linear and angular velocity: diff --git a/diff_drive_controller/test/diff_drive_raw_velocity.test b/diff_drive_controller/test/diff_drive_raw_velocity.test new file mode 100644 index 000000000..1fc21dd97 --- /dev/null +++ b/diff_drive_controller/test/diff_drive_raw_velocity.test @@ -0,0 +1,18 @@ + + + + + + diffbot_controller: + estimate_velocity_from_position: false + + + + + + + + diff --git a/diff_drive_controller/test/diff_drive_raw_velocity_invalid.test b/diff_drive_controller/test/diff_drive_raw_velocity_invalid.test new file mode 100644 index 000000000..dd647553a --- /dev/null +++ b/diff_drive_controller/test/diff_drive_raw_velocity_invalid.test @@ -0,0 +1,18 @@ + + + + + + diffbot_controller: + estimate_velocity_from_position: something_invalid + + + + + + + + From 2ca6776edafdedef53337b03eb33996d02cd0ce7 Mon Sep 17 00:00:00 2001 From: Eric Tappan Date: Fri, 18 Dec 2015 14:58:01 -0500 Subject: [PATCH 3/3] Removed debug flags. --- diff_drive_controller/src/diff_drive_controller.cpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 737aeaa30..343b59149 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -270,8 +270,6 @@ namespace diff_drive_controller{ else { bool trust_velocity = !estimate_velocity_from_position_; - bool never_trusted = estimate_velocity_from_position_; - bool found_nan = false; double left_pos = 0.0; double right_pos = 0.0; double left_vel = 0.0; @@ -293,7 +291,6 @@ namespace diff_drive_controller{ if (std::isnan(lv) || std::isnan(rv)) { trust_velocity = false; - found_nan = true; continue; }