Initial import of FaRui Campus ADS v3.2

master/src/Localization/autoware_ekf_localizer/test/test_aged_object_queue.cpp

@@ -0,0 +1,106 @@
+// Copyright 2023 The Autoware Contributors
+//
+// 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.
+
+#include "autoware/ekf_localizer/aged_object_queue.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(AgedObjectQueue, DiscardsObjectWhenAgeReachesMaximum)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 1
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 2
+  EXPECT_EQ(queue.size(), 1U);
+
+  queue.pop_increment_age();  // age = 3
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, MultipleObjects)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  EXPECT_EQ(queue.size(), 1U);
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 1
+  EXPECT_EQ(queue.size(), 1U);
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 2
+
+  queue.push("b");
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"a"});  // age of a = 3
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 1
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 2
+  EXPECT_EQ(queue.size(), 1U);
+
+  EXPECT_EQ(queue.pop_increment_age(), std::string{"b"});  // age of b = 3
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, Empty)
+{
+  AgedObjectQueue<std::string> queue(2);
+
+  EXPECT_TRUE(queue.empty());
+
+  queue.push("a");
+
+  EXPECT_FALSE(queue.empty());
+
+  queue.pop_increment_age();
+  queue.pop_increment_age();
+
+  EXPECT_TRUE(queue.empty());
+}
+
+TEST(AgedObjectQueue, Clear)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+  queue.push("b");
+
+  EXPECT_EQ(queue.size(), 2U);
+
+  queue.clear();
+
+  EXPECT_EQ(queue.size(), 0U);
+}
+
+TEST(AgedObjectQueue, Back)
+{
+  AgedObjectQueue<std::string> queue(3);
+
+  queue.push("a");
+
+  EXPECT_EQ(queue.back(), std::string{"a"});
+  queue.push("b");
+
+  EXPECT_EQ(queue.back(), std::string{"b"});
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_covariance.cpp

@@ -0,0 +1,84 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/covariance.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(EKFCovarianceToPoseMessageCovariance, SmokeTest)
+{
+  {
+    Matrix6d p = Matrix6d::Zero();
+    p(0, 0) = 1.;
+    p(0, 1) = 2.;
+    p(0, 2) = 3.;
+    p(1, 0) = 4.;
+    p(1, 1) = 5.;
+    p(1, 2) = 6.;
+    p(2, 0) = 7.;
+    p(2, 1) = 8.;
+    p(2, 2) = 9.;
+
+    std::array<double, 36> covariance = ekf_covariance_to_pose_message_covariance(p);
+    EXPECT_EQ(covariance[0], 1.);
+    EXPECT_EQ(covariance[1], 2.);
+    EXPECT_EQ(covariance[5], 3.);
+    EXPECT_EQ(covariance[6], 4.);
+    EXPECT_EQ(covariance[7], 5.);
+    EXPECT_EQ(covariance[11], 6.);
+    EXPECT_EQ(covariance[30], 7.);
+    EXPECT_EQ(covariance[31], 8.);
+    EXPECT_EQ(covariance[35], 9.);
+  }
+
+  // ensure other elements are zero
+  {
+    Matrix6d p = Matrix6d::Zero();
+    std::array<double, 36> covariance = ekf_covariance_to_pose_message_covariance(p);
+    for (double e : covariance) {
+      EXPECT_EQ(e, 0.);
+    }
+  }
+}
+
+TEST(EKFCovarianceToTwistMessageCovariance, SmokeTest)
+{
+  {
+    Matrix6d p = Matrix6d::Zero();
+    p(4, 4) = 1.;
+    p(4, 5) = 2.;
+    p(5, 4) = 3.;
+    p(5, 5) = 4.;
+
+    std::array<double, 36> covariance = ekf_covariance_to_twist_message_covariance(p);
+    EXPECT_EQ(covariance[0], 1.);
+    EXPECT_EQ(covariance[5], 2.);
+    EXPECT_EQ(covariance[30], 3.);
+    EXPECT_EQ(covariance[35], 4.);
+  }
+
+  // ensure other elements are zero
+  {
+    Matrix6d p = Matrix6d::Zero();
+    std::array<double, 36> covariance = ekf_covariance_to_twist_message_covariance(p);
+    for (double e : covariance) {
+      EXPECT_EQ(e, 0.);
+    }
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_diagnostics.cpp

@@ -0,0 +1,197 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/diagnostics.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(TestEkfDiagnostics, check_process_activated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  bool is_activated = true;
+  stat = check_process_activated(is_activated);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_activated = false;
+  stat = check_process_activated(is_activated);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_updated)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+  const size_t no_update_count_threshold_warn = 50;
+  const size_t no_update_count_threshold_error = 250;
+
+  size_t no_update_count = 0;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 1;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 49;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  no_update_count = 50;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  no_update_count = 249;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+
+  no_update_count = 250;
+  stat = check_measurement_updated(
+    measurement_type, no_update_count, no_update_count_threshold_warn,
+    no_update_count_threshold_error);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_queue_size)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+
+  size_t queue_size = 0;  // not effect for stat.level
+  stat = check_measurement_queue_size(measurement_type, queue_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  queue_size = 1;  // not effect for stat.level
+  stat = check_measurement_queue_size(measurement_type, queue_size);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_delay_gate)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";  // not effect for stat.level
+  const double delay_time = 0.1;                // not effect for stat.level
+  const double delay_time_threshold = 1.0;      // not effect for stat.level
+
+  bool is_passed_delay_gate = true;
+  stat = check_measurement_delay_gate(
+    measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_passed_delay_gate = false;
+  stat = check_measurement_delay_gate(
+    measurement_type, is_passed_delay_gate, delay_time, delay_time_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestEkfDiagnostics, check_measurement_mahalanobis_gate)
+{
+  diagnostic_msgs::msg::DiagnosticStatus stat;
+
+  const std::string measurement_type = "pose";        // not effect for stat.level
+  const double mahalanobis_distance = 0.1;            // not effect for stat.level
+  const double mahalanobis_distance_threshold = 1.0;  // not effect for stat.level
+
+  bool is_passed_mahalanobis_gate = true;
+  stat = check_measurement_mahalanobis_gate(
+    measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+    mahalanobis_distance_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+
+  is_passed_mahalanobis_gate = false;
+  stat = check_measurement_mahalanobis_gate(
+    measurement_type, is_passed_mahalanobis_gate, mahalanobis_distance,
+    mahalanobis_distance_threshold);
+  EXPECT_EQ(stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+}
+
+TEST(TestLocalizationErrorMonitorDiagnostics, merge_diagnostic_status)
+{
+  diagnostic_msgs::msg::DiagnosticStatus merged_stat;
+  std::vector<diagnostic_msgs::msg::DiagnosticStatus> stat_array(2);
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(1).message = "OK";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::OK);
+  EXPECT_EQ(merged_stat.message, "OK");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(1).message = "OK";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN0");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(1).message = "WARN1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(1).message = "WARN1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::WARN);
+  EXPECT_EQ(merged_stat.message, "WARN0; WARN1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::OK;
+  stat_array.at(0).message = "OK";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "ERROR1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::WARN;
+  stat_array.at(0).message = "WARN0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "WARN0; ERROR1");
+
+  stat_array.at(0).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(0).message = "ERROR0";
+  stat_array.at(1).level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
+  stat_array.at(1).message = "ERROR1";
+  merged_stat = merge_diagnostic_status(stat_array);
+  EXPECT_EQ(merged_stat.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR);
+  EXPECT_EQ(merged_stat.message, "ERROR0; ERROR1");
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_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("autoware_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)

master/src/Localization/autoware_ekf_localizer/test/test_ekf_localizer_mahalanobis.py

@@ -0,0 +1,229 @@
+#!/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("autoware_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)
+        rclpy.spin_once(self.test_node, timeout_sec=0.1)
+
+        # Send pose that should be ignored by mahalanobis gate in ekf_localizer
+        pub_pose = self.test_node.create_publisher(
+            PoseWithCovarianceStamped, "/in_pose_with_covariance", 10
+        )
+        pose = PoseWithCovarianceStamped()
+        pose.header.frame_id = "map"
+        pose.pose.pose.position.x = 1000000.0
+        pose.pose.pose.position.y = 1000000.0
+        pose.pose.pose.position.z = 10.0
+        pose.pose.pose.orientation.x = 0.0
+        pose.pose.pose.orientation.y = 0.0
+        pose.pose.pose.orientation.z = 0.0
+        pose.pose.pose.orientation.w = 1.0
+        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_pose.publish(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)
+
+        # Assert msg to be at the origin
+        self.assertEqual(msg_buffer[-1].pose.pose.position.x, 0.0)
+        self.assertEqual(msg_buffer[-1].pose.pose.position.y, 0.0)
+        self.assertEqual(msg_buffer[-1].pose.pose.position.z, 0.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)

master/src/Localization/autoware_ekf_localizer/test/test_mahalanobis.cpp

@@ -0,0 +1,66 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/mahalanobis.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+constexpr double tolerance = 1e-8;
+
+TEST(squared_mahalanobis, SmokeTest)
+{
+  {
+    Eigen::Vector2d x(0, 1);
+    Eigen::Vector2d y(3, 2);
+    Eigen::Matrix2d c;
+    c << 10, 0, 0, 10;
+
+    EXPECT_NEAR(squared_mahalanobis(x, y, c), 1.0, tolerance);
+  }
+
+  {
+    Eigen::Vector2d x(4, 1);
+    Eigen::Vector2d y(1, 5);
+    Eigen::Matrix2d c;
+    c << 5, 0, 0, 5;
+
+    EXPECT_NEAR(squared_mahalanobis(x, y, c), 5.0, tolerance);
+  }
+}
+
+TEST(mahalanobis, SmokeTest)
+{
+  {
+    Eigen::Vector2d x(0, 1);
+    Eigen::Vector2d y(3, 2);
+    Eigen::Matrix2d c;
+    c << 10, 0, 0, 10;
+
+    EXPECT_NEAR(mahalanobis(x, y, c), 1.0, tolerance);
+  }
+
+  {
+    Eigen::Vector2d x(4, 1);
+    Eigen::Vector2d y(1, 5);
+    Eigen::Matrix2d c;
+    c << 5, 0, 0, 5;
+
+    EXPECT_NEAR(mahalanobis(x, y, c), std::sqrt(5.0), tolerance);
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_measurement.cpp

@@ -0,0 +1,86 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/measurement.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(Measurement, pose_measurement_matrix)
+{
+  const Eigen::Matrix<double, 3, 6> m = pose_measurement_matrix();
+  Eigen::Matrix<double, 3, 6> expected;
+  expected << 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0;
+  EXPECT_EQ((m - expected).norm(), 0);
+}
+
+TEST(Measurement, twist_measurement_matrix)
+{
+  const Eigen::Matrix<double, 2, 6> m = twist_measurement_matrix();
+  Eigen::Matrix<double, 2, 6> expected;
+  expected << 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1;
+  EXPECT_EQ((m - expected).norm(), 0);
+}
+
+TEST(Measurement, pose_measurement_covariance)
+{
+  {
+    const std::array<double, 36> covariance = {1, 2, 0, 0, 0, 3, 4, 5, 0, 0, 0, 6,
+                                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                               0, 0, 0, 0, 0, 0, 7, 8, 0, 0, 0, 9};
+
+    const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2);
+
+    Eigen::Matrix3d expected;
+    expected << 2, 4, 6, 8, 10, 12, 14, 16, 18;
+
+    EXPECT_EQ((m - expected).norm(), 0.);
+  }
+
+  {
+    // Make sure that other elements are not changed
+    std::array<double, 36> covariance{};
+    covariance.fill(0);
+    const Eigen::Matrix3d m = pose_measurement_covariance(covariance, 2.);
+    EXPECT_EQ(m.norm(), 0);
+  }
+}
+
+TEST(Measurement, twist_measurement_covariance)
+{
+  {
+    const std::array<double, 36> covariance = {1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 0, 6,
+                                               0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+                                               0, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 4};
+
+    const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2);
+
+    Eigen::Matrix2d expected;
+    expected << 2, 4, 6, 8;
+
+    EXPECT_EQ((m - expected).norm(), 0.);
+  }
+
+  {
+    // Make sure that other elements are not changed
+    std::array<double, 36> covariance{};
+    covariance.fill(0);
+    const Eigen::Matrix2d m = twist_measurement_covariance(covariance, 2.);
+    EXPECT_EQ(m.norm(), 0);
+  }
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_numeric.cpp

@@ -0,0 +1,52 @@
+// Copyright 2022 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/numeric.hpp"
+
+#include <Eigen/Core>
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(Numeric, has_nan)
+{
+  const Eigen::VectorXd empty(0);
+  const double inf = std::numeric_limits<double>::infinity();
+  const double nan = std::nan("");
+
+  EXPECT_FALSE(has_nan(empty));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 0., 1.)));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(1e16, 0., 1.)));
+  EXPECT_FALSE(has_nan(Eigen::Vector3d(0., 1., inf)));
+
+  EXPECT_TRUE(has_nan(Eigen::Vector3d(nan, 1., 0.)));
+}
+
+TEST(Numeric, has_inf)
+{
+  const Eigen::VectorXd empty(0);
+  const double inf = std::numeric_limits<double>::infinity();
+  const double nan = std::nan("");
+
+  EXPECT_FALSE(has_inf(empty));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(0., 0., 1.)));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(1e16, 0., 1.)));
+  EXPECT_FALSE(has_inf(Eigen::Vector3d(nan, 1., 0.)));
+
+  EXPECT_TRUE(has_inf(Eigen::Vector3d(0., 1., inf)));
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_state_transition.cpp

