From 9dab59b183f3987ec75d8d889f6a21740c521c97 Mon Sep 17 00:00:00 2001 From: Enrique Fernandez Date: Wed, 3 Jun 2015 14:09:11 -0400 Subject: [PATCH] Fix odometry twist computation The twist is computed as the relative transformation in SE(2) between the previous and current odometry pose, divided by the time step dt. --- diff_drive_controller/CMakeLists.txt | 3 + .../include/diff_drive_controller/odometry.h | 52 +++++++++--- diff_drive_controller/package.xml | 2 + .../src/diff_drive_controller.cpp | 6 +- diff_drive_controller/src/odometry.cpp | 83 ++++++++++++++----- 5 files changed, 108 insertions(+), 38 deletions(-) diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 8f8dcf6fe..1484bb6fa 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -9,6 +9,8 @@ find_package(catkin REQUIRED COMPONENTS urdf ) +find_package(sophus REQUIRED) + catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} @@ -16,6 +18,7 @@ catkin_package( include_directories( include ${catkin_INCLUDE_DIRS} + include ${sophus_INCLUDE_DIRS} ) add_library(${PROJECT_NAME} src/diff_drive_controller.cpp src/odometry.cpp src/speed_limiter.cpp) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index 82397857d..262371e0f 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -43,6 +43,7 @@ #define ODOMETRY_H_ #include +#include #include #include #include @@ -63,6 +64,9 @@ namespace diff_drive_controller /// Integration function, used to integrate the odometry: typedef boost::function IntegrationFunction; + /// SO(2) and SE(2) Lie Groups: + typedef Sophus::SE2d SE2; + /** * \brief Constructor * Timestamp will get the current time value @@ -91,8 +95,19 @@ namespace diff_drive_controller * \param linear Linear velocity [m/s] * \param angular Angular velocity [rad/s] * \param time Current time + * \return true if the odometry is actually updated + */ + bool updateOpenLoop(double linear, double angular, const ros::Time &time); + + /** + * \brief Update the odometry twist with the previous and current odometry + * pose + * \param p0 Previous odometry pose + * \param p1 Current odometry pose + * \param time Current time + * \return true if the odometry twist is actually updated */ - void updateOpenLoop(double linear, double angular, const ros::Time &time); + bool updateTwist(const SE2& p0, const SE2& p1, const ros::Time& time); /** * \brief heading getter @@ -122,21 +137,30 @@ namespace diff_drive_controller } /** - * \brief linear velocity getter - * \return linear velocity [m/s] + * \brief x velocity getter + * \return x velocity [m/s] + */ + double getVx() const + { + return v_x_; + } + + /** + * \brief y velocity getter + * \return y velocity [m/s] */ - double getLinear() const + double getVy() const { - return linear_; + return v_y_; } /** - * \brief angular velocity getter - * \return angular velocity [rad/s] + * \brief yaw velocity getter + * \return yaw velocity [rad/s] */ - double getAngular() const + double getVyaw() const { - return angular_; + return v_yaw_; } /** @@ -186,8 +210,9 @@ namespace diff_drive_controller double heading_; // [rad] /// Current velocity: - double linear_; // [m/s] - double angular_; // [rad/s] + double v_x_; // [m/s] + double v_y_; // [m/s] + double v_yaw_; // [rad/s] /// Wheel kinematic parameters [m]: double wheel_separation_; @@ -199,8 +224,9 @@ namespace diff_drive_controller /// Rolling mean accumulators for the linar and angular velocities: size_t velocity_rolling_window_size_; - RollingMeanAcc linear_acc_; - RollingMeanAcc angular_acc_; + RollingMeanAcc v_x_acc_; + RollingMeanAcc v_y_acc_; + RollingMeanAcc v_yaw_acc_; /// Integration funcion, used to integrate the odometry: IntegrationFunction integrate_fun_; diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index b802fc07b..c77ec20c3 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -15,6 +15,8 @@ catkin + sophus + controller_interface nav_msgs realtime_tools diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 99957b9af..ab721d6b7 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -350,8 +350,9 @@ namespace diff_drive_controller{ odom_pub_->msg_.pose.pose.position.x = odometry_.getX(); odom_pub_->msg_.pose.pose.position.y = odometry_.getY(); odom_pub_->msg_.pose.pose.orientation = orientation; - odom_pub_->msg_.twist.twist.linear.x = odometry_.getLinear(); - odom_pub_->msg_.twist.twist.angular.z = odometry_.getAngular(); + odom_pub_->msg_.twist.twist.linear.x = odometry_.getVx(); + odom_pub_->msg_.twist.twist.linear.y = odometry_.getVy(); + odom_pub_->msg_.twist.twist.angular.z = odometry_.getVyaw(); odom_pub_->unlockAndPublish(); } @@ -618,7 +619,6 @@ namespace diff_drive_controller{ (0) (0) (0) (static_cast(pose_cov_list[3])) (0) (0) (0) (0) (0) (0) (static_cast(pose_cov_list[4])) (0) (0) (0) (0) (0) (0) (static_cast(pose_cov_list[5])); - odom_pub_->msg_.twist.twist.linear.y = 0; odom_pub_->msg_.twist.twist.linear.z = 0; odom_pub_->msg_.twist.twist.angular.x = 0; odom_pub_->msg_.twist.twist.angular.y = 0; diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index 003572d6e..466f5ecfc 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -41,6 +41,8 @@ #include +#include + #include namespace diff_drive_controller @@ -52,15 +54,17 @@ namespace diff_drive_controller , x_(0.0) , y_(0.0) , heading_(0.0) - , linear_(0.0) - , angular_(0.0) + , v_x_(0.0) + , v_y_(0.0) + , v_yaw_(0.0) , wheel_separation_(0.0) , wheel_radius_(0.0) , left_wheel_old_pos_(0.0) , right_wheel_old_pos_(0.0) , velocity_rolling_window_size_(velocity_rolling_window_size) - , linear_acc_(RollingWindow::window_size = velocity_rolling_window_size) - , angular_acc_(RollingWindow::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) , integrate_fun_(boost::bind(&Odometry::integrateExact, this, _1, _2)) { } @@ -90,9 +94,36 @@ namespace diff_drive_controller 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_; + /// Safe current state: + const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_)); + /// Integrate odometry: integrate_fun_(linear, angular); + /// Update twist: + const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_)); + + return updateTwist(p0, p1, time); + } + + bool Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) + { + /// Safe current state: + const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_)); + + /// Integrate odometry: + const double dt = (time - timestamp_).toSec(); + timestamp_ = time; + integrate_fun_(linear * dt, angular * dt); + + /// Update twist: + const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_)); + + return updateTwist(p0, p1, time); + } + + bool Odometry::updateTwist(const SE2& p0, const SE2& p1, const ros::Time& time) + { /// We cannot estimate the speed with very small time intervals: const double dt = (time - timestamp_).toSec(); if (dt < 0.0001) @@ -100,26 +131,33 @@ namespace diff_drive_controller timestamp_ = time; - /// Estimate speeds using a rolling mean to filter them out: - linear_acc_(linear/dt); - angular_acc_(angular/dt); + /// Compute relative transformation: + const SE2 p = p0.inverse() * p1; - linear_ = bacc::rolling_mean(linear_acc_); - angular_ = bacc::rolling_mean(angular_acc_); + /// Retrieve rotation and translation: + /// Note that we don't use the log from SE(2) because we didn't use exp + /// to create p0 and p1. + /// So instead of: + /// + /// const SE2::Tangent v = p.log(); + /// + /// we use the following: + const SE2::ConstTranslationReference t = p.translation(); - return true; - } + v_x_ = t[0]; + v_y_ = t[1]; + v_yaw_ = p.so2().log(); - void Odometry::updateOpenLoop(double linear, double angular, const ros::Time &time) - { - /// Save last linear and angular velocity: - linear_ = linear; - angular_ = angular; + /// Estimate speeds using a rolling mean to filter them out: + v_x_acc_(v_x_/dt); + v_y_acc_(v_y_/dt); + v_yaw_acc_(v_yaw_/dt); - /// Integrate odometry: - const double dt = (time - timestamp_).toSec(); - timestamp_ = time; - integrate_fun_(linear * dt, angular * dt); + v_x_ = bacc::rolling_mean(v_x_acc_); + v_y_ = bacc::rolling_mean(v_y_acc_); + v_yaw_ = bacc::rolling_mean(v_yaw_acc_); + + return true; } void Odometry::setWheelParams(double wheel_separation, double wheel_radius) @@ -167,8 +205,9 @@ namespace diff_drive_controller void Odometry::resetAccumulators() { - linear_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); - angular_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + v_x_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + v_y_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); + v_yaw_acc_ = RollingMeanAcc(RollingWindow::window_size = velocity_rolling_window_size_); } } // namespace diff_drive_controller