Fix fuse optimizer unit test. The rclcpp::wait_for_message call was t…

…hrowing an exception 'subscription already associated with a wait set'. Switched to a standard subscriber instead.

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<geometry_msgs::msg::PoseWithCovarianceStamped>(
     "/relative_pose", 5);
 
+  auto odom_subscriber =
+    node->create_subscription<nav_msgs::msg::Odometry>(
+    "/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