diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 9b861f5b9..929c97bbf 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -11,6 +11,7 @@ find_package( fuse_publishers fuse_variables geometry_msgs + imu_transformer message_generation nav_msgs pluginlib @@ -149,6 +150,7 @@ install( if(CATKIN_ENABLE_TESTING) find_package(rostest REQUIRED) + find_package(catkin REQUIRED COMPONENTS angles) # Lint tests roslint_add_test() @@ -227,6 +229,22 @@ if(CATKIN_ENABLE_TESTING) CXX_STANDARD_REQUIRED YES ) + # Imu2D tests + add_rostest_gtest( + test_imu_2d + test/imu_2d.test + test/test_imu_2d.cpp + ) + target_link_libraries(test_imu_2d + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + set_target_properties(test_imu_2d + PROPERTIES + CXX_STANDARD 14 + CXX_STANDARD_REQUIRED YES + ) + # Unicycle2D Ignition tests add_rostest_gtest( test_unicycle_2d_ignition diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 98a3b842e..52fd8f037 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -24,6 +24,7 @@ fuse_publishers fuse_variables geometry_msgs + imu_transformer nav_msgs pluginlib roscpp @@ -39,6 +40,7 @@ message_runtime + angles benchmark + controller_manager + diff_drive_controller + gazebo_ros + joint_state_controller + robot_state_publisher rostest + xacro diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index 0874faccc..cda069bac 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -40,7 +40,8 @@ #include #include #include -#include +#include +#include #include #include @@ -105,47 +106,61 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) auto transaction = fuse_core::Transaction::make_shared(); transaction->stamp(msg->header.stamp); + // Transform to target frame + sensor_msgs::Imu imu_transformed; + try + { + tf_buffer_.transform(*msg, imu_transformed, params_.twist_target_frame); + } + catch (const tf2::TransformException& ex) + { + ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform IMU message with stamp " << msg->header.stamp << " to target frame " + << params_.twist_target_frame + << ". Error: " << ex.what()); + return; + } + // Handle the orientation data (treat it as a pose, but with only orientation indices used) - auto pose = std::make_unique(); - pose->header = msg->header; - pose->pose.pose.orientation = msg->orientation; - pose->pose.covariance[21] = msg->orientation_covariance[0]; - pose->pose.covariance[22] = msg->orientation_covariance[1]; - pose->pose.covariance[23] = msg->orientation_covariance[2]; - pose->pose.covariance[27] = msg->orientation_covariance[3]; - pose->pose.covariance[28] = msg->orientation_covariance[4]; - pose->pose.covariance[29] = msg->orientation_covariance[5]; - pose->pose.covariance[33] = msg->orientation_covariance[6]; - pose->pose.covariance[34] = msg->orientation_covariance[7]; - pose->pose.covariance[35] = msg->orientation_covariance[8]; + geometry_msgs::PoseWithCovarianceStamped pose; + pose.header = imu_transformed.header; + pose.pose.pose.orientation = imu_transformed.orientation; + pose.pose.covariance[21] = imu_transformed.orientation_covariance[0]; + pose.pose.covariance[22] = imu_transformed.orientation_covariance[1]; + pose.pose.covariance[23] = imu_transformed.orientation_covariance[2]; + pose.pose.covariance[27] = imu_transformed.orientation_covariance[3]; + pose.pose.covariance[28] = imu_transformed.orientation_covariance[4]; + pose.pose.covariance[29] = imu_transformed.orientation_covariance[5]; + pose.pose.covariance[33] = imu_transformed.orientation_covariance[6]; + pose.pose.covariance[34] = imu_transformed.orientation_covariance[7]; + pose.pose.covariance[35] = imu_transformed.orientation_covariance[8]; geometry_msgs::TwistWithCovarianceStamped twist; - twist.header = msg->header; - twist.twist.twist.angular = msg->angular_velocity; - twist.twist.covariance[21] = msg->angular_velocity_covariance[0]; - twist.twist.covariance[22] = msg->angular_velocity_covariance[1]; - twist.twist.covariance[23] = msg->angular_velocity_covariance[2]; - twist.twist.covariance[27] = msg->angular_velocity_covariance[3]; - twist.twist.covariance[28] = msg->angular_velocity_covariance[4]; - twist.twist.covariance[29] = msg->angular_velocity_covariance[5]; - twist.twist.covariance[33] = msg->angular_velocity_covariance[6]; - twist.twist.covariance[34] = msg->angular_velocity_covariance[7]; - twist.twist.covariance[35] = msg->angular_velocity_covariance[8]; + twist.header = imu_transformed.header; + twist.twist.twist.angular = imu_transformed.angular_velocity; + twist.twist.covariance[21] = imu_transformed.angular_velocity_covariance[0]; + twist.twist.covariance[22] = imu_transformed.angular_velocity_covariance[1]; + twist.twist.covariance[23] = imu_transformed.angular_velocity_covariance[2]; + twist.twist.covariance[27] = imu_transformed.angular_velocity_covariance[3]; + twist.twist.covariance[28] = imu_transformed.angular_velocity_covariance[4]; + twist.twist.covariance[29] = imu_transformed.angular_velocity_covariance[5]; + twist.twist.covariance[33] = imu_transformed.angular_velocity_covariance[6]; + twist.twist.covariance[34] = imu_transformed.angular_velocity_covariance[7]; + twist.twist.covariance[35] = imu_transformed.angular_velocity_covariance[8]; const bool validate = !params_.disable_checks; if (params_.differential) { - processDifferential(*pose, twist, validate, *transaction); + processDifferential(pose, twist, validate, *transaction); } else { common::processAbsolutePoseWithCovariance( name(), device_id_, - *pose, + pose, params_.pose_loss, - params_.orientation_target_frame, + params_.twist_target_frame, {}, params_.orientation_indices, tf_buffer_, @@ -171,17 +186,17 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) // Handle the acceleration data geometry_msgs::AccelWithCovarianceStamped accel; - accel.header = msg->header; - accel.accel.accel.linear = msg->linear_acceleration; - accel.accel.covariance[0] = msg->linear_acceleration_covariance[0]; - accel.accel.covariance[1] = msg->linear_acceleration_covariance[1]; - accel.accel.covariance[2] = msg->linear_acceleration_covariance[2]; - accel.accel.covariance[6] = msg->linear_acceleration_covariance[3]; - accel.accel.covariance[7] = msg->linear_acceleration_covariance[4]; - accel.accel.covariance[8] = msg->linear_acceleration_covariance[5]; - accel.accel.covariance[12] = msg->linear_acceleration_covariance[6]; - accel.accel.covariance[13] = msg->linear_acceleration_covariance[7]; - accel.accel.covariance[14] = msg->linear_acceleration_covariance[8]; + accel.header = imu_transformed.header; + accel.accel.accel.linear = imu_transformed.linear_acceleration; + accel.accel.covariance[0] = imu_transformed.linear_acceleration_covariance[0]; + accel.accel.covariance[1] = imu_transformed.linear_acceleration_covariance[1]; + accel.accel.covariance[2] = imu_transformed.linear_acceleration_covariance[2]; + accel.accel.covariance[6] = imu_transformed.linear_acceleration_covariance[3]; + accel.accel.covariance[7] = imu_transformed.linear_acceleration_covariance[4]; + accel.accel.covariance[8] = imu_transformed.linear_acceleration_covariance[5]; + accel.accel.covariance[12] = imu_transformed.linear_acceleration_covariance[6]; + accel.accel.covariance[13] = imu_transformed.linear_acceleration_covariance[7]; + accel.accel.covariance[14] = imu_transformed.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity if (params_.remove_gravitational_acceleration) @@ -190,7 +205,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) accel_gravity.z = params_.gravitational_acceleration; geometry_msgs::TransformStamped orientation_trans; tf2::Quaternion imu_orientation; - tf2::fromMsg(msg->orientation, imu_orientation); + tf2::fromMsg(imu_transformed.orientation, imu_orientation); orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse()); tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp accel.accel.accel.linear.x -= accel_gravity.x; @@ -203,7 +218,7 @@ void Imu2D::process(const sensor_msgs::Imu::ConstPtr& msg) device_id_, accel, params_.linear_acceleration_loss, - params_.acceleration_target_frame, + params_.twist_target_frame, params_.linear_acceleration_indices, tf_buffer_, validate, @@ -218,52 +233,28 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, fuse_core::Transaction& transaction) { - auto transformed_pose = std::make_unique(); - transformed_pose->header.frame_id = - params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; - - if (!common::transformMessage(tf_buffer_, pose, *transformed_pose)) - { - ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp - << " to orientation target frame " - << params_.orientation_target_frame); - return; - } - if (!previous_pose_) { - previous_pose_ = std::move(transformed_pose); + previous_pose_ = std::make_unique(); + *previous_pose_ = pose; return; } if (params_.use_twist_covariance) { - geometry_msgs::TwistWithCovarianceStamped transformed_twist; - transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(tf_buffer_, twist, transformed_twist)) - { - ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp - << " to twist target frame " - << params_.twist_target_frame); - } - else - { - common::processDifferentialPoseWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); - } + common::processDifferentialPoseWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + pose, + twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); } else { @@ -271,7 +262,7 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& name(), device_id_, *previous_pose_, - *transformed_pose, + pose, params_.independent, params_.minimum_pose_relative_covariance, params_.pose_loss, @@ -281,7 +272,7 @@ void Imu2D::processDifferential(const geometry_msgs::PoseWithCovarianceStamped& transaction); } - previous_pose_ = std::move(transformed_pose); + *previous_pose_ = pose; } } // namespace fuse_models diff --git a/fuse_models/test/diffbot.launch b/fuse_models/test/diffbot.launch new file mode 100644 index 000000000..3b344e0e6 --- /dev/null +++ b/fuse_models/test/diffbot.launch @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/fuse_models/test/diffbot.xacro b/fuse_models/test/diffbot.xacro new file mode 100644 index 000000000..0d4331f43 --- /dev/null +++ b/fuse_models/test/diffbot.xacro @@ -0,0 +1,114 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + true + 50.0 + true + + imu + imu_link + 50.0 + imu_link + 1.0e-6 + 0 0 0 + 0 0 0 + false + + + + + + + + / + gazebo_ros_control/DefaultRobotHWSim + + + + + + + base_link + ground_truth_odom + true + 50.0 + + + + + + Gazebo/Green + + + + Gazebo/Purple + + diff --git a/fuse_models/test/diffbot.yaml b/fuse_models/test/diffbot.yaml new file mode 100644 index 000000000..f537d14ff --- /dev/null +++ b/fuse_models/test/diffbot.yaml @@ -0,0 +1,6 @@ +gazebo_ros_control: + pid_gains: + wheel_0_joint: + p: &p 100.0 + wheel_1_joint: + p: *p diff --git a/fuse_models/test/diffbot_controllers.yaml b/fuse_models/test/diffbot_controllers.yaml new file mode 100644 index 000000000..ecdff2829 --- /dev/null +++ b/fuse_models/test/diffbot_controllers.yaml @@ -0,0 +1,16 @@ +# Publish all joint states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 + +# Diff drive controller +diffbot_controller: + type: "diff_drive_controller/DiffDriveController" + left_wheel: 'wheel_0_joint' + right_wheel: 'wheel_1_joint' + publish_rate: 50.0 # defaults to 50 + initial_pose_covariance_diagonal: [0.001, 0.001, 0.01] + cmd_vel_timeout: 20.0 # we test this separately, give plenty for the other tests + k_l: 0.01 + k_r: 0.01 + wheel_resolution: 0.001 diff --git a/fuse_models/test/imu.yaml b/fuse_models/test/imu.yaml new file mode 100644 index 000000000..580c786d5 --- /dev/null +++ b/fuse_models/test/imu.yaml @@ -0,0 +1,7 @@ +imu: + topic: /imu + angular_velocity_dimensions: ['yaw'] + differential: true + orientation_dimensions: ['yaw'] + orientation_target_frame: &target_frame base_link + twist_target_frame: *target_frame diff --git a/fuse_models/test/imu_2d.test b/fuse_models/test/imu_2d.test new file mode 100644 index 000000000..6443db369 --- /dev/null +++ b/fuse_models/test/imu_2d.test @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/fuse_models/test/test_imu_2d.cpp b/fuse_models/test/test_imu_2d.cpp new file mode 100644 index 000000000..f1530b2a4 --- /dev/null +++ b/fuse_models/test/test_imu_2d.cpp @@ -0,0 +1,331 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, Clearpath Robotics + * 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 copyright holder 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 HOLDER 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. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Imu2D test fixture + */ +class Imu2DTestFixture : public ::testing::Test +{ +public: + Imu2DTestFixture() + { + cmd_vel_publisher_ = node_handle_.advertise("/diffbot_controller/cmd_vel", 10); + odom_subscriber_ = node_handle_.subscribe("odom", 10, &Imu2DTestFixture::odomCallback, this); + + cmd_vel_timer_ = node_handle_.createTimer(ros::Duration(0.1), &Imu2DTestFixture::cmdVelTimerCallback, this); + } + + void transactionCallback(const fuse_core::Transaction::SharedPtr& transaction) + { + std::lock_guard lock(last_transaction_mutex_); + last_transaction_ = *transaction; + } + + void setCmdVel(const geometry_msgs::Twist& cmd_vel) + { + cmd_vel_ = cmd_vel; + } + + nav_msgs::Odometry getLastOdom() const + { + std::lock_guard lock(last_odom_mutex_); + return last_odom_; + } + + fuse_core::Transaction getLastTransaction() const + { + std::lock_guard lock(last_transaction_mutex_); + return last_transaction_; + } + +private: + void odomCallback(const nav_msgs::Odometry::ConstPtr& msg) + { + std::lock_guard lock(last_odom_mutex_); + last_odom_ = *msg; + } + + void cmdVelTimerCallback(const ros::TimerEvent&) + { + cmd_vel_publisher_.publish(cmd_vel_); + } + + ros::NodeHandle node_handle_; //!< A node handle + ros::Publisher cmd_vel_publisher_; //!< The command velocity publisher + ros::Subscriber odom_subscriber_; //!< The wheel odometry subscriber + ros::Timer cmd_vel_timer_; //!< A timer to send the command velocity to the robot continuously; if the command + //!< velocity is only sent once, after some time the controller considers the command + //!< velocity publisher hang up and resets the command velocity it uses to zero + + geometry_msgs::Twist cmd_vel_; //!< The command velocity to send to the robot + nav_msgs::Odometry last_odom_; //!< The last odometry measurement received + + fuse_core::Transaction last_transaction_; //!< The last transaction generated + + mutable std::mutex last_odom_mutex_; //!< A mutex to protect last_odom_ access + mutable std::mutex last_transaction_mutex_; //!< A mutex to protect last_transaction_ access +}; + +TEST_F(Imu2DTestFixture, BaseFrameYawAngularVelocity) +{ + // Create an IMU sensor and register the transaction callback + const std::string imu_2d_name{ "imu" }; + + fuse_models::Imu2D imu_2d; + imu_2d.initialize(imu_2d_name, std::bind(&Imu2DTestFixture::transactionCallback, this, std::placeholders::_1)); + imu_2d.start(); + + // Make the robot turn in place for a few seconds + geometry_msgs::Twist twist; + twist.angular.z = 0.5; + + setCmdVel(twist); + + using namespace std::chrono_literals; // NOLINT(build/namespaces) + std::this_thread::sleep_for(3s); + + // Confirm the last transaction has the expect number of variables and constraints generated by the imu sensor model + // source; if no transaction was received, the last transaction would not have any added variables + const auto last_transaction = getLastTransaction(); + + // That is: + // * 5 variables: + // * 1 variable for the angular velocity + // * 4 variables for the current and previous position and orientation + // * 2 constraints: + // * 1 constraints for the absolute angular velocity + // * 1 constraints for the relative pose between the current and previous position and orientation + // + const auto added_variables = last_transaction.addedVariables(); + const auto added_constraints = last_transaction.addedConstraints(); + + ASSERT_EQ(5u, boost::size(added_variables)); + ASSERT_EQ(2u, boost::size(added_constraints)); + + // With all constraints generated by the imu sensor model source + for (const auto& constraint : added_constraints) + { + ASSERT_EQ(imu_2d_name, constraint.source()); + } + + // And no variables or constraints are removed + ASSERT_TRUE(boost::empty(last_transaction.removedVariables())); + ASSERT_TRUE(boost::empty(last_transaction.removedConstraints())); + + // Confirm the last transaction generated matches the last wheel odometry received + // + // Note that we are not synchronizing the odometry measurements with the last transaction stamp or involved stamps, + // under the assumption the velocity is steady at the time it is being sampled. However, this is likely the reason the + // tolerance in the checks below is too high. With 1.0e-3 most of the checks fail even when things are working + // properly. + const auto last_odom = getLastOdom(); + + // First, confirm the fuse_variables::VelocityAngular2DStamped variable yaw is the same as the wheel odometry twist + // angular velocity along the z axis + const auto& w = last_odom.twist.twist.angular.z; + + // That is, ensure there is one fuse_variables::VelocityAngular2DStamped variable + EXPECT_EQ(1, std::count_if(added_variables.begin(), added_variables.end(), + [](const auto& variable) -> bool { // NOLINT(whitespace/braces) + return dynamic_cast(&variable); + })); // NOLINT(whitespace/braces) + + // And iterate over all added variables to process it + for (const auto& variable : added_variables) + { + // Skip if not a fuse_variables::VelocityAngular2DStamped variable + const auto velocity_angular = dynamic_cast(&variable); + if (!velocity_angular) + { + continue; + } + + // Check the angular velocity yaw is the same as the wheel odometry one + EXPECT_NEAR(w, velocity_angular->yaw(), 1.0e-2); + } + + // Second, confirm the fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint mean is the same as the + // wheel odometry twist angular velocity along the z axis + + // That is, ensure there is one fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint + EXPECT_EQ(1, std::count_if(added_constraints.begin(), added_constraints.end(), + [](const auto& constraint) -> bool { // NOLINT(whitespace/braces) + return dynamic_cast( + &constraint); + })); // NOLINT(whitespace/braces) + + // And iterate over all added constraints to process it + for (const auto& constraint : added_constraints) + { + // Skip if not a fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint constraint + const auto velocity_angular = + dynamic_cast(&constraint); + if (!velocity_angular) + { + continue; + } + + // Check the angular velocity mean has a single value and it is the same as the wheel odometry one + const auto& mean = velocity_angular->mean(); + ASSERT_EQ(1u, mean.size()); + EXPECT_NEAR(w, mean[0], 1.0e-2); + } + + // Third, confirm the relative yaw orientation velocity between the current and the previous + // fuse_variables::Orientation2DStamped variables is the same as the wheel odometry twist angular velocity along the z + // axis + + // That is, ensure there are two fuse_variables::Orientation2DStamped variables + EXPECT_EQ(2, std::count_if(added_variables.begin(), added_variables.end(), + [](const auto& variable) -> bool { // NOLINT(whitespace/braces) + return dynamic_cast(&variable); + })); // NOLINT(whitespace/braces) + + // Iterate over all added variables to process retrieve the fuse_variables::Orientation2DStamped variables + std::vector orientations; + for (const auto& variable : added_variables) + { + // Skip if not a fuse_variables::VelocityAngular2DStamped variable + const auto orientation = dynamic_cast(&variable); + if (!orientation) + { + continue; + } + + orientations.push_back(orientation); + } + + // Sort them by stamp + std::sort(orientations.begin(), orientations.end(), + [](const auto& lhs, const auto& rhs) { return lhs->stamp() < rhs->stamp(); }); // NOLINT(whitespace/braces) + + // And check the angular velocity is the same as the wheel odometry one + // + // We need the time and orientation deltas in order to compute the angular velocity + const auto dt = (orientations[1]->stamp() - orientations[0]->stamp()).toSec(); + ASSERT_LT(0.0, dt); + + const auto orientation_delta = angles::shortest_angular_distance(orientations[0]->yaw(), orientations[1]->yaw()); + + EXPECT_NEAR(w, orientation_delta / dt, 1.0e-2); + + // Finally, confirm the fuse_constraints::RelativePose2DStampedConstraint constraint mean is the same as the wheel + // odometry twist angular velocity along the z axis + + // That is, ensure there is one fuse_constraints::RelativePose2DStampedConstraint constraint + EXPECT_EQ(1, + std::count_if(added_constraints.begin(), added_constraints.end(), + [](const auto& constraint) -> bool { // NOLINT(whitespace/braces) + return dynamic_cast(&constraint); + })); // NOLINT(whitespace/braces) + + // And iterate over all added constraints to process it + for (const auto& constraint : added_constraints) + { + // Skip if not a fuse_constraints::RelativePose2DStampedConstraint constraint + const auto relative_pose = dynamic_cast(&constraint); + if (!relative_pose) + { + continue; + } + + // Check the relative pose mean orientation (3rd component) is the same as the wheel odometry one, when multiplied + // by the time delta between the current and previous position and orientation variables + // + // We retrieve the current and previous positions, i.e. position2 and position1, and compute the time delta between + // them. Alternatively, we could use the time delta previously computed for the orientation variables. + const auto& delta = relative_pose->delta(); + + const auto& variables = relative_pose->variables(); + const auto& position1_uuid = variables[0]; + const auto& position2_uuid = variables[2]; + + const auto position1_iter = std::find_if(added_variables.begin(), added_variables.end(), + [&position1_uuid](const auto& variable) { // NOLINT(whitespace/braces) + return variable.uuid() == position1_uuid; + }); // NOLINT(whitespace/braces) + ASSERT_NE(added_variables.end(), position1_iter); + + const auto position1 = dynamic_cast(&*position1_iter); + ASSERT_TRUE(position1); + + const auto position2_iter = std::find_if(added_variables.begin(), added_variables.end(), + [&position2_uuid](const auto& variable) { // NOLINT(whitespace/braces) + return variable.uuid() == position2_uuid; + }); // NOLINT(whitespace/braces) + ASSERT_NE(added_variables.end(), position2_iter); + + const auto position2 = dynamic_cast(&*position2_iter); + ASSERT_TRUE(position2); + + const auto dt = position2->stamp().toSec() - position1->stamp().toSec(); + ASSERT_LT(0.0, dt); + + ASSERT_EQ(3u, delta.size()); + EXPECT_NEAR(w, delta[2] / dt, 1.0e-2); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "imu_2d_test"); + auto spinner = ros::AsyncSpinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/fuse_models/test/wheel.xacro b/fuse_models/test/wheel.xacro new file mode 100644 index 000000000..85f67d91e --- /dev/null +++ b/fuse_models/test/wheel.xacro @@ -0,0 +1,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/VelocityJointInterface + + + hardware_interface/VelocityJointInterface + 1 + 1 + + + + + Gazebo/Red + + +