diff --git a/diff_drive_controller/CMakeLists.txt b/diff_drive_controller/CMakeLists.txt index 1ddea11db..252f922cf 100644 --- a/diff_drive_controller/CMakeLists.txt +++ b/diff_drive_controller/CMakeLists.txt @@ -83,6 +83,9 @@ if (CATKIN_ENABLE_TESTING) ${catkin_INCLUDE_DIRS} ${EIGEN_INCLUDE_DIRS}) + catkin_add_gtest(odometry_test test/odometry_test.cpp) + target_link_libraries(odometry_test ${PROJECT_NAME}) + add_executable(diffbot test/diffbot.cpp) target_link_libraries(diffbot ${catkin_LIBRARIES}) diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.h b/diff_drive_controller/include/diff_drive_controller/odometry.h index d92766a97..739f3ff8c 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.h +++ b/diff_drive_controller/include/diff_drive_controller/odometry.h @@ -134,6 +134,18 @@ namespace diff_drive_controller bool updateOpenLoop(const double linear, const double angular, const ros::Time &time); + /** + * \brief Update the odometry twist with the (internal) incremental pose, + * since the last update/call to this method; this resets the (internal) + * incremental pose + * \param[in] time Current time, used to compute the time step/increment, + * which is used to divide the (internal) incremental pose + * by dt and obtain the twist + * \return true if twist is actually updated; it won't be updated if the + * time step/increment is very small, to avoid division by zero + */ + bool updateTwist(const ros::Time& time); + /** * \brief Heading getter * \return Heading [rad] @@ -276,17 +288,12 @@ namespace diff_drive_controller const double v_l, const double v_r, const ros::Time& time); /** - * \brief Update the odometry twist with the previous and current odometry - * pose - * \param[in] p0 Previous odometry pose - * \param[in] p1 Current odometry pose - * \param[in] v_l Left wheel velocity [rad/s] - * \param[in] v_r Right wheel velocity [rad/s] - * \param[in] time Current time - * \return true if the odometry twist is actually updated + * \brief Updates the (internal) incremental odometry with latest left and + * right wheel position increments + * \param[in] dp_l Left wheel position increment [rad] + * \param[in] dp_r Right wheel position increment [rad] */ - bool updateTwist(const SE2& p0, const SE2& p1, - const double v_l, const double v_r, const ros::Time& time); + void updateIncrementalPose(const double dp_l, const double dp_r); /** * \brief Update the measurement covariance @@ -303,19 +310,31 @@ namespace diff_drive_controller /// Current timestamp: ros::Time timestamp_; + /// Timestamp for last twist computed, ie. since when the (internal) + /// incremental pose has been computed: + ros::Time timestamp_twist_; + /// Current pose: double x_; // [m] double y_; // [m] double heading_; // [rad] + /// Current incremental pose: + double d_x_; // [m] + double d_y_; // [m] + double d_yaw_; // [rad] + /// Current velocity: - double v_x_; // [m/s] - double v_y_; // [m/s] - double v_yaw_; // [rad/s] + double v_x_; // [m/s] + double v_y_; // [m/s] + double v_yaw_; // [rad/s] /// Pose covariance: PoseCovariance pose_covariance_; + /// Incremental Pose covariance: + PoseCovariance incremental_pose_covariance_; + /// Twist (and minimum twist) covariance: TwistCovariance twist_covariance_; TwistCovariance minimum_twist_covariance_; diff --git a/diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h b/diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h new file mode 100644 index 000000000..e05c0d3a8 --- /dev/null +++ b/diff_drive_controller/include/diff_drive_controller/rigid_body_motion.h @@ -0,0 +1,105 @@ +/********************************************************************* + * 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 RIGID_BODY_MOTION_H_ +#define RIGID_BODY_MOTION_H_ + +#include + +#include + +namespace diff_drive_controller +{ + +/** + * \brief Compute rigid body motion in SE(2) using element-wise equations. + * \param[in, out] x Pose x component + * \param[in, out] y Pose y component + * \param[in, out] yaw Pose yaw component + * \param[in] v_x Velocity/Twist x component + * \param[in] v_y Velocity/Twist y component + * \param[in] v_yaw Velocity/Twist yaw component + * \param[in] dt Time step + */ +static void integrate_motion(double& x, double &y, double &yaw, + const double v_x, const double v_y, const double v_yaw, + const double dt) +{ + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + + x += (v_x * cos_yaw - v_y * sin_yaw) * dt; + y += (v_x * sin_yaw + v_y * cos_yaw) * dt; + yaw += v_yaw * dt; +} + +/** + * \brief Compute rigid body motion in SE(2) using element-wise equations. + * Also computes the Jacobians wrt the pose and the velocity/twist. + * \param[in, out] x Pose x component + * \param[in, out] y Pose y component + * \param[in, out] yaw Pose yaw component + * \param[in] v_x Velocity/Twist x component + * \param[in] v_y Velocity/Twist y component + * \param[in] v_yaw Velocity/Twist yaw component + * \param[in] dt Time step + */ +static void integrate_motion(double& x, double &y, double &yaw, + const double v_x, const double v_y, const double v_yaw, + const double dt, + Eigen::Matrix3d& J_pose, Eigen::Matrix3d& J_twist) +{ + const double cos_yaw = std::cos(yaw); + const double sin_yaw = std::sin(yaw); + + x += (v_x * cos_yaw - v_y * sin_yaw) * dt; + y += (v_x * sin_yaw + v_y * cos_yaw) * dt; + yaw += v_yaw * dt; + + J_pose << 1.0, 0.0, (-v_x * sin_yaw - v_y * cos_yaw) * dt, + 0.0, 1.0, ( v_x * cos_yaw - v_y * sin_yaw) * dt, + 0.0, 0.0, 1.0; + + J_twist << cos_yaw * dt, -sin_yaw * dt, 0.0, + sin_yaw * dt, cos_yaw * dt, 0.0, + 0.0, 0.0, dt; +} + +} + +#endif /* RIGID_BODY_MOTION_H_ */ diff --git a/diff_drive_controller/package.xml b/diff_drive_controller/package.xml index fd471b171..92350110f 100644 --- a/diff_drive_controller/package.xml +++ b/diff_drive_controller/package.xml @@ -46,6 +46,7 @@ rostest + gtest std_srvs controller_manager control_toolbox diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index 5b14eff6b..a9ebf4202 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -620,6 +620,12 @@ namespace diff_drive_controller if (last_odom_publish_time_ + publish_period_ < time + half_period && odom_pub_->trylock()) { + // Update twist: + // Note that the twist must be computed at the same frequency that it gets + // published, because otherwise the code that uses it cannot apply the + // same period it was used to compute it. + odometry_.updateTwist(time); + last_odom_publish_time_ = time; // Populate odom message and publish: @@ -646,6 +652,12 @@ namespace diff_drive_controller last_odom_tf_publish_time_ + publish_period_ < time + half_period && tf_odom_pub_->trylock()) { + // Note that the tf odometry doesn't need the twist, only the pose. + // In the current implementation the pose is computed on its own, ie. w/o + // using the (internal) incremental pose. Therefore, it's always up to + // date with every control cycle and can be published at any rate because + // it doesn't depend on any period. + last_odom_tf_publish_time_ = time; // Populate tf odometry frame message and publish: diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index ca7f0fbe0..b6bc6d284 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -58,12 +58,16 @@ namespace diff_drive_controller Odometry::Odometry(size_t velocity_rolling_window_size) : timestamp_(0.0) + , timestamp_twist_(0.0) , x_(0.0) , y_(0.0) , heading_(0.0) , v_x_(0.0) , v_y_(0.0) , v_yaw_(0.0) + , d_x_(0.0) + , d_y_(0.0) + , d_yaw_(0.0) , wheel_separation_(0.0) , left_wheel_radius_(0.0) , right_wheel_radius_(0.0) @@ -86,19 +90,19 @@ namespace diff_drive_controller minimum_twist_covariance_.setIdentity(); minimum_twist_covariance_ *= DEFAULT_MINIMUM_TWIST_COVARIANCE; - // There's no need to initialize the twist covariance because it's updated - // from scratch on each cycle, but it's safer to initialize it anyway: twist_covariance_ = minimum_twist_covariance_; pose_covariance_.setIdentity(); pose_covariance_ *= DEFAULT_POSE_COVARIANCE; + + incremental_pose_covariance_.setZero(); } void Odometry::init(const ros::Time& time) { // Reset accumulators and timestamp: resetAccumulators(); - timestamp_ = time; + timestamp_ = timestamp_twist_ = time; } bool Odometry::updateCloseLoop( @@ -138,6 +142,7 @@ namespace diff_drive_controller /// Compute time step: const double dt = (time - timestamp_).toSec(); + timestamp_ = time; /// Update pose and twist: return update(v_l * dt, v_r * dt, v_l, v_r, time); @@ -148,9 +153,6 @@ namespace diff_drive_controller const double v_l, const double v_r, const ros::Time& time) { - /// Safe current state: - const SE2 p0(SE2::Scalar(heading_), SE2::Point(x_, y_)); - /// Integrate odometry pose: IntegrateFunction::PoseJacobian J_pose; IntegrateFunction::MeasJacobian J_meas; @@ -163,75 +165,69 @@ namespace diff_drive_controller pose_covariance_ = J_pose * pose_covariance_ * J_pose.transpose() + J_meas * meas_covariance_ * J_meas.transpose(); - /// Safe new state: - const SE2 p1(SE2::Scalar(heading_), SE2::Point(x_, y_)); + /// Update incremental pose: + // @todo in principle there's no need to use v_l, v_r at all, but this + // should be decided from outside, so here we should use: + // updateIncrementalPose(v_l * dt, v_r * dt); + // which could be equivalent to dp_l, dp_r or not. + // Note that we need to obtain dt from the time (which it's not used now) + updateIncrementalPose(dp_l, dp_r); - /// Update twist: - return updateTwist(p0, p1, v_l, v_r, time); + return true; } - bool Odometry::updateTwist(const SE2& p0, const SE2& p1, - double v_l, double v_r, const ros::Time& time) + void Odometry::updateIncrementalPose(const double dp_l, const double dp_r) { - /// We cannot estimate the speed with very small time intervals: - const double dt = (time - timestamp_).toSec(); - if (dt < 0.0001) - return false; + /// Integrate incremental odometry pose: + IntegrateFunction::PoseJacobian J_pose; + IntegrateFunction::MeasJacobian J_meas; + (*integrate_fun_)(d_x_, d_y_, d_yaw_, dp_l, dp_r, J_pose, J_meas); - timestamp_ = time; + /// Update Measurement Covariance with the wheel joint position increments: + updateMeasCovariance(dp_l, dp_r); - /// Compute relative transformation: - const SE2 p = p0.inverse() * p1; + /// Update incremental pose covariance: + incremental_pose_covariance_ = + J_pose * incremental_pose_covariance_ * J_pose.transpose() + + J_meas * meas_covariance_ * J_meas.transpose(); + } - /// 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(); + bool Odometry::updateTwist(const ros::Time& time) + { + /// We cannot estimate the speed with very small time intervals: + const double dt = (time - timestamp_twist_).toSec(); + if (dt < 0.0001) + { + return false; + } - v_x_ = t[0]; - v_y_ = t[1]; - v_yaw_ = p.so2().log(); + timestamp_twist_ = time; /// 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); + const double f = 1.0 / dt; + + v_x_acc_(d_x_ * f); + v_y_acc_(d_y_ * f); + v_yaw_acc_(d_yaw_ * f); v_x_ = bacc::rolling_mean(v_x_acc_); v_y_ = bacc::rolling_mean(v_y_acc_); v_yaw_ = bacc::rolling_mean(v_yaw_acc_); - /// Integrate odometry twist: - /// Note that this is done this way because it isn't trivial to compute the - /// Jacobians for the relative transformation between p0 and p1 - const double dp_l = v_l * dt; - const double dp_r = v_r * dt; - - IntegrateFunction::PoseJacobian J_dummy; - IntegrateFunction::MeasJacobian J_meas; - double x = 0.0, y = 0.0, yaw = 0.0; - (*integrate_fun_)(x, y, yaw, dp_l, dp_r, J_dummy, J_meas); - - /// Include the Jacobian of dividing by dt, which is equivalent to divide - /// all the elements of the other Jacobian by dt: - J_meas /= dt; - - /// Update Measurement Covariance with the wheel joint velocites: - updateMeasCovariance(dp_l, dp_r); - /// Update twist covariance: - twist_covariance_ = J_meas * meas_covariance_ * J_meas.transpose(); + IntegrateFunction::PoseJacobian J_twist = + IntegrateFunction::PoseJacobian::Identity() * f; + twist_covariance_ = J_twist * incremental_pose_covariance_ * J_twist.transpose(); /// Add minimum (diagonal) covariance to avoid ill-conditioned covariance /// matrices, i.e. with a very large condition number, which would make /// inverse or Cholesky decomposition fail on many algorithms: twist_covariance_ += minimum_twist_covariance_; + /// Reset incremental pose and its covariance: + d_x_ = d_y_ = d_yaw_ = 0.0; + incremental_pose_covariance_.setZero(); + return true; } diff --git a/diff_drive_controller/test/diff_drive_test.cpp b/diff_drive_controller/test/diff_drive_test.cpp index 1f3ba1279..ebc1b4655 100644 --- a/diff_drive_controller/test/diff_drive_test.cpp +++ b/diff_drive_controller/test/diff_drive_test.cpp @@ -23,7 +23,7 @@ // 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 Bence Magyar /// \author Enrique Fernandez @@ -31,11 +31,14 @@ #include "test_common.h" #include +#include #include -#define POSE_COVARIANCE_MAX_CONDITION_NUMBER 1e8 -#define TWIST_COVARIANCE_MAX_CONDITION_NUMBER 1e8 +#include + +const double POSE_COVARIANCE_MAX_CONDITION_NUMBER = 1e8; +const double TWIST_COVARIANCE_MAX_CONDITION_NUMBER = POSE_COVARIANCE_MAX_CONDITION_NUMBER; // TEST CASES TEST_F(DiffDriveControllerTest, testNoMove) @@ -60,25 +63,25 @@ TEST_F(DiffDriveControllerTest, testNoMove) nav_msgs::Odometry new_odom = getLastOdom(); // check if the robot didn't moved, changes on all fields should be ~~0 - EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS); - EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); - EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); + EXPECT_NEAR(old_odom.pose.pose.position.x, new_odom.pose.pose.position.x, std::numeric_limits::epsilon()); + EXPECT_NEAR(old_odom.pose.pose.position.y, new_odom.pose.pose.position.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(old_odom.pose.pose.position.z, new_odom.pose.pose.position.z, std::numeric_limits::epsilon()); // convert to rpy and test that way double roll_old, pitch_old, yaw_old; double roll_new, pitch_new, yaw_new; tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); - EXPECT_LT(fabs(roll_new - roll_old), EPS); - EXPECT_LT(fabs(pitch_new - pitch_old), EPS); - EXPECT_LT(fabs(yaw_new - yaw_old), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + EXPECT_NEAR(roll_old , roll_new , std::numeric_limits::epsilon()); + EXPECT_NEAR(pitch_old, pitch_new, std::numeric_limits::epsilon()); + EXPECT_NEAR(yaw_old , yaw_new , std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.x, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.z, std::numeric_limits::epsilon()); - EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.x, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.z, std::numeric_limits::epsilon()); // propagation double x = old_odom.pose.pose.position.x; @@ -92,10 +95,10 @@ TEST_F(DiffDriveControllerTest, testNoMove) const double dt = (new_odom.header.stamp - old_odom.header.stamp).toSec(); propagate(x, y, yaw, v_x, v_y, v_yaw, dt); - EXPECT_LT(std::abs(x - new_odom.pose.pose.position.x), EPS); - EXPECT_LT(std::abs(y - new_odom.pose.pose.position.y), EPS); - EXPECT_LT(angles::shortest_angular_distance(yaw, - tf::getYaw(new_odom.pose.pose.orientation)), EPS); + EXPECT_NEAR(new_odom.pose.pose.position.x, x, std::numeric_limits::epsilon()); + EXPECT_NEAR(new_odom.pose.pose.position.y, y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, angles::shortest_angular_distance(yaw, + tf::getYaw(new_odom.pose.pose.orientation)), std::numeric_limits::epsilon()); // covariance using namespace diff_drive_controller; @@ -147,7 +150,83 @@ TEST_F(DiffDriveControllerTest, testNoMove) // where Odometry::DEFAULT_MINIMUM_TWIST_COVARIANCE == 1e-9 TwistCovariance minimum_twist_covariance = 1e-9 * TwistCovariance::Identity(); EXPECT_TRUE(((twist_covariance - minimum_twist_covariance).array() == 0).all()) - << "Twist covariance=\n" << twist_covariance.format(HeavyFmt); + << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n" + << "Minimum twist covariance =\n" << minimum_twist_covariance.format(HeavyFmt); + + // Take the last two odometry messages: + std::vector odoms = getLastOdoms(); + + EXPECT_GT(odoms.size(), 1); + + // Take pose and twist state and covariance for the current odometry: + const ros::Time t_0 = odoms[0].header.stamp; + + const double x_0 = odoms[0].pose.pose.position.x; + const double y_0 = odoms[0].pose.pose.position.y; + const double yaw_0 = tf::getYaw(odoms[0].pose.pose.orientation); + + const double v_x_0 = odoms[0].twist.twist.linear.x; + const double v_y_0 = odoms[0].twist.twist.linear.y; + const double v_yaw_0 = odoms[0].twist.twist.angular.z; + + PoseCovariance pose_covariance_0; + msgToCovariance(odoms[0].pose.covariance, pose_covariance_0, no_tag()); + + TwistCovariance twist_covariance_0; + msgToCovariance(odoms[0].twist.covariance, twist_covariance_0, no_tag()); + + // Take pose and twist state and covariance for the previous odometry: + const ros::Time t_1 = odoms[1].header.stamp; + + const double x_1 = odoms[1].pose.pose.position.x; + const double y_1 = odoms[1].pose.pose.position.y; + const double yaw_1 = tf::getYaw(odoms[1].pose.pose.orientation); + + const double v_x_1 = odoms[1].twist.twist.linear.x; + const double v_y_1 = odoms[1].twist.twist.linear.y; + const double v_yaw_1 = odoms[1].twist.twist.angular.z; + + PoseCovariance pose_covariance_1; + msgToCovariance(odoms[1].pose.covariance, pose_covariance_1, no_tag()); + + TwistCovariance twist_covariance_1; + msgToCovariance(odoms[1].twist.covariance, twist_covariance_1, no_tag()); + + // Integrate motion: + const double dt_odoms = (t_0 - t_1).toSec(); + x = x_1; + y = y_1; + yaw = yaw_1; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_0, v_y_0, v_yaw_0, + dt_odoms, + J_pose, J_twist); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_0_corrected = + twist_covariance_0 - 1e-9 * TwistCovariance::Identity(); + + // Propagate covariance: + PoseCovariance pose_covariance_0_expected = + J_pose * pose_covariance_1 * J_pose.transpose() + + J_twist * twist_covariance_0_corrected * J_twist.transpose(); + + // Check new pose is equal to the expected one: + EXPECT_EQ(x_0, x); + EXPECT_EQ(y_0, y); + EXPECT_EQ(yaw_0, yaw); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < std::numeric_limits::epsilon()).all()) + << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt); } TEST_F(DiffDriveControllerTest, testForward) @@ -178,23 +257,23 @@ TEST_F(DiffDriveControllerTest, testForward) const double dy = new_odom.pose.pose.position.y - old_odom.pose.pose.position.y; const double dz = new_odom.pose.pose.position.z - old_odom.pose.pose.position.z; EXPECT_NEAR(sqrt(dx*dx + dy*dy), 1.0, POSITION_TOLERANCE); - EXPECT_LT(fabs(dz), EPS); + EXPECT_NEAR(0.0, dz, std::numeric_limits::epsilon()); // convert to rpy and test that way double roll_old, pitch_old, yaw_old; double roll_new, pitch_new, yaw_new; tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); - EXPECT_LT(fabs(roll_new - roll_old), EPS); - EXPECT_LT(fabs(pitch_new - pitch_old), EPS); - EXPECT_LT(fabs(yaw_new - yaw_old), EPS); - EXPECT_NEAR(fabs(new_odom.twist.twist.linear.x), cmd_vel.linear.x, EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + EXPECT_NEAR(roll_old , roll_new , std::numeric_limits::epsilon()); + EXPECT_NEAR(pitch_old, pitch_new, std::numeric_limits::epsilon()); + EXPECT_NEAR(yaw_old , yaw_new , std::numeric_limits::epsilon()); + EXPECT_NEAR(cmd_vel.linear.x, new_odom.twist.twist.linear.x, EPS); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.z, std::numeric_limits::epsilon()); - EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.angular.z), EPS); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.x, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.z, std::numeric_limits::epsilon()); // propagation double x = old_odom.pose.pose.position.x; @@ -208,10 +287,10 @@ TEST_F(DiffDriveControllerTest, testForward) const double dt = (new_odom.header.stamp - old_odom.header.stamp).toSec(); propagate(x, y, yaw, v_x, v_y, v_yaw, dt); - EXPECT_LT(std::abs(x - new_odom.pose.pose.position.x), EPS); - EXPECT_LT(std::abs(y - new_odom.pose.pose.position.y), EPS); - EXPECT_LT(angles::shortest_angular_distance(yaw, - tf::getYaw(new_odom.pose.pose.orientation)), EPS); + EXPECT_NEAR(new_odom.pose.pose.position.x, x, EPS); + EXPECT_NEAR(new_odom.pose.pose.position.y, y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, angles::shortest_angular_distance(yaw, + tf::getYaw(new_odom.pose.pose.orientation)), std::numeric_limits::epsilon()); // covariance using namespace diff_drive_controller; @@ -255,6 +334,81 @@ TEST_F(DiffDriveControllerTest, testForward) TWIST_COVARIANCE_MAX_CONDITION_NUMBER) << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n" << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt); + + // Take the last two odometry messages: + std::vector odoms = getLastOdoms(); + + EXPECT_GT(odoms.size(), 1); + + // Take pose and twist state and covariance for the current odometry: + const ros::Time t_0 = odoms[0].header.stamp; + + const double x_0 = odoms[0].pose.pose.position.x; + const double y_0 = odoms[0].pose.pose.position.y; + const double yaw_0 = tf::getYaw(odoms[0].pose.pose.orientation); + + const double v_x_0 = odoms[0].twist.twist.linear.x; + const double v_y_0 = odoms[0].twist.twist.linear.y; + const double v_yaw_0 = odoms[0].twist.twist.angular.z; + + PoseCovariance pose_covariance_0; + msgToCovariance(odoms[0].pose.covariance, pose_covariance_0, no_tag()); + + TwistCovariance twist_covariance_0; + msgToCovariance(odoms[0].twist.covariance, twist_covariance_0, no_tag()); + + // Take pose and twist state and covariance for the previous odometry: + const ros::Time t_1 = odoms[1].header.stamp; + + const double x_1 = odoms[1].pose.pose.position.x; + const double y_1 = odoms[1].pose.pose.position.y; + const double yaw_1 = tf::getYaw(odoms[1].pose.pose.orientation); + + const double v_x_1 = odoms[1].twist.twist.linear.x; + const double v_y_1 = odoms[1].twist.twist.linear.y; + const double v_yaw_1 = odoms[1].twist.twist.angular.z; + + PoseCovariance pose_covariance_1; + msgToCovariance(odoms[1].pose.covariance, pose_covariance_1, no_tag()); + + TwistCovariance twist_covariance_1; + msgToCovariance(odoms[1].twist.covariance, twist_covariance_1, no_tag()); + + // Integrate motion: + const double dt_odoms = (t_0 - t_1).toSec(); + x = x_1; + y = y_1; + yaw = yaw_1; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_0, v_y_0, v_yaw_0, + dt_odoms, + J_pose, J_twist); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_0_corrected = + twist_covariance_0 - 1e-9 * TwistCovariance::Identity(); + + // Propagate covariance: + PoseCovariance pose_covariance_0_expected = + J_pose * pose_covariance_1 * J_pose.transpose() + + J_twist * twist_covariance_0_corrected * J_twist.transpose(); + + // Check new pose is equal to the expected one: + EXPECT_NEAR(x_0, x, POSITION_TOLERANCE); + EXPECT_NEAR(y_0, y, std::numeric_limits::epsilon()); + EXPECT_NEAR(yaw_0, yaw, std::numeric_limits::epsilon()); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all()) + << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt); } TEST_F(DiffDriveControllerTest, testTurn) @@ -281,26 +435,30 @@ TEST_F(DiffDriveControllerTest, testTurn) nav_msgs::Odometry new_odom = getLastOdom(); // check if the robot rotated PI around z, changes in the other fields should be ~~0 - EXPECT_LT(fabs(new_odom.pose.pose.position.x - old_odom.pose.pose.position.x), EPS); - EXPECT_LT(fabs(new_odom.pose.pose.position.y - old_odom.pose.pose.position.y), EPS); - EXPECT_LT(fabs(new_odom.pose.pose.position.z - old_odom.pose.pose.position.z), EPS); + EXPECT_NEAR(old_odom.pose.pose.position.x, new_odom.pose.pose.position.x, std::numeric_limits::epsilon()); + // Cannot use std::numeric_limits::epsilon() in the next check because + // it's to strict for the test_diff_drive_multipliers rostest: + EXPECT_NEAR(old_odom.pose.pose.position.y, new_odom.pose.pose.position.y, 1e-13); + EXPECT_NEAR(old_odom.pose.pose.position.z, new_odom.pose.pose.position.z, std::numeric_limits::epsilon()); // convert to rpy and test that way double roll_old, pitch_old, yaw_old; double roll_new, pitch_new, yaw_new; tf::Matrix3x3(tfQuatFromGeomQuat(old_odom.pose.pose.orientation)).getRPY(roll_old, pitch_old, yaw_old); tf::Matrix3x3(tfQuatFromGeomQuat(new_odom.pose.pose.orientation)).getRPY(roll_new, pitch_new, yaw_new); - EXPECT_LT(fabs(roll_new - roll_old), EPS); - EXPECT_LT(fabs(pitch_new - pitch_old), EPS); - EXPECT_NEAR(fabs(yaw_new - yaw_old), M_PI, ORIENTATION_TOLERANCE); + EXPECT_NEAR(roll_old , roll_new , std::numeric_limits::epsilon()); + EXPECT_NEAR(pitch_old, pitch_new, std::numeric_limits::epsilon()); + EXPECT_NEAR(M_PI, fabs(yaw_new - yaw_old), ORIENTATION_TOLERANCE); - EXPECT_LT(fabs(new_odom.twist.twist.linear.x), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.y), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.linear.z), EPS); + // Cannot use std::numeric_limits::epsilon() in the next check because + // it's to strict for the test_diff_drive_multipliers rostest: + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.x, 1e-12); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.linear.z, std::numeric_limits::epsilon()); - EXPECT_LT(fabs(new_odom.twist.twist.angular.x), EPS); - EXPECT_LT(fabs(new_odom.twist.twist.angular.y), EPS); - EXPECT_NEAR(fabs(new_odom.twist.twist.angular.z), M_PI/10.0, EPS); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.x, std::numeric_limits::epsilon()); + EXPECT_NEAR(0.0, new_odom.twist.twist.angular.y, std::numeric_limits::epsilon()); + EXPECT_NEAR(M_PI/10.0, new_odom.twist.twist.angular.z, EPS); // propagation double x = old_odom.pose.pose.position.x; @@ -314,10 +472,12 @@ TEST_F(DiffDriveControllerTest, testTurn) const double dt = (new_odom.header.stamp - old_odom.header.stamp).toSec(); propagate(x, y, yaw, v_x, v_y, v_yaw, dt); - EXPECT_LT(std::abs(x - new_odom.pose.pose.position.x), EPS); - EXPECT_LT(std::abs(y - new_odom.pose.pose.position.y), EPS); - EXPECT_LT(angles::shortest_angular_distance(yaw, - tf::getYaw(new_odom.pose.pose.orientation)), EPS); + EXPECT_NEAR(new_odom.pose.pose.position.x, x, EPS); + // Cannot use std::numeric_limits::epsilon() in the next check because + // it's to strict for the test_diff_drive_multipliers rostest: + EXPECT_NEAR(new_odom.pose.pose.position.y, y, 1e-13); + EXPECT_NEAR(0.0, angles::shortest_angular_distance(yaw, + tf::getYaw(new_odom.pose.pose.orientation)), ORIENTATION_TOLERANCE); // covariance using namespace diff_drive_controller; @@ -364,6 +524,81 @@ TEST_F(DiffDriveControllerTest, testTurn) TWIST_COVARIANCE_MAX_CONDITION_NUMBER) << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n" << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt); + + // Take the last two odometry messages: + std::vector odoms = getLastOdoms(); + + EXPECT_GT(odoms.size(), 1); + + // Take pose and twist state and covariance for the current odometry: + const ros::Time t_0 = odoms[0].header.stamp; + + const double x_0 = odoms[0].pose.pose.position.x; + const double y_0 = odoms[0].pose.pose.position.y; + const double yaw_0 = tf::getYaw(odoms[0].pose.pose.orientation); + + const double v_x_0 = odoms[0].twist.twist.linear.x; + const double v_y_0 = odoms[0].twist.twist.linear.y; + const double v_yaw_0 = odoms[0].twist.twist.angular.z; + + PoseCovariance pose_covariance_0; + msgToCovariance(odoms[0].pose.covariance, pose_covariance_0, no_tag()); + + TwistCovariance twist_covariance_0; + msgToCovariance(odoms[0].twist.covariance, twist_covariance_0, no_tag()); + + // Take pose and twist state and covariance for the previous odometry: + const ros::Time t_1 = odoms[1].header.stamp; + + const double x_1 = odoms[1].pose.pose.position.x; + const double y_1 = odoms[1].pose.pose.position.y; + const double yaw_1 = tf::getYaw(odoms[1].pose.pose.orientation); + + const double v_x_1 = odoms[1].twist.twist.linear.x; + const double v_y_1 = odoms[1].twist.twist.linear.y; + const double v_yaw_1 = odoms[1].twist.twist.angular.z; + + PoseCovariance pose_covariance_1; + msgToCovariance(odoms[1].pose.covariance, pose_covariance_1, no_tag()); + + TwistCovariance twist_covariance_1; + msgToCovariance(odoms[1].twist.covariance, twist_covariance_1, no_tag()); + + // Integrate motion: + const double dt_odoms = (t_0 - t_1).toSec(); + x = x_1; + y = y_1; + yaw = yaw_1; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_0, v_y_0, v_yaw_0, + dt_odoms, + J_pose, J_twist); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_0_corrected = + twist_covariance_0 - 1e-9 * TwistCovariance::Identity(); + + // Propagate covariance: + PoseCovariance pose_covariance_0_expected = + J_pose * pose_covariance_1 * J_pose.transpose() + + J_twist * twist_covariance_0_corrected * J_twist.transpose(); + + // Check new pose is equal to the expected one: + EXPECT_NEAR(x_0, x, std::numeric_limits::epsilon()); + EXPECT_NEAR(y_0, y, std::numeric_limits::epsilon()); + EXPECT_NEAR(yaw_0, yaw, ORIENTATION_TOLERANCE); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_0_expected - pose_covariance_0).array().abs() < 1e-5).all()) + << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_0_expected.format(HeavyFmt); } TEST_F(DiffDriveControllerTest, testMoveX) diff --git a/diff_drive_controller/test/odometry_test.cpp b/diff_drive_controller/test/odometry_test.cpp new file mode 100644 index 000000000..a84260837 --- /dev/null +++ b/diff_drive_controller/test/odometry_test.cpp @@ -0,0 +1,642 @@ +/////////////////////////////////////////////////////////////////////////////// +// Copyright (C) 2016, Clearpath Robotics Inc. +// +// 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 PAL Robotics, Inc. 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 Fernandez + +#include +#include +#include + +#include + +#include + +#include + +const double POSE_COVARIANCE_MAX_CONDITION_NUMBER = 1e8; +const double TWIST_COVARIANCE_MAX_CONDITION_NUMBER = POSE_COVARIANCE_MAX_CONDITION_NUMBER; + +const double WHEEL_SEPARATION = 0.5; +const double LEFT_WHEEL_RADIUS = 0.1; +const double RIGHT_WHEEL_RADIUS = LEFT_WHEEL_RADIUS; + +const double K_L = 0.01; +const double K_R = K_L; + +const size_t CONTROL_STEPS = 100; +const double CONTROL_PERIOD = 0.02; // [s] +const double POSITION_INCREMENT = 0.02; // [rad] + +const double EPS_INTEGRATE_MOTION_COVARIANCE = std::numeric_limits::epsilon(); + +/** + * \brief Setup the odometry params + * \param[in, out] odometry Odometry + */ +static void setupOdometry(diff_drive_controller::Odometry& odometry) +{ + odometry.setWheelParams(WHEEL_SEPARATION, + LEFT_WHEEL_RADIUS, RIGHT_WHEEL_RADIUS); + + odometry.setMeasCovarianceParams(K_L, K_R); +} + +static void moveOdometry(diff_drive_controller::Odometry& odometry, + double& left_position, double& right_position, + ros::Time& t, + const size_t control_steps, const double control_period, + const double left_position_increment, + const double right_position_increment) +{ + const double left_velocity = left_position_increment / control_period; + const double right_velocity = right_position_increment / control_period; + + for (size_t i = 0; i < control_steps; ++i) + { + left_position += left_position_increment; + right_position += right_position_increment; + + odometry.updateCloseLoop(left_position, right_position, + left_velocity, right_velocity, t); + + t += ros::Duration(control_period); + } +} + +TEST(OdometryTest, testInitial) +{ + // Setup odometry: + diff_drive_controller::Odometry odometry(1); + setupOdometry(odometry); + + // Check initial odometry pose and twist is zero: + EXPECT_EQ(0.0, odometry.getX()); + EXPECT_EQ(0.0, odometry.getY()); + EXPECT_EQ(0.0, odometry.getHeading()); + + EXPECT_EQ(0.0, odometry.getVx()); + EXPECT_EQ(0.0, odometry.getVy()); + EXPECT_EQ(0.0, odometry.getVyaw()); + + // Check initial odometry pose and twist covariance are valid: + using namespace diff_drive_controller; + + typedef Odometry::PoseCovariance PoseCovariance; + typedef Odometry::TwistCovariance TwistCovariance; + + const PoseCovariance pose_covariance = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance = odometry.getTwistCovariance(); + + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + typedef Eigen::SelfAdjointEigenSolver PoseEigenSolver; + typedef Eigen::SelfAdjointEigenSolver TwistEigenSolver; + + PoseEigenSolver pose_eigensolver(pose_covariance); + TwistEigenSolver twist_eigensolver(twist_covariance); + + EXPECT_TRUE(isSymmetric(pose_covariance)) + << "Pose covariance =\n" << pose_covariance.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance, no_tag())) + << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance) << "\n" + << "Eigenvalues = " << pose_eigensolver.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance)) + << "Twist covariance =\n" << twist_covariance.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance, no_tag())) + << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance) << "\n" + << "Eigenvalues = " << twist_eigensolver.eigenvalues().transpose().format(HeavyFmt); + + // Check initial odometry pose and twist covariance are the small expected + // ones: + PoseCovariance pose_covariance_expected = PoseCovariance::Identity(); + pose_covariance_expected *= diff_drive_controller::Odometry::DEFAULT_POSE_COVARIANCE; + + const TwistCovariance twist_covariance_expected = odometry.getMinimumTwistCovariance(); + + EXPECT_TRUE(((pose_covariance_expected - pose_covariance).array() == 0.0).all()) + << "Pose covariance actual =\n" << pose_covariance.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_expected.format(HeavyFmt); + EXPECT_TRUE(((twist_covariance_expected - twist_covariance).array() == 0.0).all()) + << "Twist covariance actual =\n" << twist_covariance.format(HeavyFmt) + << "\nTwist covariance expected =\n" << twist_covariance_expected.format(HeavyFmt); +} + +TEST(OdometryTest, testIntegrateMotionNoMoveFromInitial) +{ + // Setup odometry: + using namespace diff_drive_controller; + + typedef Odometry::PoseCovariance PoseCovariance; + typedef Odometry::TwistCovariance TwistCovariance; + + diff_drive_controller::Odometry odometry(1); + setupOdometry(odometry); + + // Save initial/current pose and twist state and covariance: + const double x_0 = odometry.getX(); + const double y_0 = odometry.getY(); + const double yaw_0 = odometry.getHeading(); + + const double v_x_0 = odometry.getVx(); + const double v_y_0 = odometry.getVy(); + const double v_yaw_0 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_0 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_0 = odometry.getTwistCovariance(); + + // Update the odometry moving the wheels forward: + double left_position = 0.0; + double right_position = 0.0; + + const ros::Time t0(0.0); + ros::Time t(t0); + moveOdometry(odometry, left_position, right_position, t, + CONTROL_STEPS, CONTROL_PERIOD, 0.0, 0.0); + + // Update the twist (from the internal incremental odometry pose): + EXPECT_TRUE(odometry.updateTwist(t)); + + // Retrieve new/current pose and twist state and covariance: + const double x_1 = odometry.getX(); + const double y_1 = odometry.getY(); + const double yaw_1 = odometry.getHeading(); + + const double v_x_1 = odometry.getVx(); + const double v_y_1 = odometry.getVy(); + const double v_yaw_1 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_1 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_1 = odometry.getTwistCovariance(); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_1_corrected = + twist_covariance_1 - odometry.getMinimumTwistCovariance(); + + // Integrate motion: + const double dt = (t - t0).toSec(); + double x = x_0; + double y = y_0; + double yaw = yaw_0; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_1, v_y_1, v_yaw_1, + dt, + J_pose, J_twist); + + // Propagate covariance: + PoseCovariance pose_covariance_1_expected = + J_pose * pose_covariance_0 * J_pose.transpose() + + J_twist * twist_covariance_1_corrected * J_twist.transpose(); + + // Check odometry pose and twist is zero (no move): + EXPECT_EQ(0.0, x_1); + EXPECT_EQ(0.0, y_1); + EXPECT_EQ(0.0, yaw_1); + + EXPECT_EQ(0.0, v_x_1); + EXPECT_EQ(0.0, v_y_1); + EXPECT_EQ(0.0, v_yaw_1); + + // Check new pose is equal to the expected one: + EXPECT_EQ(x_1, x); + EXPECT_EQ(y_1, y); + EXPECT_EQ(yaw_1, yaw); + + // Check all pose and twist covariances are valid: + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + typedef Eigen::SelfAdjointEigenSolver PoseEigenSolver; + typedef Eigen::SelfAdjointEigenSolver TwistEigenSolver; + + PoseEigenSolver pose_eigensolver_0(pose_covariance_0); + TwistEigenSolver twist_eigensolver_0(twist_covariance_0); + + PoseEigenSolver pose_eigensolver_1(pose_covariance_1); + TwistEigenSolver twist_eigensolver_1(twist_covariance_1); + + EXPECT_TRUE(isSymmetric(pose_covariance_0)) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_0, no_tag())) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_0), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_0) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_0)) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_0, no_tag())) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_0), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_0) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(pose_covariance_1)) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_1, no_tag())) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_1), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_1) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_1)) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_1, no_tag())) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_1), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_1) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_1_expected - pose_covariance_1).array().abs() < EPS_INTEGRATE_MOTION_COVARIANCE).all()) + << "Pose covariance actual =\n" << pose_covariance_1.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_1_expected.format(HeavyFmt); + + // Check initial odometry pose and twist covariance are the small expected + // ones: + PoseCovariance pose_covariance_expected = PoseCovariance::Identity(); + pose_covariance_expected *= diff_drive_controller::Odometry::DEFAULT_POSE_COVARIANCE; + + const TwistCovariance twist_covariance_expected = odometry.getMinimumTwistCovariance(); + + EXPECT_TRUE(((pose_covariance_expected - pose_covariance_1).array() == 0.0).all()) + << "Pose covariance actual =\n" << pose_covariance_1.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_expected.format(HeavyFmt); + EXPECT_TRUE(((twist_covariance_expected - twist_covariance_1).array() == 0.0).all()) + << "Twist covariance actual =\n" << twist_covariance_1.format(HeavyFmt) + << "\nTwist covariance expected =\n" << twist_covariance_expected.format(HeavyFmt); +} + +TEST(OdometryTest, testIntegrateMotionForwardFromInitial) +{ + // Setup odometry: + using namespace diff_drive_controller; + + typedef Odometry::PoseCovariance PoseCovariance; + typedef Odometry::TwistCovariance TwistCovariance; + + diff_drive_controller::Odometry odometry(1); + setupOdometry(odometry); + + // Save initial/current pose and twist state and covariance: + const double x_0 = odometry.getX(); + const double y_0 = odometry.getY(); + const double yaw_0 = odometry.getHeading(); + + const double v_x_0 = odometry.getVx(); + const double v_y_0 = odometry.getVy(); + const double v_yaw_0 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_0 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_0 = odometry.getTwistCovariance(); + + // Update the odometry moving the wheels forward: + double left_position = 0.0; + double right_position = 0.0; + + const ros::Time t0(0.0); + ros::Time t(t0); + moveOdometry(odometry, left_position, right_position, t, + CONTROL_STEPS, CONTROL_PERIOD, POSITION_INCREMENT, POSITION_INCREMENT); + + // Update the twist (from the internal incremental odometry pose): + EXPECT_TRUE(odometry.updateTwist(t)); + + // Retrieve new/current pose and twist state and covariance: + const double x_1 = odometry.getX(); + const double y_1 = odometry.getY(); + const double yaw_1 = odometry.getHeading(); + + const double v_x_1 = odometry.getVx(); + const double v_y_1 = odometry.getVy(); + const double v_yaw_1 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_1 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_1 = odometry.getTwistCovariance(); + + // Integrate motion: + const double dt = (t - t0).toSec(); + double x = x_0; + double y = y_0; + double yaw = yaw_0; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_1, v_y_1, v_yaw_1, + dt, + J_pose, J_twist); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_1_corrected = + twist_covariance_1 - odometry.getMinimumTwistCovariance(); + + // Propagate covariance: + PoseCovariance pose_covariance_1_expected = + J_pose * pose_covariance_0 * J_pose.transpose() + + J_twist * twist_covariance_1_corrected * J_twist.transpose(); + + // Check new pose is equal to the expected one: + EXPECT_EQ(x_1, x); + EXPECT_EQ(y_1, y); + EXPECT_EQ(yaw_1, yaw); + + // Check all pose and twist covariances are valid: + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + typedef Eigen::SelfAdjointEigenSolver PoseEigenSolver; + typedef Eigen::SelfAdjointEigenSolver TwistEigenSolver; + + PoseEigenSolver pose_eigensolver_0(pose_covariance_0); + TwistEigenSolver twist_eigensolver_0(twist_covariance_0); + + PoseEigenSolver pose_eigensolver_1(pose_covariance_1); + TwistEigenSolver twist_eigensolver_1(twist_covariance_1); + + EXPECT_TRUE(isSymmetric(pose_covariance_0)) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_0, no_tag())) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_0), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_0) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_0)) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_0, no_tag())) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_0), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_0) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(pose_covariance_1)) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_1, no_tag())) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_1), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_1) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_1)) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_1, no_tag())) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_1), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_1) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_1_expected - pose_covariance_1).array().abs() < EPS_INTEGRATE_MOTION_COVARIANCE).all()) + << "Pose covariance actual =\n" << pose_covariance_1.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_1_expected.format(HeavyFmt); +} + +TEST(OdometryTest, testIntegrateMotionForwardFromNotInitial) +{ + // Setup odometry: + using namespace diff_drive_controller; + + typedef Odometry::PoseCovariance PoseCovariance; + typedef Odometry::TwistCovariance TwistCovariance; + + diff_drive_controller::Odometry odometry(1); + setupOdometry(odometry); + + // Update the odometry moving the wheels forward and turning: + double left_position = 0.0; + double right_position = 0.0; + + const ros::Time t0(0.0); + ros::Time t(t0); + moveOdometry(odometry, left_position, right_position, t, + CONTROL_STEPS, CONTROL_PERIOD, POSITION_INCREMENT, 2 * POSITION_INCREMENT); + + // Update the twist (from the internal incremental odometry pose): + EXPECT_TRUE(odometry.updateTwist(t)); + + // Save initial/current pose and twist state and covariance: + const double x_0 = odometry.getX(); + const double y_0 = odometry.getY(); + const double yaw_0 = odometry.getHeading(); + + const double v_x_0 = odometry.getVx(); + const double v_y_0 = odometry.getVy(); + const double v_yaw_0 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_0 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_0 = odometry.getTwistCovariance(); + + // Update the odometry moving the wheels forward: + const ros::Time t1(t); + moveOdometry(odometry, left_position, right_position, t, + CONTROL_STEPS, CONTROL_PERIOD, POSITION_INCREMENT, POSITION_INCREMENT); + + // Update the twist (from the internal incremental odometry pose): + EXPECT_TRUE(odometry.updateTwist(t)); + + // Retrieve new/current pose and twist state and covariance: + const double x_1 = odometry.getX(); + const double y_1 = odometry.getY(); + const double yaw_1 = odometry.getHeading(); + + const double v_x_1 = odometry.getVx(); + const double v_y_1 = odometry.getVy(); + const double v_yaw_1 = odometry.getVyaw(); + + const PoseCovariance pose_covariance_1 = odometry.getPoseCovariance(); + const TwistCovariance twist_covariance_1 = odometry.getTwistCovariance(); + + // Integrate motion: + const double dt = (t - t1).toSec(); + double x = x_0; + double y = y_0; + double yaw = yaw_0; + Eigen::Matrix3d J_pose, J_twist; + diff_drive_controller::integrate_motion(x, y, yaw, + v_x_1, v_y_1, v_yaw_1, + dt, + J_pose, J_twist); + + // Correct the twist covariance removing the minimum twist covariance, which + // is always added to it to avoid ill-conditioned covariance matrices. + // We need to do this to obtain the correct expected pose covariance, + // because this one doesn't include the minimum twist covariance. + // We could also set the minimum twist covariance to zero, but that would + // generate a zero covariance matrix when the robot doesn't move (as in this + // test): + const TwistCovariance twist_covariance_1_corrected = + twist_covariance_1 - odometry.getMinimumTwistCovariance(); + + // Propagate covariance: + PoseCovariance pose_covariance_1_expected = + J_pose * pose_covariance_0 * J_pose.transpose() + + J_twist * twist_covariance_1_corrected * J_twist.transpose(); + + // Check new pose is equal to the expected one: + // Note that at this point the pose is not computed using the (internal) + // incremental pose, so we have an error greater than the double eps! + EXPECT_NEAR(x_1, x, 1e-14); + EXPECT_NEAR(y_1, y, std::numeric_limits::epsilon()); + EXPECT_NEAR(yaw_1, yaw, 1e-14); + + // Check all pose and twist covariances are valid: + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + typedef Eigen::SelfAdjointEigenSolver PoseEigenSolver; + typedef Eigen::SelfAdjointEigenSolver TwistEigenSolver; + + PoseEigenSolver pose_eigensolver_0(pose_covariance_0); + TwistEigenSolver twist_eigensolver_0(twist_covariance_0); + + PoseEigenSolver pose_eigensolver_1(pose_covariance_1); + TwistEigenSolver twist_eigensolver_1(twist_covariance_1); + + EXPECT_TRUE(isSymmetric(pose_covariance_0)) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_0, no_tag())) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_0), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_0) << "\n" + << "Eigenvalues = " << pose_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_0)) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_0, no_tag())) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_0), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_0.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_0) << "\n" + << "Eigenvalues = " << twist_eigensolver_0.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(pose_covariance_1)) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(pose_covariance_1, no_tag())) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(pose_covariance_1), + POSE_COVARIANCE_MAX_CONDITION_NUMBER) + << "Pose covariance =\n" << pose_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(pose_covariance_1) << "\n" + << "Eigenvalues = " << pose_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + EXPECT_TRUE(isSymmetric(twist_covariance_1)) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt); + EXPECT_TRUE(isPositiveDefinite(twist_covariance_1, no_tag())) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + EXPECT_LT(conditionNumber(twist_covariance_1), + TWIST_COVARIANCE_MAX_CONDITION_NUMBER) + << "Twist covariance =\n" << twist_covariance_1.format(HeavyFmt) << "\n" + << "Condition number = " << conditionNumber(twist_covariance_1) << "\n" + << "Eigenvalues = " << twist_eigensolver_1.eigenvalues().transpose().format(HeavyFmt); + + // Check the initial pose and twist state and covariance aren't zero or the + // initial ones (since it's been moved before retrieving it): + EXPECT_NE(0.0, x_0); + EXPECT_NE(0.0, y_0); + EXPECT_NE(0.0, yaw_0); + + EXPECT_NE(0.0, v_x_0); + EXPECT_NE(0.0, v_y_0); + EXPECT_NE(0.0, v_yaw_0); + + PoseCovariance pose_covariance_expected = PoseCovariance::Identity(); + pose_covariance_expected *= diff_drive_controller::Odometry::DEFAULT_POSE_COVARIANCE; + + const TwistCovariance twist_covariance_expected = odometry.getMinimumTwistCovariance(); + + EXPECT_FALSE(((pose_covariance_expected - pose_covariance_0).array() == 0.0).all()) + << "Pose covariance actual =\n" << pose_covariance_0.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_expected.format(HeavyFmt); + EXPECT_FALSE(((twist_covariance_expected - twist_covariance_0).array() == 0.0).all()) + << "Twist covariance actual =\n" << twist_covariance_0.format(HeavyFmt) + << "\nTwist covariance expected =\n" << twist_covariance_expected.format(HeavyFmt); + + // Check new pose covariance is equal to the expected one: + EXPECT_TRUE(((pose_covariance_1_expected - pose_covariance_1).array().abs() < EPS_INTEGRATE_MOTION_COVARIANCE).all()) + << "Pose covariance actual =\n" << pose_covariance_1.format(HeavyFmt) + << "\nPose covariance expected =\n" << pose_covariance_1_expected.format(HeavyFmt); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/diff_drive_controller/test/test_common.h b/diff_drive_controller/test/test_common.h index 93c4443d3..8c0b39b55 100644 --- a/diff_drive_controller/test/test_common.h +++ b/diff_drive_controller/test/test_common.h @@ -23,7 +23,7 @@ // 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 Bence Magyar @@ -63,6 +63,7 @@ class DiffDriveControllerTest : public ::testing::Test , odom_sub(nh.subscribe("odom", 100, &DiffDriveControllerTest::odomCallback, this)) , cmd_vel_limited_sub(nh.subscribe("cmd_vel_limited", 100, &DiffDriveControllerTest::cmdVelLimitedCallback, this)) , state_sub(nh.subscribe("state", 100, &DiffDriveControllerTest::diffDriveControllerStateCallback, this)) + , last_odoms(2) , last_states(3) , start_srv(nh.serviceClient("start")) , stop_srv(nh.serviceClient("stop")) @@ -76,7 +77,8 @@ class DiffDriveControllerTest : public ::testing::Test state_sub.shutdown(); } - nav_msgs::Odometry getLastOdom(){ return last_odom; } + nav_msgs::Odometry getLastOdom(){ return last_odoms[0]; } + std::vector getLastOdoms(){ return last_odoms; } geometry_msgs::TwistStamped getLastCmdVelLimited(){ return last_cmd_vel_limited; } diff_drive_controller::DiffDriveControllerState getLastState(){ return last_states[0]; } std::vector getLastStates(){ return last_states; } @@ -131,7 +133,7 @@ class DiffDriveControllerTest : public ::testing::Test ros::Subscriber odom_sub; ros::Subscriber cmd_vel_limited_sub; ros::Subscriber state_sub; - nav_msgs::Odometry last_odom; + std::vector last_odoms; geometry_msgs::TwistStamped last_cmd_vel_limited; std::vector last_states; @@ -140,23 +142,38 @@ class DiffDriveControllerTest : public ::testing::Test void odomCallback(const nav_msgs::Odometry& odom) { - ROS_INFO_STREAM("Odometry callback reveived: pos.x: " << odom.pose.pose.position.x + ROS_INFO_STREAM("Odometry callback reveived (" << odom.header.seq + << "): pos.x: " << odom.pose.pose.position.x << ", orient.z: " << odom.pose.pose.orientation.z << ", lin_est: " << odom.twist.twist.linear.x << ", ang_est: " << odom.twist.twist.angular.z); - last_odom = odom; + + if (last_odoms[0].header.seq > 0) + { + EXPECT_EQ(last_odoms[0].header.seq + 1, odom.header.seq); + } + + last_odoms[1] = last_odoms[0]; + last_odoms[0] = odom; } void cmdVelLimitedCallback(const geometry_msgs::TwistStamped& twist) { ROS_INFO_STREAM("Twist callback reveived: linear: " << twist.twist.linear.x << ", angular: " << twist.twist.angular.z); + last_cmd_vel_limited = twist; } void diffDriveControllerStateCallback(const diff_drive_controller::DiffDriveControllerState& msg) { - ROS_INFO("Joint trajectory controller state callback reveived"); + ROS_INFO_STREAM("Joint trajectory controller state callback reveived(" + << msg.header.seq << ")"); + + if (last_states[0].header.seq > 0) + { + EXPECT_EQ(last_states[0].header.seq + 1, msg.header.seq); + } last_states[2] = last_states[1]; last_states[1] = last_states[0];