fix(ekf_localizer): skip update of z/roll/pitch when mahalanobis gate enabled

localization/ekf_localizer/CMakeLists.txt

@@ -41,6 +41,14 @@ function(add_testcase filepath)
 endfunction()
 
 if(BUILD_TESTING)
+  add_ros_test(
+    test/test_ekf_localizer_launch.py
+    TIMEOUT "30"
+  )
+  add_ros_test(
+    test/test_ekf_localizer_mahalanobis.py
+    TIMEOUT "30"
+  )
   find_package(ament_cmake_gtest REQUIRED)
 
   set(TEST_FILES

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp

@@ -254,7 +254,8 @@ class EKFLocalizer : public rclcpp::Node
   /**
    * @brief update simple1DFilter
    */
-  void updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose);
+  void updateSimple1DFilters(
+    const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step);
 
   /**
    * @brief initialize simple1DFilter

localization/ekf_localizer/package.xml

@@ -33,6 +33,7 @@
   <test_depend>ament_cmake_ros</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>autoware_lint_common</test_depend>
+  <test_depend>ros_testing</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

localization/ekf_localizer/src/ekf_localizer.cpp

@@ -345,8 +345,6 @@ void EKFLocalizer::callbackPoseWithCovariance(
   }
 
   pose_queue_.push(msg);
-
-  updateSimple1DFilters(*msg);
 }
 
 /*
@@ -448,6 +446,7 @@ void EKFLocalizer::measurementUpdatePose(const geometry_msgs::msg::PoseWithCovar
     poseMeasurementCovariance(pose.pose.covariance, params_.pose_smoothing_steps);
 
   ekf_.updateWithDelay(y, C, R, delay_step);
+  updateSimple1DFilters(pose, params_.pose_smoothing_steps);
 
   // debug
   const Eigen::MatrixXd X_result = ekf_.getLatestX();
@@ -600,16 +599,18 @@ void EKFLocalizer::publishEstimateResult()
   pub_debug_->publish(msg);
 }
 
-void EKFLocalizer::updateSimple1DFilters(const geometry_msgs::msg::PoseWithCovarianceStamped & pose)
+void EKFLocalizer::updateSimple1DFilters(
+  const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const size_t smoothing_step)
 {
   double z = pose.pose.pose.position.z;
 
   const auto rpy = tier4_autoware_utils::getRPY(pose.pose.pose.orientation);
 
   using COV_IDX = tier4_autoware_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
-  double z_dev = pose.pose.covariance[COV_IDX::Z_Z];
-  double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL];
-  double pitch_dev = pose.pose.covariance[COV_IDX::PITCH_PITCH];
+  double z_dev = pose.pose.covariance[COV_IDX::Z_Z] * static_cast<double>(smoothing_step);
+  double roll_dev = pose.pose.covariance[COV_IDX::ROLL_ROLL] * static_cast<double>(smoothing_step);
+  double pitch_dev =
+    pose.pose.covariance[COV_IDX::PITCH_PITCH] * static_cast<double>(smoothing_step);
 
   z_filter_.update(z, z_dev, pose.header.stamp);
   roll_filter_.update(rpy.x, roll_dev, pose.header.stamp);

localization/ekf_localizer/test/test_ekf_localizer_launch.py

@@ -0,0 +1,170 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import time
+import unittest
+
+from ament_index_python import get_package_share_directory
+from geometry_msgs.msg import PoseWithCovarianceStamped
+import launch
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import AnyLaunchDescriptionSource
+from launch.logging import get_logger
+import launch_testing
+from nav_msgs.msg import Odometry
+import pytest
+import rclpy
+from std_srvs.srv import SetBool
+
+logger = get_logger(__name__)
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    test_ekf_localizer_launch_file = os.path.join(
+        get_package_share_directory("ekf_localizer"),
+        "launch",
+        "ekf_localizer.launch.xml",
+    )
+    ekf_localizer = IncludeLaunchDescription(
+        AnyLaunchDescriptionSource(test_ekf_localizer_launch_file),
+    )
+
+    return launch.LaunchDescription(
+        [
+            ekf_localizer,
+            # Start tests right away - no need to wait for anything
+            launch_testing.actions.ReadyToTest(),
+        ]
+    )
+
+
+class TestEKFLocalizer(unittest.TestCase):
+    @classmethod
+    def setUpClass(cls):
+        # Initialize the ROS context for the test node
+        rclpy.init()
+
+    @classmethod
+    def tearDownClass(cls):
+        # Shutdown the ROS context
+        rclpy.shutdown()
+
+    def setUp(self):
+        # Create a ROS node for tests
+        self.test_node = rclpy.create_node("test_node")
+        self.evaluation_time = 0.2  # 200ms
+
+    def tearDown(self):
+        self.test_node.destroy_node()
+
+    @staticmethod
+    def print_message(stat):
+        logger.debug("===========================")
+        logger.debug(stat)
+
+    def test_node_link(self):
+        # Trigger ekf_localizer to activate the node
+        cli_trigger = self.test_node.create_client(SetBool, "/trigger_node")
+        while not cli_trigger.wait_for_service(timeout_sec=1.0):
+            continue
+
+        request = SetBool.Request()
+        request.data = True
+        future = cli_trigger.call_async(request)
+        rclpy.spin_until_future_complete(self.test_node, future)
+
+        if future.result() is not None:
+            self.test_node.get_logger().info("Result of bool service: %s" % future.result().message)
+        else:
+            self.test_node.get_logger().error(
+                "Exception while calling service: %r" % future.exception()
+            )
+
+        # Send initial pose
+        pub_init_pose = self.test_node.create_publisher(
+            PoseWithCovarianceStamped, "/initialpose3d", 10
+        )
+        init_pose = PoseWithCovarianceStamped()
+        init_pose.header.frame_id = "map"
+        init_pose.pose.pose.position.x = 0.0
+        init_pose.pose.pose.position.y = 0.0
+        init_pose.pose.pose.position.z = 0.0
+        init_pose.pose.pose.orientation.x = 0.0
+        init_pose.pose.pose.orientation.y = 0.0
+        init_pose.pose.pose.orientation.z = 0.0
+        init_pose.pose.pose.orientation.w = 1.0
+        init_pose.pose.covariance = [
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.0,
+            0.01,
+        ]
+        pub_init_pose.publish(init_pose)
+
+        # Receive Odometry
+        msg_buffer = []
+        self.test_node.create_subscription(
+            Odometry, "/ekf_odom", lambda msg: msg_buffer.append(msg), 10
+        )
+
+        # Wait until the node publishes some topic
+        end_time = time.time() + self.evaluation_time
+        while time.time() < end_time:
+            rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Check if the EKF outputs some Odometry
+        self.assertTrue(len(msg_buffer) > 0)
+
+
+@launch_testing.post_shutdown_test()
+class TestProcessOutput(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that process exits with code 0: no error
+        launch_testing.asserts.assertExitCodes(proc_info)