From 8da4017a9df6a7db1ee92afdf3d6c912fc19ea29 Mon Sep 17 00:00:00 2001 From: Kento Yabuuchi Date: Mon, 29 May 2023 15:03:35 +0900 Subject: [PATCH] feature(pf): combine some packages related to particle filter (#59) * create yabloc_particle_filter Signed-off-by: Kento Yabuuchi * combine gnss_particle_corrector Signed-off-by: Kento Yabuuchi * combine ll2_cost_map Signed-off-by: Kento Yabuuchi * combine camera_particle_corrector Signed-off-by: Kento Yabuuchi * fix launch files Signed-off-by: Kento Yabuuchi * split README & remove obsolete scripts Signed-off-by: Kento Yabuuchi * fix config path of multi_camera mode Signed-off-by: Kento Yabuuchi --------- Signed-off-by: Kento Yabuuchi --- .../camera_particle_corrector/CMakeLists.txt | 45 ------- .../camera_particle_corrector/package.xml | 29 ----- .../gnss_particle_corrector/CMakeLists.txt | 43 ------- .../gnss_particle_corrector/package.xml | 26 ---- .../ll2_cost_map/CMakeLists.txt | 38 ------ .../particle_filter/ll2_cost_map/package.xml | 28 ----- .../CMakeLists.txt | 67 ---------- .../launch/run.launch.xml | 20 --- .../script/particle_array_to_marker_array.py | 81 ------------ .../script/particle_array_to_pose_array.py | 50 -------- .../CMakeLists.txt | 24 ---- .../package.xml | 23 ---- .../yabloc_launch/launch/impl/pf.launch.xml | 6 +- .../yabloc_launch/launch/yabloc.launch.xml | 6 +- .../launch/yabloc_multi_camera.launch.xml | 8 +- localization/yabloc/yabloc_launch/package.xml | 6 +- .../yabloc_particle_filter/CMakeLists.txt | 118 ++++++++++++++++++ .../README.md | 32 ++--- .../camera_particle_corrector.param.yaml | 0 .../config/gnss_particle_corrector.param.yaml | 0 .../config/predictor.param.yaml | 0 .../docs/camera_corrector.md} | 2 +- .../docs/gnss_corrector.md} | 23 +++- .../camera_particle_corrector.hpp | 7 +- .../camera_corrector}/fast_cos.hpp | 0 .../camera_corrector}/logit.hpp | 0 .../yabloc_particle_filter}/common/mean.hpp | 10 +- .../common/prediction_util.hpp | 2 +- .../common/visualize.hpp | 6 +- .../correction/abst_corrector.hpp | 11 +- .../correction/correction_util.hpp | 7 +- .../gnss_particle_corrector.hpp | 4 +- .../gnss_corrector}/weight_manager.hpp | 0 .../ll2_cost_map/direct_cost_map.hpp | 0 .../ll2_cost_map/hierarchical_cost_map.hpp | 0 .../experimental/suspension_adaptor.hpp | 0 .../prediction/predictor.hpp | 10 +- .../prediction/resampler.hpp | 9 +- .../prediction/resampling_history.hpp | 0 .../msg/Particle.msg | 0 .../msg/ParticleArray.msg | 0 .../package.xml | 10 +- .../camera_particle_corrector_core.cpp | 6 +- .../camera_particle_corrector_node.cpp | 2 +- .../src/camera_corrector}/filt_lsd.cpp | 2 +- .../src/camera_corrector}/logit.cpp | 2 +- .../src/common/mean.cpp | 17 ++- .../src/common/particle_visualize_node.cpp | 6 +- .../src/common/visualize.cpp | 2 +- .../src/correction/abst_corrector.cpp | 2 +- .../src/correction/correction_util.cpp | 6 +- .../gnss_corrector}/gnss_corrector_core.cpp | 2 +- .../gnss_corrector}/gnss_corrector_node.cpp | 2 +- .../src/ll2_cost_map}/direct_cost_map.cpp | 2 +- .../ll2_cost_map}/hierarchical_cost_map.cpp | 4 +- .../src/prediction/predictor.cpp | 10 +- .../src/prediction/predictor_node.cpp | 2 +- .../src/prediction/resampler.cpp | 2 +- .../test/CMakeLists.txt | 0 .../test/src/test_resampler.cpp | 6 +- 60 files changed, 238 insertions(+), 588 deletions(-) delete mode 100644 localization/yabloc/particle_filter/camera_particle_corrector/CMakeLists.txt delete mode 100644 localization/yabloc/particle_filter/camera_particle_corrector/package.xml delete mode 100644 localization/yabloc/particle_filter/gnss_particle_corrector/CMakeLists.txt delete mode 100644 localization/yabloc/particle_filter/gnss_particle_corrector/package.xml delete mode 100644 localization/yabloc/particle_filter/ll2_cost_map/CMakeLists.txt delete mode 100644 localization/yabloc/particle_filter/ll2_cost_map/package.xml delete mode 100644 localization/yabloc/particle_filter/modularized_particle_filter/CMakeLists.txt delete mode 100644 localization/yabloc/particle_filter/modularized_particle_filter/launch/run.launch.xml delete mode 100755 localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_marker_array.py delete mode 100755 localization/yabloc/particle_filter/modularized_particle_filter/script/particle_array_to_pose_array.py delete mode 100644 localization/yabloc/particle_filter/modularized_particle_filter_msgs/CMakeLists.txt delete mode 100644 localization/yabloc/particle_filter/modularized_particle_filter_msgs/package.xml create mode 100644 localization/yabloc/yabloc_particle_filter/CMakeLists.txt rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/README.md (68%) rename localization/yabloc/{particle_filter/camera_particle_corrector => yabloc_particle_filter}/config/camera_particle_corrector.param.yaml (100%) rename localization/yabloc/{particle_filter/gnss_particle_corrector => yabloc_particle_filter}/config/gnss_particle_corrector.param.yaml (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/config/predictor.param.yaml (100%) rename localization/yabloc/{particle_filter/camera_particle_corrector/README.md => yabloc_particle_filter/docs/camera_corrector.md} (97%) rename localization/yabloc/{particle_filter/gnss_particle_corrector/README.md => yabloc_particle_filter/docs/gnss_corrector.md} (65%) rename localization/yabloc/{particle_filter/camera_particle_corrector/include/camera_particle_corrector => yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector}/camera_particle_corrector.hpp (93%) rename localization/yabloc/{particle_filter/camera_particle_corrector/include/camera_particle_corrector => yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector}/fast_cos.hpp (100%) rename localization/yabloc/{particle_filter/camera_particle_corrector/include/camera_particle_corrector => yabloc_particle_filter/include/yabloc_particle_filter/camera_corrector}/logit.hpp (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/common/mean.hpp (73%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/common/prediction_util.hpp (98%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/common/visualize.hpp (86%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/correction/abst_corrector.hpp (83%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/correction/correction_util.hpp (79%) rename localization/yabloc/{particle_filter/gnss_particle_corrector/include/gnss_particle_corrector => yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector}/gnss_particle_corrector.hpp (95%) rename localization/yabloc/{particle_filter/gnss_particle_corrector/include/gnss_particle_corrector => yabloc_particle_filter/include/yabloc_particle_filter/gnss_corrector}/weight_manager.hpp (100%) rename localization/yabloc/{particle_filter/ll2_cost_map/include => yabloc_particle_filter/include/yabloc_particle_filter}/ll2_cost_map/direct_cost_map.hpp (100%) rename localization/yabloc/{particle_filter/ll2_cost_map/include => yabloc_particle_filter/include/yabloc_particle_filter}/ll2_cost_map/hierarchical_cost_map.hpp (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/prediction/experimental/suspension_adaptor.hpp (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/prediction/predictor.hpp (91%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/prediction/resampler.hpp (88%) rename localization/yabloc/{particle_filter/modularized_particle_filter/include/modularized_particle_filter => yabloc_particle_filter/include/yabloc_particle_filter}/prediction/resampling_history.hpp (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter_msgs => yabloc_particle_filter}/msg/Particle.msg (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter_msgs => yabloc_particle_filter}/msg/ParticleArray.msg (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/package.xml (76%) rename localization/yabloc/{particle_filter/camera_particle_corrector/src => yabloc_particle_filter/src/camera_corrector}/camera_particle_corrector_core.cpp (98%) rename localization/yabloc/{particle_filter/camera_particle_corrector/src => yabloc_particle_filter/src/camera_corrector}/camera_particle_corrector_node.cpp (91%) rename localization/yabloc/{particle_filter/camera_particle_corrector/src => yabloc_particle_filter/src/camera_corrector}/filt_lsd.cpp (97%) rename localization/yabloc/{particle_filter/camera_particle_corrector/src => yabloc_particle_filter/src/camera_corrector}/logit.cpp (95%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/common/mean.cpp (87%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/common/particle_visualize_node.cpp (94%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/common/visualize.cpp (97%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/correction/abst_corrector.cpp (97%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/correction/correction_util.cpp (83%) rename localization/yabloc/{particle_filter/gnss_particle_corrector/src => yabloc_particle_filter/src/gnss_corrector}/gnss_corrector_core.cpp (99%) rename localization/yabloc/{particle_filter/gnss_particle_corrector/src => yabloc_particle_filter/src/gnss_corrector}/gnss_corrector_node.cpp (91%) rename localization/yabloc/{particle_filter/ll2_cost_map/src => yabloc_particle_filter/src/ll2_cost_map}/direct_cost_map.cpp (98%) rename localization/yabloc/{particle_filter/ll2_cost_map/src => yabloc_particle_filter/src/ll2_cost_map}/hierarchical_cost_map.cpp (98%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/prediction/predictor.cpp (97%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/prediction/predictor_node.cpp (92%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/src/prediction/resampler.cpp (98%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/test/CMakeLists.txt (100%) rename localization/yabloc/{particle_filter/modularized_particle_filter => yabloc_particle_filter}/test/src/test_resampler.cpp (96%) 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;