diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/CMakeLists.txt b/localization/yabloc/particle_filter/camera_particle_corrector/CMakeLists.txt
deleted file mode 100644
index f79ffd6f09692..0000000000000
--- a/localization/yabloc/particle_filter/camera_particle_corrector/CMakeLists.txt
+++ /dev/null
@@ -1,45 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(camera_particle_corrector)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
- set(CMAKE_CXX_STANDARD_REQUIRED ON)
- set(CMAKE_CXX_EXTENSIONS OFF)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- # add_compile_options(-Wall -Wextra -Wpedantic -Werror)
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# ===================================================
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-# ===================================================
-# Eigen3
-find_package(Eigen3 REQUIRED)
-
-# Sophus
-find_package(Sophus REQUIRED)
-
-# PCL
-find_package(PCL REQUIRED COMPONENTS common)
-
-# glog
-find_package(glog REQUIRED)
-
-# ===================================================
-# Executable
-set(TARGET camera_particle_corrector_node)
-ament_auto_add_executable(${TARGET}
- src/camera_particle_corrector_node.cpp
- src/filt_lsd.cpp
- src/logit.cpp
- src/camera_particle_corrector_core.cpp)
-target_include_directories(${TARGET} PUBLIC include)
-target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
-target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES} glog::glog)
-
-# ===================================================
-ament_auto_package(INSTALL_TO_SHARE config)
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/package.xml b/localization/yabloc/particle_filter/camera_particle_corrector/package.xml
deleted file mode 100644
index 2fd997590e6ea..0000000000000
--- a/localization/yabloc/particle_filter/camera_particle_corrector/package.xml
+++ /dev/null
@@ -1,29 +0,0 @@
-
-
-
- camera_particle_corrector
- 0.1.0
- The camera particle filter corrector package
- Kento Yabuuchi
- Apache License 2.0
-
- ament_cmake_ros
-
- rclcpp
- tf2
- std_msgs
- std_srvs
- geometry_msgs
- sensor_msgs
- visualization_msgs
- ll2_cost_map
-
- modularized_particle_filter
- yabloc_common
- libgoogle-glog-dev
-
-
-
- ament_cmake
-
-
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/CMakeLists.txt b/localization/yabloc/particle_filter/gnss_particle_corrector/CMakeLists.txt
deleted file mode 100644
index f461f50054cf9..0000000000000
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/CMakeLists.txt
+++ /dev/null
@@ -1,43 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(gnss_particle_corrector)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
- set(CMAKE_CXX_STANDARD_REQUIRED ON)
- set(CMAKE_CXX_EXTENSIONS OFF)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror)
-endif()
-
-# ===================================================
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-# ===================================================
-# Eigen3
-find_package(Eigen3 REQUIRED)
-
-# Sophus
-find_package(Sophus REQUIRED)
-
-# ===================================================
-# GeographicLib
-find_package(PkgConfig)
-find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
- PATH_SUFFIXES GeographicLib
-)
-set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
-find_library(GeographicLib_LIBRARIES NAMES Geographic)
-
-# ===================================================
-# Executable
-set(TARGET gnss_particle_corrector_node)
-ament_auto_add_executable(${TARGET} src/gnss_corrector_node.cpp src/gnss_corrector_core.cpp)
-target_include_directories(${TARGET} PUBLIC include)
-target_include_directories(${TARGET} PUBLIC SYSTEM ${GeographicLib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
-target_link_libraries(${TARGET} Sophus::Sophus)
-
-# ===================================================
-ament_auto_package(INSTALL_TO_SHARE config)
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/package.xml b/localization/yabloc/particle_filter/gnss_particle_corrector/package.xml
deleted file mode 100644
index 033c78a2059ab..0000000000000
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/package.xml
+++ /dev/null
@@ -1,26 +0,0 @@
-
-
-
- gnss_particle_corrector
- 0.1.0
- The GNSS particle filter corrector package
- Kento Yabuuchi
- Apache License 2.0
-
- ament_cmake_ros
-
- rclcpp
- tf2
- std_msgs
- geometry_msgs
- sensor_msgs
- visualization_msgs
- geographiclib
-
- modularized_particle_filter
- yabloc_common
-
-
- ament_cmake
-
-
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/CMakeLists.txt b/localization/yabloc/particle_filter/ll2_cost_map/CMakeLists.txt
deleted file mode 100644
index 463ca21374a9f..0000000000000
--- a/localization/yabloc/particle_filter/ll2_cost_map/CMakeLists.txt
+++ /dev/null
@@ -1,38 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(ll2_cost_map)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
- set(CMAKE_CXX_STANDARD_REQUIRED ON)
- set(CMAKE_CXX_EXTENSIONS OFF)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- # add_compile_options(-Wall -Wextra -Wpedantic -Werror)
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# ===================================================
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-# ===================================================
-# Eigen3
-find_package(Eigen3 REQUIRED)
-
-# PCL
-find_package(PCL REQUIRED COMPONENTS common)
-
-# ===================================================
-# Library
-ament_auto_add_library(${PROJECT_NAME}
- SHARED
- src/hierarchical_cost_map.cpp
- src/direct_cost_map.cpp
-)
-target_include_directories(${PROJECT_NAME} PUBLIC include)
-target_include_directories(${PROJECT_NAME} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
-target_link_libraries(${PROJECT_NAME} ${PCL_LIBRARIES})
-
-# ===================================================
-ament_auto_package()
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/package.xml b/localization/yabloc/particle_filter/ll2_cost_map/package.xml
deleted file mode 100644
index bf1b51207fed9..0000000000000
--- a/localization/yabloc/particle_filter/ll2_cost_map/package.xml
+++ /dev/null
@@ -1,28 +0,0 @@
-
-
-
- ll2_cost_map
- 0.1.0
- The package converts cost map from lanelet2
- Kento Yabuuchi
- Apache License 2.0
-
- ament_cmake_ros
-
- rclcpp
- tf2
- std_msgs
- std_srvs
- geometry_msgs
- sensor_msgs
- visualization_msgs
-
- ground_msgs
- ll2_decomposer
- yabloc_common
-
-
- ament_cmake
-
-
-
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/CMakeLists.txt b/localization/yabloc/particle_filter/modularized_particle_filter/CMakeLists.txt
deleted file mode 100644
index d681c2d3c84ed..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter/CMakeLists.txt
+++ /dev/null
@@ -1,67 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(modularized_particle_filter)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic -Werror)
-endif()
-
-find_package(ament_cmake_auto REQUIRED)
-
-# PCL
-find_package(PCL REQUIRED)
-
-# Sophus
-find_package(Sophus REQUIRED)
-
-ament_auto_find_build_dependencies()
-
-# ===================================================
-# Libraries
-ament_auto_add_library(
- abst_corrector
- SHARED
- src/correction/abst_corrector.cpp
- src/common/visualize.cpp
- src/common/mean.cpp
-)
-target_include_directories(abst_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS})
-target_link_libraries(abst_corrector Sophus::Sophus ${PCL_LIBRARIES})
-
-ament_auto_add_library(predictor
- src/prediction/predictor.cpp
- src/prediction/resampler.cpp
- src/common/visualize.cpp
- src/common/mean.cpp
-)
-target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS})
-target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES})
-
-# ===================================================
-# Executables
-ament_auto_add_executable(predictor_node
- src/prediction/predictor_node.cpp
-)
-target_link_libraries(predictor_node predictor)
-
-ament_auto_add_executable(particle_visualize
- src/common/particle_visualize_node.cpp
-)
-
-# ===================================================
-# TEST
-if(BUILD_TESTING)
- add_subdirectory(test)
-endif()
-
-# ===================================================
-install(PROGRAMS
- script/particle_array_to_marker_array.py
- script/particle_array_to_pose_array.py
- DESTINATION lib/${PROJECT_NAME}
-)
-
-ament_auto_package(INSTALL_TO_SHARE launch config)
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/launch/run.launch.xml b/localization/yabloc/particle_filter/modularized_particle_filter/launch/run.launch.xml
deleted file mode 100644
index 741b7c67a3dac..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter/launch/run.launch.xml
+++ /dev/null
@@ -1,20 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_marker_array.py b/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_marker_array.py
deleted file mode 100755
index bdaa09ee70c5e..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_marker_array.py
+++ /dev/null
@@ -1,81 +0,0 @@
-#!/usr/bin/env python3
-
-import copy
-import numpy
-
-import rclpy
-from rclpy.node import Node
-
-import modularized_particle_filter_msgs.msg
-import visualization_msgs.msg
-
-
-class ParticleArrayToMarkerArray(Node):
- def __init__(self):
- super().__init__('particle_array_to_marker_array')
- self.__marker_array = None
- self._publisher = self.create_publisher(
- visualization_msgs.msg.MarkerArray,
- "pub/marker_array", 10)
- self._subscription = self.create_subscription(
- modularized_particle_filter_msgs.msg.ParticleArray,
- "sub/particle_array",
- self.on_particle_array, 10)
-
- def on_particle_array(self, msg):
- if self.__marker_array is None or len(self.__marker_array.markers) != len(msg.particles):
- self.__marker_array = visualization_msgs.msg.MarkerArray()
- for i in range(len(msg.particles)):
- marker = visualization_msgs.msg.Marker()
- marker.header.frame_id = "map"
- marker.frame_locked = True
- marker.scale.x = 0.30 # arrow length
- marker.scale.y = 0.10 # arrow width
- marker.scale.z = 0.10 # arrow height
- marker.color.r = 1.0
- marker.color.g = 0.1
- marker.color.b = 0.0
- marker.color.a = 1.0
- marker.id = i
- marker.type = visualization_msgs.msg.Marker.ARROW
- self.__marker_array.markers.append(marker)
-
- max_weight = max(map(lambda particle: particle.weight, msg.particles))
- max_weight = max(0.1, max_weight)
-
- for i, particle in enumerate(msg.particles):
- self.__marker_array.markers[i].pose = copy.deepcopy(particle.pose)
- self.__marker_array.markers[i].color.r, \
- self.__marker_array.markers[i].color.g, \
- self.__marker_array.markers[i].color.b = ParticleArrayToMarkerArray.calculate_weight_color(
- particle.weight / max_weight)
-
- self._publisher.publish(self.__marker_array)
-
- @staticmethod
- def sigmoid(x, gain=1, offset_x=0):
- return ((numpy.tanh(((x+offset_x)*gain)/2)+1)/2)
-
- @staticmethod
- def calculate_weight_color(x, gain=10, offset_x=0.2, offset_green=0.6):
- x = (x * 2) - 1
- red = ParticleArrayToMarkerArray.sigmoid(x, gain, -1*offset_x)
- blue = 1-ParticleArrayToMarkerArray.sigmoid(x, gain, offset_x)
- green = ParticleArrayToMarkerArray.sigmoid(x, gain, offset_green) + \
- (1-ParticleArrayToMarkerArray.sigmoid(x, gain, -1*offset_green)) - 1.0
- return red, green, blue
-
-
-def main(args=None):
- rclpy.init(args=args)
-
- node = ParticleArrayToMarkerArray()
-
- rclpy.spin(node)
-
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_pose_array.py b/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_pose_array.py
deleted file mode 100755
index eda09a0643a44..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_pose_array.py
+++ /dev/null
@@ -1,50 +0,0 @@
-#!/usr/bin/env python3
-
-import copy
-import numpy
-
-import rclpy
-from rclpy.node import Node
-
-import modularized_particle_filter_msgs.msg
-import geometry_msgs.msg
-
-
-class ParticleArrayToPoseArray(Node):
- def __init__(self):
- super().__init__('particle_array_to_pose_array')
-
- self._publisher = self.create_publisher(
- geometry_msgs.msg.PoseArray, "pub/pose_array", 10)
-
- self._subscription = self.create_subscription(
- modularized_particle_filter_msgs.msg.ParticleArray, "sub/particle_array", self.on_particle_array, 10)
-
- self.__pose_array = None
-
- def on_particle_array(self, msg):
- if self.__pose_array is None or len(self.__pose_array.poses) != len(msg.particles):
- self.__pose_array = geometry_msgs.msg.PoseArray()
- for i in range(len(msg.particles)):
- self.__pose_array.poses.append(geometry_msgs.msg.Pose())
-
- self.__pose_array.header = msg.header;
- for i, particle in enumerate(msg.particles):
- self.__pose_array.poses[i] = copy.deepcopy(particle.pose)
-
- self._publisher.publish(self.__pose_array)
-
-
-def main(args=None):
- rclpy.init(args=args)
-
- node = ParticleArrayToPoseArray()
-
- rclpy.spin(node)
-
- node.destroy_node()
- rclpy.shutdown()
-
-
-if __name__ == '__main__':
- main()
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/CMakeLists.txt b/localization/yabloc/particle_filter/modularized_particle_filter_msgs/CMakeLists.txt
deleted file mode 100644
index 5b8c1822905a3..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/CMakeLists.txt
+++ /dev/null
@@ -1,24 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(modularized_particle_filter_msgs)
-
-if(NOT CMAKE_CXX_STANDARD)
- set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
- add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-find_package(ament_cmake_auto REQUIRED)
-ament_auto_find_build_dependencies()
-
-rosidl_generate_interfaces(${PROJECT_NAME}
- "msg/Particle.msg"
- "msg/ParticleArray.msg"
- DEPENDENCIES
- std_msgs
- geometry_msgs
- sensor_msgs
-)
-
-ament_package()
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/package.xml b/localization/yabloc/particle_filter/modularized_particle_filter_msgs/package.xml
deleted file mode 100644
index ce8c8d19ec880..0000000000000
--- a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/package.xml
+++ /dev/null
@@ -1,23 +0,0 @@
-
-
-
- modularized_particle_filter_msgs
- 0.1.0
- The modularized_particle_filter_msgs package
- Kento Yabuuchi
- Apache License 2.0
-
- ament_cmake_ros
- rosidl_default_generators
-
- std_msgs
- geometry_msgs
- sensor_msgs
-
- rosidl_default_runtime
- rosidl_interface_packages
-
-
- ament_cmake
-
-
diff --git a/localization/yabloc/yabloc_launch/launch/impl/pf.launch.xml b/localization/yabloc/yabloc_launch/launch/impl/pf.launch.xml
index ffc46ae807278..5fa79159b3bc8 100644
--- a/localization/yabloc/yabloc_launch/launch/impl/pf.launch.xml
+++ b/localization/yabloc/yabloc_launch/launch/impl/pf.launch.xml
@@ -5,7 +5,7 @@
-
+
@@ -23,7 +23,7 @@
-
+
@@ -37,7 +37,7 @@
-
+
diff --git a/localization/yabloc/yabloc_launch/launch/yabloc.launch.xml b/localization/yabloc/yabloc_launch/launch/yabloc.launch.xml
index 9e1b9bd680c3d..2727fe2672514 100644
--- a/localization/yabloc/yabloc_launch/launch/yabloc.launch.xml
+++ b/localization/yabloc/yabloc_launch/launch/yabloc.launch.xml
@@ -19,9 +19,9 @@
-
-
-
+
+
+
diff --git a/localization/yabloc/yabloc_launch/launch/yabloc_multi_camera.launch.xml b/localization/yabloc/yabloc_launch/launch/yabloc_multi_camera.launch.xml
index 6e1d77720dbb0..49bea50929ccc 100644
--- a/localization/yabloc/yabloc_launch/launch/yabloc_multi_camera.launch.xml
+++ b/localization/yabloc/yabloc_launch/launch/yabloc_multi_camera.launch.xml
@@ -25,11 +25,11 @@
-
+
-
-
-
+
+
+
diff --git a/localization/yabloc/yabloc_launch/package.xml b/localization/yabloc/yabloc_launch/package.xml
index a3dfae7a7e62c..fd24e4396699c 100644
--- a/localization/yabloc/yabloc_launch/package.xml
+++ b/localization/yabloc/yabloc_launch/package.xml
@@ -12,17 +12,15 @@
rclpy
autoware_auto_control_msgs
- yabloc_initializer
+ yabloc_pose_initializer
yabloc_imgproc
+ yabloc_particle_filter
ground_server
ll2_decomposer
twist_converter
twist_estimator
-
- camera_particle_corrector
- gnss_particle_corrector
rviz2_initialpose_plugins
rviz2_overlay_plugins
diff --git a/localization/yabloc/yabloc_particle_filter/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt
new file mode 100644
index 0000000000000..d3891a2140791
--- /dev/null
+++ b/localization/yabloc/yabloc_particle_filter/CMakeLists.txt
@@ -0,0 +1,118 @@
+cmake_minimum_required(VERSION 3.5)
+project(yabloc_particle_filter)
+
+if(NOT CMAKE_CXX_STANDARD)
+ set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+ add_compile_options(-Wall -Wextra -Wpedantic -Werror)
+endif()
+
+# ===================================================
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+# ===================================================
+# PCL
+find_package(PCL REQUIRED)
+
+# Sophus
+find_package(Sophus REQUIRED)
+
+# glog
+find_package(glog REQUIRED)
+
+# GeographicLib
+find_package(PkgConfig)
+find_path(GeographicLib_INCLUDE_DIR GeographicLib/Config.h
+ PATH_SUFFIXES GeographicLib
+)
+set(GeographicLib_INCLUDE_DIRS ${GeographicLib_INCLUDE_DIR})
+find_library(GeographicLib_LIBRARIES NAMES Geographic)
+
+# ===================================================
+# Message & Service
+rosidl_generate_interfaces(${PROJECT_NAME}
+ "msg/Particle.msg"
+ "msg/ParticleArray.msg"
+ DEPENDENCIES
+ std_msgs
+ geometry_msgs
+)
+
+# ===================================================
+# Libraries
+# correction library
+ament_auto_add_library(
+ abst_corrector
+ SHARED
+ src/correction/abst_corrector.cpp
+ src/common/visualize.cpp
+ src/common/mean.cpp
+)
+target_include_directories(abst_corrector SYSTEM PRIVATE ${PCL_INCLUDE_DIRS})
+target_link_libraries(abst_corrector Sophus::Sophus ${PCL_LIBRARIES})
+rosidl_target_interfaces(abst_corrector ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# Prediction library
+ament_auto_add_library(predictor
+ src/prediction/predictor.cpp
+ src/prediction/resampler.cpp
+ src/common/visualize.cpp
+ src/common/mean.cpp
+)
+target_include_directories(predictor SYSTEM PRIVATE ${PCL_INCLUDE_DIRS})
+target_link_libraries(predictor Sophus::Sophus ${PCL_LIBRARIES})
+rosidl_target_interfaces(predictor ${PROJECT_NAME} "rosidl_typesupport_cpp")
+
+# Cost map Library
+ament_auto_add_library(ll2_cost_map
+ SHARED
+ src/ll2_cost_map/hierarchical_cost_map.cpp
+ src/ll2_cost_map/direct_cost_map.cpp
+)
+target_include_directories(ll2_cost_map PUBLIC include)
+target_include_directories(ll2_cost_map SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+target_link_libraries(ll2_cost_map ${PCL_LIBRARIES})
+
+# ===================================================
+# Executables
+# Predictor
+ament_auto_add_executable(predictor_node
+ src/prediction/predictor_node.cpp
+)
+target_link_libraries(predictor_node predictor)
+
+# Visualizer
+ament_auto_add_executable(particle_visualize
+ src/common/particle_visualize_node.cpp
+)
+
+# GNSS corrector
+set(TARGET gnss_particle_corrector_node)
+ament_auto_add_executable(${TARGET}
+ src/gnss_corrector/gnss_corrector_node.cpp
+ src/gnss_corrector/gnss_corrector_core.cpp)
+target_include_directories(${TARGET} PUBLIC include)
+target_include_directories(${TARGET} PUBLIC SYSTEM ${GeographicLib_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS})
+target_link_libraries(${TARGET} Sophus::Sophus)
+
+# Camera corrector
+set(TARGET camera_particle_corrector_node)
+ament_auto_add_executable(${TARGET}
+ src/camera_corrector/camera_particle_corrector_node.cpp
+ src/camera_corrector/filt_lsd.cpp
+ src/camera_corrector/logit.cpp
+ src/camera_corrector/camera_particle_corrector_core.cpp)
+target_include_directories(${TARGET} PUBLIC include)
+target_include_directories(${TARGET} SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS})
+target_link_libraries(${TARGET} Sophus::Sophus ${PCL_LIBRARIES} glog::glog)
+
+# ===================================================
+# TEST
+if(BUILD_TESTING)
+ add_subdirectory(test)
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE config)
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/README.md b/localization/yabloc/yabloc_particle_filter/README.md
similarity index 68%
rename from localization/yabloc/particle_filter/modularized_particle_filter/README.md
rename to localization/yabloc/yabloc_particle_filter/README.md
index a14ba0b131efb..7fe0f925c7c63 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/README.md
+++ b/localization/yabloc/yabloc_particle_filter/README.md
@@ -1,4 +1,4 @@
-# Modularized Particle Filter
+# YabLoc Particle Filter
## Purpose
@@ -9,11 +9,12 @@
## How to test
```shell
-colcon test --event-handlers console_cohesion+ --packages-select modularized_particle_filter
+colcon test --event-handlers console_cohesion+ --packages-select yabloc_particle_filter
colcon test-result --verbose --all
```
-## Predictor
+
+## Predictor node
### Input
@@ -22,14 +23,14 @@ colcon test-result --verbose --all
| `/initialpose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | to specity the initial position of particles |
| `/twist` | `geometry_msgs::msg::TwistStamped` | linear velocity and angular velocity of prediction update |
| `/twist_cov` | `geometry_msgs::msg::TwistWithCovarianceStamped` | linear velocity and angular velocity of prediction update |
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted by corrector nodes |
+| `/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by corrector nodes |
| `/height` | `std_msgs::msg::Float32` | ground height |
### Output
| Name | Type | Description |
|-------------------------------|--------------------------------------------------------|---------------------------------------|
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted by predictor nodes |
+| `/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by predictor nodes |
| `/predicted_particles_marker` | `visualization_msgs::msg::MarkerArray` | markers for particle visualization |
| `/pose` | `geometry_msgs::msg::PoseStamped` | weighted mean of particles |
| `/pose_with_covariance` | `geometry_msgs::msg::PoseWithCovarianceStamped` | weighted mean of particles |
@@ -46,22 +47,7 @@ colcon test-result --verbose --all
| `static_angular_covariance` | double | 0.01 | to override the covariance of `/twist`. When using `/twist_cov`, it has no effect |
| `cov_xx_yy` | list | [2.0,0.25] | the covariance of initial pose |
-## Corrector
-
-### Input
-
-| Name | Type | Description |
-|------------------------|--------------------------------------------------------|-------------------------------------------|
-| `/predicted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles predicted by the predictor node |
-
-### Output
-
-| Name | Type | Description |
-|-----------------------|--------------------------------------------------------|------------------------------------------|
-| `/weighted_particles` | `modularized_particle_filter_msgs::msg::ParticleArray` | particles weighted by the corrector node |
-
-### Parameters
-| Name | Type | Default | Description |
-|--------------|------|---------|-------------------------------------------------------------------|
-| `/visualize` | bool | false | whether particles are also published in visualization_msgs or not |
+## Other nodes
+- [camera_corrector](docs/camera_corrector.md)
+- [gnss_corrector](docs/gnss_corrector.md)
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/config/camera_particle_corrector.param.yaml b/localization/yabloc/yabloc_particle_filter/config/camera_particle_corrector.param.yaml
similarity index 100%
rename from localization/yabloc/particle_filter/camera_particle_corrector/config/camera_particle_corrector.param.yaml
rename to localization/yabloc/yabloc_particle_filter/config/camera_particle_corrector.param.yaml
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/config/gnss_particle_corrector.param.yaml b/localization/yabloc/yabloc_particle_filter/config/gnss_particle_corrector.param.yaml
similarity index 100%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/config/gnss_particle_corrector.param.yaml
rename to localization/yabloc/yabloc_particle_filter/config/gnss_particle_corrector.param.yaml
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/config/predictor.param.yaml b/localization/yabloc/yabloc_particle_filter/config/predictor.param.yaml
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter/config/predictor.param.yaml
rename to localization/yabloc/yabloc_particle_filter/config/predictor.param.yaml
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/README.md b/localization/yabloc/yabloc_particle_filter/docs/camera_corrector.md
similarity index 97%
rename from localization/yabloc/particle_filter/camera_particle_corrector/README.md
rename to localization/yabloc/yabloc_particle_filter/docs/camera_corrector.md
index a57a9f9a228f7..50a4b516105fe 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/README.md
+++ b/localization/yabloc/yabloc_particle_filter/docs/camera_corrector.md
@@ -34,4 +34,4 @@
| `max_range` | float | 80 | width of heararchical cost map |
| `gamma` | float | 40.0 | gamma value of the intensity gradient of the cost map |
| `min_prob` | float | 0.1 | minimum particle weight the corrector node gives |
-| `far_weight_gain` | float | 0.001 | `exp(-far_weight_gain_ * squared_distance_from_camera)` is reflected in the weight (If this is large, the nearby landmarks will be more important.)|
+| `far_weight_gain` | float | 0.001 | `exp(-far_weight_gain_ * squared_distance_from_camera)` is reflected in the weight (If this is large, the nearby landmarks will be more important.)|
\ No newline at end of file
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/README.md b/localization/yabloc/yabloc_particle_filter/docs/gnss_corrector.md
similarity index 65%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/README.md
rename to localization/yabloc/yabloc_particle_filter/docs/gnss_corrector.md
index 860edfa55c177..892df47fce65b 100644
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/README.md
+++ b/localization/yabloc/yabloc_particle_filter/docs/gnss_corrector.md
@@ -1,3 +1,24 @@
+## GNSS Corrector
+
+### Input
+
+| Name | Type | Description |
+|------------------------|--------------------------------------------------------|-------------------------------------------|
+| `/predicted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles predicted by the predictor node |
+
+### Output
+
+| Name | Type | Description |
+|-----------------------|--------------------------------------------------------|------------------------------------------|
+| `/weighted_particles` | `yabloc_particle_filter::msg::ParticleArray` | particles weighted by the corrector node |
+
+### Parameters
+
+| Name | Type | Default | Description |
+|--------------|------|---------|-------------------------------------------------------------------|
+| `/visualize` | bool | false | whether particles are also published in visualization_msgs or not |
+
+
# GNSS Particle Corrector
## Purpose
@@ -30,4 +51,4 @@
| Name | Type | Default | Description |
|-----------------------------------|-------|---------|-------------------------------------------------------------------------------------------------------|
| `/ignore_less_than_float` | bool | true | if this is true, only FIX or FLOAT is used for correction (No effect when using pose_with_covariance) |
-| `/mahalanobis_distance_threshold` | float | 20.f | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. |
\ No newline at end of file
+| `/mahalanobis_distance_threshold` | float | 20.f | if the Mahalanobis distance to the GNSS for particle exceeds this, the correction skips. |
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/camera_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp
similarity index 93%
rename from localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/camera_particle_corrector.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp
index d77b21e184ec8..061db3bc589aa 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/camera_particle_corrector.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp
@@ -14,10 +14,10 @@
#pragma once
-#include
-#include
#include
#include
+#include
+#include
#include
#include
@@ -82,7 +82,8 @@ class CameraParticleCorrector : public modularized_particle_filter::AbstCorrecto
std::pair split_line_segments(const PointCloud2 & msg);
- float compute_logit(const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position);
+ float compute_logit(
+ const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position);
pcl::PointCloud evaluate_cloud(
const LineSegments & line_segments_cloud, const Eigen::Vector3f & self_position);
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/fast_cos.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/fast_cos.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/fast_cos.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/fast_cos.hpp
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/logit.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/camera_particle_corrector/include/camera_particle_corrector/logit.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector/logit.hpp
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/mean.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp
similarity index 73%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/mean.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp
index 3388ba72dfed6..e8c243b59c268 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/mean.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/mean.hpp
@@ -15,22 +15,22 @@
#ifndef MODULARIZED_PARTICLE_FILTER__COMMON__MEAN_HPP_
#define MODULARIZED_PARTICLE_FILTER__COMMON__MEAN_HPP_
-#include
+#include
+#include
#include
-#include
namespace yabloc
{
namespace modularized_particle_filter
{
geometry_msgs::msg::Pose mean_pose(
- const modularized_particle_filter_msgs::msg::ParticleArray & particle_array);
+ const yabloc_particle_filter::msg::ParticleArray & particle_array);
Eigen::Matrix3f std_of_distribution(
- const modularized_particle_filter_msgs::msg::ParticleArray & particle_array);
+ const yabloc_particle_filter::msg::ParticleArray & particle_array);
-float std_of_weight(const modularized_particle_filter_msgs::msg::ParticleArray & particle_array);
+float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array);
} // namespace modularized_particle_filter
} // namespace yabloc
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/prediction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp
similarity index 98%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/prediction_util.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp
index b48f8dd249229..33f5ada7ef294 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/prediction_util.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/prediction_util.hpp
@@ -15,7 +15,7 @@
#ifndef MODULARIZED_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_
#define MODULARIZED_PARTICLE_FILTER__COMMON__PREDICTION_UTIL_HPP_
-#include
+#include
#include
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/visualize.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp
similarity index 86%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/visualize.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp
index c661fed475058..55c8a4a99a3f5 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/common/visualize.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/common/visualize.hpp
@@ -16,8 +16,8 @@
#define MODULARIZED_PARTICLE_FILTER__COMMON__VISUALIZE_HPP_
#include
+#include
-#include
#include
#include
@@ -29,8 +29,8 @@ class ParticleVisualizer
{
public:
using MarkerArray = visualization_msgs::msg::MarkerArray;
- using Particle = modularized_particle_filter_msgs::msg::Particle;
- using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+ using Particle = yabloc_particle_filter::msg::Particle;
+ using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
ParticleVisualizer(rclcpp::Node & node);
void publish(const ParticleArray & msg);
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/abst_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abst_corrector.hpp
similarity index 83%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/abst_corrector.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abst_corrector.hpp
index 14000bc663a7d..528470117db53 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/abst_corrector.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/abst_corrector.hpp
@@ -15,12 +15,11 @@
#ifndef MODULARIZED_PARTICLE_FILTER__CORRECTION__ABST_CORRECTOR_HPP_
#define MODULARIZED_PARTICLE_FILTER__CORRECTION__ABST_CORRECTOR_HPP_
-#include "modularized_particle_filter/common/mean.hpp"
-#include "modularized_particle_filter/common/visualize.hpp"
+#include "yabloc_particle_filter/common/mean.hpp"
+#include "yabloc_particle_filter/common/visualize.hpp"
#include
-
-#include
+#include
#include
@@ -31,8 +30,8 @@ namespace modularized_particle_filter
class AbstCorrector : public rclcpp::Node
{
public:
- using Particle = modularized_particle_filter_msgs::msg::Particle;
- using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+ using Particle = yabloc_particle_filter::msg::Particle;
+ using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
AbstCorrector(const std::string & node_name);
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/correction_util.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp
similarity index 79%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/correction_util.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp
index 7b3c7d2368ef5..4ab29cad36766 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/correction/correction_util.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/correction/correction_util.hpp
@@ -16,8 +16,7 @@
#define MODULARIZED_PARTICLE_FILTER__CORRECTION__CORRECTION_UTIL_HPP_
#include "rclcpp/rclcpp.hpp"
-
-#include "modularized_particle_filter_msgs/msg/particle_array.hpp"
+#include "yabloc_particle_filter/msg/particle_array.hpp"
#include
@@ -27,8 +26,8 @@ namespace yabloc
{
namespace modularized_particle_filter
{
-std::optional find_synced_particles(
- boost::circular_buffer circular_buffer,
+std::optional find_synced_particles(
+ boost::circular_buffer circular_buffer,
rclcpp::Time time);
}
} // namespace yabloc
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp
similarity index 95%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp
index 2f175e8ff7942..4aa47e6d76d80 100644
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/gnss_particle_corrector.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp
@@ -15,10 +15,10 @@
#ifndef GNSS_PARTILCE_CORRECTOR__GNSS_PARTICLE_CORRECTOR_HPP_
#define GNSS_PARTILCE_CORRECTOR__GNSS_PARTICLE_CORRECTOR_HPP_
-#include "gnss_particle_corrector/weight_manager.hpp"
+#include "yabloc_particle_filter/gnss_corrector/weight_manager.hpp"
#include
-#include
+#include
#include
#include
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/weight_manager.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/include/gnss_particle_corrector/weight_manager.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector/weight_manager.hpp
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/include/ll2_cost_map/direct_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/ll2_cost_map/include/ll2_cost_map/direct_cost_map.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/include/ll2_cost_map/hierarchical_cost_map.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/ll2_cost_map/include/ll2_cost_map/hierarchical_cost_map.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/experimental/suspension_adaptor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/experimental/suspension_adaptor.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/experimental/suspension_adaptor.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/experimental/suspension_adaptor.hpp
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/predictor.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp
similarity index 91%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/predictor.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp
index 8e0de264b2583..a8a047575d730 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/predictor.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/predictor.hpp
@@ -15,18 +15,18 @@
#ifndef MODULARIZED_PARTICLE_FILTER__PREDICTION__PREDICTOR_HPP_
#define MODULARIZED_PARTICLE_FILTER__PREDICTION__PREDICTOR_HPP_
-#include "modularized_particle_filter/common/visualize.hpp"
-#include "modularized_particle_filter/prediction/experimental/suspension_adaptor.hpp"
-#include "modularized_particle_filter/prediction/resampler.hpp"
+#include "yabloc_particle_filter/common/visualize.hpp"
+#include "yabloc_particle_filter/prediction/experimental/suspension_adaptor.hpp"
+#include "yabloc_particle_filter/prediction/resampler.hpp"
#include
#include
+#include
#include
#include
#include
#include
-#include
#include
#include
@@ -38,7 +38,7 @@ class Predictor : public rclcpp::Node
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
- using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+ using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
using PoseStamped = geometry_msgs::msg::PoseStamped;
using PoseCovStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
using TwistCovStamped = geometry_msgs::msg::TwistWithCovarianceStamped;
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/resampler.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp
similarity index 88%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/resampler.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp
index 382e85b9d32b1..a8ff634b23af0 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/resampler.hpp
+++ b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampler.hpp
@@ -15,12 +15,11 @@
#ifndef MODULARIZED_PARTICLE_FILTER__PREDICTION__RESAMPLER_HPP_
#define MODULARIZED_PARTICLE_FILTER__PREDICTION__RESAMPLER_HPP_
-#include "modularized_particle_filter/prediction/resampling_history.hpp"
+#include "yabloc_particle_filter/msg/particle_array.hpp"
+#include "yabloc_particle_filter/prediction/resampling_history.hpp"
#include
-#include "modularized_particle_filter_msgs/msg/particle_array.hpp"
-
namespace yabloc::modularized_particle_filter
{
class resampling_skip_exception : public std::runtime_error
@@ -32,8 +31,8 @@ class resampling_skip_exception : public std::runtime_error
class RetroactiveResampler
{
public:
- using Particle = modularized_particle_filter_msgs::msg::Particle;
- using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+ using Particle = yabloc_particle_filter::msg::Particle;
+ using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
RetroactiveResampler(int number_of_particles, int max_history_num);
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/resampling_history.hpp b/localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter/include/modularized_particle_filter/prediction/resampling_history.hpp
rename to localization/yabloc/yabloc_particle_filter/include/yabloc_particle_filter/prediction/resampling_history.hpp
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/msg/Particle.msg b/localization/yabloc/yabloc_particle_filter/msg/Particle.msg
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter_msgs/msg/Particle.msg
rename to localization/yabloc/yabloc_particle_filter/msg/Particle.msg
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter_msgs/msg/ParticleArray.msg b/localization/yabloc/yabloc_particle_filter/msg/ParticleArray.msg
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter_msgs/msg/ParticleArray.msg
rename to localization/yabloc/yabloc_particle_filter/msg/ParticleArray.msg
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/package.xml b/localization/yabloc/yabloc_particle_filter/package.xml
similarity index 76%
rename from localization/yabloc/particle_filter/modularized_particle_filter/package.xml
rename to localization/yabloc/yabloc_particle_filter/package.xml
index b7a9e0b111a96..ecdab9117849f 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/package.xml
+++ b/localization/yabloc/yabloc_particle_filter/package.xml
@@ -1,9 +1,9 @@
- modularized_particle_filter
+ yabloc_particle_filter
0.1.0
- The modularized_particle_filter package
+ The particle filter package
Kento Yabuuchi
Apache License 2.0
@@ -14,18 +14,22 @@
tf2
tf2_ros
std_msgs
+ std_srvs
geometry_msgs
sensor_msgs
visualization_msgs
tf2_sensor_msgs
tf2_geometry_msgs
+ libgoogle-glog-dev
sophus
yabloc_common
- modularized_particle_filter_msgs
ament_cmake_gtest
+ rosidl_default_runtime
+ rosidl_interface_packages
+
ament_cmake
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp
similarity index 98%
rename from localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_core.cpp
rename to localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp
index f2ed64c774ca5..7415c9b46b609 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_core.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_core.cpp
@@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "camera_particle_corrector/camera_particle_corrector.hpp"
-#include "camera_particle_corrector/fast_cos.hpp"
-#include "camera_particle_corrector/logit.hpp"
+#include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp"
+#include "yabloc_particle_filter/camera_corrector/fast_cos.hpp"
+#include "yabloc_particle_filter/camera_corrector/logit.hpp"
#include
#include
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp
similarity index 91%
rename from localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_node.cpp
rename to localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp
index ec9580e63b85a..eadbb3f294124 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/src/camera_particle_corrector_node.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/camera_particle_corrector_node.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "camera_particle_corrector/camera_particle_corrector.hpp"
+#include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp"
#include
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/src/filt_lsd.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_lsd.cpp
similarity index 97%
rename from localization/yabloc/particle_filter/camera_particle_corrector/src/filt_lsd.cpp
rename to localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_lsd.cpp
index bb2574f546b64..8ef194e3385f8 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/src/filt_lsd.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/filt_lsd.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "camera_particle_corrector/camera_particle_corrector.hpp"
+#include "yabloc_particle_filter/camera_corrector/camera_particle_corrector.hpp"
#include
#include
diff --git a/localization/yabloc/particle_filter/camera_particle_corrector/src/logit.cpp b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp
similarity index 95%
rename from localization/yabloc/particle_filter/camera_particle_corrector/src/logit.cpp
rename to localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp
index 2188f52d88ae8..c03a33368fb9b 100644
--- a/localization/yabloc/particle_filter/camera_particle_corrector/src/logit.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/camera_corrector/logit.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "camera_particle_corrector/logit.hpp"
+#include "yabloc_particle_filter/camera_corrector/logit.hpp"
#include
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/mean.cpp b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp
similarity index 87%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/common/mean.cpp
rename to localization/yabloc/yabloc_particle_filter/src/common/mean.cpp
index 77d7743a75986..e67c248b4d25b 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/mean.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/common/mean.cpp
@@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/common/mean.hpp"
+#include "yabloc_particle_filter/common/mean.hpp"
-#include
+#include
#include
#include
#include
@@ -44,10 +44,10 @@ double mean_radian(const std::vector & angles, const std::vector
} // namespace
geometry_msgs::msg::Pose mean_pose(
- const modularized_particle_filter_msgs::msg::ParticleArray & particle_array)
+ const yabloc_particle_filter::msg::ParticleArray & particle_array)
{
using Pose = geometry_msgs::msg::Pose;
- using Particle = modularized_particle_filter_msgs::msg::Particle;
+ using Particle = yabloc_particle_filter::msg::Particle;
Pose mean_pose;
@@ -89,10 +89,9 @@ geometry_msgs::msg::Pose mean_pose(
return mean_pose;
}
-Eigen::Matrix3f std_of_distribution(
- const modularized_particle_filter_msgs::msg::ParticleArray & array)
+Eigen::Matrix3f std_of_distribution(const yabloc_particle_filter::msg::ParticleArray & array)
{
- using Particle = modularized_particle_filter_msgs::msg::Particle;
+ using Particle = yabloc_particle_filter::msg::Particle;
auto ori = mean_pose(array).orientation;
Eigen::Quaternionf orientation(ori.w, ori.x, ori.y, ori.z);
float invN = 1.f / array.particles.size();
@@ -114,9 +113,9 @@ Eigen::Matrix3f std_of_distribution(
return sigma;
}
-float std_of_weight(const modularized_particle_filter_msgs::msg::ParticleArray & particle_array)
+float std_of_weight(const yabloc_particle_filter::msg::ParticleArray & particle_array)
{
- using Particle = modularized_particle_filter_msgs::msg::Particle;
+ using Particle = yabloc_particle_filter::msg::Particle;
const float invN = 1.f / particle_array.particles.size();
float mean = 0;
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/particle_visualize_node.cpp b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp
similarity index 94%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/common/particle_visualize_node.cpp
rename to localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp
index 6b7a187656bb1..efe84cec7fb6c 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/particle_visualize_node.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/common/particle_visualize_node.cpp
@@ -15,8 +15,8 @@
#include "yabloc_common/color.hpp"
#include
+#include
-#include
#include
#include
@@ -25,8 +25,8 @@ namespace yabloc::modularized_particle_fitler
class ParticleVisualize : public rclcpp::Node
{
public:
- using Particle = modularized_particle_filter_msgs::msg::Particle;
- using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+ using Particle = yabloc_particle_filter::msg::Particle;
+ using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
using Marker = visualization_msgs::msg::Marker;
using MarkerArray = visualization_msgs::msg::MarkerArray;
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/visualize.cpp b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp
similarity index 97%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/common/visualize.cpp
rename to localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp
index a30c48f6f5e3f..f4c34d5b5ab6e 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/common/visualize.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/common/visualize.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/common/visualize.hpp"
+#include "yabloc_particle_filter/common/visualize.hpp"
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/correction/abst_corrector.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/abst_corrector.cpp
similarity index 97%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/correction/abst_corrector.cpp
rename to localization/yabloc/yabloc_particle_filter/src/correction/abst_corrector.cpp
index d1f52e4109962..8de7c1417d58f 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/correction/abst_corrector.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/correction/abst_corrector.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/correction/abst_corrector.hpp"
+#include "yabloc_particle_filter/correction/abst_corrector.hpp"
namespace yabloc::modularized_particle_filter
{
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/correction/correction_util.cpp b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp
similarity index 83%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/correction/correction_util.cpp
rename to localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp
index 72718a3ed9923..dfe3ea27b87ba 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/correction/correction_util.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/correction/correction_util.cpp
@@ -12,12 +12,12 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/correction/correction_util.hpp"
+#include "yabloc_particle_filter/correction/correction_util.hpp"
namespace yabloc::modularized_particle_filter
{
-std::optional find_synced_particles(
- boost::circular_buffer circular_buffer,
+std::optional find_synced_particles(
+ boost::circular_buffer circular_buffer,
rclcpp::Time time)
{
for (int i{1}; i < static_cast(circular_buffer.size()); i++) {
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp
similarity index 99%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp
rename to localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp
index a1047953541d7..a459b85563557 100644
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_core.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_core.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "gnss_particle_corrector/gnss_particle_corrector.hpp"
+#include "yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp"
#include
#include
diff --git a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_node.cpp b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp
similarity index 91%
rename from localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_node.cpp
rename to localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp
index 7cb3cb3d3cba3..439e8a7657574 100644
--- a/localization/yabloc/particle_filter/gnss_particle_corrector/src/gnss_corrector_node.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/gnss_corrector/gnss_corrector_node.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "gnss_particle_corrector/gnss_particle_corrector.hpp"
+#include "yabloc_particle_filter/gnss_corrector/gnss_particle_corrector.hpp"
int main(int argc, char * argv[])
{
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/src/direct_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp
similarity index 98%
rename from localization/yabloc/particle_filter/ll2_cost_map/src/direct_cost_map.cpp
rename to localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp
index 7dcce68d9e2af..f39aa6ceaf25d 100644
--- a/localization/yabloc/particle_filter/ll2_cost_map/src/direct_cost_map.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/direct_cost_map.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "ll2_cost_map/direct_cost_map.hpp"
+#include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp"
namespace yabloc
{
diff --git a/localization/yabloc/particle_filter/ll2_cost_map/src/hierarchical_cost_map.cpp b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp
similarity index 98%
rename from localization/yabloc/particle_filter/ll2_cost_map/src/hierarchical_cost_map.cpp
rename to localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp
index b33c3cbbc596e..0abc109113441 100644
--- a/localization/yabloc/particle_filter/ll2_cost_map/src/hierarchical_cost_map.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/ll2_cost_map/hierarchical_cost_map.cpp
@@ -12,9 +12,9 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "ll2_cost_map/hierarchical_cost_map.hpp"
+#include "yabloc_particle_filter/ll2_cost_map/hierarchical_cost_map.hpp"
-#include "ll2_cost_map/direct_cost_map.hpp"
+#include "yabloc_particle_filter/ll2_cost_map/direct_cost_map.hpp"
#include
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp
similarity index 97%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor.cpp
rename to localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp
index 9486ec9936395..479e62314bc6e 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor.cpp
@@ -12,11 +12,11 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/prediction/predictor.hpp"
+#include "yabloc_particle_filter/prediction/predictor.hpp"
-#include "modularized_particle_filter/common/mean.hpp"
-#include "modularized_particle_filter/common/prediction_util.hpp"
-#include "modularized_particle_filter/prediction/resampler.hpp"
+#include "yabloc_particle_filter/common/mean.hpp"
+#include "yabloc_particle_filter/common/prediction_util.hpp"
+#include "yabloc_particle_filter/prediction/resampler.hpp"
#include
#include
@@ -98,7 +98,7 @@ void Predictor::on_initial_pose(const PoseCovStamped::ConstSharedPtr initialpose
void Predictor::initialize_particles(const PoseCovStamped & initialpose)
{
RCLCPP_INFO_STREAM(this->get_logger(), "initialize_particles");
- modularized_particle_filter_msgs::msg::ParticleArray particle_array{};
+ yabloc_particle_filter::msg::ParticleArray particle_array{};
particle_array.header = initialpose.header;
particle_array.id = 0;
particle_array.particles.resize(number_of_particles_);
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor_node.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp
similarity index 92%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor_node.cpp
rename to localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp
index bb87da5e8f01f..81c756908b0f8 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/predictor_node.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/prediction/predictor_node.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/prediction/predictor.hpp"
+#include "yabloc_particle_filter/prediction/predictor.hpp"
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/resampler.cpp b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp
similarity index 98%
rename from localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/resampler.cpp
rename to localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp
index f2cb5eeec13c5..44f98f3e6d469 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/src/prediction/resampler.cpp
+++ b/localization/yabloc/yabloc_particle_filter/src/prediction/resampler.cpp
@@ -12,7 +12,7 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/prediction/resampler.hpp"
+#include "yabloc_particle_filter/prediction/resampler.hpp"
#include
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/test/CMakeLists.txt b/localization/yabloc/yabloc_particle_filter/test/CMakeLists.txt
similarity index 100%
rename from localization/yabloc/particle_filter/modularized_particle_filter/test/CMakeLists.txt
rename to localization/yabloc/yabloc_particle_filter/test/CMakeLists.txt
diff --git a/localization/yabloc/particle_filter/modularized_particle_filter/test/src/test_resampler.cpp b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp
similarity index 96%
rename from localization/yabloc/particle_filter/modularized_particle_filter/test/src/test_resampler.cpp
rename to localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp
index 81e3695d2f1e0..4d823a36e0d94 100644
--- a/localization/yabloc/particle_filter/modularized_particle_filter/test/src/test_resampler.cpp
+++ b/localization/yabloc/yabloc_particle_filter/test/src/test_resampler.cpp
@@ -12,15 +12,15 @@
// See the License for the specific language governing permissions and
// limitations under the License.
-#include "modularized_particle_filter/prediction/resampler.hpp"
+#include "yabloc_particle_filter/prediction/resampler.hpp"
#include
#include
namespace mpf = yabloc::modularized_particle_filter;
-using Particle = modularized_particle_filter_msgs::msg::Particle;
-using ParticleArray = modularized_particle_filter_msgs::msg::ParticleArray;
+using Particle = yabloc_particle_filter::msg::Particle;
+using ParticleArray = yabloc_particle_filter::msg::ParticleArray;
constexpr int PARTICLE_COUNT = 10;
constexpr int HISTORY_SIZE = 10;