Add fixed lag smoother test

fuse_optimizers/CMakeLists.txt

@@ -212,4 +212,34 @@ if(CATKIN_ENABLE_TESTING)
       CXX_STANDARD 17
       CXX_STANDARD_REQUIRED YES
   )
+
+  # Fixed-lag Smoother test
+  add_rostest_gtest(test_fixed_lag_smoother
+    test/fixed_lag_smoother.test
+    test/test_fixed_lag_smoother.cpp
+  )
+  add_dependencies(test_fixed_lag_smoother
+    fixed_lag_smoother_node
+  )
+  target_include_directories(test_fixed_lag_smoother
+    PRIVATE
+      include
+      ${catkin_INCLUDE_DIRS}
+      ${fuse_models_INCLUDE_DIRS}
+      ${geometry_msgs_INCLUDE_DIRS}
+      ${nav_msgs_INCLUDE_DIRS}
+      ${rostest_INCLUDE_DIRS}
+  )
+  target_link_libraries(test_fixed_lag_smoother
+    ${catkin_LIBRARIES}
+    ${fuse_models_LIBRARIES}
+    ${geometry_msgs_LIBRARIES}
+    ${nav_msgs_LIBRARIES}
+    ${rostest_LIBRARIES}
+  )
+  set_target_properties(test_fixed_lag_smoother
+    PROPERTIES
+      CXX_STANDARD 14
+      CXX_STANDARD_REQUIRED YES
+  )
 endif()

fuse_optimizers/test/fixed_lag_smoother.test

@@ -0,0 +1,49 @@
+<?xml version="1.0"?>
+<launch>
+  <node name="fixed_lag" pkg="fuse_optimizers" type="fixed_lag_smoother_node" output="screen">
+    <rosparam subst_value="true">
+      optimization_frequency: 2.0
+      transaction_timeout: 5.0
+      lag_duration: 5.0
+
+      motion_models:
+        unicycle_motion_model:
+          type: fuse_models::Unicycle2D
+
+      sensor_models:
+        unicycle_ignition_sensor:
+          type: fuse_models::Unicycle2DIgnition
+          motion_models: [unicycle_motion_model]
+          ignition: true
+        pose_sensor:
+          type: fuse_models::Pose2D
+          motion_models: [unicycle_motion_model]
+
+      publishers:
+        odometry_publisher:
+          type: fuse_models::Odometry2DPublisher
+
+      unicycle_motion_model:
+        buffer_length: 5.0
+        process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+
+      unicycle_ignition_sensor:
+        publish_on_startup: false
+        initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+        initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1]
+
+      pose_sensor:
+        differential: true
+        topic: relative_pose
+        position_dimensions: ['x', 'y']
+        orientation_dimensions: ['yaw']
+
+      odometry_publisher:
+        topic: odom
+        world_frame_id: map
+        publish_tf: false
+    </rosparam>
+  </node>
+
+  <test test-name="FixedLagSmoother" pkg="fuse_optimizers" type="test_fixed_lag_smoother" />
+</launch>

fuse_optimizers/test/test_fixed_lag_smoother.cpp

@@ -0,0 +1,101 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, Clearpath Robotics
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <fuse_models/SetPose.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <nav_msgs/Odometry.h>
+#include <ros/ros.h>
+
+#include <gtest/gtest.h>
+
+
+TEST(FixedLagIgnition, SetInitialStateToPiOrientation)
+{
+  // Time should be valid after ros::init() returns in main(). But it doesn't hurt to verify.
+  ASSERT_TRUE(ros::Time::waitForValid(ros::WallDuration(1.0)));
+
+  auto node_handle = ros::NodeHandle();
+  auto relative_pose_publisher = node_handle.advertise<geometry_msgs::PoseWithCovarianceStamped>("/relative_pose", 1);
+
+  // Wait for the optimizer to be ready
+  ASSERT_TRUE(ros::service::waitForService("/fixed_lag/set_pose", ros::Duration(1.0)));
+  ASSERT_TRUE(ros::service::waitForService("/fixed_lag/reset", ros::Duration(1.0)));
+
+  // Set the initial pose to something far away from zero
+  fuse_models::SetPose::Request req;
+  req.pose.header.frame_id = "map";
+  req.pose.header.stamp = ros::Time(1, 0);
+  req.pose.pose.pose.position.x = 100.1;
+  req.pose.pose.pose.position.y = 100.2;
+  req.pose.pose.pose.position.z = 0.0;
+  req.pose.pose.pose.orientation.x = 0.0;
+  req.pose.pose.pose.orientation.y = 0.0;
+  req.pose.pose.pose.orientation.z = 1.0; // yaw = pi rad
+  req.pose.pose.pose.orientation.w = 0.0;
+  req.pose.pose.covariance[0] = 1.0;
+  req.pose.pose.covariance[7] = 1.0;
+  req.pose.pose.covariance[35] = 1.0;
+  fuse_models::SetPose::Response res;
+  ros::service::call("/fixed_lag/set_pose", req, res);
+  ASSERT_TRUE(res.success);
+
+  // Wait for the optimizer to process all queued transactions
+  ros::Time result_timeout = ros::Time::now() + ros::Duration(3.0);
+  auto odom_msg = nav_msgs::Odometry::ConstPtr();
+  while ((!odom_msg || odom_msg->header.stamp != ros::Time(3, 0)) &&
+         (ros::Time::now() < result_timeout))
+  {
+    odom_msg = ros::topic::waitForMessage<nav_msgs::Odometry>("/odom", ros::Duration(1.0));
+  }
+  ASSERT_TRUE(static_cast<bool>(odom_msg));
+  ASSERT_EQ(odom_msg->header.stamp, ros::Time(1, 0));
+
+  // 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.1);
+  EXPECT_NEAR(100.2, odom_msg->pose.pose.position.y, 0.1);
+  EXPECT_NEAR(1.0, std::abs(odom_msg->pose.pose.orientation.z), 0.1);  // pi == -pi
+  EXPECT_NEAR(0.0, odom_msg->pose.pose.orientation.w, 0.1);
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  ros::init(argc, argv, "fixed_lag_smoother_test");
+  auto spinner = ros::AsyncSpinner(1);
+  spinner.start();
+  int ret = RUN_ALL_TESTS();
+  spinner.stop();
+  ros::shutdown();
+  return ret;
+}