From 1913f629efc998ddc42a081c1dc6b7e82d41ae77 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Sun, 28 Apr 2024 17:00:15 -0700 Subject: [PATCH 1/2] Fix fuse optimizer unit test. The rclcpp::wait_for_message call was throwing an exception 'subscription already associated with a wait set'. Switched to a standard subscriber instead. --- .../launch_tests/test_fixed_lag_ignition.cpp | 33 +++++++++++++------ 1 file changed, 23 insertions(+), 10 deletions(-) diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp index 2895bc8d..b974ec6e 100644 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp @@ -65,8 +65,14 @@ class FixedLagIgnitionFixture : public ::testing::Test executor_.reset(); } + void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) + { + received_odom_msg_ = msg; + } + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; + nav_msgs::msg::Odometry::SharedPtr received_odom_msg_; }; TEST_F(FixedLagIgnitionFixture, SetInitialState) @@ -78,6 +84,10 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) node->create_publisher( "/relative_pose", 5); + auto odom_subscriber = + node->create_subscription( + "/odom", 5, std::bind(&FixedLagIgnitionFixture::odom_callback, this, std::placeholders::_1)); + // Time should be valid after rclcpp::init() returns in main(). But it doesn't hurt to verify. ASSERT_TRUE(node->get_clock()->wait_until_started(rclcpp::Duration::from_seconds(1.0))); @@ -131,6 +141,10 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) pose_msg1.pose.covariance[35] = 1.0; relative_pose_publisher->publish(pose_msg1); + /// @todo(swilliams) Understand why the subscriber does not receive all the messages + /// unless I force a delay between publishing. + rclcpp::sleep_for(std::chrono::milliseconds(100)); + auto pose_msg2 = geometry_msgs::msg::PoseWithCovarianceStamped(); pose_msg2.header.stamp = rclcpp::Time(3, 0, RCL_ROS_TIME); pose_msg2.header.frame_id = "base_link"; @@ -146,23 +160,22 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) pose_msg2.pose.covariance[35] = 1.0; relative_pose_publisher->publish(pose_msg2); - // Wait for the optimizer to process all queued transactions + // Wait for the optimizer to process all queued transactions and publish the last odometry msg rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(1.0); - auto odom_msg = nav_msgs::msg::Odometry(); - while ((odom_msg.header.stamp != rclcpp::Time(3, 0, RCL_ROS_TIME)) && - (node->now() < result_timeout)) + while ((!received_odom_msg_ || received_odom_msg_->header.stamp != rclcpp::Time(3, 0, + RCL_ROS_TIME)) && (node->now() < result_timeout)) { - rclcpp::wait_for_message(odom_msg, node, "/odom", std::chrono::seconds(1)); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } - ASSERT_EQ(rclcpp::Time(odom_msg.header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME)); + ASSERT_EQ(rclcpp::Time(received_odom_msg_->header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME)); // The optimizer is configured for 0 iterations, so it should return the initial variable values // If we did our job correctly, the initial variable values should be the same as the service call // state, give or take the motion model forward prediction. - EXPECT_NEAR(100.1, odom_msg.pose.pose.position.x, 0.10); - EXPECT_NEAR(100.2, odom_msg.pose.pose.position.y, 0.10); - EXPECT_NEAR(0.8660, odom_msg.pose.pose.orientation.z, 0.10); - EXPECT_NEAR(0.5000, odom_msg.pose.pose.orientation.w, 0.10); + EXPECT_NEAR(100.1, received_odom_msg_->pose.pose.position.x, 0.10); + EXPECT_NEAR(100.2, received_odom_msg_->pose.pose.position.y, 0.10); + EXPECT_NEAR(0.8660, received_odom_msg_->pose.pose.orientation.z, 0.10); + EXPECT_NEAR(0.5000, received_odom_msg_->pose.pose.orientation.w, 0.10); } // NOTE(CH3): This main is required because the test is manually run by a launch test From bd8e60cc74da018cb14411bc0d9e81c18e4bb6b8 Mon Sep 17 00:00:00 2001 From: Stephen Williams Date: Mon, 29 Apr 2024 10:18:54 -0700 Subject: [PATCH 2/2] Fix threading problem in unit test --- .../launch_tests/test_fixed_lag_ignition.cpp | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) diff --git a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp index b974ec6e..eafd79d6 100644 --- a/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp +++ b/fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp @@ -33,6 +33,10 @@ */ #include +#include +#include +#include + #include #include #include @@ -67,12 +71,20 @@ class FixedLagIgnitionFixture : public ::testing::Test void odom_callback(const nav_msgs::msg::Odometry::SharedPtr msg) { + std::lock_guard lock(received_odom_mutex_); received_odom_msg_ = msg; } + nav_msgs::msg::Odometry::SharedPtr get_last_odom_msg() + { + std::lock_guard lock(received_odom_mutex_); + return received_odom_msg_; + } + std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; nav_msgs::msg::Odometry::SharedPtr received_odom_msg_; + std::mutex received_odom_mutex_; }; TEST_F(FixedLagIgnitionFixture, SetInitialState) @@ -162,20 +174,23 @@ TEST_F(FixedLagIgnitionFixture, SetInitialState) // Wait for the optimizer to process all queued transactions and publish the last odometry msg rclcpp::Time result_timeout = node->now() + rclcpp::Duration::from_seconds(1.0); - while ((!received_odom_msg_ || received_odom_msg_->header.stamp != rclcpp::Time(3, 0, + auto odom_msg = nav_msgs::msg::Odometry::SharedPtr(); + while ((!odom_msg || odom_msg->header.stamp != rclcpp::Time(3, 0, RCL_ROS_TIME)) && (node->now() < result_timeout)) { rclcpp::sleep_for(std::chrono::milliseconds(100)); + odom_msg = this->get_last_odom_msg(); } - ASSERT_EQ(rclcpp::Time(received_odom_msg_->header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME)); + ASSERT_TRUE(static_cast(odom_msg)); + ASSERT_EQ(rclcpp::Time(odom_msg->header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME)); // The optimizer is configured for 0 iterations, so it should return the initial variable values // If we did our job correctly, the initial variable values should be the same as the service call // state, give or take the motion model forward prediction. - EXPECT_NEAR(100.1, received_odom_msg_->pose.pose.position.x, 0.10); - EXPECT_NEAR(100.2, received_odom_msg_->pose.pose.position.y, 0.10); - EXPECT_NEAR(0.8660, received_odom_msg_->pose.pose.orientation.z, 0.10); - EXPECT_NEAR(0.5000, received_odom_msg_->pose.pose.orientation.w, 0.10); + EXPECT_NEAR(100.1, odom_msg->pose.pose.position.x, 0.10); + EXPECT_NEAR(100.2, odom_msg->pose.pose.position.y, 0.10); + EXPECT_NEAR(0.8660, odom_msg->pose.pose.orientation.z, 0.10); + EXPECT_NEAR(0.5000, odom_msg->pose.pose.orientation.w, 0.10); } // NOTE(CH3): This main is required because the test is manually run by a launch test