Fix fuse optimizer unit test (locusrobotics#369)

* 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.

fuse_optimizers/test/launch_tests/test_fixed_lag_ignition.cpp

@@ -33,6 +33,10 @@
  */
 #include <gtest/gtest.h>
 
+#include <memory>
+#include <mutex>
+#include <thread>
+
 #include <fuse_msgs/srv/set_pose.hpp>
 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -65,8 +69,22 @@ class FixedLagIgnitionFixture : public ::testing::Test
     executor_.reset();
   }
 
+  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)
@@ -78,6 +96,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 +153,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 +172,25 @@ 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))
+  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::wait_for_message(odom_msg, node, "/odom", std::chrono::seconds(1));
+    rclcpp::sleep_for(std::chrono::milliseconds(100));
+    odom_msg = this->get_last_odom_msg();
   }
-  ASSERT_EQ(rclcpp::Time(odom_msg.header.stamp), rclcpp::Time(3, 0, RCL_ROS_TIME));
+  ASSERT_TRUE(static_cast<bool>(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, 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, 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