kiks_avoidance

.gitignore

@@ -70,3 +70,6 @@ docs/logs/portal/
 
 # Configuration files
 crane_bringup/config/diagnostics.yaml
+
+# Documents
+*.pdf

AGENTS.md

@@ -170,6 +170,12 @@ docker compose up -d
 - 独自 lint: `crane_lint_common`
 - C++20, 主なフラグ: `-Wall -Wextra -Wpedantic -g`
 
+### コメントと言語
+
+- すべてのソースコード内コメントは日本語で記述してください。
+- 外部論文・式番号（例: 式(1), Eq.(2)）や外部API名などは必要に応じて英語併記可。
+- ドキュメント・PR説明・レビューコメントも原則日本語（必要に応じて英語併記）。
+
 ### テスト構成
 
 - GTest によるユニットテスト（各パッケージ `test/`）
@@ -301,8 +307,8 @@ FILTER_BRANCH_SQUELCH_WARNING=1 git filter-branch --force --index-filter \
 
 ### 粒度
 
-- 1コミット=1論理変更。コード/ドキュメント/設定は分離
-- 各コミットはビルド可能かつ動作する状態
+- 1コミット=1論理変更。
+- コード/ドキュメント/設定は分離
 
 ### チェックリスト
 

crane_local_planner/CMakeLists.txt

