diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..a85b791 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,27 @@ +## Jira Link: + +## Description + +Describe problems, if any, clearly and concisely. +Describe any changes that have been made in this pull request. + +## Type of Change + +- [ ] Bugfix +- [ ] Enhancement +- [ ] New feature +- [ ] Breaking change (fix or feature that would cause the existing functionality to not work as expected) + +## How Has This Been Tested? + +- [ ] New unit tests added. +- [ ] Manual tested. + +## Checklist: + +- [ ] Using Branch Name Convention + - `feature/JIRA-ID-SHORT-DESCRIPTION` if has a JIRA ticket + - `enhancement/SHORT-DESCRIPTION` if has/has no JIRA ticket and contain enhancement + - `hotfix/SHORT-DESCRIPTION` if the change doesn't need to be tested (urgent) +- [ ] I have commented on my code, particularly in hard-to-understand areas. +- [ ] I have made the documentation for the corresponding changes. \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml new file mode 100644 index 0000000..c0bef04 --- /dev/null +++ b/.github/workflows/ci.yml @@ -0,0 +1,21 @@ +name: CI +on: + workflow_dispatch: + pull_request: + push: + branches: [master] +jobs: + build-and-test: + name: Build and Test + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4.1.1 + with: + path: hurocup + + - name: Setup workspace + uses: ichiro-its/ros2-ws-action/setup@v1.0.1 + + - name: Build workspace + uses: ichiro-its/ros2-ws-action/build@v1.0.1 diff --git a/.github/workflows/dispatch-pr-bot.yml b/.github/workflows/dispatch-pr-bot.yml new file mode 100644 index 0000000..016b0cb --- /dev/null +++ b/.github/workflows/dispatch-pr-bot.yml @@ -0,0 +1,21 @@ +name: Dispatch Discord PR Bot + +on: + pull_request: + types: [opened, closed, reopened] + workflow_dispatch: + +jobs: + dispatch-discord-pr-bot: + runs-on: ubuntu-latest + steps: + - uses: peter-evans/repository-dispatch@v3 + with: + token: ${{ secrets.GH_TOKEN }} + repository: ichiro-its/${{ vars.DISCORD_PR_BOT_REPO }} + event-type: pull_request + client-payload: | + { + "repository": "${{ github.event.repository.name }}", + "pr_type": "${{ github.event.action || 'opened' }}" + } diff --git a/CMakeLists.txt b/CMakeLists.txt index 95e5b28..03e38d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,35 +1,93 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(basho) +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +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) + add_compile_options(-Wall -Wextra -Wpedantic -fPIC) endif() -# find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(aruku REQUIRED) +find_package(aruku_interfaces REQUIRED) +find_package(atama_interfaces REQUIRED) +find_package(gyakuenki_interfaces REQUIRED) +find_package(jitsuyo REQUIRED) +find_package(kansei REQUIRED) +find_package(kansei_interfaces REQUIRED) +find_package(keisan REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(basho_interfaces REQUIRED) +include_directories(${EIGEN3_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} SHARED + "src/${PROJECT_NAME}/config/node/config_node.cpp" + "src/${PROJECT_NAME}/node/basho_node.cpp" + "src/${PROJECT_NAME}/localization/mcl.cpp" + "src/${PROJECT_NAME}/localization/fusion.cpp" + "src/${PROJECT_NAME}/process/localization.cpp") + +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $) + +ament_target_dependencies(${PROJECT_NAME} + aruku + aruku_interfaces + atama_interfaces + gyakuenki_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + rclcpp + std_msgs + basho_interfaces) + +install(DIRECTORY "include" DESTINATION ".") + +install(TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION "lib" + LIBRARY DESTINATION "lib" + RUNTIME DESTINATION "bin") -add_executable(basho_node src/basho_node.cpp) -target_include_directories(basho_node PUBLIC +add_executable(main "src/basho_main.cpp") +target_include_directories(main PUBLIC $ - $) -target_compile_features(basho_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + $) +target_link_libraries(main ${PROJECT_NAME}) -install(TARGETS basho_node +install(TARGETS + main DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() +ament_export_dependencies( + ament_index_cpp + aruku + aruku_interfaces + atama_interfaces + gyakuenki_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + rclcpp + std_msgs + basho_interfaces) +ament_export_include_directories("include") +ament_export_libraries(${PROJECT_NAME}) ament_package() diff --git a/README.md b/README.md new file mode 100644 index 0000000..1cbad6e --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +# basho +Localization (MCL) Package for ROS 2 Project. diff --git a/include/basho/basho.hpp b/include/basho/basho.hpp new file mode 100644 index 0000000..d3910ce --- /dev/null +++ b/include/basho/basho.hpp @@ -0,0 +1,28 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__BASHO_HPP_ +#define BASHO__BASHO_HPP_ + +#include "basho/node/basho_node.hpp" +#include "basho/localization/mcl.hpp" +#include "basho/localization/fusion.hpp" + +#endif // BASHO__BASHO_HPP_ diff --git a/include/basho/config/config.hpp b/include/basho/config/config.hpp new file mode 100644 index 0000000..84cf12c --- /dev/null +++ b/include/basho/config/config.hpp @@ -0,0 +1,26 @@ +// Copyright (c) 2026 ICHIRO ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__CONFIG__CONFIG_HPP_ +#define BASHO__CONFIG__CONFIG_HPP_ + +#include "basho/config/node/config_node.hpp" + +#endif // BASHO__CONFIG__CONFIG_HPP_ diff --git a/include/basho/config/node/config_node.hpp b/include/basho/config/node/config_node.hpp new file mode 100644 index 0000000..155f41d --- /dev/null +++ b/include/basho/config/node/config_node.hpp @@ -0,0 +1,60 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ +#define BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ + +#include +#include + +#include "basho/process/localization.hpp" +#include "basho_interfaces/srv/set_config.hpp" +#include "basho_interfaces/srv/get_config.hpp" +#include "basho_interfaces/srv/save_config.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace basho +{ + +class ConfigNode +{ +public: + using GetConfig = basho_interfaces::srv::GetConfig; + using SaveConfig = basho_interfaces::srv::SaveConfig; + using SetConfig = basho_interfaces::srv::SetConfig; + + explicit ConfigNode(rclcpp::Node::SharedPtr node, const std::string & path, + const std::shared_ptr & localization); + +private: + std::string get_node_prefix() const; + + std::shared_ptr localization; + + rclcpp::Node::SharedPtr node; + + rclcpp::Service::SharedPtr get_config_server; + rclcpp::Service::SharedPtr save_config_server; + rclcpp::Service::SharedPtr set_config_server; +}; + +} // namespace basho + +#endif // BASHO__CONFIG__NODE__CONFIG_NODE_HPP_ diff --git a/include/basho/localization/fusion.hpp b/include/basho/localization/fusion.hpp new file mode 100644 index 0000000..0b8a5ff --- /dev/null +++ b/include/basho/localization/fusion.hpp @@ -0,0 +1,52 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__LOCALIZATION__FUSION_HPP_ +#define BASHO__LOCALIZATION__FUSION_HPP_ + +#include "keisan/keisan.hpp" + +namespace basho { + +class Fusion2D { +public: + Fusion2D(); + ~Fusion2D() = default; + + void init(double x, double y); + void predict(double dx, double dy); + void update(double mx, double my); + + bool is_initialized() const { return initialized; } + + keisan::Point2 getPosition() const; + +private: + keisan::Matrix<2,1> Xk; // state [x, y] + keisan::Matrix<2,2> P; // covariance + keisan::Matrix<2,2> Q; // process noise + keisan::Matrix<2,2> R; // measurement noise + + bool initialized = false; +}; + +} // namespace basho + +#endif // BASHO__LOCALIZATION__FUSION_HPP_ diff --git a/include/basho/localization/mcl.hpp b/include/basho/localization/mcl.hpp new file mode 100644 index 0000000..28285fa --- /dev/null +++ b/include/basho/localization/mcl.hpp @@ -0,0 +1,184 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__LOCALIZATION__MCL_HPP_ +#define BASHO__LOCALIZATION__MCL_HPP_ + +#include +#include + +#include "keisan/keisan.hpp" +#include "keisan/hungarian.hpp" +#include "basho/model/field.hpp" + +namespace basho +{ + +struct ProjectedObject +{ + std::string label; + keisan::Point3 position; +}; + +struct Particle +{ + keisan::Point2 position; + keisan::Angle orientation; + double weight; +}; + +struct LandmarkGroup { + std::vector* projected; + std::vector* landmarks; +}; + +struct ParticleCluster { + keisan::Point2 mean; + double weight_sum; + int count; +}; + +class MCL +{ +public: + enum { INIT, TRACKING, UNCERTAIN, LOST }; + + MCL(); + + void localize(); + void reset() { initial_localization = true; } + void clear_projected_objects(); + void set_initial_position(const keisan::Point2 & position) { initial_position = position; } + + keisan::Point2 get_position() const { return position; } + keisan::Point2 get_delta_position() const { return delta_position; } + keisan::Point2 get_estimated_position() const { return estimated_position; } + keisan::Angle get_orientation() const { return orientation; } + const std::vector & get_particles() const { return particles; } + + keisan::Point2 get_odometry_position() const { return odometry_position; } + + void set_delta_position(const keisan::Point2 & delta) { delta_position = delta; } + void set_orientation(const keisan::Angle & yaw) { orientation = yaw; } + void set_orientation_roll(const keisan::Angle & roll) { orientation_roll = roll; } + void set_orientation_pitch(const keisan::Angle & pitch) { orientation_pitch = pitch; } + + void set_head_pan(const keisan::Angle & pan) { head_pan = pan; } + void set_head_tilt(const keisan::Angle & tilt) { head_tilt = tilt; } + + void set_odometry_position(const keisan::Point2 & odom) { odometry_position = odom; } + + int num_particles; + double min_centered_ratio; + double short_term_avg_ratio; + double long_term_avg_ratio; + double base_sigma; + double range_sigma; + double chi_square_gate; + double uncertainty_threshold; + keisan::Point2 max_object_distance; + bool generalized_landmark; + bool print_debug; + + bool apply_localization; + int state; + + // IPM + std::vector projected_X; + std::vector projected_L; + std::vector projected_T; + std::vector projected_intersection; + std::vector projected_goalpost; + int num_projected_objects; + +private: + // MCL Steps + void init_particles(); + void update_motion(); + void calculate_weight(); + void resample_particles_wheel(); + void resample_particles_low_variance(); + void estimate_position(); + + // Likelihood Calculations + double calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2); + keisan::Point2 rotate_to_global_frame(const keisan::Point2 & local_pos, + const keisan::Angle & theta); + + double calculate_total_likelihood(const Particle & particle); + double calculate_object_likelihood(const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle); + + keisan::Matrix<27, 27> calculate_cost_matrix(const Particle & particle, + const std::vector & detected_landmarks, + const std::vector & landmarks); + + double calculate_pose_covariance(); + + bool is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle); + + bool compute_goalpost_pose_hypothesis(keisan::Point2 & out_pos); + + void print_particles(); + + keisan::Point2 position; + keisan::Point2 delta_position; + keisan::Angle orientation; + keisan::Angle orientation_roll; + keisan::Angle orientation_pitch; + + keisan::Angle head_pan; + keisan::Angle head_tilt; + + keisan::Point2 initial_position; + keisan::Point2 estimated_position; + keisan::Point2 odometry_position; + + Field field; + std::vector particles; + std::vector best_cluster_particles; + double centered_ratio; + + std::mt19937 rand_gen; + keisan::Hungarian<27> hungarian; + + double weight_avg; + double short_term_avg; + double long_term_avg; + double last_weight_avg; + double prob; + + bool initial_localization; + bool initial_avg_weight; + int pending_accept_count; + keisan::Point2 pending_estimated_position; + + double xvar; + double yvar; + keisan::Matrix<2,2> pose_cov; + double pose_uncertainty; + + int lost_count; +}; + +} // namespace basho + +#endif // BASHO__LOCALIZATION__MCL_HPP_ diff --git a/include/basho/model/field.hpp b/include/basho/model/field.hpp new file mode 100644 index 0000000..ec8a834 --- /dev/null +++ b/include/basho/model/field.hpp @@ -0,0 +1,94 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__MODEL__FIELD_HPP_ +#define BASHO__MODEL__FIELD_HPP_ + +#include + +#include "keisan/keisan.hpp" + +namespace basho +{ + +struct Field +{ +public: + int width; + int length; + std::vector landmarks_L; + std::vector landmarks_T; + std::vector landmarks_X; + std::vector landmarks_goalpost; + std::vector landmarks_intersection; + + Field() + : width(600), + length(900), + landmarks_L({ + {0.0, 0.0}, // Top-left corner (touchline x goal line) + {0.0, 600.0}, // Bottom-left corner (touchline x goal line) + {900.0, 0.0}, // Top-right corner (touchline x goal line) + {900.0, 600.0}, // Bottom-right corner (touchline x goal line) + {200.0, 100.0}, // Left penalty area - top corner + {200.0, 500.0}, // Left penalty area - bottom corner + {700.0, 100.0}, // Right penalty area - top corner + {700.0, 500.0}, // Right penalty area - bottom corner + {100.0, 150.0}, // Left goal area - top corner + {100.0, 450.0}, // Left goal area - bottom corner + {800.0, 150.0}, // Right goal area - top corner + {800.0, 450.0} // Right goal area - bottom corner + }), + landmarks_T({ + {450.0, 0.0}, // Halfway line x top touchline + {450.0, 600.0}, // Halfway line x bottom touchline + {0.0, 100.0}, // Left penalty area - top intersection with touchline + {0.0, 500.0}, // Left penalty area - bottom intersection with touchline + {900.0, 100.0}, // Right penalty area - top intersection with touchline + {900.0, 500.0}, // Right penalty area - bottom intersection with touchline + {0.0, 150.0}, // Left goal area - top intersection with touchline + {0.0, 450.0}, // Left goal area - bottom intersection with touchline + {900.0, 150.0}, // Right goal area - top intersection with touchline + {900.0, 450.0} // Right goal area - bottom intersection with touchline + }), + landmarks_X({ + {450.0, 300.0}, // Center mark + {450.0, 375.0}, // Halfway line (lower intersection region) + {450.0, 225.0}, // Halfway line (upper intersection region) + {150.0, 300.0}, // Left penalty mark + {750.0, 300.0} // Right penalty mark + }), + landmarks_goalpost({ + {0.0, 210.0}, // Left goal - upper goalpost (on goal line) + {0.0, 390.0}, // Left goal - lower goalpost (on goal line) + {900.0, 210.0}, // Right goal - upper goalpost (on goal line) + {900.0, 390.0} // Right goal - lower goalpost (on goal line) + }) + { + landmarks_intersection.reserve(landmarks_L.size() + landmarks_T.size() + landmarks_X.size()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_L.begin(), landmarks_L.end()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_T.begin(), landmarks_T.end()); + landmarks_intersection.insert(landmarks_intersection.end(), landmarks_X.begin(), landmarks_X.end()); + } +}; + +} // namespace basho + +#endif // BASHO__MODEL__FIELD_HPP_ diff --git a/include/basho/node/basho_node.hpp b/include/basho/node/basho_node.hpp new file mode 100644 index 0000000..848a8c9 --- /dev/null +++ b/include/basho/node/basho_node.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__NODE__BASHO_NODE_HPP_ +#define BASHO__NODE__BASHO_NODE_HPP_ + +#include +#include +#include + +#include "aruku_interfaces/msg/point2.hpp" +#include "aruku_interfaces/msg/status.hpp" +#include "atama_interfaces/msg/head.hpp" +#include "basho/localization/mcl.hpp" +#include "basho/config/node/config_node.hpp" +#include "basho/process/localization.hpp" +#include "basho_interfaces/msg/particle.hpp" +#include "basho_interfaces/msg/particles.hpp" +#include "gyakuenki_interfaces/msg/projected_objects.hpp" +#include "kansei_interfaces/msg/status.hpp" + +namespace basho +{ + +class BashoNode +{ +public: + using MeasurementStatus = kansei_interfaces::msg::Status; + using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects; + using Particle = basho_interfaces::msg::Particle; + using Particles = basho_interfaces::msg::Particles; + using Point2 = aruku_interfaces::msg::Point2; + using WalkingStatus = aruku_interfaces::msg::Status; + using HeadData = atama_interfaces::msg::Head; + + static std::string get_node_prefix(); + + void update(); + + BashoNode( + const std::shared_ptr & node, + const std::shared_ptr & localization, + const std::string & path + ); + + void run_config_service(const std::string & path); + +private: + void publish_odometry(); + void publish_particles(); + + rclcpp::Node::SharedPtr node; + + rclcpp::Publisher::SharedPtr set_odometry_publisher; + rclcpp::Publisher::SharedPtr fused_position_publisher; + rclcpp::Publisher::SharedPtr particles_publisher; + + rclcpp::Subscription::SharedPtr measurement_status_subscriber; + rclcpp::Subscription::SharedPtr projected_objects_subscriber; + rclcpp::Subscription::SharedPtr walking_status_subscriber; + rclcpp::Subscription::SharedPtr delta_position_subscriber; + rclcpp::Subscription::SharedPtr init_localization_subscriber; + rclcpp::Subscription::SharedPtr head_data_subscriber; + + std::shared_ptr localization; + + std::shared_ptr config_node; +}; + +} // namespace basho + +#endif // BASHO__NODE__BASHO_NODE_HPP_ diff --git a/include/basho/process/localization.hpp b/include/basho/process/localization.hpp new file mode 100644 index 0000000..9759a52 --- /dev/null +++ b/include/basho/process/localization.hpp @@ -0,0 +1,62 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#ifndef BASHO__PROCESS__LOCALIZATION_HPP_ +#define BASHO__PROCESS__LOCALIZATION_HPP_ + +#include +#include + +#include "keisan/keisan.hpp" +#include "nlohmann/json.hpp" +#include "basho/localization/mcl.hpp" +#include "basho/localization/fusion.hpp" + +namespace basho +{ + +class Localization +{ +public: + explicit Localization(std::shared_ptr mcl, const std::string & config_path); + + void load_config(); + void set_config(const nlohmann::json & json); + void save_config(); + void sync_config(); + + void reset(keisan::Point2 init_position); + void update(); + + keisan::Point2 getFusedPose() const; + + std::shared_ptr get_mcl() const; + + std::string config_path; + std::string config_name; + +private: + std::shared_ptr mcl; + Fusion2D fusion; +}; + +} // namespace basho + +#endif // BASHO__PROCESS__LOCALIZATION_HPP_ diff --git a/package.xml b/package.xml index 6d5aac4..27e2e3d 100644 --- a/package.xml +++ b/package.xml @@ -3,15 +3,23 @@ basho 0.0.0 - Localization Package for ROS 2 Project + Localization (MCL) Package for ROS 2 Project mbsaloka MIT License - ament_cmake - + ament_index_cpp + aruku + aruku_interfaces + jitsuyo + kansei + kansei_interfaces + keisan + nlohmann-json-dev + rclcpp + std_msgs + basho_interfaces ament_lint_auto ament_lint_common - ament_cmake diff --git a/src/basho/config/node/config_node.cpp b/src/basho/config/node/config_node.cpp new file mode 100644 index 0000000..e84bf3b --- /dev/null +++ b/src/basho/config/node/config_node.cpp @@ -0,0 +1,97 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include + +#include "basho/config/node/config_node.hpp" +#include "jitsuyo/config.hpp" +#include "nlohmann/json.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace basho +{ + +ConfigNode::ConfigNode( + rclcpp::Node::SharedPtr node, + const std::string & path, + const std::shared_ptr & localization) +: node(node), localization(localization) +{ + get_config_server = node->create_service( + get_node_prefix() + "/get_config", + [this, path](GetConfig::Request::SharedPtr, GetConfig::Response::SharedPtr response) { + try { + nlohmann::ordered_json data; + if (!jitsuyo::load_config(path, "localization.json", data)) { + RCLCPP_ERROR(this->node->get_logger(), "Failed to load localization config"); + return; + } + response->json = data.dump(); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); + + save_config_server = node->create_service( + get_node_prefix() + "/save_config", + [this, path](SaveConfig::Request::SharedPtr request, SaveConfig::Response::SharedPtr response) { + try { + response->status = false; + nlohmann::ordered_json data = nlohmann::ordered_json::parse(request->json); + + if (!jitsuyo::save_config(path, "localization.json", data)) { + RCLCPP_ERROR(this->node->get_logger(), "Failed to save localization config"); + return; + } + + response->status = true; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); + + set_config_server = node->create_service( + get_node_prefix() + "/set_config", + [this](SetConfig::Request::SharedPtr request, SetConfig::Response::SharedPtr response) { + try { + response->status = false; + nlohmann::ordered_json data = nlohmann::ordered_json::parse(request->json); + + this->localization->set_config(data); + this->localization->get_mcl()->reset(); + + response->status = true; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->node->get_logger(), "Exception: %s", e.what()); + } + } + ); +} + +std::string ConfigNode::get_node_prefix() const +{ + return "basho/config"; +} + +} // namespace basho diff --git a/src/basho/localization/fusion.cpp b/src/basho/localization/fusion.cpp new file mode 100644 index 0000000..7000d34 --- /dev/null +++ b/src/basho/localization/fusion.cpp @@ -0,0 +1,81 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "basho/localization/fusion.hpp" + +namespace basho { + +Fusion2D::Fusion2D() +{ + Xk = keisan::Matrix<2,1>::zero(); + P = keisan::Matrix<2,2>::identity() * 0.01; + Q = keisan::Matrix<2,2>::identity() * 0.01*0.01; + R = keisan::Matrix<2,2>::identity() * 0.5*0.5; + + initialized = false; +} + +void Fusion2D::init(double x, double y) +{ + Xk[0][0] = x; + Xk[1][0] = y; + + P = keisan::Matrix<2,2>::identity() * 0.01; + + initialized = true; +} + +void Fusion2D::predict(double dx, double dy) +{ + // Predict state using delta odometry + Xk[0][0] += dx; + Xk[1][0] += dy; + + // Predict covariance + P = P + Q; +} + +void Fusion2D::update(double mx, double my) +{ + keisan::Matrix<2,1> z; + z[0][0] = mx; + z[1][0] = my; + + // Kalman Gain + keisan::Matrix<2,2> S = P + R; + keisan::Matrix<2,2> K = P * S.inverse2(); + + // Update state + Xk = Xk + K*(z - Xk); + + // Update covariance + keisan::Matrix<2,2> I = keisan::Matrix<2,2>::identity(); + P = (I - K)*P; +} + +keisan::Point2 Fusion2D::getPosition() const +{ + keisan::Point2 pos; + pos.x = Xk[0][0]; + pos.y = Xk[1][0]; + return pos; +} + +} // namespace basho diff --git a/src/basho/localization/mcl.cpp b/src/basho/localization/mcl.cpp new file mode 100644 index 0000000..4461baa --- /dev/null +++ b/src/basho/localization/mcl.cpp @@ -0,0 +1,724 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include + +#include "basho/localization/mcl.hpp" + +#include "keisan/keisan.hpp" + +using keisan::literals::operator""_deg; + +namespace basho +{ + +MCL::MCL() +: position(0.0, 0.0), orientation(0_deg), orientation_roll(0_deg), orientation_pitch(0_deg), state(INIT), + delta_position(0.0, 0.0), estimated_position(0.0, 0.0), print_debug(false), initial_position(-1.0, -1.0), + weight_avg(0.0), initial_localization(true), apply_localization(false), prob(0.0), last_weight_avg(0.0), + short_term_avg(0.0), long_term_avg(0.0), rand_gen(std::random_device{}()), pending_accept_count(0), + num_projected_objects(0), min_centered_ratio(0.0), pose_uncertainty(0.0), initial_avg_weight(true), + num_particles(0), short_term_avg_ratio(0.0), long_term_avg_ratio(0.0), base_sigma(0.0), range_sigma(0.0), + chi_square_gate(0.0), uncertainty_threshold(0.0), max_object_distance(0.0, 0.0), centered_ratio(0.0) +{ +} + +void MCL::localize() +{ + if (initial_localization) { + init_particles(); + initial_localization = false; + initial_avg_weight = true; + state = INIT; + clear_projected_objects(); + return; + } + + update_motion(); + + bool evaluate_particles = num_projected_objects > 0; + evaluate_particles &= fabs(orientation_roll.degree()) <= 30; + evaluate_particles &= fabs(orientation_pitch.degree()) <= 30; + evaluate_particles &= fabs(head_pan.degree()) <= 60; + + apply_localization = false; + + if (evaluate_particles) { + calculate_weight(); + if (initial_avg_weight) { + short_term_avg = weight_avg; + long_term_avg = weight_avg; + + initial_avg_weight = false; + } + + short_term_avg += short_term_avg_ratio * (weight_avg - short_term_avg); + long_term_avg += long_term_avg_ratio * (weight_avg - long_term_avg); + + prob = keisan::clamp(1.0 - short_term_avg / (long_term_avg + 1e-6), 0.0, 1.0); + } + + estimate_position(); + pose_uncertainty = calculate_pose_covariance(); + + if (centered_ratio < min_centered_ratio) { + state = LOST; + lost_count++; + } else if (pose_uncertainty < uncertainty_threshold * 1.5) { + position = estimated_position; + apply_localization = true; + state = pose_uncertainty < uncertainty_threshold ? TRACKING : UNCERTAIN; + lost_count = 0; + } else { + state = LOST; + lost_count++; + } + + if ((state == UNCERTAIN || state == LOST) && evaluate_particles) { + // resample_particles_wheel(); + resample_particles_low_variance(); + } + + print_particles(); + clear_projected_objects(); +} + +void MCL::init_particles() +{ + double uniform_weight = 1.0 / num_particles; + particles.clear(); + particles.resize(num_particles); + estimated_position = initial_position; + + if (initial_position.x == -1 && initial_position.y == -1) { + std::uniform_int_distribution xrg(-10, 910); + std::uniform_int_distribution yrg(-10, 610); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } + } else { + std::normal_distribution xrg(initial_position.x, 50); + std::normal_distribution yrg(initial_position.y, 50); + + for (int i = 0; i < num_particles; ++i) { + particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + particles[i].orientation = orientation; + particles[i].weight = uniform_weight; + } + } +} + +void MCL::clear_projected_objects() +{ + projected_X.clear(); + projected_L.clear(); + projected_T.clear(); + projected_intersection.clear(); + projected_goalpost.clear(); + num_projected_objects = 0; +} + +void MCL::update_motion() +{ + double k_trans = 0.05; + double min_trans_noise = 0.5; + + double sigma_theta = 5.0; + + double dx = delta_position.x; + double dy = delta_position.y; + double trans = std::sqrt(dx*dx + dy*dy); + + double sigma_trans = k_trans * trans + min_trans_noise; + + std::normal_distribution noise_x(0.0, sigma_trans); + std::normal_distribution noise_y(0.0, sigma_trans); + std::normal_distribution noise_theta(0.0, sigma_theta); + + for (auto & p : particles) { + p.position.x += dx + noise_x(rand_gen); + p.position.y += dy + noise_y(rand_gen); + + p.orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + p.position.x = keisan::clamp(p.position.x, -10.0, 910.0); + p.position.y = keisan::clamp(p.position.y, -10.0, 610.0); + } + + estimated_position += delta_position; +} + +void MCL::calculate_weight() +{ + double sum_weight = 0.0; + + for (auto & p : particles) { + p.weight = calculate_total_likelihood(p); + sum_weight += p.weight; + } + + if (!std::isnan(sum_weight) && sum_weight > 0) { + weight_avg = sum_weight / num_particles; + for (auto & p : particles) { + p.weight /= sum_weight; + } + } else { + weight_avg = 1e-9; + } +} + +void MCL::resample_particles_wheel() +{ + std::vector new_particles(num_particles); + std::uniform_int_distribution rand_index(0, num_particles - 1); + int index = rand_index(rand_gen); + + double beta = 0.0; + double max_weight = 0.0; + + // determine resample interval area + int interval_x[2] = {-10, 910}; + int interval_y[2] = {-10, 610}; + + // resample particles + std::uniform_real_distribution rand_prob(0.0, 1.0); + std::uniform_real_distribution rand_beta(0.0, 2.0 * max_weight); + std::uniform_int_distribution xrg(interval_x[0], interval_x[1]); + std::uniform_int_distribution yrg(interval_y[0], interval_y[1]); + + // adaptive noise + double max_sigma = 3.0; + double min_sigma = 0.1; + + double sigma_factor = 1.0 - prob; + double resample_sigma_x = min_sigma + sigma_factor * (max_sigma - min_sigma); + double resample_sigma_y = min_sigma + sigma_factor * (max_sigma - min_sigma); + + std::normal_distribution noise_x(0.0, resample_sigma_x); + std::normal_distribution noise_y(0.0, resample_sigma_y); + std::normal_distribution noise_theta(0.0, 5.0); + + for (int i = 0; i < num_particles; ++i) { + if (rand_prob(rand_gen) < prob) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + new_particles[i].weight = 0.0; + } else { + beta += rand_beta(rand_gen); + + while (beta > particles[index].weight) { + beta -= particles[index].weight; + index = (index + 1) % num_particles; + } + new_particles[i] = particles[index]; + + new_particles[i].position.x += noise_x(rand_gen); + new_particles[i].position.y += noise_y(rand_gen); + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + } + + // check is particle out of bound + if (new_particles[i].position.x < 0.0 || new_particles[i].position.x > 900.0 || + new_particles[i].position.y < 0.0 || new_particles[i].position.y > 600.0) { + new_particles[i].position = keisan::Point2(xrg(rand_gen), yrg(rand_gen)); + new_particles[i].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + new_particles[i].weight = 0.0; + } + } + + particles = new_particles; +} + +void MCL::resample_particles_low_variance() +{ + std::vector new_particles; + new_particles.reserve(num_particles); + + // Perform systematic sampling to reduce variance compared to random wheel sampling + std::uniform_real_distribution dist_u(0.0, 1.0 / num_particles); + double r = dist_u(rand_gen); + double c = particles[0].weight; + int i = 0; + + // Sample particles based on cumulative weight distribution + for (int m = 0; m < num_particles; ++m) { + double U = r + (double)m / num_particles; + + while (U > c && i < num_particles - 1) { + i++; + c += particles[i].weight; + } + + new_particles.push_back(particles[i]); + } + + // Random particle injection. Used for recovery when particle set collapses or localization is lost + int num_random = prob * num_particles; + + // Limit injection to avoid destroying the current distribution (max 20%) + num_random = std::min(num_random, (int)(0.2 * num_particles)); + + // Noise for injected particles (around estimated position) + // TOOD: get noise value from config + std::normal_distribution noise_x(0.0, 75.0); + std::normal_distribution noise_y(0.0, 75.0); + std::normal_distribution noise_theta(0.0, 8.0); + + for (int i = 0; i < num_random; ++i) { + int idx = num_particles - 1 - i; + + // Inject particle near estimated position + new_particles[idx].position.x = estimated_position.x + noise_x(rand_gen); + new_particles[idx].position.y = estimated_position.y + noise_y(rand_gen); + + // Add orientation noise + new_particles[idx].orientation = + (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + // Assign small weight so it does not dominate immediately + new_particles[idx].weight = 1e-3; + } + + int num_inject_odom = 0; + int num_inject_goalpost = 0; + + // If localization is lost, inject random particles near odometry position to increase chances of recovery + if (state == LOST && lost_count > 10) { + num_inject_odom = 0.1 * num_particles; + for (int i = num_random; i < num_random + num_inject_odom; ++i) { + int idx = num_particles - 1 - i; + + new_particles[idx].position.x = odometry_position.x + noise_x(rand_gen); + new_particles[idx].position.y = odometry_position.y + noise_y(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + new_particles[idx].weight = 1e-3; + } + } + + // Inject particles around goalpost hypothesis if available + if (projected_goalpost.size() >= 2) { + keisan::Point2 goalpost_hypothesis; + int num_inject_goalpost = 0.1 * num_particles; + + if (compute_goalpost_pose_hypothesis(goalpost_hypothesis)) { + for (int i = num_random + num_inject_odom; i < num_random + num_inject_goalpost + num_inject_goalpost; ++i) { + int idx = num_particles - 1 - i; + + new_particles[idx].position.x = goalpost_hypothesis.x + noise_x(rand_gen); + new_particles[idx].position.y = goalpost_hypothesis.y + noise_y(rand_gen); + new_particles[idx].orientation = (orientation + keisan::make_degree(noise_theta(rand_gen))).normalize(); + + new_particles[idx].weight = 1e-3; + } + } + } + + // Maintain particle diversity and prevent collapse + std::normal_distribution jitter_x(0.0, 2.0); + std::normal_distribution jitter_y(0.0, 2.0); + + for (auto & p : new_particles) { + p.position.x += jitter_x(rand_gen); + p.position.y += jitter_y(rand_gen); + + // Clamp to map boundaries + p.position.x = keisan::clamp(p.position.x, -10.0, 910.0); + p.position.y = keisan::clamp(p.position.y, -10.0, 610.0); + } + + particles = new_particles; +} + +void MCL::estimate_position() +{ + double cluster_radius = 50.0; + double min_weight = 1e-4; + + std::vector clusters; + + for (auto & p : particles) { + + if (p.weight < min_weight) continue; + + bool assigned = false; + + for (auto & c : clusters) { + double dist = calculate_distance(p.position, c.mean); + + if (dist < cluster_radius) { + double new_weight = c.weight_sum + p.weight; + + c.mean.x = (c.mean.x * c.weight_sum + p.position.x * p.weight) / new_weight; + c.mean.y = (c.mean.y * c.weight_sum + p.position.y * p.weight) / new_weight; + + c.weight_sum = new_weight; + c.count++; + assigned = true; + break; + } + } + + // create new cluster + if (!assigned) { + ParticleCluster c; + c.mean = p.position; + c.weight_sum = p.weight; + c.count = 1; + clusters.push_back(c); + } + } + + double best_weight = -1.0; + ParticleCluster* best_cluster = nullptr; + + for (auto & c : clusters) { + if (c.weight_sum > best_weight) { + best_weight = c.weight_sum; + best_cluster = &c; + } + } + + if (best_cluster == nullptr) return; + + best_cluster_particles.clear(); + + for (auto & p : particles) { + if (p.weight < min_weight) continue; + + double dist = calculate_distance(p.position, best_cluster->mean); + if (dist < cluster_radius) { + best_cluster_particles.push_back(&p); + } + } + + estimated_position = best_cluster->mean; + + estimated_position.x = keisan::clamp(estimated_position.x, -10.0, 910.0); + estimated_position.y = keisan::clamp(estimated_position.y, -10.0, 610.0); + + centered_ratio = (double)best_cluster_particles.size() / num_particles; +} + +double MCL::calculate_pose_covariance() +{ + if (best_cluster_particles.empty()) { + return 1e9; + } + + double c_xx = 0.0; + double c_yy = 0.0; + double c_xy = 0.0; + double w_sum = 0.0; + + for (auto p : best_cluster_particles) { + double dx = p->position.x - estimated_position.x; + double dy = p->position.y - estimated_position.y; + + c_xx += p->weight * dx * dx; + c_yy += p->weight * dy * dy; + c_xy += p->weight * dx * dy; + w_sum += p->weight; + } + + if (w_sum < 1e-9) return 1e9; + + pose_cov[0][0] = c_xx / w_sum; + pose_cov[1][1] = c_yy / w_sum; + pose_cov[0][1] = c_xy / w_sum; + pose_cov[1][0] = c_xy / w_sum; + + double det = pose_cov[0][0]*pose_cov[1][1] - pose_cov[0][1]*pose_cov[1][0]; + double trace = pose_cov[0][0] + pose_cov[1][1]; + double lambda_max = trace/2 + sqrt((trace*trace)/4 - det); + + return sqrt(lambda_max); +} + +double MCL::calculate_distance(const keisan::Point2 & point_1, const keisan::Point2 & point_2) +{ + double dx = point_1.x - point_2.x; + double dy = point_1.y - point_2.y; + + return sqrt(dx * dx + dy * dy); +} + +keisan::Point2 MCL::rotate_to_global_frame( + const keisan::Point2& local_pos, const keisan::Angle & theta) +{ + // Robot frame: X forward+, Y left+, CW positive + double x_g = local_pos.x * theta.cos() + local_pos.y * theta.sin(); + double y_g = local_pos.x * theta.sin() - local_pos.y * theta.cos(); + return {x_g, y_g}; +} + +double MCL::calculate_total_likelihood(const Particle & particle) +{ + double log_total = 0.0; + int valid_associations = 0; + + std::vector landmark_groups = { + {&projected_goalpost, &field.landmarks_goalpost} + }; + + if (generalized_landmark) { + landmark_groups.push_back({&projected_intersection, &field.landmarks_intersection}); + } else { + landmark_groups.push_back({&projected_X, &field.landmarks_X}); + landmark_groups.push_back({&projected_L, &field.landmarks_L}); + landmark_groups.push_back({&projected_T, &field.landmarks_T}); + } + + for (const auto & group : landmark_groups) { + if (group.projected->empty() || group.landmarks->empty()) { + continue; + } + + auto cost_matrix = calculate_cost_matrix(particle, *group.projected, *group.landmarks); + auto result = hungarian.solve(cost_matrix, group.landmarks->size()); + + for (int i = 0; i < group.projected->size(); ++i) { + for (int j = 0; j < group.landmarks->size(); ++j) { + if (result[i][j] == 1) { + if (cost_matrix[i][j] >= 1e8) { + log_total += std::log(1e-3); + valid_associations++; + continue; + } + + double likelihood = + calculate_object_likelihood((*group.projected)[i], (*group.landmarks)[j], particle); + log_total += std::log(std::max(likelihood, 1e-6)); + valid_associations++; + } + } + } + } + + if (valid_associations == 0) { + return 1.0; + } + + return std::exp(log_total / valid_associations); +} + +double MCL::calculate_object_likelihood( + const ProjectedObject & measurement, + const keisan::Point2 & landmark, + const Particle & particle) +{ + double dx = measurement.position.x * 100; + double dy = measurement.position.y * 100; + + keisan::Point2 global_pos = rotate_to_global_frame({dx, dy}, particle.orientation); + + double px = particle.position.x + global_pos.x; + double py = particle.position.y + global_pos.y; + + double ex = landmark.x - px; + double ey = landmark.y - py; + + double dist = std::sqrt(dx*dx + dy*dy); + double sigma = base_sigma + range_sigma * dist; + double var = sigma * sigma; + + double d2 = (ex*ex + ey*ey) / var; + + return std::exp(-0.5 * d2); +} + +keisan::Matrix<27, 27> MCL::calculate_cost_matrix( + const Particle & particle, + const std::vector & detected_landmarks, + const std::vector & landmarks) +{ + auto cost_matrix = keisan::Matrix<27, 27>::infinite(); + + for (int i = 0; i < detected_landmarks.size(); ++i) { + double dx = detected_landmarks[i].position.x * 100; + double dy = detected_landmarks[i].position.y * 100; + + keisan::Point2 global_pos = rotate_to_global_frame({dx, dy}, particle.orientation); + + double px = particle.position.x + global_pos.x; + double py = particle.position.y + global_pos.y; + + double dist = std::sqrt(dx*dx + dy*dy); + double sigma = base_sigma + range_sigma * dist; + double var = sigma * sigma; + + for (int j = 0; j < landmarks.size(); ++j) { + if (!is_landmark_in_fov(landmarks[j], particle)) { + cost_matrix[i][j] = 1e9; + continue; + } + + double ex = landmarks[j].x - px; + double ey = landmarks[j].y - py; + + // Squared Mahalanobis distance + double d2 = (ex*ex + ey*ey) / var; + + // Chi-square gating to reject unlikely associations + if (d2 < chi_square_gate) { + cost_matrix[i][j] = d2; + } else { + cost_matrix[i][j] = 1e9; + } + } + } + + return cost_matrix; +} + +bool MCL::is_landmark_in_fov(const keisan::Point2 & landmark, const Particle & particle) +{ + double dx_world = landmark.x - particle.position.x; + double dy_world = landmark.y - particle.position.y; + + double cos_ori = particle.orientation.cos(); + double sin_ori = particle.orientation.sin(); + double dx_robot = dx_world * cos_ori + dy_world * sin_ori; + double dy_robot = -dx_world * sin_ori + dy_world * cos_ori; + + auto angle_to_landmark = keisan::signed_arctan(dy_robot, dx_robot); + auto gaze = -head_pan; + + auto diff = (angle_to_landmark - gaze).normalize(); + + return std::abs(diff.degree()) <= 45.0; +} + +bool MCL::compute_goalpost_pose_hypothesis(keisan::Point2 & out_pos) +{ + keisan::Point2 p1_local = {projected_goalpost[0].position.x * 100.0, + projected_goalpost[0].position.y * 100.0}; + keisan::Point2 p2_local = {projected_goalpost[1].position.x * 100.0, + projected_goalpost[1].position.y * 100.0}; + + double r1 = calculate_distance({0.0, 0.0}, p1_local); + double r2 = calculate_distance({0.0, 0.0}, p2_local); + + double dist_posts = calculate_distance(p1_local, p2_local); + if (dist_posts < 120.0 || dist_posts > 280.0) { + return false; + } + + keisan::Point2 p1_global_rel = rotate_to_global_frame(p1_local, orientation); + keisan::Point2 p2_global_rel = rotate_to_global_frame(p2_local, orientation); + + // Determine goal side (home/away) + double mid_global_rel_x = (p1_global_rel.x + p2_global_rel.x) * 0.5; + bool is_home_goal = mid_global_rel_x < 0.0; + + double goal_x = is_home_goal ? 0.0 : 900.0; + + // Assign lower/upper posts based on Y + // r_lower is distance to the post closer to the bottom of the field (y=210), + // r_upper is closer to top (y=390) + keisan::Point2 p_lower_local, p_upper_local; + double r_lower, r_upper; + + if (p1_global_rel.y < p2_global_rel.y) { + p_lower_local = p1_local; r_lower = r1; + p_upper_local = p2_local; r_upper = r2; + } else { + p_lower_local = p2_local; r_lower = r2; + p_upper_local = p1_local; r_upper = r1; + } + + // Trilateration calculation + // Known landmarks: + // A = (goal_x, 210) distance: r_lower + // B = (goal_x, 390) distance: r_upper + // (1) r_lower^2 = (x - goal_x)^2 + (y - 210)^2 + // (2) r_upper^2 = (x - goal_x)^2 + (y - 390)^2 + // => (1)-(2): r_lower^2 - r_upper^2 = (y-210)^2 - (y-390)^2 = 360y - 108000 + // => y = (r_lower^2 - r_upper^2 + 108000) / 360 + + double y_robot = (r_lower * r_lower - r_upper * r_upper + 108000.0) / 360.0; + + // From (1): (x - goal_x)^2 = r_lower^2 - (y - 210)^2 + double dy = y_robot - 210.0; + double x_sq = r_lower * r_lower - dy * dy; + + if (x_sq < 0.0) { + return false; + } + + double x_offset = sqrt(x_sq); + + // Robot assumed inside field + double x_robot = is_home_goal ? x_offset : (900.0 - x_offset); + + out_pos.x = x_robot; + out_pos.y = y_robot; + + return true; +} + +void MCL::print_particles() { + if (!print_debug) { + return; + } + + printf("\033c"); + printf("Particles num: %d\n", particles.size()); + printf("Prob: %.5f\n", prob); + printf("Short term avg: %.8f\n", short_term_avg); + printf("Long term avg: %.8f\n", long_term_avg); + printf("Last weight avg: %.8f\n", last_weight_avg); + printf("Weight avg: %.8f\n", weight_avg); + + printf("Minimal Centered Ratio: %.2f\n", min_centered_ratio); + printf("Centered Ratio: %.2f\n", centered_ratio); + printf("Minimal Centered Particles: %.0f\n", min_centered_ratio * num_particles); + + printf("Best cluster particles: %d\n", best_cluster_particles.size()); + for (int i = 0; i < best_cluster_particles.size(); ++i) { + printf("Centered Particle %d weight: %.5f [%.2f, %.2f, %.2f]\n", + i, + best_cluster_particles[i]->weight, + best_cluster_particles[i]->position.x, + best_cluster_particles[i]->position.y, + best_cluster_particles[i]->orientation.degree()); + if (i >= 5) { + printf("... and %d more\n", best_cluster_particles.size() - 5); + break; + } + } + + printf("Num objects: %d\n", num_projected_objects); + printf("Num particles: %d\n", num_particles); + printf("Initial Localization %d\n", initial_localization); + printf("Pose estimation: [%.2f, %.2f])\n", estimated_position.x, estimated_position.y); + printf("Pose covariance uncertainty: %.2f\n", pose_uncertainty); + printf("STATE: [%s]\n", + (state == INIT) ? "INIT" : + (state == TRACKING) ? "TRACKING" : + (state == UNCERTAIN) ? "UNCERTAIN" : + (state == LOST) ? "LOST" : "UNKNOWN"); +} + +} // namespace basho diff --git a/src/basho/node/basho_node.cpp b/src/basho/node/basho_node.cpp new file mode 100644 index 0000000..54da123 --- /dev/null +++ b/src/basho/node/basho_node.cpp @@ -0,0 +1,198 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include "basho/node/basho_node.hpp" + +#include + +#include "aruku/walking/walking.hpp" +#include "kansei/measurement/measurement.hpp" + +namespace basho +{ + +std::string BashoNode::get_node_prefix() +{ + return "localization"; +} + +BashoNode::BashoNode( + const rclcpp::Node::SharedPtr & node, + const std::shared_ptr & localization, + const std::string & path +) : node(node), + localization(localization) +{ + measurement_status_subscriber = + node->create_subscription( + kansei::measurement::MeasurementNode::status_topic(), + 10, + [this](const MeasurementStatus::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + + mcl->set_orientation(keisan::make_degree(msg->orientation.yaw)); + mcl->set_orientation_roll(keisan::make_degree(msg->orientation.roll)); + mcl->set_orientation_pitch(keisan::make_degree(msg->orientation.pitch)); + }); + + delta_position_subscriber = + node->create_subscription( + aruku::WalkingNode::delta_position_topic(), + 10, + [this](const Point2::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + mcl->set_delta_position({msg->x, msg->y}); + }); + + walking_status_subscriber = + node->create_subscription( + aruku::WalkingNode::status_topic(), + 10, + [this](const WalkingStatus::SharedPtr message) + { + auto mcl = this->localization->get_mcl(); + mcl->set_odometry_position({message->odometry.x, message->odometry.y}); + }); + + projected_objects_subscriber = + node->create_subscription( + "/gyakuenki_cpp/projected_objects", + 10, + [this](const ProjectedObjects::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + + mcl->clear_projected_objects(); + + for (const auto & obj : msg->projected_objects) { + const bool ignore = + obj.label == "ball" || + obj.label == "robot" || + obj.label == "self" || + std::abs(obj.position.x * 100) > mcl->max_object_distance.x || + std::abs(obj.position.y * 100) > mcl->max_object_distance.y; + + if (ignore) { + continue; + } + + ProjectedObject projected{ + obj.label, + {obj.position.x, obj.position.y, obj.position.z} + }; + + if (obj.label == "goalpost") { + mcl->projected_goalpost.push_back(projected); + } else if (mcl->generalized_landmark) { + mcl->projected_intersection.push_back(projected); + } else if (obj.label == "X-Intersection") { + mcl->projected_X.push_back(projected); + } else if (obj.label == "L-Intersection") { + mcl->projected_L.push_back(projected); + } else if (obj.label == "T-Intersection") { + mcl->projected_T.push_back(projected); + } + + mcl->num_projected_objects++; + } + }); + + init_localization_subscriber = + node->create_subscription( + get_node_prefix() + "/init_localization", + 10, + [this](const Point2::SharedPtr msg) { + this->localization->reset({msg->x, msg->y}); + }); + + head_data_subscriber = + node->create_subscription( + "head/set_head_data", + 10, + [this](const HeadData::SharedPtr msg) { + auto mcl = this->localization->get_mcl(); + mcl->set_head_pan(keisan::make_degree(msg->pan_angle)); + mcl->set_head_tilt(keisan::make_degree(msg->tilt_angle)); + }); + + set_odometry_publisher = + node->create_publisher(aruku::WalkingNode::set_odometry_topic(), 10); + + particles_publisher = + node->create_publisher(get_node_prefix() + "/particles", 10); + + fused_position_publisher = + node->create_publisher(get_node_prefix() + "/fused_pose", 10); +} + +void BashoNode::update() +{ + localization->update(); + publish_particles(); + + auto fused = localization->getFusedPose(); + Point2 msg; + msg.x = fused.x; + msg.y = fused.y; + fused_position_publisher->publish(msg); +} + +void BashoNode::publish_odometry() +{ + auto mcl = localization->get_mcl(); + + Point2 msg; + msg.x = mcl->get_position().x; + msg.y = mcl->get_position().y; + + set_odometry_publisher->publish(msg); +} + +void BashoNode::publish_particles() +{ + auto mcl = localization->get_mcl(); + + Particles msg; + double max_weight = 0.0; + + for (const auto & p : mcl->get_particles()) { + Particle particle; + particle.x = p.position.x; + particle.y = p.position.y; + particle.orientation = p.orientation.degree(); + particle.weight = p.weight; + + msg.particles.push_back(particle); + max_weight = std::max(max_weight, p.weight); + } + + msg.estimated_position.x = mcl->get_estimated_position().x; + msg.estimated_position.y = mcl->get_estimated_position().y; + msg.estimated_position.orientation = mcl->get_orientation().degree(); + msg.estimated_position.weight = max_weight; + + particles_publisher->publish(msg); +} + +void BashoNode::run_config_service(const std::string & path) +{ + config_node = std::make_shared(node, path, localization); +} + +} // namespace basho diff --git a/src/basho/process/localization.cpp b/src/basho/process/localization.cpp new file mode 100644 index 0000000..d34b94d --- /dev/null +++ b/src/basho/process/localization.cpp @@ -0,0 +1,161 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include +#include +#include +#include +#include +#include + +#include "basho/process/localization.hpp" + +#include "keisan/keisan.hpp" +#include "jitsuyo/config.hpp" +#include "nlohmann/json.hpp" + +#include "unistd.h" + +using namespace keisan::literals; + +namespace basho +{ + +Localization::Localization(std::shared_ptr mcl, const std::string & config_path) +: config_name("localization.json"), config_path(config_path), mcl(mcl) +{ +} + +void Localization::load_config() +{ + nlohmann::json data; + if (!jitsuyo::load_config(config_path, config_name, data)) { + throw std::runtime_error("Failed to find config file `localization.json`"); + } + + set_config(data); +} + +void Localization::set_config(const nlohmann::json & json) +{ + + bool valid_config = true; + + nlohmann::json localization_section; + if (jitsuyo::assign_val(json, "localization", localization_section)) { + bool valid_section = true; + + double max_distance_x = 0.0; + double max_distance_y = 0.0; + + valid_section &= jitsuyo::assign_val(localization_section, "num_particles", mcl->num_particles); + valid_section &= jitsuyo::assign_val(localization_section, "min_centered_ratio", mcl->min_centered_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "short_term_avg_ratio", mcl->short_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "long_term_avg_ratio", mcl->long_term_avg_ratio); + valid_section &= jitsuyo::assign_val(localization_section, "base_sigma", mcl->base_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "range_sigma", mcl->range_sigma); + valid_section &= jitsuyo::assign_val(localization_section, "chi_square_gate", mcl->chi_square_gate); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_x", max_distance_x); + valid_section &= jitsuyo::assign_val(localization_section, "max_distance_y", max_distance_y); + valid_section &= jitsuyo::assign_val(localization_section, "uncertainty_threshold", mcl->uncertainty_threshold); + valid_section &= jitsuyo::assign_val(localization_section, "generalized_landmark", mcl->generalized_landmark); + valid_section &= jitsuyo::assign_val(localization_section, "debug", mcl->print_debug); + + mcl->max_object_distance = {max_distance_x, max_distance_y}; + + if (!valid_section) { + std::cout << "Error found at section `localization`" << std::endl; + valid_config = false; + } + } else { + valid_config = false; + } + + if (!valid_config) { + throw std::runtime_error("Failed to load config file `localization.json`"); + } +} + +void Localization::save_config() +{ + nlohmann::json config; + + config["localization"] = { + {"num_particles", mcl->num_particles}, + {"min_centered_ratio", mcl->min_centered_ratio}, + {"short_term_avg_ratio", mcl->short_term_avg_ratio}, + {"long_term_avg_ratio", mcl->long_term_avg_ratio}, + {"base_sigma", mcl->base_sigma}, + {"range_sigma", mcl->range_sigma}, + {"chi_square_gate", mcl->chi_square_gate}, + {"max_distance_x", mcl->max_object_distance.x}, + {"max_distance_y", mcl->max_object_distance.y}, + {"uncertainty_threshold", mcl->uncertainty_threshold}, + {"generalized_landmark", mcl->generalized_landmark}, + {"debug", mcl->print_debug} + }; + + jitsuyo::save_config(config_path, config_name, config); +} + +void Localization::sync_config() +{ + load_config(); + save_config(); +} + +void Localization::reset(keisan::Point2 init_position) +{ + mcl->set_initial_position(init_position); + fusion.init(init_position.x, init_position.y); + mcl->reset(); +} + +void Localization::update() +{ + mcl->localize(); + + auto delta = mcl->get_delta_position(); + + if (!fusion.is_initialized()) { + auto odom_pos = mcl->get_odometry_position(); + fusion.init(odom_pos.x, odom_pos.y); + } + + fusion.predict(delta.x, delta.y); + + if (mcl->apply_localization) { + auto pos = mcl->get_position(); + fusion.update(pos.x, pos.y); + mcl->apply_localization = false; + } +} + +keisan::Point2 Localization::getFusedPose() const +{ + return fusion.getPosition(); +} + +std::shared_ptr Localization::get_mcl() const +{ + return mcl; +} + +} // namespace basho diff --git a/src/basho_main.cpp b/src/basho_main.cpp new file mode 100644 index 0000000..b584f92 --- /dev/null +++ b/src/basho_main.cpp @@ -0,0 +1,58 @@ +// Copyright (c) 2026 Ichiro ITS +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL +// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +// THE SOFTWARE. + +#include + +#include "basho/basho.hpp" + +using namespace std::chrono_literals; + +int main(int argc, char ** argv) +{ + auto args = rclcpp::init_and_remove_ros_arguments(argc, argv); + + if (argc < 2) { + std::cerr << "Please specify the path!" << std::endl; + return 0; + } + + const std::string & path = args[1]; + + auto node = std::make_shared("basho_node"); + + auto localization = std::make_shared(std::make_shared(), path); + localization->sync_config(); + + auto basho_node = std::make_shared(node, localization, path); + basho_node->run_config_service(path); + + rclcpp::Rate rcl_rate(8ms); + while (rclcpp::ok()) { + rcl_rate.sleep(); + + rclcpp::spin_some(node); + + basho_node->update(); + } + + rclcpp::shutdown(); + + return 0; +} \ No newline at end of file diff --git a/src/basho_node.cpp b/src/basho_node.cpp deleted file mode 100644 index 98ef1d9..0000000 --- a/src/basho_node.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include - -int main(int argc, char ** argv) -{ - (void) argc; - (void) argv; - - printf("hello world basho package\n"); - return 0; -}