Add fixed lag smoother test

[efernandez]

Add fixed lag smoother test to test set_pose with orientation equal to pi, which is failing to be optimized with this error:

E20230628 15:01:20.425174 436794 trust_region_minimizer.cc:97] Terminating: Number of consecutive invalid steps more than Solver::Options::max_num_consecutive_invalid_steps: 5

Full test output error:

build/fuse_optimizers$ make run_tests_fuse_optimizers_rostest_test_fixed_lag_smoother.test
...
[ INFO] [/fixed_lag] [1687986631.450961780]: Received a set_pose request (stamp: 1.000000000, x: 100.1, y: 100.2, yaw: 3.14159)
WARNING: Logging before InitGoogleLogging() is written to STDERR
E20230628 17:10:31.922817 460509 trust_region_minimizer.cc:97] Terminating: Number of consecutive invalid steps more than Solver::Options::max_num_consecutive_invalid_steps: 5
[FATAL] [/fixed_lag] [1687986631.923664848]: Optimization failed after updating the graph with the transaction with timestamp 1.000000000. Leaving optimization loop and requesting node shutdown...
Graph:
HashGraph
  constraints:
   - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: d9b80224-7a7f-400b-8861-df56ff803f44
  variable: 68206cae-31ec-59a8-be15-f76649ad0557
  mean: 0
  sqrt_info: 10

   - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 45925eab-c9c4-4f62-90ff-e8f159df0382
  variable: bc1b435d-b576-5103-9214-1db9b4646b2f
  mean: 0 0
  sqrt_info: 10  0
 0 10

   - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: c02eeb3e-0cc4-4f59-90c5-d34f443d2712
  variable: b09b83b7-cb28-5243-8c6f-242bc2586105
  mean: 0 0
  sqrt_info: 10  0
 0 10

   - fuse_constraints::AbsoluteOrientation2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 4153b9c5-a5f3-4165-8b32-e05aee60a249
  variable: e6881748-8a90-5837-b2c5-2d36a74417f9
  mean: 3.14159
  sqrt_info: 1

   - fuse_constraints::AbsolutePosition2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 169b2316-85c1-439d-8e14-1aa6a0329115
  variable: 574f418d-ebfb-595e-9432-1b463dc9d659
  mean: 100.1 100.2
  sqrt_info: 1 0
0 1

  variables:
   - fuse_variables::AccelerationLinear2DStamped:
  uuid: bc1b435d-b576-5103-9214-1db9b4646b2f
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 0
  - y: 0

     on_hold: false
   - fuse_variables::VelocityAngular2DStamped:
  uuid: 68206cae-31ec-59a8-be15-f76649ad0557
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 1
  data:
  - yaw: 0

     on_hold: false
   - fuse_variables::VelocityLinear2DStamped:
  uuid: b09b83b7-cb28-5243-8c6f-242bc2586105
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 0
  - y: 0

     on_hold: false
   - fuse_variables::Orientation2DStamped:
  uuid: e6881748-8a90-5837-b2c5-2d36a74417f9
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 1
  data:
  - yaw: 3.14159

     on_hold: false
   - fuse_variables::Position2DStamped:
  uuid: 574f418d-ebfb-595e-9432-1b463dc9d659
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 100.1
  - y: 100.2

     on_hold: false

Transaction:
Stamp: 1.000000000
Involved Timestamps:
 - 1.000000000
Added Variables:
 - fuse_variables::Position2DStamped:
  uuid: 574f418d-ebfb-595e-9432-1b463dc9d659
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 100.1
  - y: 100.2

 - fuse_variables::Orientation2DStamped:
  uuid: e6881748-8a90-5837-b2c5-2d36a74417f9
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 1
  data:
  - yaw: 3.14159

 - fuse_variables::VelocityLinear2DStamped:
  uuid: b09b83b7-cb28-5243-8c6f-242bc2586105
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 0
  - y: 0

 - fuse_variables::VelocityAngular2DStamped:
  uuid: 68206cae-31ec-59a8-be15-f76649ad0557
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 1
  data:
  - yaw: 0

 - fuse_variables::AccelerationLinear2DStamped:
  uuid: bc1b435d-b576-5103-9214-1db9b4646b2f
  stamp: 1.000000000
  device_id: 00000000-0000-0000-0000-000000000000
  size: 2
  data:
  - x: 0
  - y: 0

Added Constraints:
 - fuse_constraints::AbsolutePosition2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 169b2316-85c1-439d-8e14-1aa6a0329115
  variable: 574f418d-ebfb-595e-9432-1b463dc9d659
  mean: 100.1 100.2
  sqrt_info: 1 0
0 1

 - fuse_constraints::AbsoluteOrientation2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 4153b9c5-a5f3-4165-8b32-e05aee60a249
  variable: e6881748-8a90-5837-b2c5-2d36a74417f9
  mean: 3.14159
  sqrt_info: 1

 - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: c02eeb3e-0cc4-4f59-90c5-d34f443d2712
  variable: b09b83b7-cb28-5243-8c6f-242bc2586105
  mean: 0 0
  sqrt_info: 10  0
 0 10

 - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: d9b80224-7a7f-400b-8861-df56ff803f44
  variable: 68206cae-31ec-59a8-be15-f76649ad0557
  mean: 0
  sqrt_info: 10

 - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint
  source: unicycle_ignition_sensor
  uuid: 45925eab-c9c4-4f62-90ff-e8f159df0382
  variable: bc1b435d-b576-5103-9214-1db9b4646b2f
  mean: 0 0
  sqrt_info: 10  0
 0 10

Removed Variables:
Removed Constraints:

[ INFO] [/fixed_lag] [1687986631.924233109]: 
Solver Summary (v 2.0.0-eigen-(3.4.0)-lapack-suitesparse-(5.13.0)-cxsparse-(3.2.0)-eigensparse-no_openmp)

                                     Original                  Reduced
Parameter blocks                            5                        5
Parameters                                  8                        8
Residual blocks                             5                        5
Residuals                                   8                        8

Minimizer                        TRUST_REGION

Sparse linear algebra library    SUITE_SPARSE
Trust region strategy     LEVENBERG_MARQUARDT

                                        Given                     Used
Linear solver          SPARSE_NORMAL_CHOLESKY   SPARSE_NORMAL_CHOLESKY
Threads                                     1                        1
Linear solver ordering              AUTOMATIC                        5

Cost:
Initial                          0.000000e+00

Minimizer iterations                        5
Successful steps                            1
Unsuccessful steps                          4

Time (in seconds):
Preprocessor                         0.000283

  Residual only evaluation           0.000000 (0)
  Jacobian & residual evaluation     0.000015 (1)
  Linear solver                      0.000108 (5)
Minimizer                            0.000331

Postprocessor                        0.000011
Total                                0.000625

Termination:                          FAILURE (Number of consecutive invalid steps more than Solver::Options::max_num_consecutive_invalid_steps: 5)

[Testcase: testFixedLagSmoother] ... ok

[ROSTEST]-----------------------------------------------------------------------

[fuse_optimizers.rosunit-FixedLagSmoother/SetInitialStateToPiOrientation][FAILURE]
/home/efernandez/NIX/devel/colcon_workspace/src/fuse/fuse_optimizers/test/test_fixed_lag_smoother.cpp:80
Value of: static_cast<bool>(odom_msg)
  Actual: false
Expected: true
--------------------------------------------------------------------------------

A fix for this would be required before merging this!