[PI Sprint 24/25 / PD-456] - [Feature] Implement Localization

CMakeLists.txt

@@ -18,6 +18,7 @@ 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)
@@ -47,6 +48,7 @@ ament_target_dependencies(${PROJECT_NAME}
   aruku
   aruku_interfaces
   atama_interfaces
+  gyakuenki_interfaces
   jitsuyo
   kansei
   kansei_interfaces
@@ -112,6 +114,7 @@ ament_export_dependencies(
   aruku
   aruku_interfaces
   atama_interfaces
+  gyakuenki_interfaces
   jitsuyo
   kansei
   kansei_interfaces

include/suiryoku/locomotion/model/field.hpp

@@ -0,0 +1,81 @@
+// Copyright (c) 2025 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 SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_
+#define SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_
+
+#include <vector>
+
+#include "keisan/keisan.hpp"
+
+namespace suiryoku
+{
+
+struct Field
+{
+public:
+  int width;
+  int length;
+  std::vector<keisan::Point2> landmarks_L;
+  std::vector<keisan::Point2> landmarks_T;
+  std::vector<keisan::Point2> landmarks_X;
+  std::vector<keisan::Point2> landmarks_goalpost;
+
+  Field()
+  : width(600),
+    length(900),
+    landmarks_L({
+      {0.0, 0.0},
+      {0.0, 600.0},
+      {100.0, 50.0},
+      {100.0, 550.0},
+      {900.0, 0.0},
+      {900.0, 600.0},
+      {800.0, 50.0},
+      {800.0, 550.0}
+    }),
+    landmarks_T({
+      {0.0, 50.0},
+      {0.0, 550.0},
+      {450.0, 0.0},
+      {450.0, 600.0},
+      {900.0, 50.0},
+      {900.0, 550.0}
+    }),
+    landmarks_X({
+      {210, 300},
+      {690, 300},
+      {450, 300},
+      {450, 375},
+      {450, 225}
+    }),
+    landmarks_goalpost({
+      {0.0, 170.0},
+      {0.0, 430.0},
+      {900.0, 170.0},
+      {900.0, 430.0}
+    })
+  {
+  }
+};
+
+}  // namespace suiryoku
+
+#endif  // SUIRYOKU__LOCOMOTION__MODEL__FIELD_HPP_

include/suiryoku/locomotion/model/robot.hpp

@@ -24,10 +24,24 @@
 #include <string>
 
 #include "keisan/keisan.hpp"
+#include "suiryoku/locomotion/model/field.hpp"
 
 namespace suiryoku
 {
 
+struct ProjectedObject
+{
+  std::string label;
+  keisan::Point3 position;
+};
+
+struct Particle
+{
+  keisan::Point2 position;
+  keisan::Angle<double> orientation;
+  double weight;
+};
+
 class Robot
 {
 public:
@@ -36,10 +50,35 @@ class Robot
   keisan::Angle<double> get_pan() const;
   keisan::Angle<double> get_tilt() const;
 
+  // localizations
+  void localize(bool initial_localization = false);
+  void init_particles(bool initial_localization);
+  void resample_particles();
+  void update_motion();
+  void calculate_weight();
+  void estimate_position();
+  void print_particles();
+  void print_estimate_position();
+  double calculate_total_likelihood(const Particle & particle);
+  double calculate_object_likelihood(const ProjectedObject & measurement, const Particle & particle);
+  double get_sum_weight();
+
+  Field field;
+  std::vector<Particle> particles;
+  keisan::Point2 estimated_position;
+  int num_particles;
+
+  bool use_localization;
+  bool apply_localization;
+
+  // IPM
+  std::vector<ProjectedObject> projected_objects;
+
   // member for getting
   bool is_calibrated;
   keisan::Angle<double> orientation;
   keisan::Point2 position;
+  keisan::Point2 delta_position;
 
   bool is_walking;
 
@@ -57,6 +96,11 @@ class Robot
   double y_speed;
   double a_speed;
   bool aim_on;
+
+private:
+  double xvar;
+  double yvar;
+  int kidnap_counter;
 };
 
 }  // namespace suiryoku

include/suiryoku/locomotion/node/locomotion_node.hpp

@@ -28,6 +28,7 @@
 #include "aruku_interfaces/msg/set_walking.hpp"
 #include "aruku_interfaces/msg/status.hpp"
 #include "atama_interfaces/msg/head.hpp"
+#include "gyakuenki_interfaces/msg/projected_objects.hpp"
 #include "kansei_interfaces/msg/status.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "suiryoku/locomotion/model/robot.hpp"
@@ -42,6 +43,7 @@ class LocomotionNode
   using Head = atama_interfaces::msg::Head;
   using MeasurementStatus = kansei_interfaces::msg::Status;
   using Point2 = aruku_interfaces::msg::Point2;
+  using ProjectedObjects = gyakuenki_interfaces::msg::ProjectedObjects;
   using SetWalking = aruku_interfaces::msg::SetWalking;
   using WalkingStatus = aruku_interfaces::msg::Status;
 
@@ -69,6 +71,10 @@ class LocomotionNode
 
   rclcpp::Subscription<Head>::SharedPtr head_subscriber;
 
+  rclcpp::Subscription<ProjectedObjects>::SharedPtr projected_objects_subscriber;
+
+  rclcpp::Subscription<Point2>::SharedPtr delta_position_subscriber;
+
   std::shared_ptr<Locomotion> locomotion;
   std::shared_ptr<Robot> robot;
 

include/suiryoku/locomotion/process/locomotion.hpp

@@ -167,6 +167,8 @@ class Locomotion
   keisan::Angle<double> right_kick_target_pan;
   keisan::Angle<double> right_kick_target_tilt;
 
+  bool localization_enable;
+
   std::shared_ptr<Robot> robot;
 };