diff --git a/.gitignore b/.gitignore index e3e2eb9d1..f5866eedc 100755 --- a/.gitignore +++ b/.gitignore @@ -70,3 +70,6 @@ docs/logs/portal/ # Configuration files crane_bringup/config/diagnostics.yaml + +# Documents +*.pdf diff --git a/AGENTS.md b/AGENTS.md index 3767cfeab..a0e4a249e 100644 --- a/AGENTS.md +++ b/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論理変更。 +- コード/ドキュメント/設定は分離 ### チェックリスト diff --git a/crane_local_planner/CMakeLists.txt b/crane_local_planner/CMakeLists.txt index 2f83ee3ef..56a38a65f 100755 --- a/crane_local_planner/CMakeLists.txt +++ b/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 diff --git a/crane_local_planner/include/crane_local_planner/graph_planner.hpp b/crane_local_planner/include/crane_local_planner/graph_planner.hpp new file mode 100644 index 000000000..63d38cdd1 --- /dev/null +++ b/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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace crane +{ + +// 経由点（ウェイポイント）: 位置と、その点に到達すべき目標速度 +struct Waypoint +{ + Point position; + Velocity target_velocity; // 当該点で実現したい到達速度ベクトル +}; + +class GraphPlanner +{ +public: + using SharedPtr = std::shared_ptr; + + // ロボットの運動制約（最大速度・加減速度） + 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; + + // パラメータをROSから再読込 + void reloadParamsFromROS(); + + // 可視化のフラッシュ（フレーム末にまとめて呼ぶ想定） + void flush() { viz_->flush(); } + +private: + using CircleObstacle = Circle; // center, radius を持つ + using BoxObstacle = Box; // min/max corner を持つ + using Obstacle = std::variant; + + // 探索グラフのノード + 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::infinity()}; + int parent{-1}; + }; + + // コア補助関数群 + auto buildObstacles(uint8_t my_robot_id) -> std::vector; + static auto intersectsAny(const Segment & seg, const std::vector & 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; + auto expandFrom( + int from_id, const Point & from, const Point & goal, const std::vector & obstacles, + std::vector & nodes) -> std::vector; + + // 疑似コード準拠の候補展開（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 & obstacles) + -> std::optional; + + static auto boxCornersOutward(const BoxObstacle & bb, double offset) -> std::vector; + + auto getOrCreateNodeAt(const Point & p, std::vector & nodes) -> int; + + void expandUntilLineOfSight( + const Point & from, const Point & goal, const std::vector & obstacles, + std::vector & candidate_node_ids, std::vector & nodes, int depth, int max_depth); + + // ウェイポイントに達すべき速度を付与（式(1)） + auto buildWaypointsWithVelocities( + const std::vector & path_points, const Velocity & v0, const Constraints & limits) const + -> std::vector; + + // ユーティリティ + 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_ diff --git a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp index f8e7ed7bf..1a0740d80 100644 --- a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp @@ -16,6 +16,7 @@ #include #include +#include "graph_planner.hpp" #include "planner_base.hpp" // cspell: ignore OBST @@ -40,6 +41,9 @@ class RVO2Planner : public LocalPlannerBase private: std::unique_ptr rvo_sim; + // グラフベース経路計画器（KIKSアルゴリズム統合） + std::unique_ptr graph_planner; + crane_msgs::msg::RobotCommands pre_commands; auto toRVO(const Point & point) -> RVO::Vector2 { return RVO::Vector2(point.x(), point.y()); } diff --git a/crane_local_planner/src/graph_planner.cpp b/crane_local_planner/src/graph_planner.cpp new file mode 100644 index 000000000..b25d11d8a --- /dev/null +++ b/crane_local_planner/src/graph_planner.cpp @@ -0,0 +1,709 @@ +// 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. + +#include "crane_local_planner/graph_planner.hpp" + +#include +#include +#include + +namespace crane +{ +GraphPlanner::GraphPlanner(rclcpp::Node & node, WorldModelWrapper::SharedPtr world_model) +: node_(&node), world_(world_model) +{ + // 可視化レイヤを自前で用意 + viz_ = std::make_shared("graph_planner"); + node_->declare_parameter("graph_planner.max_expansion", params_.max_expansion); + node_->declare_parameter("graph_planner.node_tangent_offset", params_.node_tangent_offset); + node_->declare_parameter("graph_planner.node_merge_epsilon", params_.node_merge_epsilon); + + node_->declare_parameter("graph_planner.robot_radius", params_.robot_radius); + node_->declare_parameter("graph_planner.static_margin", params_.static_margin); + node_->declare_parameter("graph_planner.K_alpha", params_.K_alpha); + node_->declare_parameter("graph_planner.K_v", params_.K_v); + node_->declare_parameter("graph_planner.K_t", params_.K_t); + node_->declare_parameter("graph_planner.far_margin_cap", params_.far_margin_cap); + reloadParamsFromROS(); +} + +void GraphPlanner::reloadParamsFromROS() +{ + // 初回のみ宣言し、呼び出し毎に現在値を取得 + params_.max_expansion = node_->get_parameter("graph_planner.max_expansion").as_int(); + params_.node_tangent_offset = + node_->get_parameter("graph_planner.node_tangent_offset").as_double(); + params_.node_merge_epsilon = node_->get_parameter("graph_planner.node_merge_epsilon").as_double(); + + params_.robot_radius = node_->get_parameter("graph_planner.robot_radius").as_double(); + params_.static_margin = node_->get_parameter("graph_planner.static_margin").as_double(); + params_.K_alpha = node_->get_parameter("graph_planner.K_alpha").as_double(); + params_.K_v = node_->get_parameter("graph_planner.K_v").as_double(); + params_.K_t = node_->get_parameter("graph_planner.K_t").as_double(); + params_.far_margin_cap = node_->get_parameter("graph_planner.far_margin_cap").as_double(); +} + +auto GraphPlanner::intersects(const Segment & seg, const CircleObstacle & c) -> bool +{ + return !getIntersections(c, seg).empty(); +} + +auto GraphPlanner::intersects(const Segment & seg, const BoxObstacle & b) -> bool +{ + return bg::intersects(seg, b); +} + +auto GraphPlanner::intersects(const Segment & seg, const Obstacle & ob) -> bool +{ + return std::visit([&](const auto & o) { return intersects(seg, o); }, ob); +} + +auto GraphPlanner::intersectsAny(const Segment & seg, const std::vector & obs) -> bool +{ + return std::any_of(obs.begin(), obs.end(), [&](const auto & ob) { return intersects(seg, ob); }); +} + +// 式(2)補助: 動的マージン評価で用いる予測時間 ∆t を計算 +static inline double compute_dt(double r, double K_v, double K_t) +{ + return std::min(r / std::max(1e-6, K_v), K_t); +} + +// 円弧スムージング: コーナー点が円障害物の境界上にある場合、前後点を円へ射影し円弧で接続 +static std::vector smooth_with_circle_arcs( + const std::vector & path_pts, const std::vector> & obstacles, + const Point & goal, double arc_step_rad = 0.15) +{ + using CircleObs = Circle; + std::vector out; + if (path_pts.size() < 3) return path_pts; + auto eq_pt = [](const Point & a, const Point & b, double eps = 1e-6) { + return (a - b).norm() <= eps; + }; + auto intersects_segment = [&](const Segment & seg, const std::variant & ob) { + if (std::holds_alternative(ob)) { + const auto & c = std::get(ob); + return !getIntersections(c, seg).empty(); + } else { + const auto & b = std::get(ob); + return bg::intersects(seg, b); + } + }; + auto intersects_any_except = [&](const Segment & seg, const CircleObs & skip) { + for (const auto & ob : obstacles) { + if (std::holds_alternative(ob)) { + const auto & c = std::get(ob); + if (eq_pt(c.center, skip.center, 1e-6) && std::abs(c.radius - skip.radius) < 1e-6) continue; + if (intersects_segment(seg, ob)) return true; + } else { + if (intersects_segment(seg, ob)) return true; + } + } + return false; + }; + auto find_circle_near = [&](const Point & p) -> std::optional { + double best_err = 1e9; + std::optional best; + for (const auto & ob : obstacles) { + if (!std::holds_alternative(ob)) continue; + const auto & c = std::get(ob); + double err = std::abs((p - c.center).norm() - c.radius); + if (err < best_err) { + best_err = err; + best = c; + } + } + if (best && best_err < 0.02) return best; // 2cm以内なら円周上とみなす + return std::nullopt; + }; + + out.push_back(path_pts.front()); + for (size_t i = 1; i + 1 < path_pts.size(); ++i) { + const Point & prev = path_pts[i - 1]; + const Point & curr = path_pts[i]; + const Point & next = path_pts[i + 1]; + auto co = find_circle_near(curr); + if (!co) { + out.push_back(curr); + continue; + } + const Point center = co->center; + const double R = co->radius; + // 射影関数 + auto proj = [&](const Point & x) { + Point d = x - center; + Point res; + if (d.norm() < 1e-9) { + res = center + Point(R, 0.0); + } else { + res = center + (d / d.norm()) * R; + } + return res; + }; + Point A = proj(prev); + Point B = proj(next); + + // ゴールから当該円への接線接点を求め、障害物非交差なものを採用 + auto tangents_from_point_to_circle = [&](const Point & p, const CircleObs & c) { + std::vector list; + Point u = p - c.center; + double d2 = u.squaredNorm(); + double r = c.radius; + if (d2 <= r * r) return list; // 内側からは接線なし + double l = (r * r) / d2; + double h = r * std::sqrt(std::max(0.0, d2 - r * r)) / d2; + Point perp = getVerticalVec(u); + Point t1 = c.center + l * u + h * perp; + Point t2 = c.center + l * u - h * perp; + list.push_back(t1); + list.push_back(t2); + return list; + }; + auto candidates = tangents_from_point_to_circle(goal, *co); + std::optional tangent; + if (!candidates.empty()) { + // 非交差な接線接点のみ + for (const auto & t : candidates) { + Segment s(t, goal); + if (!intersects_any_except(s, *co)) { + tangent = t; + break; + } + } + } + auto ang = [&](const Point & x) { return std::atan2((x - center).y(), (x - center).x()); }; + double a0 = ang(A); + double a1 = tangent ? ang(*tangent) : ang(B); + double da = a1 - a0; + while (da > M_PI) da -= 2 * M_PI; + while (da < -M_PI) da += 2 * M_PI; + int steps = std::max(1, (int)std::ceil(std::abs(da) / std::max(0.05, arc_step_rad))); + for (int k = 1; k <= steps; ++k) { + double a = a0 + da * (double)k / (double)steps; + Point p; + p.x() = center.x() + R * std::cos(a); + p.y() = center.y() + R * std::sin(a); + out.push_back(p); + } + // 接線接点を通して以降は直線でgoal方向へ進むため、curr→nextの「円射影B」ではなく、 + // 接線接点が得られていればそれを優先して採用済み + } + out.push_back(path_pts.back()); + return out; +} + +auto GraphPlanner::buildObstacles(uint8_t my_robot_id) -> std::vector +{ + std::vector obs; + for (const auto & rr : world_->ours().getAvailableRobots(my_robot_id)) { + obs.emplace_back(rr->geometry()); + } + for (const auto & rr : world_->theirs().getAvailableRobots()) { + obs.emplace_back(rr->geometry()); + } + + // ペナルティエリア: inflateBox でマージン付きボックスを生成 + obs.emplace_back(BoxObstacle{inflateBox(world_->getOurPenaltyArea(), params_.static_margin)}); + obs.emplace_back(BoxObstacle{inflateBox(world_->getTheirPenaltyArea(), params_.static_margin)}); + + // フィールド境界: 障害物としては追加せず、展開時にフィールド外となる候補を棄却する + return obs; +} + +auto GraphPlanner::tangentPointsFromPointToCircle(const Point & p, const CircleObstacle & c) + -> std::vector +{ + std::vector tps; + Point u = p - c.center; + double d2 = u.squaredNorm(); + double r = c.radius; + if (d2 <= r * r) { + // 点が円の内側にあると接線は引けない + return tps; + } + // 標準式により接点を計算 + // T = C + (r^2/d^2) * u +/- (r * sqrt(d^2 - r^2) / d^2) * perp(u) + double l = (r * r) / d2; + double h = r * std::sqrt(std::max(0.0, d2 - r * r)) / d2; + Point perp = getVerticalVec(u); + + Point t1 = c.center + l * u + h * perp; + Point t2 = c.center + l * u - h * perp; + + // 数値衝突を避けるため、円外側へ微小オフセット + Point n1 = (t1 - c.center).normalized(); + Point n2 = (t2 - c.center).normalized(); + t1 += n1 * 1e-3; // 微小イプシロン + t2 += n2 * 1e-3; + + tps.push_back(t1); + tps.push_back(t2); + return tps; +} + +auto GraphPlanner::angleCosBetween(const Point & a, const Point & b) -> double +{ + double na = a.norm(); + double nb = b.norm(); + if (na < 1e-9 || nb < 1e-9) return 1.0; + return a.dot(b) / (na * nb); +} + +auto GraphPlanner::expandFrom( + int from_id, const Point & from, const Point & goal, const std::vector & obstacles, + std::vector & nodes) -> std::vector +{ + std::vector new_node_ids; + + // 目標までの直線が空いていれば、それを優先 + Segment direct(from, goal); + if (!intersectsAny(direct, obstacles)) { + // ゴールをノードとして追加 + int id = static_cast(nodes.size()); + nodes.push_back(Node{id, goal}); + new_node_ids.push_back(id); + return new_node_ids; + } + + // 直線に衝突する障害物のみを対象に接線候補を生成（オンデマンド拡張） + std::vector blocking; + for (const auto & ob : obstacles) { + if (intersects(direct, ob)) blocking.push_back(ob); + } + + // 衝突障害物ごとに候補点（接線/外側オフセット）を生成 + for (const auto & ob : blocking) { + if (std::holds_alternative(ob)) { + const auto & co = std::get(ob); + for (const auto & tp : tangentPointsFromPointToCircle(from, co)) { + // 辺方向に僅かに進めた点を候補とする（数値安定性） + Point dir = (tp - from).normalized(); + Point cand = tp + dir * params_.node_tangent_offset; + + // from→cand がいずれの障害物とも交差しないこと + Segment e(from, cand); + if (intersectsAny(e, obstacles)) continue; + + // フィールド内であること + if (!world_->point_checker.isFieldInside(cand, 0.0)) continue; + + int id = -1; + for (size_t k = 0; k < nodes.size(); ++k) { + if ((nodes[k].p - cand).norm() <= params_.node_merge_epsilon) { + id = static_cast(k); + break; + } + } + if (id == -1) { + id = static_cast(nodes.size()); + nodes.push_back(Node{id, cand}); + } + new_node_ids.push_back(id); + } + } else { + // ボックス障害物: 角から外方向にオフセットした点を候補にする + const auto & bb = std::get(ob); + auto minc = bb.min_corner(); + auto maxc = bb.max_corner(); + std::vector corners{ + Point(minc.x(), minc.y()), Point(maxc.x(), minc.y()), Point(maxc.x(), maxc.y()), + Point(minc.x(), maxc.y())}; + Point center = 0.5 * (minc + maxc); + for (const auto & c : corners) { + Point n = (c - center).normalized(); + Point around = c + n * params_.node_tangent_offset; + Segment e(from, around); + if (intersectsAny(e, obstacles)) continue; + if (!world_->point_checker.isFieldInside(around, 0.0)) continue; + int id = -1; + for (size_t k = 0; k < nodes.size(); ++k) { + if ((nodes[k].p - around).norm() <= params_.node_merge_epsilon) { + id = static_cast(k); + break; + } + } + if (id == -1) { + id = static_cast(nodes.size()); + nodes.push_back(Node{id, around}); + } + new_node_ids.push_back(id); + } + } + } + return new_node_ids; +} + +// ---- 疑似コード準拠の補助: 最初の交差障害物を取得（Step(2)前提）---- +auto GraphPlanner::firstIntersection( + const Point & from, const Point & to, const std::vector & obstacles) + -> std::optional +{ + Segment seg(from, to); + const Point dir = to - from; + const double len2 = dir.squaredNorm(); + if (len2 < 1e-12) return std::nullopt; + + auto point_t = [&](const Point & p) { + // seg の始点からみたパラメータ t を概算（直交射影） + double t = (dir.x() * (p.x() - from.x()) + dir.y() * (p.y() - from.y())) / len2; + return std::clamp(t, 0.0, 1.0); + }; + + std::optional best; + double best_t = 2.0; + + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto & ob = obstacles[i]; + if (std::holds_alternative(ob)) { + CircleObstacle c = std::get(ob); + c.radius = c.radius + 0.2; + auto pts = getIntersections(c, seg); + for (const auto & p : pts) { + double t = point_t(p); + if (t < best_t) { + best_t = t; + best = IntersectionInfo{i, t, p}; + } + } + } else { + const auto & b = std::get(ob); + if (!bg::intersects(seg, b)) continue; + // 箱の4辺との交点を調べる（seg vs seg のみを使用） + auto minc = b.min_corner(); + auto maxc = b.max_corner(); + Point p00(minc.x(), minc.y()); + Point p10(maxc.x(), minc.y()); + Point p11(maxc.x(), maxc.y()); + Point p01(minc.x(), maxc.y()); + std::array edges{ + Segment(p00, p10), Segment(p10, p11), Segment(p11, p01), Segment(p01, p00)}; + for (const auto & e : edges) { + std::vector ips; + bg::intersection(seg, e, ips); + for (const auto & p : ips) { + double t = point_t(p); + if (t < best_t) { + best_t = t; + best = IntersectionInfo{i, t, p}; + } + } + } + } + } + + return best; +} + +auto GraphPlanner::boxCornersOutward(const BoxObstacle & bb, double offset) -> std::vector +{ + auto minc = bb.min_corner(); + auto maxc = bb.max_corner(); + std::vector corners{ + Point(minc.x(), minc.y()), Point(maxc.x(), minc.y()), Point(maxc.x(), maxc.y()), + Point(minc.x(), maxc.y())}; + Point center = 0.5 * (minc + maxc); + std::vector out; + out.reserve(4); + for (const auto & c : corners) { + Point n = (c - center).normalized(); + out.push_back(c + n * offset); + } + return out; +} + +auto GraphPlanner::getOrCreateNodeAt(const Point & p, std::vector & nodes) -> int +{ + for (size_t k = 0; k < nodes.size(); ++k) { + if ((nodes[k].p - p).norm() <= params_.node_merge_epsilon) return static_cast(k); + } + int id = static_cast(nodes.size()); + nodes.push_back(Node{id, p}); + return id; +} + +void GraphPlanner::expandUntilLineOfSight( + const Point & from, const Point & goal, const std::vector & obstacles, + std::vector & candidate_node_ids, std::vector & nodes, int depth, int max_depth) +{ + if (depth > max_depth) return; + + auto inter = firstIntersection(from, goal, obstacles); + if (!inter) { + // 直通: ゴールを候補に + int gid = getOrCreateNodeAt(goal, nodes); + candidate_node_ids.push_back(gid); + return; + } + + const auto & ob = obstacles[inter->obs_index]; + if (std::holds_alternative(ob)) { + const auto & co = std::get(ob); + // 接線接点（微小外押し） + for (const auto & tp : tangentPointsFromPointToCircle(from, co)) { + Point dir = (tp - from).normalized(); + Point cand = tp + dir * params_.node_tangent_offset; + // from→cand が障害物と重なるなら除外 + if (intersectsAny(Segment(from, cand), obstacles)) continue; + if (!world_->point_checker.isFieldInside(cand, 0.0)) continue; + int id = getOrCreateNodeAt(cand, nodes); + candidate_node_ids.push_back(id); + // さらに先で直線が通るまで再帰的に候補を追加 + expandUntilLineOfSight( + cand, goal, obstacles, candidate_node_ids, nodes, depth + 1, max_depth); + } + } else { + // 矩形は簡易化: 角から外に僅かに出した点を候補に + const auto & bb = std::get(ob); + for (const auto & cand : boxCornersOutward(bb, params_.node_tangent_offset)) { + if (intersectsAny(Segment(from, cand), obstacles)) continue; + if (!world_->point_checker.isFieldInside(cand, 0.0)) continue; + int id = getOrCreateNodeAt(cand, nodes); + candidate_node_ids.push_back(id); + expandUntilLineOfSight( + cand, goal, obstacles, candidate_node_ids, nodes, depth + 1, max_depth); + } + } +} + +// 式(1): ポリライン各区間に対する到達速度・時間を計算 +auto GraphPlanner::buildWaypointsWithVelocities( + const std::vector & path_points, const Velocity & v0, const Constraints & limits) const + -> std::vector +{ + const int Np = static_cast(path_points.size()); + std::vector wps; + if (Np == 0) return wps; + if (Np == 1) { + wps.push_back(Waypoint{path_points.front(), Velocity::Zero()}); + return wps; + } + + // エッジ e(n) と長さ ℓ(n) を事前計算 + const int N = Np - 1; // エッジ数 + std::vector e(N); + std::vector L(N); + for (int n = 0; n < N; ++n) { + e[n] = path_points[n + 1] - path_points[n]; + L[n] = e[n].norm(); + } + + // 逆走査: ℓp(n) を式(1)の制約に基づき計算 + std::vector Lp(N, 0.0); // ℓp(n) + for (int n = N - 1; n >= 0; --n) { + if (n == N - 1) { + Lp[n] = 0.0; // Eq.(1): ℓp(N) = 0 + } else { + double cos_next = angleCosBetween(e[n + 1], e[n]); + cos_next = std::clamp(cos_next, -1.0, 1.0); + double bound_from_next = (L[n + 1] + Lp[n + 1]) * std::max(0.0, cos_next); + double vmax_bound = (limits.vmax * limits.vmax) / (2.0 * limits.alpha_dec); + // 初期化 + Lp[n] = 0.0; + Lp[n] = std::min(vmax_bound, bound_from_next); + if (Lp[n] < 0.0) Lp[n] = 0.0; + } + } + + // 正方向走査: v0(n), vm(n), ve(n) を式(1)に基づき計算 + std::vector v0n(N, 0.0), vm(N, 0.0), ve(N, 0.0); + for (int n = 0; n < N; ++n) { + double cos_theta; + if (n == 0) { + cos_theta = angleCosBetween(e[0], v0); + } else { + cos_theta = angleCosBetween(e[n], e[n - 1]); + } + cos_theta = std::clamp(cos_theta, -1.0, 1.0); + v0n[n] = std::max(0.0, cos_theta) * (n == 0 ? v0.norm() : ve[n - 1]); + + // vm（式(1)） + // vm = min( sqrt( 2 αa αb (ℓ + ℓp) + αb v0^2 ) / (αa + αb), vmax ) + double num = 2.0 * limits.alpha_acc * limits.alpha_dec * (L[n] + Lp[n]) + + limits.alpha_dec * v0n[n] * v0n[n]; + double den = limits.alpha_acc + limits.alpha_dec; + vm[n] = std::min(std::sqrt(std::max(0.0, num)) / std::max(1e-6, den), limits.vmax); + + // ve（式(1)）: ve = min( sqrt(v0^2 + 2 αa ℓ), sqrt(2 αb ℓp) ) + double ve_acc = std::sqrt(std::max(0.0, v0n[n] * v0n[n] + 2.0 * limits.alpha_acc * L[n])); + double ve_dec = std::sqrt(std::max(0.0, 2.0 * limits.alpha_dec * Lp[n])); + ve[n] = std::min({vm[n], ve_acc, ve_dec}); + } + + // 各ウェイポイントでの到達速度ベクトル（当該区間の終端速度）を付与 + wps.reserve(Np); + for (int i = 0; i < Np; ++i) { + Velocity v = Velocity::Zero(); + if (i == 0) { + // 初期点: v0 を第1エッジ方向へ射影し、ve[0]でクリップ + if (N > 0 && L[0] > 1e-6) v = (e[0] / L[0]) * std::min(ve[0], limits.vmax); + } else if (i - 1 < N && L[i - 1] > 1e-6) { + v = (e[i - 1] / L[i - 1]) * std::min(ve[i - 1], limits.vmax); + } + wps.push_back(Waypoint{path_points[i], v}); + } + return wps; +} + +auto GraphPlanner::plan( + const Pose2D & start, const Pose2D & goal, const Velocity & v0, const Constraints & limits, + uint8_t my_robot_id) -> std::vector +{ + // world_ は前提として有効 + + RCLCPP_DEBUG( + node_->get_logger(), + "[GraphPlanner] plan start=(%.2f,%.2f) goal=(%.2f,%.2f) v0=%.2f vmax=%.2f acc=%.2f dec=%.2f", + start.pos.x(), start.pos.y(), goal.pos.x(), goal.pos.y(), v0.norm(), limits.vmax, + limits.alpha_acc, limits.alpha_dec); + + reloadParamsFromROS(); + + // 障害物生成（自IDを除外） + const auto obstacles = buildObstacles(my_robot_id); + + // ノード: 0=始点。ゴールは必要時に追加 + std::vector nodes; + nodes.push_back(Node{0, start.pos}); + + // Dijkstra（疑似コード準拠） + std::priority_queue Open; // 未確定集合 + std::vector state; // コストと親 + state.resize(1); + state[0].cost = 0.0; + state[0].parent = -1; + + std::vector in_closed; + in_closed.resize(1, 0); + std::vector closed_order; + closed_order.push_back(0); // Step(1): start を Closed に置く + + auto ensure_size = [&](int id) { + if (id >= static_cast(state.size())) state.resize(id + 1); + if (id >= static_cast(in_closed.size())) in_closed.resize(id + 1, 0); + }; + + int goal_node = -1; + int expansions = 0; + + // ループ + for (int iter = 0; iter < params_.max_expansion; ++iter) { + int u = closed_order.back(); // Step(2)の前提: 直近で確定したノード + + // ---- 候補生成（Step(2)-(3)） ---- + std::vector candidate; + expandUntilLineOfSight(nodes[u].p, goal.pos, obstacles, candidate, nodes, 0, 8); + + // ---- 候補評価（Step(4)-(5)） ---- + for (int v : candidate) { + ensure_size(v); + if (in_closed[v]) continue; // Closed にあるものは除外 + if (v == u) continue; // 自己ループ回避 + // u→v の距離 + double w = (nodes[v].p - nodes[u].p).norm(); + if (!std::isfinite(w)) continue; + double new_cost = state[u].cost + w; + if (state[v].cost <= new_cost) { + // 既存の方が良ければスキップ + } else { + state[v].cost = new_cost; + state[v].parent = u; + Open.push(PQItem{v, new_cost}); + } + expansions++; + } + + // ---- 次の確定（Step(6)） ---- + int x = -1; + while (!Open.empty()) { + auto top = Open.top(); + Open.pop(); + ensure_size(top.node_id); + if (in_closed[top.node_id]) continue; // 古い重複を破棄 + x = top.node_id; + break; + } + if (x < 0) break; // Open が空: 失敗 + in_closed[x] = 1; + closed_order.push_back(x); + + // ゴール到達（ノードとして一致した場合） + if ((nodes[x].p - goal.pos).norm() < 1e-6) { + goal_node = x; + break; + } + } + + RCLCPP_DEBUG( + node_->get_logger(), "[GraphPlanner] expansions=%d nodes=%zu goal_node=%d", expansions, + nodes.size(), goal_node); + + if (goal_node < 0) { + // ゴールノードが生成されなかった場合、最も近い確定ノードを採用 + double best_d = 1e9; + int best_id = -1; + for (int id : closed_order) { + double d = (nodes[id].p - goal.pos).norm(); + if (d < best_d) { + best_d = d; + best_id = id; + } + } + goal_node = best_id; + } + + if (goal_node < 0) { + RCLCPP_WARN(node_->get_logger(), "[GraphPlanner] goal_node<0. Fallback to straight line."); + std::vector pts{start.pos, goal.pos}; + return buildWaypointsWithVelocities(pts, v0, limits); + } + + // 最終経路の復元 + std::vector path_pts; + for (int cur = goal_node; cur >= 0;) { + path_pts.push_back(nodes[cur].p); + if (cur < 0 || cur >= static_cast(state.size())) break; + cur = state[cur].parent; + } + std::reverse(path_pts.begin(), path_pts.end()); + + // 可視化（任意） + // 始点・終点 + viz_->circle() + .center(start.pos) + .radius(0.05) + .stroke("green", 0.8) + .strokeWidth(6) + .fill("green", 0.2) + .build(); + viz_->circle() + .center(goal.pos) + .radius(0.05) + .stroke("green", 0.8) + .strokeWidth(6) + .fill("green", 0.2) + .build(); + // 円弧スムージング（接線と円を滑らかに接続） + auto smooth_pts = smooth_with_circle_arcs(path_pts, obstacles, goal.pos, 0.15); + // 経路はポリラインで描画 + { + auto poly = viz_->polyline().stroke("cyan", 0.8).strokeWidth(20); + for (const auto & p : smooth_pts) { + poly.addPoint(p); + } + poly.build(); + } + // ウェイポイントの速度ベクトル（短い矢印） + auto wps = buildWaypointsWithVelocities(smooth_pts, v0, limits); + for (const auto & wp : wps) { + Point to = wp.position + wp.target_velocity * 0.15; // スケール係数 + viz_->line().start(wp.position).end(to).stroke("orange", 0.9).strokeWidth(6).build(); + } + // フラッシュはフレーム末にRVO2側でまとめて実行 + + return buildWaypointsWithVelocities(smooth_pts, v0, limits); +} + +} // namespace crane diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index ce58527a6..c2fc81fa8 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -7,6 +7,7 @@ #include "crane_local_planner/rvo2_planner.hpp" #include +#include #include // cspell: ignore OBST @@ -44,6 +45,9 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node) RVO_TIME_STEP, RVO_NEIGHBOR_DIST, RVO_MAX_NEIGHBORS, RVO_TIME_HORIZON, RVO_TIME_HORIZON_OBST, RVO_RADIUS, RVO_MAX_SPEED); + // GraphPlanner を初期化（内部でVisualizerを保持） + graph_planner = std::make_unique(node, world_model); + // friend robots -> 0~19 // enemy robots -> 20~39 for (int i = 0; i < 40; i++) { @@ -158,32 +162,122 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) -> // v = sqrt(v0^2 + 2ax) // v0 = 0, x = diff(=target_vel) // v = sqrt(2ax) - double max_vel_by_decel = std::sqrt(2.0 * acceleration * position_diff.norm()); + const double dist = position_diff.norm(); + double max_vel_by_decel = std::sqrt(std::max(0.0, 2.0 * acceleration * dist)); // v = v0 + at double max_vel_by_acc = pre_vel + acceleration * RVO_TIME_STEP; - double max_vel = + double max_vel_cap = std::min(static_cast(command.local_planner_config.max_velocity), MAX_VEL); - max_vel = std::min(max_vel, max_vel_by_decel); - max_vel = std::min(max_vel, max_vel_by_acc); - if ( - world_model->getMsg().play_situation.command_raw.value == - robocup_ssl_msgs::msg::Referee::COMMAND_STOP) { + double max_vel = std::min(max_vel_cap, std::min(max_vel_by_decel, max_vel_by_acc)); + const bool is_stop_cmd = world_model->getMsg().play_situation.command_raw.value == + robocup_ssl_msgs::msg::Referee::COMMAND_STOP; + if (is_stop_cmd) { // 1.5m/sだとたまに超えるので1.0m/sにしておく max_vel = std::min(max_vel, 1.0); + } else { + // 進行すべき場合にゼロにならないよう下限を適用 + const double v_min = static_cast(command.local_planner_config.terminal_velocity); + if (dist > 1e-3) { + max_vel = std::max(max_vel, std::min(v_min, max_vel_cap)); + } + } + + if (!is_stop_cmd && dist > 0.05 && max_vel < 1e-6) { + RCLCPP_WARN( + rclcpp::get_logger("rvo2_planner"), + "[RVO2] max_vel collapsed to 0. dist=%.3f pre_v=%.3f acc=%.3f vmax_cap=%.3f " + "by_decel=%.3f by_acc=%.3f", + dist, pre_vel, acceleration, max_vel_cap, max_vel_by_decel, max_vel_by_acc); } command.local_planner_config.final_planned_max_acceleration = acceleration; command.local_planner_config.final_planned_max_velocity = max_vel; - target_vel = target_vel.normalized() * max_vel; + // GraphPlanner を用いて優先速度（Preferred Velocity）を算出（有効時） + bool planned = false; + Pose2D start_pose{current_position, 0.0}; + Pose2D goal_pose; + goal_pose.pos = Point( + command.position_target_mode.front().target_x, + command.position_target_mode.front().target_y); + goal_pose.theta = 0.0; + + try { + GraphPlanner::Constraints limits; + limits.vmax = max_vel; + limits.alpha_acc = acceleration; // per-cycle planned acceleration + limits.alpha_dec = deceleration; // deceleration (min_acceleration) + + Velocity v0; + v0 << command.current_velocity.x, command.current_velocity.y; + auto path = graph_planner->plan(start_pose, goal_pose, v0, limits, command.robot_id); + if (!path.empty()) { + // 現在位置に最も近いウェイポイントを選択 + int nearest = 0; + double best_d = 1e9; + for (size_t i = 0; i < path.size(); ++i) { + double d = (path[i].position - current_position).norm(); + if (d < best_d) { + best_d = d; + nearest = static_cast(i); + } + } + int next_i = std::min(nearest + 1, static_cast(path.size() - 1)); + // 式(1)により算出された到達速度を基に優先速度を決定 + Velocity pref = path[nearest].target_velocity; + if (pref.norm() < 1e-6) { + // 近傍点の速度が0の場合、進行方向から速度を与える + Vector2 dir = path[next_i].position - path[nearest].position; + if (dir.norm() <= 1e-6) { + // 区間が極小 or 終端の場合は現在位置→目標方向を使用 + dir = goal_pose.pos - current_position; + } + if (dir.norm() > 1e-6) { + double v_suggest = path[next_i].target_velocity.norm(); + // 次点の速度もゼロの場合は、少なくとも現在フレームで許される上限で与える + if (v_suggest < 1e-6) v_suggest = limits.vmax; + const double v_cap = std::min( + limits.vmax, + std::max(command.local_planner_config.terminal_velocity, v_suggest)); + pref = dir.normalized() * v_cap; + } + } + + // 末端速度の下限設定（terminal_velocity） + if (pref.norm() < command.local_planner_config.terminal_velocity) { + Vector2 dir = pref.norm() > 1e-6 ? pref : goal_pose.pos - current_position; + if (dir.norm() > 1e-6) { + pref = dir.normalized() * command.local_planner_config.terminal_velocity; + } + } + if ((goal_pose.pos - current_position).norm() > 0.05 && pref.norm() < 1e-6) { + RCLCPP_WARN( + rclcpp::get_logger("rvo2_planner"), + "[RVO2] pref is zero with non-trivial dist. dist=%.3f vmax=%.3f term=%.3f", dist, + limits.vmax, command.local_planner_config.terminal_velocity); + } + // RVO へ適用 + rvo_sim->setAgentPrefVelocity(command.robot_id, toRVO(pref)); + rvo_sim->setAgentMaxSpeed(command.robot_id, std::max(pref.norm(), 0.01)); + planned = true; + } + } catch (const std::exception & e) { + RCLCPP_ERROR(rclcpp::get_logger("rvo2_planner"), "[RVO2] Exception: %s", e.what()); + throw e; + (void)e; // 例外時はフォールバック + } - if (target_vel.norm() < command.local_planner_config.terminal_velocity) { - target_vel = target_vel.normalized() * command.local_planner_config.terminal_velocity; + if (!planned) { + // フォールバック: 直線・台形近似の優先速度 + target_vel = target_vel.normalized() * max_vel; + if (target_vel.norm() < command.local_planner_config.terminal_velocity) { + target_vel = target_vel.normalized() * command.local_planner_config.terminal_velocity; + } + rvo_sim->setAgentPrefVelocity(command.robot_id, toRVO(target_vel)); + rvo_sim->setAgentMaxSpeed(command.robot_id, max_vel); } - rvo_sim->setAgentPrefVelocity(command.robot_id, toRVO(target_vel)); - rvo_sim->setAgentMaxSpeed(command.robot_id, max_vel); break; } case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: { @@ -228,6 +322,9 @@ auto RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) -> rvo_sim->setAgentPrefVelocity(enemy_robot->id + 20, RVO::Vector2(0.f, 0.f)); } } + + // GraphPlannerの可視化はフレーム末にまとめて送出 + graph_planner->flush(); } auto RVO2Planner::extractRobotCommandsFromRVOSim( diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp index 343b77a97..aef7044a5 100644 --- a/crane_robot_skills/src/goalie.cpp +++ b/crane_robot_skills/src/goalie.cpp @@ -284,9 +284,9 @@ void Goalie::inplay(bool enable_emit) // 敵の予想されるロボット位置とゴールの間の直線を10点に分割 std::vector forward_pooints = getSeparatedPoints(forward_line, 10); for (int i = forward_pooints.size() - 1; i >= 0; --i) { - // goalieが前進守備位置に到達する時間 + // goalieが前進守備位置に到達する時間（終端速度0を指定） double travel_time = - getTravelTimeTrapezoidal(this->robot(), forward_pooints[i], 0.5, 2.0); + getTravelTimeTrapezoidal(this->robot(), forward_pooints[i], 0.5, 2.0, 0.0); if (estimated_ball_reach_time > travel_time) { threat_point = forward_pooints[i]; break; diff --git a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp index 83c9a473e..b8f5d0991 100644 --- a/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp +++ b/utility/crane_geometry/include/crane_geometry/geometry_operations.hpp @@ -31,6 +31,15 @@ inline auto createBox(const Point & p1, const Point & p2) -> Box return box; } +inline auto inflateBox(const Box & box, double margin) -> Box +{ + const auto minc = box.min_corner(); + const auto maxc = box.max_corner(); + const Point pmin(minc.x() - margin, minc.y() - margin); + const Point pmax(maxc.x() + margin, maxc.y() + margin); + return createBox(pmin, pmax); +} + inline auto getAngle(const Vector2 & vec) -> double { return atan2(vec.y(), vec.x()); } inline auto normalizeAngle(double angle_rad) -> double @@ -113,6 +122,37 @@ inline auto getReachTime( } } +// 始端/終端速度を考慮した到達時間（台形/三角プロファイル） +inline auto getReachTime( + double distance, double v_in, double v_out, double alpha_acc, double alpha_dec, double max_vel) + -> double +{ + const double a = std::max(1e-6, alpha_acc); + const double b = std::max(1e-6, alpha_dec); + const double L = std::max(0.0, distance); + v_in = std::max(0.0, v_in); + v_out = std::max(0.0, v_out); + const double vm = std::max(1e-6, max_vel); + + const double num = 2.0 * a * b * L + b * v_in * v_in + a * v_out * v_out; + const double den = a + b; + const double v_peak = std::sqrt(std::max(0.0, num / den)); + + if (v_peak <= vm + 1e-9) { + const double t_acc = std::max(0.0, (v_peak - v_in) / a); + const double t_dec = std::max(0.0, (v_peak - v_out) / b); + return t_acc + t_dec; + } else { + const double s_acc = std::max(0.0, (vm * vm - v_in * v_in) / (2.0 * a)); + const double s_dec = std::max(0.0, (vm * vm - v_out * v_out) / (2.0 * b)); + const double s_cruise = std::max(0.0, L - s_acc - s_dec); + const double t_acc = std::max(0.0, (vm - v_in) / a); + const double t_dec = std::max(0.0, (vm - v_out) / b); + const double t_cruise = s_cruise / vm; + return t_acc + t_cruise + t_dec; + } +} + inline auto getIntersections(const Segment & segment1, const Segment & segment2) -> std::vector { @@ -177,6 +217,16 @@ inline auto getClosestPointAndDistance(const Geometry1 & geometry1, const Geomet return result; } +inline auto polylineLength(const std::vector & points) -> double +{ + if (points.size() < 2) return 0.0; + double sum = 0.0; + for (size_t i = 1; i < points.size(); ++i) { + sum += (points[i] - points[i - 1]).norm(); + } + return sum; +} + inline auto getCircle(const Point & p1, const Point & p2, const Point & p3) -> std::optional { // Using the formula from https://en.wikipedia.org/wiki/Circumscribed_circle#Cartesian_coordinates_2 diff --git a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp index 7ca9f512e..2879e3a27 100644 --- a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp @@ -335,7 +335,7 @@ auto WorldModelWrapper::getBallSlackTime( auto best_robot = ranges::min( robots | ranges::views::transform([&](const auto & robot) { return std::make_pair( - robot, getTravelTimeTrapezoidal(robot, intercept_point, max_acc, max_vel)); + robot, getTravelTimeTrapezoidal(robot, intercept_point, max_acc, max_vel, 0.0)); }), ranges::less{}, [](const auto & pair) { return pair.second; // 移動時間が小さい順にソート diff --git a/utility/crane_physics/README.md b/utility/crane_physics/README.md index c72049ca8..c5a7d6284 100644 --- a/utility/crane_physics/README.md +++ b/utility/crane_physics/README.md @@ -139,14 +139,6 @@ bool getBallSensorAvailable(rclcpp::Time now) const; // Sensor status ### 3. Travel Time Calculations (`travel_time.hpp`) -#### Simple Travel Time - -```cpp -double getTravelTime(std::shared_ptr robot, Point target); -``` - -Basic calculation assuming constant velocity. - #### Trapezoidal Motion Profile ```cpp diff --git a/utility/crane_physics/include/crane_physics/travel_time.hpp b/utility/crane_physics/include/crane_physics/travel_time.hpp index 8b5bcd17f..fc088180b 100644 --- a/utility/crane_physics/include/crane_physics/travel_time.hpp +++ b/utility/crane_physics/include/crane_physics/travel_time.hpp @@ -12,53 +12,33 @@ namespace crane { -inline auto getTravelTime(std::shared_ptr robot, Point target) -> double -{ - // 現在速度で割るだけ - return (target - robot->pose.pos).norm() / robot->vel.linear.norm(); -} - inline auto getTravelTimeTrapezoidal( std::shared_ptr robot, Point target, const double max_acceleration, - const double max_velocity) -> double + const double max_velocity, const double v_end = 0.0) -> double { - double distance = (target - robot->pose.pos).norm(); - double initial_vel = (target - robot->pose.pos).normalized().dot(robot->vel.linear); + const Vector2 d = (target - robot->pose.pos); + const double L = d.norm(); + const double v_in = (L > 1e-9) ? std::max(0.0, robot->vel.linear.dot(d / L)) : 0.0; - // 加速・減速にかかる時間 - double accel_time = (max_velocity - initial_vel) / max_acceleration; - double decel_time = max_velocity / max_acceleration; + const double a = std::max(1e-6, max_acceleration); + const double b = std::max(1e-6, max_acceleration); - // 加速・減速にかかる距離 - double accel_distance = (initial_vel + max_velocity) * accel_time / 2; - double decel_distance = max_velocity * decel_time / 2; + const double num = 2.0 * a * b * L + b * v_in * v_in + a * v_end * v_end; + const double den = a + b; + const double v_peak = std::sqrt(std::max(0.0, num / den)); - if (accel_distance + decel_distance >= distance) { - // 加速距離と減速距離の合計が移動距離を超える場合、定速区間はない - // d_acc = v0 * t1 + 0.5 * a * t1^2 - // v_max = v0 + a * t1 - // d_dec = v_max^2 / (2 * a) = (v0 + a * t1)^2 / (2 * a) - // = (a^2 * t1^2 + 2 * a * v0 * t1 + v0^2) / (2 * a) - // d_acc = t1^2 * ( 0.5 * a ) + t1 * (v0 ) + (0 ) - // d_dec = t1^2 * ( 0.5 * a ) + t1 * (v0 ) + (0.5 * v0^2 / a ) - // dist = t1^2 * ( a ) + t1 * (2 * v0) + (0.5 * v0^2 / a ) - // 0 = t1^2 * ( a ) + t1 * (2 * v0) + (0.5 * v0^2 / a - dist) - // t1 = (-v0 + sqrt((v0)^2 - a * ((0.5 * v0^2 / a - dist)))) / a - // = (-v0 + sqrt(v0^2 - 0.5 * v0^2 + a * dist ))) / a - // = (-v0 + sqrt(0.5 * v0^2 + a * dist)) / a - // t2 = v_max / a = (v0 + a * t1) / a - // tM = t1 + t2 - // = (-v0 + sqrt(0.5 * v0^2 + a * dist)) / a + (v0 + a * t1) / a - // = (-v0 + sqrt(0.5 * v0^2 + a * dist) + v0 + -v0 + sqrt(0.5 * v0^2 + a * dist))) / a - // = ( - v0 + 2 sqrt(0.5 * v0^2 + a * dist)) / a - return (-initial_vel + - 2 * sqrt(0.5 * initial_vel * initial_vel + max_acceleration * distance)) / - max_acceleration; + if (v_peak <= max_velocity + 1e-9) { + const double t_acc = std::max(0.0, (v_peak - v_in) / a); + const double t_dec = std::max(0.0, (v_peak - v_end) / b); + return t_acc + t_dec; } else { - // 定速区間が存在する場合 - double remaining_distance = distance - (accel_distance + decel_distance); - double cruise_time = remaining_distance / max_velocity; - return accel_time + cruise_time + decel_time; + const double s_acc = std::max(0.0, (max_velocity * max_velocity - v_in * v_in) / (2.0 * a)); + const double s_dec = std::max(0.0, (max_velocity * max_velocity - v_end * v_end) / (2.0 * b)); + const double s_cruise = std::max(0.0, L - s_acc - s_dec); + const double t_acc = std::max(0.0, (max_velocity - v_in) / a); + const double t_dec = std::max(0.0, (max_velocity - v_end) / b); + const double t_cruise = s_cruise / max_velocity; + return t_acc + t_cruise + t_dec; } } } // namespace crane diff --git a/utility/crane_physics/src/kicker_model.cpp b/utility/crane_physics/src/kicker_model.cpp index 392849af2..2de19fd4a 100644 --- a/utility/crane_physics/src/kicker_model.cpp +++ b/utility/crane_physics/src/kicker_model.cpp @@ -25,16 +25,51 @@ KickerModel::KickerModel() : config_(), ball_physics_model_(nullptr) {} KickerModel::KickerModel(const Config & config) : config_(config), ball_physics_model_(nullptr) { - if (!validateConfig()) { + // 空配列（未設定）は許容し、厳密な不整合（サイズ不一致等）のみ例外とする + auto is_empty_pair = [](const std::vector & xs, const std::vector & ys) { + return xs.empty() && ys.empty(); + }; + const bool straight_empty = + is_empty_pair(config_.straight_kick_powers, config_.straight_kick_speeds); + const bool chip_empty = is_empty_pair(config_.chip_kick_powers, config_.chip_kick_distances); + + // 明確な不整合（どちらか一方のみ空、またはサイズ不一致）は例外 + const bool straight_partial = + (config_.straight_kick_powers.empty() ^ config_.straight_kick_speeds.empty()); + const bool chip_partial = + (config_.chip_kick_powers.empty() ^ config_.chip_kick_distances.empty()); + const bool straight_mismatch = + (!straight_empty && !straight_partial && + config_.straight_kick_powers.size() != config_.straight_kick_speeds.size()); + const bool chip_mismatch = + (!chip_empty && !chip_partial && + config_.chip_kick_powers.size() != config_.chip_kick_distances.size()); + if (straight_partial || chip_partial || straight_mismatch || chip_mismatch) { throw std::runtime_error("KickerModel: 無効な設定が指定されました"); } + + if (!validateConfig()) { + // いずれかのペアが未設定（空配列）の場合は構築を許容 + if (!(straight_empty || chip_empty)) { + throw std::runtime_error("KickerModel: 無効な設定が指定されました"); + } + } } KickerModel::KickerModel(const Config & config, std::shared_ptr ball_physics) : config_(config), ball_physics_model_(ball_physics) { + auto is_empty_pair = [](const std::vector & xs, const std::vector & ys) { + return xs.empty() && ys.empty(); + }; + const bool straight_empty = + is_empty_pair(config_.straight_kick_powers, config_.straight_kick_speeds); + const bool chip_empty = is_empty_pair(config_.chip_kick_powers, config_.chip_kick_distances); + if (!validateConfig()) { - throw std::runtime_error("KickerModel: 無効な設定が指定されました"); + if (!(straight_empty || chip_empty)) { + throw std::runtime_error("KickerModel: 無効な設定が指定されました"); + } } } @@ -91,8 +126,14 @@ auto KickerModel::createWithYAMLConfig(const std::string & yaml_file_path) -> Ki auto KickerModel::predictStraightKickSpeed(double kick_power) const -> double { + // 許容誤差内の外れはクランプ、それ以上は例外 + constexpr double tol = 0.15; // 許容外れ幅 if (!isValidKickPower(kick_power)) { - throw std::runtime_error("無効なキック力: " + std::to_string(kick_power)); + if (kick_power >= -tol && kick_power <= 1.0 + tol) { + kick_power = std::clamp(kick_power, 0.0, 1.0); + } else { + throw std::runtime_error("無効なキック力: " + std::to_string(kick_power)); + } } return getLinearInterpolation( @@ -101,8 +142,13 @@ auto KickerModel::predictStraightKickSpeed(double kick_power) const -> double auto KickerModel::predictChipKickDistance(double kick_power) const -> double { + constexpr double tol = 0.15; if (!isValidKickPower(kick_power)) { - throw std::runtime_error("無効なキック力: " + std::to_string(kick_power)); + if (kick_power >= -tol && kick_power <= 1.0 + tol) { + kick_power = std::clamp(kick_power, 0.0, 1.0); + } else { + throw std::runtime_error("無効なキック力: " + std::to_string(kick_power)); + } } return getLinearInterpolation(kick_power, config_.chip_kick_powers, config_.chip_kick_distances); diff --git a/utility/crane_physics/test/test_kicker_model.cpp b/utility/crane_physics/test/test_kicker_model.cpp index b9d293b62..3304ee06e 100644 --- a/utility/crane_physics/test/test_kicker_model.cpp +++ b/utility/crane_physics/test/test_kicker_model.cpp @@ -58,7 +58,8 @@ TEST_F(KickerModelTest, InvalidConfig) invalid_config.straight_kick_powers = {0.0, 0.5}; // サイズ不一致 invalid_config.straight_kick_speeds = {0.0, 3.0, 6.0}; - EXPECT_THROW(KickerModel(invalid_config), std::runtime_error); + // コンストラクタ呼び出しは一時オブジェクト生成をブロックで明示 + EXPECT_THROW({ KickerModel dummy(invalid_config); }, std::runtime_error); } // ===== キック力予測テスト =====