import lanelet2_map_preprocessor

Signed-off-by: a-maumau <[email protected]>

.github/CODEOWNERS

@@ -89,7 +89,7 @@ localization/autoware_pose_initializer/** [email protected] isamu.takagi@tie
 localization/autoware_pose_instability_detector/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_stop_filter/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/autoware_twist2accel/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
-localization/ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
+localization/autoware_ekf_localizer/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/localization_util/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/ndt_scan_matcher/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]
 localization/yabloc/yabloc_common/** [email protected] [email protected] [email protected] [email protected] [email protected] [email protected] [email protected]

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <launch>
   <group>
-    <include file="$(find-pkg-share ekf_localizer)/launch/ekf_localizer.launch.xml">
+    <include file="$(find-pkg-share autoware_ekf_localizer)/launch/ekf_localizer.launch.xml">
       <arg name="input_initial_pose_name" value="/initialpose3d"/>
       <arg name="input_pose_with_cov_name" value="/localization/pose_estimator/pose_with_covariance"/>
       <arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>

launch/tier4_localization_launch/package.xml

@@ -19,6 +19,7 @@
 
   <exec_depend>automatic_pose_initializer</exec_depend>
   <exec_depend>autoware_ar_tag_based_localizer</exec_depend>
+  <exec_depend>autoware_ekf_localizer</exec_depend>
   <exec_depend>autoware_geo_pose_projector</exec_depend>
   <exec_depend>autoware_gyro_odometer</exec_depend>
   <exec_depend>autoware_lidar_marker_localizer</exec_depend>
@@ -29,7 +30,6 @@
   <exec_depend>eagleye_geo_pose_fusion</exec_depend>
   <exec_depend>eagleye_gnss_converter</exec_depend>
   <exec_depend>eagleye_rt</exec_depend>
-  <exec_depend>ekf_localizer</exec_depend>
   <exec_depend>ndt_scan_matcher</exec_depend>
   <exec_depend>topic_tools</exec_depend>
   <exec_depend>yabloc_common</exec_depend>

localization/ekf_localizer/CMakeLists.txt → localization/autoware_ekf_localizer/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.14)
-project(ekf_localizer)
+project(autoware_ekf_localizer)
 
 find_package(autoware_cmake REQUIRED)
 autoware_package()
@@ -42,6 +42,10 @@ function(add_testcase filepath)
 endfunction()
 
 if(BUILD_TESTING)
+  include_directories(
+    src
+  )
+
   add_launch_test(
     test/test_ekf_localizer_launch.py
     TIMEOUT "30"

localization/ekf_localizer/README.md → localization/autoware_ekf_localizer/README.md

@@ -4,7 +4,7 @@ The **Extend Kalman Filter Localizer** estimates robust and less noisy robot pos
 
 ## Flowchart
 
-The overall flowchart of the ekf_localizer is described below.
+The overall flowchart of the autoware_ekf_localizer is described below.
 
 <p align="center">
   <img src="./media/ekf_flowchart.png" width="800">
@@ -78,33 +78,33 @@ The parameters are set in `launch/ekf_localizer.launch` .
 
 ### For Node
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/node.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/node.sub_schema.json") }}
 
 ### For pose measurement
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/pose_measurement.sub_schema.json") }}
 
 ### For twist measurement
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/twist_measurement.sub_schema.json") }}
 
 ### For process noise
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/process_noise.sub_schema.json") }}
 
 note: process noise for positions x & y are calculated automatically from nonlinear dynamics.
 
 ### Simple 1D Filter Parameters
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/simple_1d_filter_parameters.sub_schema.json") }}
 
 ### For diagnostics
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/diagnostics.sub_schema.json") }}
 
 ### Misc
 
-{{ json_to_markdown("localization/ekf_localizer/schema/sub/misc.sub_schema.json") }}
+{{ json_to_markdown("localization/autoware_ekf_localizer/schema/sub/misc.sub_schema.json") }}
 
 ## How to tune EKF parameters
 

localization/ekf_localizer/launch/ekf_localizer.launch.xml → localization/autoware_ekf_localizer/launch/ekf_localizer.launch.xml

@@ -1,5 +1,5 @@
 <launch>
-  <arg name="param_file" default="$(find-pkg-share ekf_localizer)/config/ekf_localizer.param.yaml"/>
+  <arg name="param_file" default="$(find-pkg-share autoware_ekf_localizer)/config/ekf_localizer.param.yaml"/>
 
   <arg name="input_initial_pose_name" default="initialpose3d"/>
   <arg name="input_trigger_node_service_name" default="trigger_node" description="trigger node service"/>
@@ -17,7 +17,7 @@
   <arg name="output_twist_name" default="ekf_twist"/>
   <arg name="output_twist_with_covariance_name" default="ekf_twist_with_covariance"/>
 
-  <node pkg="ekf_localizer" exec="ekf_localizer_node" output="both">
+  <node pkg="autoware_ekf_localizer" exec="autoware_ekf_localizer_node" output="both">
     <remap from="in_pose_with_covariance" to="$(var input_pose_with_cov_name)"/>
 
     <remap from="in_twist_with_covariance" to="$(var input_twist_with_cov_name)"/>

localization/ekf_localizer/package.xml → localization/autoware_ekf_localizer/package.xml

@@ -1,9 +1,9 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>ekf_localizer</name>
+  <name>autoware_ekf_localizer</name>
   <version>0.1.0</version>
-  <description>The ekf_localizer package</description>
+  <description>The autoware_ekf_localizer package</description>
   <maintainer email="[email protected]">Takamasa Horibe</maintainer>
   <maintainer email="[email protected]">Yamato Ando</maintainer>
   <maintainer email="[email protected]">Takeshi Ishita</maintainer>

localization/ekf_localizer/include/ekf_localizer/aged_object_queue.hpp → localization/autoware_ekf_localizer/src/aged_object_queue.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_
-#define EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_
+#ifndef AGED_OBJECT_QUEUE_HPP_
+#define AGED_OBJECT_QUEUE_HPP_
 
 #include <cstddef>
 #include <queue>
@@ -68,4 +68,4 @@ class AgedObjectQueue
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__AGED_OBJECT_QUEUE_HPP_
+#endif  // AGED_OBJECT_QUEUE_HPP_

localization/ekf_localizer/src/covariance.cpp → localization/autoware_ekf_localizer/src/covariance.cpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/covariance.hpp"
+#include "covariance.hpp"
 
 #include "autoware/universe_utils/ros/msg_covariance.hpp"
-#include "ekf_localizer/state_index.hpp"
+#include "state_index.hpp"
 
 namespace autoware::ekf_localizer
 {

localization/ekf_localizer/include/ekf_localizer/covariance.hpp → localization/autoware_ekf_localizer/src/covariance.hpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__COVARIANCE_HPP_
-#define EKF_LOCALIZER__COVARIANCE_HPP_
+#ifndef COVARIANCE_HPP_
+#define COVARIANCE_HPP_
 
-#include "ekf_localizer/matrix_types.hpp"
+#include "matrix_types.hpp"
 
 namespace autoware::ekf_localizer
 {
@@ -25,4 +25,4 @@ std::array<double, 36> ekf_covariance_to_twist_message_covariance(const Matrix6d
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__COVARIANCE_HPP_
+#endif  // COVARIANCE_HPP_

localization/ekf_localizer/src/diagnostics.cpp → localization/autoware_ekf_localizer/src/diagnostics.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/diagnostics.hpp"
+#include "diagnostics.hpp"
 
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 

localization/ekf_localizer/include/ekf_localizer/diagnostics.hpp → localization/autoware_ekf_localizer/src/diagnostics.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__DIAGNOSTICS_HPP_
-#define EKF_LOCALIZER__DIAGNOSTICS_HPP_
+#ifndef DIAGNOSTICS_HPP_
+#define DIAGNOSTICS_HPP_
 
 #include <diagnostic_msgs/msg/diagnostic_status.hpp>
 
@@ -45,4 +45,4 @@ diagnostic_msgs::msg::DiagnosticStatus merge_diagnostic_status(
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__DIAGNOSTICS_HPP_
+#endif  // DIAGNOSTICS_HPP_

localization/ekf_localizer/src/ekf_localizer.cpp → localization/autoware_ekf_localizer/src/ekf_localizer.cpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/ekf_localizer.hpp"
+#include "ekf_localizer.hpp"
 
-#include "ekf_localizer/diagnostics.hpp"
-#include "ekf_localizer/string.hpp"
-#include "ekf_localizer/warning_message.hpp"
+#include "diagnostics.hpp"
 #include "localization_util/covariance_ellipse.hpp"
+#include "string.hpp"
+#include "warning_message.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>

localization/ekf_localizer/include/ekf_localizer/ekf_localizer.hpp → localization/autoware_ekf_localizer/src/ekf_localizer.hpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__EKF_LOCALIZER_HPP_
-#define EKF_LOCALIZER__EKF_LOCALIZER_HPP_
+#ifndef EKF_LOCALIZER_HPP_
+#define EKF_LOCALIZER_HPP_
 
-#include "ekf_localizer/aged_object_queue.hpp"
-#include "ekf_localizer/ekf_module.hpp"
-#include "ekf_localizer/hyper_parameters.hpp"
-#include "ekf_localizer/warning.hpp"
+#include "aged_object_queue.hpp"
+#include "ekf_module.hpp"
+#include "hyper_parameters.hpp"
+#include "warning.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/logger_level_configure.hpp>
@@ -198,4 +198,4 @@ class EKFLocalizer : public rclcpp::Node
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__EKF_LOCALIZER_HPP_
+#endif  // EKF_LOCALIZER_HPP_

localization/ekf_localizer/src/ekf_module.cpp → localization/autoware_ekf_localizer/src/ekf_module.cpp

@@ -12,15 +12,15 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/ekf_module.hpp"
+#include "ekf_module.hpp"
 
-#include "ekf_localizer/covariance.hpp"
-#include "ekf_localizer/mahalanobis.hpp"
-#include "ekf_localizer/matrix_types.hpp"
-#include "ekf_localizer/measurement.hpp"
-#include "ekf_localizer/numeric.hpp"
-#include "ekf_localizer/state_transition.hpp"
-#include "ekf_localizer/warning_message.hpp"
+#include "covariance.hpp"
+#include "mahalanobis.hpp"
+#include "matrix_types.hpp"
+#include "measurement.hpp"
+#include "numeric.hpp"
+#include "state_transition.hpp"
+#include "warning_message.hpp"
 
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/ros/msg_covariance.hpp>
@@ -29,6 +29,9 @@
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2/utils.h>
 
+#include <algorithm>
+#include <utility>
+
 namespace autoware::ekf_localizer
 {
 

localization/ekf_localizer/include/ekf_localizer/ekf_module.hpp → localization/autoware_ekf_localizer/src/ekf_module.hpp

@@ -12,12 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__EKF_MODULE_HPP_
-#define EKF_LOCALIZER__EKF_MODULE_HPP_
+#ifndef EKF_MODULE_HPP_
+#define EKF_MODULE_HPP_
 
-#include "ekf_localizer/hyper_parameters.hpp"
-#include "ekf_localizer/state_index.hpp"
-#include "ekf_localizer/warning.hpp"
+#include "hyper_parameters.hpp"
+#include "state_index.hpp"
+#include "warning.hpp"
 
 #include <autoware/kalman_filter/kalman_filter.hpp>
 #include <autoware/kalman_filter/time_delay_kalman_filter.hpp>
@@ -153,4 +153,4 @@ class EKFModule
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__EKF_MODULE_HPP_
+#endif  // EKF_MODULE_HPP_

localization/ekf_localizer/include/ekf_localizer/hyper_parameters.hpp → localization/autoware_ekf_localizer/src/hyper_parameters.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
-#define EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
+#ifndef HYPER_PARAMETERS_HPP_
+#define HYPER_PARAMETERS_HPP_
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -107,4 +107,4 @@ class HyperParameters
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__HYPER_PARAMETERS_HPP_
+#endif  // HYPER_PARAMETERS_HPP_

localization/ekf_localizer/src/mahalanobis.cpp → localization/autoware_ekf_localizer/src/mahalanobis.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/mahalanobis.hpp"
+#include "mahalanobis.hpp"
 
 namespace autoware::ekf_localizer
 {

localization/ekf_localizer/include/ekf_localizer/mahalanobis.hpp → localization/autoware_ekf_localizer/src/mahalanobis.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__MAHALANOBIS_HPP_
-#define EKF_LOCALIZER__MAHALANOBIS_HPP_
+#ifndef MAHALANOBIS_HPP_
+#define MAHALANOBIS_HPP_
 
 #include <Eigen/Core>
 #include <Eigen/Dense>
@@ -28,4 +28,4 @@ double mahalanobis(const Eigen::VectorXd & x, const Eigen::VectorXd & y, const E
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__MAHALANOBIS_HPP_
+#endif  // MAHALANOBIS_HPP_

localization/ekf_localizer/include/ekf_localizer/matrix_types.hpp → localization/autoware_ekf_localizer/src/matrix_types.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__MATRIX_TYPES_HPP_
-#define EKF_LOCALIZER__MATRIX_TYPES_HPP_
+#ifndef MATRIX_TYPES_HPP_
+#define MATRIX_TYPES_HPP_
 
 #include <Eigen/Core>
 
@@ -25,4 +25,4 @@ using Matrix6d = Eigen::Matrix<double, 6, 6>;
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__MATRIX_TYPES_HPP_
+#endif  // MATRIX_TYPES_HPP_

localization/ekf_localizer/src/measurement.cpp → localization/autoware_ekf_localizer/src/measurement.cpp

@@ -12,10 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "ekf_localizer/measurement.hpp"
+#include "measurement.hpp"
 
 #include "autoware/universe_utils/ros/msg_covariance.hpp"
-#include "ekf_localizer/state_index.hpp"
+#include "state_index.hpp"
 
 namespace autoware::ekf_localizer
 {

localization/ekf_localizer/include/ekf_localizer/measurement.hpp → localization/autoware_ekf_localizer/src/measurement.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef EKF_LOCALIZER__MEASUREMENT_HPP_
-#define EKF_LOCALIZER__MEASUREMENT_HPP_
+#ifndef MEASUREMENT_HPP_
+#define MEASUREMENT_HPP_
 
 #include <Eigen/Core>
 
@@ -29,4 +29,4 @@ Eigen::Matrix2d twist_measurement_covariance(
 
 }  // namespace autoware::ekf_localizer
 
-#endif  // EKF_LOCALIZER__MEASUREMENT_HPP_
+#endif  // MEASUREMENT_HPP_