[PI Sprint 25/26] [Feature] - Initialize Package

.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.

.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

.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' }}"
+            }

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(<dependency> 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
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+
+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
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-  $<INSTALL_INTERFACE:include/${PROJECT_NAME}>)
-target_compile_features(basho_node PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+  $<INSTALL_INTERFACE:include>)
+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()

README.md

@@ -0,0 +1,2 @@
+# basho
+Localization (MCL) Package for ROS 2 Project.

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_

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_

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 <memory>
+#include <string>
+
+#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> & localization);
+
+private:
+  std::string get_node_prefix() const;
+
+  std::shared_ptr<Localization> localization;
+
+  rclcpp::Node::SharedPtr node;
+
+  rclcpp::Service<GetConfig>::SharedPtr get_config_server;
+  rclcpp::Service<SaveConfig>::SharedPtr save_config_server;
+  rclcpp::Service<SetConfig>::SharedPtr set_config_server;
+};
+
+}  // namespace basho
+
+#endif  // BASHO__CONFIG__NODE__CONFIG_NODE_HPP_

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_