diff --git a/CMakeLists.txt b/CMakeLists.txt index e4f89ceac..67b8ea18b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -198,20 +198,30 @@ if(BUILD_TESTING) rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") target_link_libraries(test_ekf "${cpp_typesupport_target}") - ament_add_gtest_executable(test_ekf_localization_node_interfaces - test/test_ekf_localization_node_interfaces.cpp) - target_link_libraries(test_ekf_localization_node_interfaces ${library_name}) - - rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") - target_link_libraries(test_ekf_localization_node_interfaces "${cpp_typesupport_target}") - - add_dependencies(test_ekf_localization_node_interfaces ekf_node) - ament_add_test(test_ekf_localization_node_interfaces + ament_add_gtest_executable( + test_ekf_node_interfaces + test/test_se_node_interfaces.cpp) + target_link_libraries( + test_ekf_node_interfaces + ${library_name}) + + rosidl_get_typesupport_target( + cpp_typesupport_target + "${PROJECT_NAME}" "rosidl_typesupport_cpp") + target_link_libraries( + test_ekf_node_interfaces + "${cpp_typesupport_target}") + + add_dependencies( + test_ekf_node_interfaces + ekf_node) + ament_add_test( + test_ekf_node_interfaces GENERATE_RESULT_FOR_RETURN_CODE_ZERO TIMEOUT 300 - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_ekf_localization_node_interfaces.launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_se_node_interfaces.launch.py" + ENV NODE_TYPE=ekf + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") #### UKF TESTS ##### ament_add_gtest(test_ukf test/test_ukf.cpp) @@ -220,20 +230,30 @@ if(BUILD_TESTING) rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") target_link_libraries(test_ukf "${cpp_typesupport_target}") - ament_add_gtest_executable(test_ukf_localization_node_interfaces - test/test_ukf_localization_node_interfaces.cpp) - target_link_libraries(test_ukf_localization_node_interfaces ${library_name}) - - rosidl_get_typesupport_target(cpp_typesupport_target "${PROJECT_NAME}" "rosidl_typesupport_cpp") - target_link_libraries(test_ukf_localization_node_interfaces "${cpp_typesupport_target}") - - add_dependencies(test_ukf_localization_node_interfaces ukf_node) - ament_add_test(test_ukf_localization_node_interfaces + ament_add_gtest_executable( + test_ukf_node_interfaces + test/test_se_node_interfaces.cpp) + target_link_libraries( + test_ukf_node_interfaces + ${library_name}) + + rosidl_get_typesupport_target( + cpp_typesupport_target + "${PROJECT_NAME}" "rosidl_typesupport_cpp") + target_link_libraries( + test_ukf_node_interfaces + "${cpp_typesupport_target}") + + add_dependencies( + test_ukf_node_interfaces + ukf_node) + ament_add_test( + test_ukf_node_interfaces GENERATE_RESULT_FOR_RETURN_CODE_ZERO TIMEOUT 300 - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_ukf_localization_node_interfaces.launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test/test_se_node_interfaces.launch.py" + ENV NODE_TYPE=ukf + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}") #### DATA TESTS ##### # ament_add_gtest(test_ekf_localization_node_bag1 test/test_localization_node_bag_pose_tester.cpp) @@ -294,9 +314,9 @@ if(BUILD_TESTING) filter_base-test test_filter_base_diagnostics_timestamps test_ekf - test_ekf_localization_node_interfaces + test_ekf_node_interfaces test_ukf - test_ukf_localization_node_interfaces + test_ukf_node_interfaces #test_ekf_localization_node_bag1 #test_ekf_localization_node_bag2 #test_ekf_localization_node_bag3 diff --git a/test/test_ekf_localization_node_interfaces.cpp b/test/test_ekf_localization_node_interfaces.cpp deleted file mode 100644 index d773dfd6e..000000000 --- a/test/test_ekf_localization_node_interfaces.cpp +++ /dev/null @@ -1,1032 +0,0 @@ -/* - * Copyright (c) 2014, 2015, 2016 Charles River Analytics, 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: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. 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. - * 3. 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 "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" -#include "gtest/gtest.h" -#include "nav_msgs/msg/odometry.hpp" -#include "rclcpp/qos.hpp" -#include "rclcpp/rclcpp.hpp" -#include "robot_localization/srv/set_pose.hpp" -#include "sensor_msgs/msg/imu.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - -using namespace std::chrono_literals; - -nav_msgs::msg::Odometry filtered_; -bool stateUpdated_; - -void filterCallback(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - filtered_ = *msg; - stateUpdated_ = true; -} - -void resetFilter(rclcpp::Node::SharedPtr node_) -{ - // ros2 type service-client has been implemented - auto client = - node_->create_client("set_pose"); - auto setPoseRequest = - std::make_shared(); - - setPoseRequest->pose.pose.pose.orientation.w = 1; - setPoseRequest->pose.header.frame_id = "odom"; - - for (size_t ind = 0; ind < 36; ind += 7) { - setPoseRequest->pose.pose.covariance[ind] = 1e-6; - } - - setPoseRequest->pose.header.stamp = node_->now(); - - if (!client->wait_for_service(10s)) { - ASSERT_TRUE(false) << "service not available after waiting"; - } - - auto result = client->async_send_request(setPoseRequest); - auto ret = rclcpp::spin_until_future_complete( - node_, result, - 5s); // Wait for the result. - - double deltaX = 0.0; - double deltaY = 0.0; - double deltaZ = 0.0; - - if (ret == rclcpp::FutureReturnCode::SUCCESS) { - rclcpp::Rate(2).sleep(); - rclcpp::spin_some(node_); - deltaX = filtered_.pose.pose.position.x - - setPoseRequest->pose.pose.pose.position.x; - deltaY = filtered_.pose.pose.position.y - - setPoseRequest->pose.pose.pose.position.y; - deltaZ = filtered_.pose.pose.position.z - - setPoseRequest->pose.pose.pose.position.z; - EXPECT_LT( - ::sqrt( - deltaX * deltaX + deltaY * deltaY + deltaZ * deltaZ), - 0.1); - } else { - EXPECT_TRUE(false); - } -} - -TEST(InterfacesTest, OdomPoseBasicIO) { - stateUpdated_ = false; - - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomPoseBasicIO_testcase"); - - auto odomPub = node_->create_publisher( - "odom_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - - nav_msgs::msg::Odometry odom; - odom.pose.pose.position.x = 20.0; - odom.pose.pose.position.y = 10.0; - odom.pose.pose.position.z = -40.0; - - odom.pose.covariance[0] = 2.0; - odom.pose.covariance[7] = 2.0; - odom.pose.covariance[14] = 2.0; - - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_link"; - rclcpp::Rate loopRate(50); - for (size_t i = 0; i < 50; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - // Now check the values from the callback - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), - 0.01); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.y), - 0.01); // Configuration for this variable for this sensor is false - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), - 0.01); - - EXPECT_LT(filtered_.pose.covariance[0], 0.5); - EXPECT_LT( - filtered_.pose.covariance[7], - 0.25); // Configuration for this variable for this sensor is false - EXPECT_LT(filtered_.pose.covariance[14], 0.5); - - resetFilter(node_); -} - -TEST(InterfacesTest, OdomTwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomTwistBasicIO_testcase"); - - auto odomPub = node_->create_publisher( - "odom_input2", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - nav_msgs::msg::Odometry odom; - odom.twist.twist.linear.x = 5.0; - odom.twist.twist.linear.y = 0.0; - odom.twist.twist.linear.z = 0.0; - odom.twist.twist.angular.x = 0.0; - odom.twist.twist.angular.y = 0.0; - odom.twist.twist.angular.z = 0.0; - - for (size_t ind = 0; ind < 36; ind += 7) { - odom.twist.covariance[ind] = 1e-6; - } - - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_link"; - - rclcpp::Rate loopRate(20); - for (size_t i = 0; i < 400; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); - - resetFilter(node_); - - odom.twist.twist.linear.x = 0.0; - odom.twist.twist.linear.y = 5.0; - - for (size_t i = 0; i < 400; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 100.0), 2.0); - - resetFilter(node_); - - odom.twist.twist.linear.y = 0.0; - odom.twist.twist.linear.z = 5.0; - - for (size_t i = 0; i < 400; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 100.0), 2.0); - - resetFilter(node_); - - odom.twist.twist.linear.z = 0.0; - odom.twist.twist.linear.x = 1.0; - odom.twist.twist.angular.z = (M_PI / 2) / (100.0 * 0.05); - - for (size_t i = 0; i < 100; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), - 0.5); - - resetFilter(node_); - - odom.twist.twist.linear.x = 0.0; - odom.twist.twist.angular.z = 0.0; - odom.twist.twist.angular.x = -(M_PI / 2) / (100.0 * 0.05); - - // First, roll the vehicle on its side - for (size_t i = 0; i < 100; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - odom.twist.twist.angular.x = 0.0; - odom.twist.twist.angular.y = (M_PI / 2) / (100.0 * 0.05); - - // Now, pitch it down (positive pitch velocity in vehicle frame) - for (size_t i = 0; i < 100; ++i) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - odom.twist.twist.angular.y = 0.0; - odom.twist.twist.linear.x = 3.0; - - // We should now be on our side and facing -Y. Move forward in - // the vehicle frame X direction, and make sure Y decreases in - // the world frame. - for (size_t i = 0; i < 100; ++i) { - odom.header.stamp = node_->now(); - - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); - - resetFilter(node_); -} - -TEST(InterfacesTest, PoseBasicIO) { - auto node_ = rclcpp::Node::make_shared("InterfacesTest_PoseBasicIO_testcase"); - - auto posePub = - node_->create_publisher( - "pose_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.pose.pose.position.x = 20.0; - pose.pose.pose.position.y = 10.0; - pose.pose.pose.position.z = -40.0; - pose.pose.pose.orientation.x = 0; - pose.pose.pose.orientation.y = 0; - pose.pose.pose.orientation.z = 0; - pose.pose.pose.orientation.w = 1; - - for (size_t ind = 0; ind < 36; ind += 7) { - pose.pose.covariance[ind] = 1e-6; - } - - pose.header.frame_id = "odom"; - rclcpp::Rate loopRate(50); - - for (size_t i = 0; i < 50; ++i) { - pose.header.stamp = node_->now(); - posePub->publish(pose); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - // Now check the values from the callback - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.y), - 0.1); // Configuration for this variable for this sensor is false - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), - 0.1); - - EXPECT_LT(filtered_.pose.covariance[0], 0.5); - EXPECT_LT( - filtered_.pose.covariance[7], - 0.25); // Configuration for this variable for this sensor is false - EXPECT_LT(filtered_.pose.covariance[14], 0.5); - - resetFilter(node_); -} - -TEST(InterfacesTest, TwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_TwistBasicIO_testcase"); - - auto twistPub = - node_->create_publisher( - "twist_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - geometry_msgs::msg::TwistWithCovarianceStamped twist; - twist.twist.twist.linear.x = 5.0; - twist.twist.twist.linear.y = 0; - twist.twist.twist.linear.z = 0; - twist.twist.twist.angular.x = 0; - twist.twist.twist.angular.y = 0; - twist.twist.twist.angular.z = 0; - - for (size_t ind = 0; ind < 36; ind += 7) { - twist.twist.covariance[ind] = 1e-6; - } - - twist.header.frame_id = "base_link"; - rclcpp::Rate loopRate(20); - for (size_t i = 0; i < 400; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); - - resetFilter(node_); - - twist.twist.twist.linear.x = 0.0; - twist.twist.twist.linear.y = 5.0; - - for (size_t i = 0; i < 400; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 100.0), 2.0); - - resetFilter(node_); - - twist.twist.twist.linear.y = 0.0; - twist.twist.twist.linear.z = 5.0; - - for (size_t i = 0; i < 400; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 100.0), 2.0); - - resetFilter(node_); - - twist.twist.twist.linear.z = 0.0; - twist.twist.twist.linear.x = 1.0; - twist.twist.twist.angular.z = (M_PI / 2) / (100.0 * 0.05); - for (size_t i = 0; i < 100; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), - 0.5); - - resetFilter(node_); - - twist.twist.twist.linear.x = 0.0; - twist.twist.twist.angular.z = 0.0; - twist.twist.twist.angular.x = -(M_PI / 2) / (100.0 * 0.05); - - // First, roll the vehicle on its side - for (size_t i = 0; i < 100; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - twist.twist.twist.angular.x = 0.0; - twist.twist.twist.angular.y = (M_PI / 2) / (100.0 * 0.05); - - // Now, pitch it down (positive pitch velocity in vehicle frame) - for (size_t i = 0; i < 100; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - twist.twist.twist.angular.y = 0.0; - twist.twist.twist.linear.x = 3.0; - - // We should now be on our side and facing -Y. Move forward in - // the vehicle frame X direction, and make sure Y decreases in - // the world frame. - for (size_t i = 0; i < 100; ++i) { - twist.header.stamp = node_->now(); - twistPub->publish(twist); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); - - resetFilter(node_); -} - -TEST(InterfacesTest, ImuPoseBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuPoseBasicIO_testcase"); - - auto imuPub = node_->create_publisher( - "/imu_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - sensor_msgs::msg::Imu imu; - tf2::Quaternion quat; - quat.setRPY(M_PI / 4, -M_PI / 4, M_PI / 2); - imu.orientation = tf2::toMsg(quat); - - for (size_t ind = 0; ind < 9; ind += 4) { - imu.orientation_covariance[ind] = 1e-6; - } - - imu.header.frame_id = "base_link"; - - // Make sure the pose reset worked. Test will timeout - // if this fails. - rclcpp::Rate loopRate1(50); - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate1.sleep(); - } - - // Now check the values from the callback - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - tf2::Matrix3x3 mat(quat); - double r, p, y; - mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r - M_PI / 4), 0.1); - EXPECT_LT(::fabs(p + M_PI / 4), 0.1); - EXPECT_LT(::fabs(y - M_PI / 2), 0.1); - - EXPECT_LT(filtered_.pose.covariance[21], 0.5); - EXPECT_LT(filtered_.pose.covariance[28], 0.25); - EXPECT_LT(filtered_.pose.covariance[35], 0.5); - - resetFilter(node_); - - // Test to see if the orientation data is ignored when we set the - // first covariance value to -1 - sensor_msgs::msg::Imu imuIgnore; - imuIgnore.orientation.x = 0.1; - imuIgnore.orientation.y = 0.2; - imuIgnore.orientation.z = 0.3; - imuIgnore.orientation.w = 0.4; - imuIgnore.orientation_covariance[0] = -1; - - rclcpp::Rate loopRate2(50); - for (size_t i = 0; i < 50; ++i) { - imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); - rclcpp::spin_some(node_); - loopRate2.sleep(); - } - rclcpp::spin_some(node_); - - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - mat.setRotation(quat); - mat.getRPY(r, p, y); - - EXPECT_LT(::fabs(r), 1e-3); - EXPECT_LT(::fabs(p), 1e-3); - EXPECT_LT(::fabs(y), 1e-3); - - resetFilter(node_); -} - -TEST(InterfacesTest, ImuTwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuTwistBasicIO_testcase"); - - auto imuPub = node_->create_publisher( - "/imu_input1", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - sensor_msgs::msg::Imu imu; - tf2::Quaternion quat; - imu.angular_velocity.x = (M_PI / 2.0); - - for (size_t ind = 0; ind < 9; ind += 4) { - imu.angular_velocity_covariance[ind] = 1e-6; - } - - imu.header.frame_id = "base_link"; - rclcpp::Rate loopRate(50); - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - // Now check the values from the callback - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - tf2::Matrix3x3 mat(quat); - double r, p, y; - mat.getRPY(r, p, y); - - // Tolerances may seem loose, but the initial state covariances - // are tiny, so the filter is sluggish to accept velocity data - EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7); - EXPECT_LT(::fabs(p), 0.1); - EXPECT_LT(::fabs(y), 0.1); - - EXPECT_LT(filtered_.twist.covariance[21], 1e-3); - EXPECT_LT(filtered_.pose.covariance[21], 0.1); - - resetFilter(node_); - - imu.angular_velocity.x = 0.0; - imu.angular_velocity.y = -(M_PI / 3.0); - - // Make sure the pose reset worked. Test will timeout - // if this fails. - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - // Now check the values from the callback - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - mat.setRotation(quat); - mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 0.1); - EXPECT_LT(::fabs(p + M_PI / 3.0), 0.7); - EXPECT_LT(::fabs(y), 0.1); - - EXPECT_LT(filtered_.twist.covariance[28], 1e-3); - EXPECT_LT(filtered_.pose.covariance[28], 0.1); - - resetFilter(node_); - - imu.angular_velocity.y = 0; - imu.angular_velocity.z = M_PI / 4.0; - - // Make sure the pose reset worked. Test will timeout - // if this fails. - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - // Now check the values from the callback - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - mat.setRotation(quat); - mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 0.1); - EXPECT_LT(::fabs(p), 0.1); - EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7); - - EXPECT_LT(filtered_.twist.covariance[35], 1e-3); - EXPECT_LT(filtered_.pose.covariance[35], 0.1); - - resetFilter(node_); - - // Test to see if the angular velocity data is ignored when we set the - // first covariance value to -1 - sensor_msgs::msg::Imu imuIgnore; - imuIgnore.angular_velocity.x = 100; - imuIgnore.angular_velocity.y = 100; - imuIgnore.angular_velocity.z = 100; - imuIgnore.angular_velocity_covariance[0] = -1; - - for (size_t i = 0; i < 50; ++i) { - imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - mat.setRotation(quat); - mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 1e-3); - EXPECT_LT(::fabs(p), 1e-3); - EXPECT_LT(::fabs(y), 1e-3); - - resetFilter(node_); -} - -TEST(InterfacesTest, ImuAccBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuAccBasicIO_testcase"); - - auto imuPub = node_->create_publisher( - "imu_input2", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - sensor_msgs::msg::Imu imu; - imu.header.frame_id = "base_link"; - imu.linear_acceleration_covariance[0] = 1e-6; - imu.linear_acceleration_covariance[4] = 1e-6; - imu.linear_acceleration_covariance[8] = 1e-6; - - imu.linear_acceleration.x = 1; - imu.linear_acceleration.y = -1; - imu.linear_acceleration.z = 1; - - // Move with constant acceleration for 1 second, - // then check our velocity. - rclcpp::Rate loopRate(50); - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.4); - EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.4); - EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.4); - - imu.linear_acceleration.x = 0.0; - imu.linear_acceleration.y = 0.0; - imu.linear_acceleration.z = 0.0; - - // Now move for another second, and see where we end up - for (size_t i = 0; i < 50; ++i) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.8), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.8), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.8), 0.4); - - resetFilter(node_); - - // Test to see if the linear acceleration data is ignored when we set the - // first covariance value to -1 - sensor_msgs::msg::Imu imuIgnore; - imuIgnore.linear_acceleration.x = 1000; - imuIgnore.linear_acceleration.y = 1000; - imuIgnore.linear_acceleration.z = 1000; - imuIgnore.linear_acceleration_covariance[0] = -1; - - for (size_t i = 0; i < 50; ++i) { - imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3); - - resetFilter(node_); -} - -TEST(InterfacesTest, OdomDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomDifferentialIO_testcase"); - - auto odomPub = node_->create_publisher( - "/odom_input1", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - nav_msgs::msg::Odometry odom; - odom.pose.pose.position.x = 20.0; - odom.pose.pose.position.y = 10.0; - odom.pose.pose.position.z = -40.0; - - odom.pose.pose.orientation.w = 1; - - odom.pose.covariance[0] = 2.0; - odom.pose.covariance[7] = 2.0; - odom.pose.covariance[14] = 2.0; - odom.pose.covariance[21] = 0.2; - odom.pose.covariance[28] = 0.2; - odom.pose.covariance[35] = 0.2; - - odom.header.frame_id = "odom"; - odom.child_frame_id = "base_link"; - - // No guaranteeing that the zero state - // we're expecting to see here isn't just - // a result of zeroing it out previously, - // so check 10 times in succession. - size_t zeroCount = 0; - while (zeroCount++ < 10) { - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); - rclcpp::Rate(10).sleep(); - } - - for (size_t ind = 0; ind < 36; ind += 7) { - odom.pose.covariance[ind] = 1e-6; - } - - // Slowly move the position, and hope that the differential position keeps up - rclcpp::Rate loopRate(20); - for (size_t i = 0; i < 100; ++i) { - odom.pose.pose.position.x += 0.01; - odom.pose.pose.position.y += 0.02; - odom.pose.pose.position.z -= 0.03; - - odom.header.stamp = node_->now(); - odomPub->publish(odom); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); - - resetFilter(node_); -} - -TEST(InterfacesTest, PoseDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_PoseDifferentialIO_testcase"); - - auto posePub = - node_->create_publisher( - "/pose_input1", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - - geometry_msgs::msg::PoseWithCovarianceStamped pose; - pose.pose.pose.position.x = 20.0; - pose.pose.pose.position.y = 10.0; - pose.pose.pose.position.z = -40.0; - - pose.pose.pose.orientation.w = 1; - - pose.pose.covariance[0] = 2.0; - pose.pose.covariance[7] = 2.0; - pose.pose.covariance[14] = 2.0; - pose.pose.covariance[21] = 0.2; - pose.pose.covariance[28] = 0.2; - pose.pose.covariance[35] = 0.2; - - pose.header.frame_id = "odom"; - - // No guaranteeing that the zero state - // we're expecting to see here isn't just - // a result of zeroing it out previously, - // so check 10 times in succession. - size_t zeroCount = 0; - while (zeroCount++ < 10) { - pose.header.stamp = node_->now(); - posePub->publish(pose); - rclcpp::spin_some(node_); - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); - - rclcpp::Rate(10).sleep(); - } - - // ...but only if we give the measurement a tiny covariance - for (size_t ind = 0; ind < 36; ind += 7) { - pose.pose.covariance[ind] = 1e-6; - } - - // Issue this location repeatedly, and see if we get - // a final reported position of (1, 2, -3) - rclcpp::Rate loopRate(20); - for (size_t i = 0; i < 100; ++i) { - pose.pose.pose.position.x += 0.01; - pose.pose.pose.position.y += 0.02; - pose.pose.pose.position.z -= 0.03; - - pose.header.stamp = node_->now(); - posePub->publish(pose); - rclcpp::spin_some(node_); - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); - - resetFilter(node_); -} - -TEST(InterfacesTest, ImuDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuDifferentialIO_testcase"); - - auto imu0Pub = node_->create_publisher( - "/imu_input0", rclcpp::SensorDataQoS()); - auto imu1Pub = node_->create_publisher( - "/imu_input1", rclcpp::SensorDataQoS()); - auto imuPub = node_->create_publisher( - "/imu_input3", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - sensor_msgs::msg::Imu imu; - imu.header.frame_id = "base_link"; - tf2::Quaternion quat; - const double roll = M_PI / 2.0; - const double pitch = -M_PI; - const double yaw = -M_PI / 4.0; - quat.setRPY(roll, pitch, yaw); - imu.orientation = tf2::toMsg(quat); - - imu.orientation_covariance[0] = 0.02; - imu.orientation_covariance[4] = 0.02; - imu.orientation_covariance[8] = 0.02; - - imu.angular_velocity_covariance[0] = 0.02; - imu.angular_velocity_covariance[4] = 0.02; - imu.angular_velocity_covariance[8] = 0.02; - - size_t setCount = 0; - while (setCount++ < 10) { - imu.header.stamp = node_->now(); - imu0Pub->publish(imu); // Use this to move the absolute orientation - imu1Pub->publish(imu); // Use this to keep velocities at 0 - rclcpp::spin_some(node_); - rclcpp::Rate(10).sleep(); - } - - size_t zeroCount = 0; - while (zeroCount++ < 10) { - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - rclcpp::Rate(10).sleep(); - } - - double rollFinal = roll; - double pitchFinal = pitch; - double yawFinal = yaw; - - // Move the orientation slowly, and see if we can push it to 0 - rclcpp::Rate loopRate(20); - for (size_t i = 0; i < 100; ++i) { - yawFinal -= 0.01 * (3.0 * M_PI / 4.0); - - quat.setRPY(rollFinal, pitchFinal, yawFinal); - - imu.orientation = tf2::toMsg(quat); - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - // Move the orientation slowly, and see if we can push it to 0 - for (size_t i = 0; i < 100; ++i) { - rollFinal += 0.01 * (M_PI / 2.0); - - quat.setRPY(rollFinal, pitchFinal, yawFinal); - - imu.orientation = tf2::toMsg(quat); - imu.header.stamp = node_->now(); - imuPub->publish(imu); - rclcpp::spin_some(node_); - - loopRate.sleep(); - } - rclcpp::spin_some(node_); - - tf2::fromMsg(filtered_.pose.pose.orientation, quat); - tf2::Matrix3x3 mat(quat); - mat.getRPY(rollFinal, pitchFinal, yawFinal); - - EXPECT_LT(::fabs(rollFinal), 0.2); - EXPECT_LT(::fabs(pitchFinal), 0.2); - EXPECT_LT(::fabs(yawFinal), 0.2); - - resetFilter(node_); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - ::testing::InitGoogleTest(&argc, argv); - - // Give ekf_localization_node time to initialize - rclcpp::Rate(0.5).sleep(); - - int ret = RUN_ALL_TESTS(); - rclcpp::shutdown(); - return ret; -} diff --git a/test/test_ekf_localization_node_interfaces.launch.py b/test/test_ekf_localization_node_interfaces.launch.py deleted file mode 100755 index cae6bd0a0..000000000 --- a/test/test_ekf_localization_node_interfaces.launch.py +++ /dev/null @@ -1,70 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2018 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Launch file for test_ekf_localization_node_interfaces.""" - -import launch -from launch import LaunchDescription -import launch_ros.actions -import os -import yaml -import sys -from launch.substitutions import EnvironmentVariable -import pathlib -import launch.actions -from launch.actions import DeclareLaunchArgument -from launch_testing.legacy import LaunchTestService -from launch.actions import ExecuteProcess -from launch import LaunchService -from ament_index_python.packages import get_package_prefix - -def generate_launch_description(): - parameters_file_dir = pathlib.Path(__file__).resolve().parent - parameters_file_path = parameters_file_dir / 'test_ekf_localization_node_interfaces.yaml' - os.environ['FILE_PATH'] = str(parameters_file_dir) - - #*****test_ekf_localization_node_interfaces.test***** - ekf_node = launch_ros.actions.Node( - package='robot_localization', - executable='ekf_node', - name='test_ekf_localization_node_interfaces_ekf', - output='screen', - parameters=[ - parameters_file_path, - str(parameters_file_path), - [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ekf_localization_node_interfaces.yaml'], - ],) - - return LaunchDescription([ - ekf_node, - ]) - - -def main(argv=sys.argv[1:]): - ld = generate_launch_description() - - test1_action = ExecuteProcess( - cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_ekf_localization_node_interfaces'], - output='screen', - ) - - lts = LaunchTestService() - lts.add_test_action(ld, test1_action) - ls = LaunchService(argv=argv) - ls.include_launch_description(ld) - return lts.run(ls) - -if __name__ == '__main__': - sys.exit(main()) diff --git a/test/test_ekf_localization_node_interfaces.yaml b/test/test_ekf_localization_node_interfaces.yaml deleted file mode 100644 index 875caa423..000000000 --- a/test/test_ekf_localization_node_interfaces.yaml +++ /dev/null @@ -1,107 +0,0 @@ -# -test_ekf_localization_node_interfaces_ekf: - ros__parameters: - debug: false - debug_out_file: debug_ekf_localization.txt - - frequency: 30.0 - - sensor_timeout: 1000.0 - - odom0: /odom_input0 - odom1: /odom_input1 - odom2: /odom_input2 - - pose0: /pose_input0 - pose1: /pose_input1 - - twist0: /twist_input0 - - imu0: /imu_input0 - imu1: /imu_input1 - imu2: /imu_input2 - imu3: /imu_input3 - - odom0_config: [true, false, true, - false, false, false, - false, false, false, - false, false, false, - false, false, false] - odom0_relative: false - - odom1_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - odom1_relative: false - - odom2_config: [false, false, false, - false, false, false, - true, true, true, - true, true, true, - false, false, false] - odom2_relative: false - - pose0_config: [true, false, true, - false, false, false, - false, false, false, - false, false, false, - false, false, false] - pose1_config: [true, true, true, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - - twist0_config: [false, false, false, - false, false, false, - true, true, true, - true, true, true, - false, false, false] - - imu0_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - imu1_config: [false, false, false, - false, false, false, - false, false, false, - true, true, true, - false, false, false] - imu2_config: [false, false, false, - false, false, false, - false, false, false, - false, false, false, - true, true, true] - imu3_config: [false, false, false, - true, true, true, - false, false, false, - false, false, false, - false, false, false] - - odom1_differential: true - pose1_differential: true - imu3_differential: true - - print_diagnostics: false - - odom_frame: odom - base_link_frame: base_link - - process_noise_covariance: [0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.4, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.03, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.06, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.025, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.002, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.004, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] diff --git a/test/test_ukf_localization_node_interfaces.cpp b/test/test_se_node_interfaces.cpp similarity index 57% rename from test/test_ukf_localization_node_interfaces.cpp rename to test/test_se_node_interfaces.cpp index 57a6b471b..4e8ff3bbf 100644 --- a/test/test_ukf_localization_node_interfaces.cpp +++ b/test/test_se_node_interfaces.cpp @@ -30,6 +30,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include #include #include @@ -48,26 +49,34 @@ using namespace std::chrono_literals; nav_msgs::msg::Odometry filtered_; -bool stateUpdated_; +bool state_updated_; + +rclcpp::Node::SharedPtr node_; +rclcpp::Publisher::SharedPtr odom0_pub_; +rclcpp::Publisher::SharedPtr odom1_pub_; +rclcpp::Publisher::SharedPtr odom2_pub_; +rclcpp::Publisher::SharedPtr pose0_pub_; +rclcpp::Publisher::SharedPtr pose1_pub_; +rclcpp::Publisher::SharedPtr twist0_pub_; +rclcpp::Publisher::SharedPtr imu0_pub_; +rclcpp::Publisher::SharedPtr imu1_pub_; +rclcpp::Publisher::SharedPtr imu2_pub_; +rclcpp::Publisher::SharedPtr imu3_pub_; +rclcpp::Client::SharedPtr reset_client_; void filterCallback(const nav_msgs::msg::Odometry::SharedPtr msg) { filtered_ = *msg; - stateUpdated_ = true; + state_updated_ = true; } -void resetFilter(rclcpp::Node::SharedPtr node_) +void resetFilter() { - auto client = - node_->create_client("reset"); - auto reset_request = - std::make_shared(); - - if (!client->wait_for_service(10s)) { - ASSERT_TRUE(false) << "service not available after waiting"; - } + // Force any callbacks to fire in the UKF + rclcpp::spin_some(node_); - auto result = client->async_send_request(reset_request); + auto reset_request = std::make_shared(); + auto result = reset_client_->async_send_request(reset_request); rclcpp::spin_until_future_complete(node_, result, 5s); // Wait for the result // Reset the output message @@ -76,15 +85,7 @@ void resetFilter(rclcpp::Node::SharedPtr node_) } TEST(InterfacesTest, OdomPoseBasicIO) { - stateUpdated_ = false; - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomPoseBasicIO_testcase"); - - auto odomPub = node_->create_publisher( - "odom_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); + state_updated_ = false; nav_msgs::msg::Odometry odom; odom.pose.pose.position.x = 20.0; @@ -97,43 +98,27 @@ TEST(InterfacesTest, OdomPoseBasicIO) { odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; - rclcpp::Rate loopRate(50); + rclcpp::Rate loop_rate(50); for (size_t i = 0; i < 50; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom0_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } // Now check the values from the callback - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), - 0.01); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.y), - 0.01); // Configuration for this variable for this sensor is false - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), - 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - odom.pose.pose.position.x), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y), 0.01); // Y is not fused + EXPECT_LT(std::abs(filtered_.pose.pose.position.z - odom.pose.pose.position.z), 0.01); EXPECT_LT(filtered_.pose.covariance[0], 0.5); - EXPECT_LT( - filtered_.pose.covariance[7], - 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Y is not fused EXPECT_LT(filtered_.pose.covariance[14], 0.6); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, OdomTwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomTwistBasicIO_testcase"); - - auto odomPub = node_->create_publisher( - "odom_input2", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); nav_msgs::msg::Odometry odom; odom.twist.twist.linear.x = 5.0; odom.twist.twist.linear.y = 0.0; @@ -148,57 +133,48 @@ TEST(InterfacesTest, OdomTwistBasicIO) { odom.header.frame_id = "odom"; odom.child_frame_id = "base_link"; - rclcpp::Rate loopRate(20); + rclcpp::Rate loop_rate(20); for (size_t i = 0; i < 400; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - 100.0), 2.0); - resetFilter(node_); + resetFilter(); odom.twist.twist.linear.x = 0.0; odom.twist.twist.linear.y = 5.0; for (size_t i = 0; i < 400; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.y - odom.twist.twist.linear.y), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y - 100.0), 2.0); - resetFilter(node_); + resetFilter(); odom.twist.twist.linear.y = 0.0; odom.twist.twist.linear.z = 5.0; for (size_t i = 0; i < 400; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.z - odom.twist.twist.linear.z), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z - 100.0), 2.0); - resetFilter(node_); + resetFilter(); odom.twist.twist.linear.z = 0.0; odom.twist.twist.linear.x = 1.0; @@ -206,23 +182,16 @@ TEST(InterfacesTest, OdomTwistBasicIO) { for (size_t i = 0; i < 100; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), - 0.5); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.twist.twist.angular.z - odom.twist.twist.angular.z), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); - resetFilter(node_); + resetFilter(); odom.twist.twist.linear.x = 0.0; odom.twist.twist.angular.z = 0.0; @@ -231,11 +200,10 @@ TEST(InterfacesTest, OdomTwistBasicIO) { // First, roll the vehicle on its side for (size_t i = 0; i < 100; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); odom.twist.twist.angular.x = 0.0; odom.twist.twist.angular.y = (M_PI / 2) / (100.0 * 0.05); @@ -243,11 +211,10 @@ TEST(InterfacesTest, OdomTwistBasicIO) { // Now, pitch it down (positive pitch velocity in vehicle frame) for (size_t i = 0; i < 100; ++i) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); odom.twist.twist.angular.y = 0.0; odom.twist.twist.linear.x = 3.0; @@ -257,31 +224,18 @@ TEST(InterfacesTest, OdomTwistBasicIO) { // the world frame. for (size_t i = 0; i < 100; ++i) { odom.header.stamp = node_->now(); - - odomPub->publish(odom); + odom2_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - odom.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y + 15), 1.0); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, PoseBasicIO) { - auto node_ = rclcpp::Node::make_shared("InterfacesTest_PoseBasicIO_testcase"); - - auto posePub = - node_->create_publisher( - "pose_input0", rclcpp::SensorDataQoS()); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(1), filterCallback); - geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.pose.pose.position.x = 20.0; pose.pose.pose.position.y = 10.0; @@ -296,45 +250,26 @@ TEST(InterfacesTest, PoseBasicIO) { } pose.header.frame_id = "odom"; - rclcpp::Rate loopRate(50); - + rclcpp::Rate loop_rate(50); for (size_t i = 0; i < 50; ++i) { pose.header.stamp = node_->now(); - posePub->publish(pose); + pose0_pub_->publish(pose); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } // Now check the values from the callback - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.y), - 0.1); // Configuration for this variable for this sensor is false - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), - 0.1); - + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - pose.pose.pose.position.x), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y), 0.1); // Y is not fused + EXPECT_LT(std::abs(filtered_.pose.pose.position.z - pose.pose.pose.position.z), 0.1); EXPECT_LT(filtered_.pose.covariance[0], 0.5); - EXPECT_LT( - filtered_.pose.covariance[7], - 0.25); // Configuration for this variable for this sensor is false + EXPECT_LT(filtered_.pose.covariance[7], 0.25); // Y is not fused EXPECT_LT(filtered_.pose.covariance[14], 0.5); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, TwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_TwistBasicIO_testcase"); - auto twistPub = - node_->create_publisher( - "twist_input0", rclcpp::QoS(5)); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(5), filterCallback); - geometry_msgs::msg::TwistWithCovarianceStamped twist; twist.twist.twist.linear.x = 5.0; twist.twist.twist.linear.y = 0; @@ -348,80 +283,68 @@ TEST(InterfacesTest, TwistBasicIO) { } twist.header.frame_id = "base_link"; - rclcpp::Rate loopRate(20); + rclcpp::Rate loop_rate(20); for (size_t i = 0; i < 400; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - 100.0), 2.0); - resetFilter(node_); + resetFilter(); twist.twist.twist.linear.x = 0.0; twist.twist.twist.linear.y = 5.0; for (size_t i = 0; i < 400; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), + std::abs(filtered_.twist.twist.linear.y - twist.twist.twist.linear.y), 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y - 100.0), 2.0); - resetFilter(node_); + resetFilter(); twist.twist.twist.linear.y = 0.0; twist.twist.twist.linear.z = 5.0; for (size_t i = 0; i < 400; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), + std::abs(filtered_.twist.twist.linear.z - twist.twist.twist.linear.z), 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 100.0), 2.0); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z - 100.0), 2.0); - resetFilter(node_); + resetFilter(); twist.twist.twist.linear.z = 0.0; twist.twist.twist.linear.x = 1.0; twist.twist.twist.angular.z = (M_PI / 2) / (100.0 * 0.05); for (size_t i = 0; i < 100; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT( - ::fabs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), - 0.1); - EXPECT_LT( - ::fabs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), - 0.5); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.twist.twist.angular.z - twist.twist.twist.angular.z), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - filtered_.pose.pose.position.y), 0.5); - resetFilter(node_); + resetFilter(); twist.twist.twist.linear.x = 0.0; twist.twist.twist.angular.z = 0.0; @@ -430,12 +353,11 @@ TEST(InterfacesTest, TwistBasicIO) { // First, roll the vehicle on its side for (size_t i = 0; i < 100; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); rclcpp::spin_some(node_); - loopRate.sleep(); + loop_rate.sleep(); } - rclcpp::spin_some(node_); twist.twist.twist.angular.x = 0.0; twist.twist.twist.angular.y = (M_PI / 2) / (100.0 * 0.05); @@ -443,12 +365,11 @@ TEST(InterfacesTest, TwistBasicIO) { // Now, pitch it down (positive pitch velocity in vehicle frame) for (size_t i = 0; i < 100; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); rclcpp::spin_some(node_); - loopRate.sleep(); + loop_rate.sleep(); } - rclcpp::spin_some(node_); twist.twist.twist.angular.y = 0.0; twist.twist.twist.linear.x = 3.0; @@ -458,30 +379,18 @@ TEST(InterfacesTest, TwistBasicIO) { // the world frame. for (size_t i = 0; i < 100; ++i) { twist.header.stamp = node_->now(); - twistPub->publish(twist); + twist0_pub_->publish(twist); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT( - ::fabs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), - 0.1); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 15), 1.0); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - twist.twist.twist.linear.x), 0.1); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y + 15), 1.0); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, ImuPoseBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuPoseBasicIO_testcase"); - auto custom_qos_profile = rclcpp::SensorDataQoS(); - auto imuPub = node_->create_publisher( - "/imu_input0", custom_qos_profile); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", custom_qos_profile, filterCallback); - sensor_msgs::msg::Imu imu; tf2::Quaternion quat; quat.setRPY(M_PI / 4, -M_PI / 4, M_PI / 2); @@ -495,12 +404,12 @@ TEST(InterfacesTest, ImuPoseBasicIO) { // Make sure the pose reset worked. Test will timeout // if this fails. - rclcpp::Rate loopRate1(50); + rclcpp::Rate loop_rate1(50); for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu0_pub_->publish(imu); + loop_rate1.sleep(); rclcpp::spin_some(node_); - loopRate1.sleep(); } // Now check the values from the callback @@ -508,17 +417,17 @@ TEST(InterfacesTest, ImuPoseBasicIO) { tf2::Matrix3x3 mat(quat); double r, p, y; mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r - M_PI / 4), 0.1); - EXPECT_LT(::fabs(p + M_PI / 4), 0.1); - EXPECT_LT(::fabs(y - M_PI / 2), 0.1); + EXPECT_LT(std::abs(r - M_PI / 4), 0.1); + EXPECT_LT(std::abs(p + M_PI / 4), 0.1); + EXPECT_LT(std::abs(y - M_PI / 2), 0.1); EXPECT_LT(filtered_.pose.covariance[21], 0.5); EXPECT_LT(filtered_.pose.covariance[28], 0.25); EXPECT_LT(filtered_.pose.covariance[35], 0.5); - resetFilter(node_); + resetFilter(); - stateUpdated_ = false; + state_updated_ = false; // Test to see if the orientation data is ignored when we set the // first covariance value to -1 @@ -529,36 +438,27 @@ TEST(InterfacesTest, ImuPoseBasicIO) { imuIgnore.orientation.w = 0.4; imuIgnore.orientation_covariance[0] = -1; - rclcpp::Rate loopRate2(50); + rclcpp::Rate loop_rate2(50); for (size_t i = 0; i < 50; ++i) { imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); - loopRate2.sleep(); + imu0_pub_->publish(imuIgnore); + loop_rate2.sleep(); rclcpp::spin_some(node_); - EXPECT_FALSE(stateUpdated_); + EXPECT_FALSE(state_updated_); } tf2::fromMsg(filtered_.pose.pose.orientation, quat); mat.setRotation(quat); mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 1e-3); - EXPECT_LT(::fabs(p), 1e-3); - EXPECT_LT(::fabs(y), 1e-3); + EXPECT_LT(std::abs(r), 1e-3); + EXPECT_LT(std::abs(p), 1e-3); + EXPECT_LT(std::abs(y), 1e-3); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, ImuTwistBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuTwistBasicIO_testcase"); - auto custom_qos_profile = rclcpp::SensorDataQoS(); - auto imuPub = node_->create_publisher( - "/imu_input1", custom_qos_profile); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", custom_qos_profile, filterCallback); - sensor_msgs::msg::Imu imu; tf2::Quaternion quat; imu.angular_velocity.x = (M_PI / 2.0); @@ -568,11 +468,11 @@ TEST(InterfacesTest, ImuTwistBasicIO) { } imu.header.frame_id = "base_link"; - rclcpp::Rate loopRate(50); + rclcpp::Rate loop_rate(50); for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); - loopRate.sleep(); + imu1_pub_->publish(imu); + loop_rate.sleep(); rclcpp::spin_some(node_); } @@ -584,14 +484,14 @@ TEST(InterfacesTest, ImuTwistBasicIO) { // Tolerances may seem loose, but the initial state covariances // are tiny, so the filter is sluggish to accept velocity data - EXPECT_LT(::fabs(r - M_PI / 2.0), 0.7); - EXPECT_LT(::fabs(p), 0.1); - EXPECT_LT(::fabs(y), 0.1); + EXPECT_LT(std::abs(r - M_PI / 2.0), 0.7); + EXPECT_LT(std::abs(p), 0.1); + EXPECT_LT(std::abs(y), 0.1); EXPECT_LT(filtered_.twist.covariance[21], 1e-3); EXPECT_LT(filtered_.pose.covariance[21], 0.1); - resetFilter(node_); + resetFilter(); imu.angular_velocity.x = 0.0; imu.angular_velocity.y = -(M_PI / 3.0); @@ -600,9 +500,9 @@ TEST(InterfacesTest, ImuTwistBasicIO) { // if this fails. for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu1_pub_->publish(imu); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } rclcpp::spin_some(node_); @@ -614,33 +514,31 @@ TEST(InterfacesTest, ImuTwistBasicIO) { EXPECT_LT(filtered_.twist.covariance[28], 1e-3); EXPECT_LT(filtered_.pose.covariance[28], 0.1); - resetFilter(node_); + resetFilter(); imu.angular_velocity.y = 0; imu.angular_velocity.z = M_PI / 4.0; - // Make sure the pose reset worked. Test will timeout - // if this fails. + // Make sure the pose reset worked. Test will timeout if this fails. for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu1_pub_->publish(imu); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); // Now check the values from the callback tf2::fromMsg(filtered_.pose.pose.orientation, quat); mat.setRotation(quat); mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 0.1); - EXPECT_LT(::fabs(p), 0.1); - EXPECT_LT(::fabs(y - M_PI / 4.0), 0.7); + EXPECT_LT(std::abs(r), 0.1); + EXPECT_LT(std::abs(p), 0.1); + EXPECT_LT(std::abs(y - M_PI / 4.0), 0.7); EXPECT_LT(filtered_.twist.covariance[35], 1e-3); EXPECT_LT(filtered_.pose.covariance[35], 0.12); - resetFilter(node_); + resetFilter(); // Test to see if the angular velocity data is ignored when we set the // first covariance value to -1 @@ -652,32 +550,22 @@ TEST(InterfacesTest, ImuTwistBasicIO) { for (size_t i = 0; i < 50; ++i) { imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); + imu1_pub_->publish(imuIgnore); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); tf2::fromMsg(filtered_.pose.pose.orientation, quat); mat.setRotation(quat); mat.getRPY(r, p, y); - EXPECT_LT(::fabs(r), 1e-3); - EXPECT_LT(::fabs(p), 1e-3); - EXPECT_LT(::fabs(y), 1e-3); + EXPECT_LT(std::abs(r), 1e-3); + EXPECT_LT(std::abs(p), 1e-3); + EXPECT_LT(std::abs(y), 1e-3); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, ImuAccBasicIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuAccBasicIO_testcase"); - auto custom_qos_profile = rclcpp::SensorDataQoS(); - auto imuPub = node_->create_publisher( - "imu_input2", custom_qos_profile); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", custom_qos_profile, filterCallback); - sensor_msgs::msg::Imu imu; imu.header.frame_id = "base_link"; imu.linear_acceleration_covariance[0] = 1e-6; @@ -690,18 +578,17 @@ TEST(InterfacesTest, ImuAccBasicIO) { // Move with constant acceleration for 1 second, // then check our velocity. - rclcpp::Rate loopRate(50); + rclcpp::Rate loop_rate(50); for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu2_pub_->publish(imu); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.twist.twist.linear.x - 1.0), 0.40); - EXPECT_LT(::fabs(filtered_.twist.twist.linear.y + 1.0), 0.40); - EXPECT_LT(::fabs(filtered_.twist.twist.linear.z - 1.0), 0.40); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.x - 1.0), 0.40); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.y + 1.0), 0.40); + EXPECT_LT(std::abs(filtered_.twist.twist.linear.z - 1.0), 0.40); imu.linear_acceleration.x = 0.0; imu.linear_acceleration.y = 0.0; @@ -710,18 +597,18 @@ TEST(InterfacesTest, ImuAccBasicIO) { // Now move for another second, and see where we end up for (size_t i = 0; i < 50; ++i) { imu.header.stamp = node_->now(); - imuPub->publish(imu); - loopRate.sleep(); + imu2_pub_->publish(imu); + loop_rate.sleep(); rclcpp::spin_some(node_); } - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1.8), 0.6); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y + 1.8), 0.6); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z - 1.8), 0.6); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - 1.8), 0.6); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y + 1.8), 0.6); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z - 1.8), 0.6); - resetFilter(node_); + resetFilter(); - stateUpdated_ = false; + state_updated_ = false; // Test to see if the linear acceleration data is ignored when we set the // first covariance value to -1 @@ -733,30 +620,20 @@ TEST(InterfacesTest, ImuAccBasicIO) { for (size_t i = 0; i < 50; ++i) { imuIgnore.header.stamp = node_->now(); - imuPub->publish(imuIgnore); + imu2_pub_->publish(imuIgnore); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); - EXPECT_FALSE(stateUpdated_); + EXPECT_FALSE(state_updated_); } - rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 1e-3); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 1e-3); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 1e-3); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x), 1e-3); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y), 1e-3); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z), 1e-3); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, OdomDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_OdomDifferentialIO_testcase"); - auto custom_qos_profile = rclcpp::SensorDataQoS(); - auto odomPub = node_->create_publisher( - "/odom_input1", custom_qos_profile); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", custom_qos_profile, filterCallback); - nav_msgs::msg::Odometry odom; odom.pose.pose.position.x = 20.0; odom.pose.pose.position.y = 10.0; @@ -781,16 +658,16 @@ TEST(InterfacesTest, OdomDifferentialIO) { size_t zeroCount = 0; while (zeroCount++ < 10) { odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom1_pub_->publish(odom); rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.w - 1), 0.01); rclcpp::Rate(10).sleep(); } @@ -799,36 +676,26 @@ TEST(InterfacesTest, OdomDifferentialIO) { } // Slowly move the position, and hope that the differential position keeps up - rclcpp::Rate loopRate(20); + rclcpp::Rate loop_rate(20); for (size_t i = 0; i < 100; ++i) { odom.pose.pose.position.x += 0.01; odom.pose.pose.position.y += 0.02; odom.pose.pose.position.z -= 0.03; odom.header.stamp = node_->now(); - odomPub->publish(odom); + odom1_pub_->publish(odom); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z + 3), 0.6); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, PoseDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_PoseDifferentialIO_testcase"); - auto posePub = - node_->create_publisher( - "/pose_input1", rclcpp::QoS(5)); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", rclcpp::QoS(5), filterCallback); - geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.pose.pose.position.x = 20.0; pose.pose.pose.position.y = 10.0; @@ -845,68 +712,50 @@ TEST(InterfacesTest, PoseDifferentialIO) { pose.header.frame_id = "odom"; - // No guaranteeing that the zero state - // we're expecting to see here isn't just - // a result of zeroing it out previously, - // so check 10 times in succession. + // No guaranteeing that the zero state we're expecting to see here isn't just a result of zeroing + // it out previously, so check 10 times in succession. size_t zeroCount = 0; while (zeroCount++ < 10) { pose.header.stamp = node_->now(); - posePub->publish(pose); + pose1_pub_->publish(pose); rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.x), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.y), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.z), 0.01); - EXPECT_LT(::fabs(filtered_.pose.pose.orientation.w - 1), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.x), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.y), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.z), 0.01); + EXPECT_LT(std::abs(filtered_.pose.pose.orientation.w - 1), 0.01); rclcpp::Rate(10).sleep(); } - // ...but only if we give the measurement a tiny covariance for (size_t ind = 0; ind < 36; ind += 7) { pose.pose.covariance[ind] = 1e-6; } - // Issue this location repeatedly, and see if we get - // a final reported position of (1, 2, -3) - rclcpp::Rate loopRate(20); + // Issue this location repeatedly, and see if we get a final reported position of (1, 2, -3) + rclcpp::Rate loop_rate(20); for (size_t i = 0; i < 100; ++i) { pose.pose.pose.position.x += 0.01; pose.pose.pose.position.y += 0.02; pose.pose.pose.position.z -= 0.03; pose.header.stamp = node_->now(); - posePub->publish(pose); + pose1_pub_->publish(pose); + loop_rate.sleep(); rclcpp::spin_some(node_); - loopRate.sleep(); } - rclcpp::spin_some(node_); - EXPECT_LT(::fabs(filtered_.pose.pose.position.x - 1), 0.2); - EXPECT_LT(::fabs(filtered_.pose.pose.position.y - 2), 0.4); - EXPECT_LT(::fabs(filtered_.pose.pose.position.z + 3), 0.6); + EXPECT_LT(std::abs(filtered_.pose.pose.position.x - 1), 0.2); + EXPECT_LT(std::abs(filtered_.pose.pose.position.y - 2), 0.4); + EXPECT_LT(std::abs(filtered_.pose.pose.position.z + 3), 0.6); - resetFilter(node_); + resetFilter(); } TEST(InterfacesTest, ImuDifferentialIO) { - auto node_ = - rclcpp::Node::make_shared("InterfacesTest_ImuDifferentialIO_testcase"); - auto custom_qos_profile = rclcpp::SensorDataQoS(); - auto imu0Pub = node_->create_publisher( - "/imu_input0", custom_qos_profile); - auto imu1Pub = node_->create_publisher( - "/imu_input1", custom_qos_profile); - auto imuPub = node_->create_publisher( - "/imu_input3", custom_qos_profile); - - auto filteredSub = node_->create_subscription( - "/odometry/filtered", custom_qos_profile, filterCallback); - sensor_msgs::msg::Imu imu; imu.header.frame_id = "base_link"; tf2::Quaternion quat; @@ -927,8 +776,8 @@ TEST(InterfacesTest, ImuDifferentialIO) { size_t setCount = 0; while (setCount++ < 10) { imu.header.stamp = node_->now(); - imu0Pub->publish(imu); // Use this to move the absolute orientation - imu1Pub->publish(imu); // Use this to keep velocities at 0 + imu0_pub_->publish(imu); // Use this to move the absolute orientation + imu1_pub_->publish(imu); // Use this to keep velocities at 0 rclcpp::spin_some(node_); rclcpp::Rate(10).sleep(); } @@ -936,7 +785,7 @@ TEST(InterfacesTest, ImuDifferentialIO) { size_t zeroCount = 0; while (zeroCount++ < 10) { imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu3_pub_->publish(imu); rclcpp::spin_some(node_); rclcpp::Rate(10).sleep(); } @@ -946,7 +795,7 @@ TEST(InterfacesTest, ImuDifferentialIO) { double yawFinal = yaw; // Move the orientation slowly, and see if we can push it to 0 - rclcpp::Rate loopRate(20); + rclcpp::Rate loop_rate(20); for (size_t i = 0; i < 100; ++i) { yawFinal -= 0.01 * (3.0 * M_PI / 4.0); @@ -954,12 +803,11 @@ TEST(InterfacesTest, ImuDifferentialIO) { imu.orientation = tf2::toMsg(quat); imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu3_pub_->publish(imu); rclcpp::spin_some(node_); - loopRate.sleep(); + loop_rate.sleep(); } - rclcpp::spin_some(node_); // Move the orientation slowly, and see if we can push it to 0 for (size_t i = 0; i < 100; ++i) { @@ -969,22 +817,21 @@ TEST(InterfacesTest, ImuDifferentialIO) { imu.orientation = tf2::toMsg(quat); imu.header.stamp = node_->now(); - imuPub->publish(imu); + imu3_pub_->publish(imu); rclcpp::spin_some(node_); - loopRate.sleep(); + loop_rate.sleep(); } - rclcpp::spin_some(node_); tf2::fromMsg(filtered_.pose.pose.orientation, quat); tf2::Matrix3x3 mat(quat); mat.getRPY(rollFinal, pitchFinal, yawFinal); - EXPECT_LT(::fabs(rollFinal), 0.2); - EXPECT_LT(::fabs(pitchFinal), 0.2); - EXPECT_LT(::fabs(yawFinal), 0.2); + EXPECT_LT(std::abs(rollFinal), 0.2); + EXPECT_LT(std::abs(pitchFinal), 0.2); + EXPECT_LT(std::abs(yawFinal), 0.2); - resetFilter(node_); + resetFilter(); } int main(int argc, char ** argv) @@ -992,10 +839,88 @@ int main(int argc, char ** argv) rclcpp::init(argc, argv); ::testing::InitGoogleTest(&argc, argv); - // Give ekf_localization_node time to initialize - rclcpp::Rate(0.5).sleep(); + node_ = rclcpp::Node::make_shared("test_se_node_interfaces"); + + odom0_pub_ = node_->create_publisher( + "odom_input0", rclcpp::SensorDataQoS()); + odom1_pub_ = node_->create_publisher( + "odom_input1", rclcpp::SensorDataQoS()); + odom2_pub_ = node_->create_publisher( + "odom_input2", rclcpp::SensorDataQoS()); + + pose0_pub_ = node_->create_publisher( + "pose_input0", rclcpp::SensorDataQoS()); + pose1_pub_ = node_->create_publisher( + "pose_input1", rclcpp::SensorDataQoS()); + + twist0_pub_ = node_->create_publisher( + "twist_input0", rclcpp::SensorDataQoS()); + + imu0_pub_ = node_->create_publisher( + "imu_input0", rclcpp::SensorDataQoS()); + imu1_pub_ = node_->create_publisher( + "imu_input1", rclcpp::SensorDataQoS()); + imu2_pub_ = node_->create_publisher( + "imu_input2", rclcpp::SensorDataQoS()); + imu3_pub_ = node_->create_publisher( + "imu_input3", rclcpp::SensorDataQoS()); + + auto filtered_sub = node_->create_subscription( + "/odometry/filtered", rclcpp::QoS(1), filterCallback); + + reset_client_ = node_->create_client("reset"); + + if (!reset_client_->wait_for_service(10s)) { + RCLCPP_ERROR(node_->get_logger(), "Reset service not available after waiting"); + return 1; + } + + int attempts = 0; + while ( + odom0_pub_->get_subscription_count() == 0 || + odom1_pub_->get_subscription_count() == 0 || + odom2_pub_->get_subscription_count() == 0 || + pose0_pub_->get_subscription_count() == 0 || + pose1_pub_->get_subscription_count() == 0 || + twist0_pub_->get_subscription_count() == 0 || + imu0_pub_->get_subscription_count() == 0 || + imu1_pub_->get_subscription_count() == 0 || + imu2_pub_->get_subscription_count() == 0 || + imu3_pub_->get_subscription_count() == 0 || + filtered_sub->get_publisher_count() == 0) + { + RCLCPP_INFO_THROTTLE( + node_->get_logger(), + *(node_->get_clock()), + 1.0, + "Waiting for all publishers and subscriptions to start..."); + + rclcpp::Rate(1.0).sleep(); + + if (++attempts == 10) { + RCLCPP_ERROR( + node_->get_logger(), + "Could not establish necessary publishers and subscribers."); + return 1; + } + } int ret = RUN_ALL_TESTS(); + + odom0_pub_.reset(); + odom1_pub_.reset(); + odom2_pub_.reset(); + pose0_pub_.reset(); + pose1_pub_.reset(); + twist0_pub_.reset(); + imu0_pub_.reset(); + imu1_pub_.reset(); + imu2_pub_.reset(); + imu3_pub_.reset(); + filtered_sub.reset(); + reset_client_.reset(); + node_.reset(); + rclcpp::shutdown(); return ret; } diff --git a/test/test_ukf_localization_node_interfaces.launch.py b/test/test_se_node_interfaces.launch.py similarity index 77% rename from test/test_ukf_localization_node_interfaces.launch.py rename to test/test_se_node_interfaces.launch.py index 0989082f7..b58ddce8b 100755 --- a/test/test_ukf_localization_node_interfaces.launch.py +++ b/test/test_se_node_interfaces.launch.py @@ -13,7 +13,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Launch file for test_ukf_localization_node_interfaces.""" +"""Launch file for test_se_node_interfaces.""" import launch from launch import LaunchDescription @@ -30,33 +30,33 @@ from launch import LaunchService from ament_index_python.packages import get_package_prefix -def generate_launch_description(): +def generate_launch_description(node_type): parameters_file_dir = pathlib.Path(__file__).resolve().parent - parameters_file_path = parameters_file_dir / 'test_ukf_localization_node_interfaces.yaml' + parameters_file_path = parameters_file_dir / 'test_se_node_interfaces.yaml' os.environ['FILE_PATH'] = str(parameters_file_dir) - #*****test_ukf_localization_node_interfaces.test***** - ukf_node = launch_ros.actions.Node( + se_node = launch_ros.actions.Node( package='robot_localization', - executable='ukf_node', - name='test_ukf_localization_node_interfaces_ukf', + executable=node_type + '_node', + name='test_se_node_interfaces', output='screen', parameters=[ parameters_file_path, str(parameters_file_path), - [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_ukf_localization_node_interfaces.yaml'], + [EnvironmentVariable(name='FILE_PATH'), os.sep, 'test_se_node_interfaces.yaml'], ],) return LaunchDescription([ - ukf_node, + se_node, ]) def main(argv=sys.argv[1:]): - ld = generate_launch_description() + node_type = os.environ['NODE_TYPE'] + ld = generate_launch_description(node_type) test1_action = ExecuteProcess( - cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_ukf_localization_node_interfaces'], + cmd=[get_package_prefix('robot_localization') + '/lib/robot_localization/test_' + node_type + '_node_interfaces'], output='screen', ) diff --git a/test/test_ukf_localization_node_interfaces.yaml b/test/test_se_node_interfaces.yaml similarity index 97% rename from test/test_ukf_localization_node_interfaces.yaml rename to test/test_se_node_interfaces.yaml index dd7e2a3c0..2a5fb8e80 100644 --- a/test/test_ukf_localization_node_interfaces.yaml +++ b/test/test_se_node_interfaces.yaml @@ -1,5 +1,5 @@ -# -test_ukf_localization_node_interfaces_ukf: +# +test_se_node_interfaces: ros__parameters: frequency: 30.0 @@ -104,7 +104,7 @@ test_ukf_localization_node_interfaces_ukf: 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.01] - + # UKF-specific settings that the EKF will ignore alpha: 0.001 kappa: 0.0 beta: 2.0