@@ -0,0 +1,101 @@
+// Copyright 2018-2019 Autoware Foundation
+//
+// 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.
+
+#define _USE_MATH_DEFINES
+#include "autoware/ekf_localizer/state_index.hpp"
+#include "autoware/ekf_localizer/state_transition.hpp"
+
+#include <gtest/gtest.h>
+
+#include <cmath>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(StateTransition, normalize_yaw)
+{
+  const double tolerance = 1e-6;
+  EXPECT_NEAR(normalize_yaw(M_PI * 4 / 3), -M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalize_yaw(-M_PI * 4 / 3), M_PI * 2 / 3, tolerance);
+  EXPECT_NEAR(normalize_yaw(M_PI * 9 / 2), M_PI * 1 / 2, tolerance);
+  EXPECT_NEAR(normalize_yaw(M_PI * 4), M_PI * 0, tolerance);
+}
+
+TEST(predict_next_state, predict_next_state)
+{
+  // This function is the definition of state transition so we just check
+  // if the calculation is done according to the formula
+  Vector6d x_curr;
+  x_curr(0) = 2.;
+  x_curr(1) = 3.;
+  x_curr(2) = M_PI / 2.;
+  x_curr(3) = M_PI / 4.;
+  x_curr(4) = 10.;
+  x_curr(5) = 2. * M_PI / 3.;
+
+  const double dt = 0.5;
+
+  const Vector6d x_next = predict_next_state(x_curr, dt);
+
+  const double tolerance = 1e-10;
+  EXPECT_NEAR(x_next(0), 2. + 10. * std::cos(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(x_next(1), 3. + 10. * std::sin(M_PI / 2. + M_PI / 4.) * 0.5, tolerance);
+  EXPECT_NEAR(x_next(2), normalize_yaw(M_PI / 2. + M_PI / 3.), tolerance);
+  EXPECT_NEAR(x_next(3), x_curr(3), tolerance);
+  EXPECT_NEAR(x_next(4), x_curr(4), tolerance);
+  EXPECT_NEAR(x_next(5), x_curr(5), tolerance);
+}
+
+TEST(create_state_transition_matrix, NumericalApproximation)
+{
+  // The transition matrix A = df / dx
+  // We check if df = A * dx approximates f(x + dx) - f(x)
+
+  {
+    // check around x = 0
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = Vector6d::Zero();
+
+    const Matrix6d a = create_state_transition_matrix(x, dt);
+    const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt);
+
+    EXPECT_LT((df - a * dx).norm(), 2e-3);
+  }
+
+  {
+    // check around random x
+    const double dt = 0.1;
+    const Vector6d dx = 0.1 * Vector6d::Ones();
+    const Vector6d x = (Vector6d() << 0.1, 0.2, 0.1, 0.4, 0.1, 0.3).finished();
+
+    const Matrix6d a = create_state_transition_matrix(x, dt);
+    const Vector6d df = predict_next_state(x + dx, dt) - predict_next_state(x, dt);
+
+    EXPECT_LT((df - a * dx).norm(), 5e-3);
+  }
+}
+
+TEST(process_noise_covariance, process_noise_covariance)
+{
+  const Matrix6d q = process_noise_covariance(1., 2., 3.);
+  EXPECT_EQ(q(2, 2), 1.);  // for yaw
+  EXPECT_EQ(q(4, 4), 2.);  // for vx
+  EXPECT_EQ(q(5, 5), 3.);  // for wz
+
+  // Make sure other elements are zero
+  EXPECT_EQ(process_noise_covariance(0, 0, 0).norm(), 0.);
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_string.cpp

@@ -0,0 +1,31 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/string.hpp"
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(erase_leading_slash, SmokeTest)
+{
+  EXPECT_EQ(erase_leading_slash("/topic"), "topic");
+  EXPECT_EQ(erase_leading_slash("topic"), "topic");  // do nothing
+
+  EXPECT_EQ(erase_leading_slash(""), "");
+  EXPECT_EQ(erase_leading_slash("/"), "");
+}
+
+}  // namespace autoware::ekf_localizer

master/src/Localization/autoware_ekf_localizer/test/test_warning_message.cpp

@@ -0,0 +1,67 @@
+// Copyright 2023 Autoware Foundation
+//
+// 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.
+
+#include "autoware/ekf_localizer/warning_message.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+namespace autoware::ekf_localizer
+{
+
+TEST(pose_delay_step_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    pose_delay_step_warning_message(6.0, 4.0).c_str(),
+    "Pose delay exceeds the compensation limit, ignored. "
+    "delay: 6.000[s], limit: 4.000[s]");
+}
+
+TEST(twist_delay_step_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    twist_delay_step_warning_message(10.0, 6.0).c_str(),
+    "Twist delay exceeds the compensation limit, ignored. "
+    "delay: 10.000[s], limit: 6.000[s]");
+}
+
+TEST(pose_delay_time_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    pose_delay_time_warning_message(-1.0).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    pose_delay_time_warning_message(-0.4).c_str(),
+    "Pose time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
+TEST(twist_delay_time_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    twist_delay_time_warning_message(-1.0).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -1.000");
+  EXPECT_STREQ(
+    twist_delay_time_warning_message(-0.4).c_str(),
+    "Twist time stamp is inappropriate, set delay to 0[s]. delay = -0.400");
+}
+
+TEST(mahalanobis_warning_message, SmokeTest)
+{
+  EXPECT_STREQ(
+    mahalanobis_warning_message(1.0, 0.5).c_str(),
+    "The Mahalanobis distance 1.0000 is over the limit 0.5000.");
+}
+
+}  // namespace autoware::ekf_localizer