@@ -20,6 +20,7 @@ find_package(Eigen3 REQUIRED)
 ament_auto_find_build_dependencies()
 
 ament_auto_add_library(${PROJECT_NAME}_component SHARED
+  src/graph_planner.cpp
   src/rvo2_planner.cpp
   src/modern_orca_planner.cpp
   src/crane_local_planner.cpp

crane_local_planner/include/crane_local_planner/graph_planner.hpp

@@ -0,0 +1,155 @@
+// Copyright (c) 2025 ibis-ssl
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file or at
+// https://opensource.org/licenses/MIT.
+
+#ifndef CRANE_LOCAL_PLANNER__GRAPH_PLANNER_HPP_
+#define CRANE_LOCAL_PLANNER__GRAPH_PLANNER_HPP_
+
+#include <crane_geometry/boost_geometry.hpp>
+#include <crane_geometry/geometry_operations.hpp>
+#include <crane_msg_wrappers/crane_visualizer_wrapper.hpp>
+#include <crane_msg_wrappers/world_model_wrapper.hpp>
+#include <limits>
+#include <memory>
+#include <optional>
+#include <queue>
+#include <rclcpp/rclcpp.hpp>
+#include <unordered_map>
+#include <variant>
+
+namespace crane
+{
+
+// 経由点（ウェイポイント）: 位置と、その点に到達すべき目標速度
+struct Waypoint
+{
+  Point position;
+  Velocity target_velocity;  // 当該点で実現したい到達速度ベクトル
+};
+
+class GraphPlanner
+{
+public:
+  using SharedPtr = std::shared_ptr<GraphPlanner>;
+
+  // ロボットの運動制約（最大速度・加減速度）
+  struct Constraints
+  {
+    double vmax{4.0};
+    double alpha_acc{4.0};  // 加速度 (α_a)
+    double alpha_dec{4.0};  // 減速度 (α_b)
+  };
+
+  // パラメータ（ROSパラメータから変更可能）
+  struct Params
+  {
+    // 一般
+    int max_expansion{300};
+    double node_tangent_offset{0.03};  // 接線接地点からの微小オフセット[m]
+    double node_merge_epsilon{0.02};   // 近接ノードのマージ許容距離[m]
+
+    // 障害物モデル（式(2)）
+    double robot_radius{0.09};
+    double static_margin{0.10};  // Ms（静的障害物マージン）
+    double K_alpha{1.0};         // Kα（加速を考慮する係数）
+    double K_v{2.0};             // Kv（速度スケール）
+    double K_t{0.6};             // Kt [s]（Δtの上限）
+    double far_margin_cap{2.0};  // 遠方障害物のマージン上限[m]
+  };
+
+  explicit GraphPlanner(rclcpp::Node & node, WorldModelWrapper::SharedPtr world_model);
+
+  // 動的障害物を考慮したグラフ探索により、始点→目標の経路を計画
+  // v0: 現在のロボット速度（2D）、limits: ロボットの運動制約
+  auto plan(
+    const Pose2D & start, const Pose2D & goal, const Velocity & v0, const Constraints & limits,
+    uint8_t my_robot_id) -> std::vector<Waypoint>;
+
+  // パラメータをROSから再読込
+  void reloadParamsFromROS();
+
+  // 可視化のフラッシュ（フレーム末にまとめて呼ぶ想定）
+  void flush() { viz_->flush(); }
+
+private:
+  using CircleObstacle = Circle;  // center, radius を持つ
+  using BoxObstacle = Box;        // min/max corner を持つ
+  using Obstacle = std::variant<CircleObstacle, BoxObstacle>;
+
+  // 探索グラフのノード
+  struct Node
+  {
+    int id;
+    Point p;
+  };
+
+  struct PQItem
+  {
+    int node_id;
+    double cost;  // 距離or時間コスト（経路依存）
+    bool operator<(const PQItem & other) const { return cost > other.cost; }
+  };
+
+  struct PathState
+  {
+    double cost{std::numeric_limits<double>::infinity()};
+    int parent{-1};
+  };
+
+  // コア補助関数群
+  auto buildObstacles(uint8_t my_robot_id) -> std::vector<Obstacle>;
+  static auto intersectsAny(const Segment & seg, const std::vector<Obstacle> & obs) -> bool;
+  static auto intersects(const Segment & seg, const Obstacle & ob) -> bool;
+  static auto intersects(const Segment & seg, const CircleObstacle & c) -> bool;
+  static auto intersects(const Segment & seg, const BoxObstacle & b) -> bool;
+
+  // 接線生成とオンデマンド拡張
+  static auto tangentPointsFromPointToCircle(const Point & p, const CircleObstacle & c)
+    -> std::vector<Point>;
+  auto expandFrom(
+    int from_id, const Point & from, const Point & goal, const std::vector<Obstacle> & obstacles,
+    std::vector<Node> & nodes) -> std::vector<int>;
+
+  // 疑似コード準拠の候補展開（Step(2)-(3)）
+  struct IntersectionInfo
+  {
+    size_t obs_index;
+    double t_param;  // [0,1] の区間での交差位置（小さいほど手前）
+    Point point;     // 交差点（代表）
+  };
+
+  static auto firstIntersection(
+    const Point & from, const Point & to, const std::vector<Obstacle> & obstacles)
+    -> std::optional<IntersectionInfo>;
+
+  static auto boxCornersOutward(const BoxObstacle & bb, double offset) -> std::vector<Point>;
+
+  auto getOrCreateNodeAt(const Point & p, std::vector<Node> & nodes) -> int;
+
+  void expandUntilLineOfSight(
+    const Point & from, const Point & goal, const std::vector<Obstacle> & obstacles,
+    std::vector<int> & candidate_node_ids, std::vector<Node> & nodes, int depth, int max_depth);
+
+  // ウェイポイントに達すべき速度を付与（式(1)）
+  auto buildWaypointsWithVelocities(
+    const std::vector<Point> & path_points, const Velocity & v0, const Constraints & limits) const
+    -> std::vector<Waypoint>;
+
+  // ユーティリティ
+  static auto angleCosBetween(const Point & a, const Point & b) -> double;
+
+private:
+  rclcpp::Node * node_;
+
+  WorldModelWrapper::SharedPtr world_;
+
+  VisualizerMessageBuilder::SharedPtr viz_;
+
+  Params params_;
+};
+
+}  // namespace crane
+
+#endif  // CRANE_LOCAL_PLANNER__GRAPH_PLANNER_HPP_

crane_local_planner/include/crane_local_planner/rvo2_planner.hpp

@@ -16,6 +16,7 @@
 #include <crane_physics/pid_controller.hpp>
 #include <memory>
 
+#include "graph_planner.hpp"
 #include "planner_base.hpp"
 
 // cspell: ignore OBST
@@ -40,6 +41,9 @@ class RVO2Planner : public LocalPlannerBase
 private:
   std::unique_ptr<RVO::RVOSimulator> rvo_sim;
 
+  // グラフベース経路計画器（KIKSアルゴリズム統合）
+  std::unique_ptr<GraphPlanner> graph_planner;
+
   crane_msgs::msg::RobotCommands pre_commands;
 
   auto toRVO(const Point & point) -> RVO::Vector2 { return RVO::Vector2(point.x(), point.y()); }