From 9ca5b64720aa3297f7d43f1c67a0a0685974cf71 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 12:44:36 +0900 Subject: [PATCH 01/12] =?UTF-8?q?fix(world=5Fmodel=5Fpublisher):=20Tracker?= =?UTF-8?q?=E6=AC=A0=E8=90=BD=E6=99=82=E3=81=AE=E7=9F=AD=E6=9C=9FLOST?= =?UTF-8?q?=E5=88=A4=E5=AE=9A=E3=82=92=E6=8A=91=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit TrackerWrapperPacketのフレームからロボットが1フレーム欠落するだけで available_vision/available_trackerが即false（LOST扱い）になる問題を修正。 robot_vision_hold_sec（デフォルト0.15秒）以内に検出されていたロボットは 次フレームでTrackerに含まれなくても状態を維持する。保持時間を超えた 場合のみリセットする。 背景: zunoh戦後半でbot=1がautoref側では検出されているのにcraneの WorldModelではdetected=Falseになり制御不能となり、autorefから ATTACKER_TOO_CLOSE_TO_DEFENSE_AREAファールを取られた。これは autoref TrackerのlosyなTrackerWrapperPacket出力を一時的に 補う短期保持で軽減できる。 --- .../world_model_data_provider.hpp | 1 + .../src/world_model_data_provider.cpp | 13 ++++++++++--- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_data_provider.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_data_provider.hpp index 2480292bc..516de523f 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_data_provider.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_data_provider.hpp @@ -202,6 +202,7 @@ class WorldModelDataProvider FieldGeometry field_geometry_; bool has_vision_updated_; int feedback_stale_timeout_ms_{100}; + double robot_vision_hold_sec_{0.15}; // エラー継続時間を算出するためのトラッキング用構造体 struct ErrorTracker diff --git a/crane_world_model_publisher/src/world_model_data_provider.cpp b/crane_world_model_publisher/src/world_model_data_provider.cpp index 98805d0e9..496d517c9 100644 --- a/crane_world_model_publisher/src/world_model_data_provider.cpp +++ b/crane_world_model_publisher/src/world_model_data_provider.cpp @@ -48,12 +48,14 @@ WorldModelDataProvider::WorldModelDataProvider(rclcpp::Node & node) node.declare_parameter("tracker_port", 10010); node.declare_parameter("use_udp_detection", false); node.declare_parameter("feedback_stale_timeout_ms", feedback_stale_timeout_ms_); + node.declare_parameter("robot_vision_hold_sec", robot_vision_hold_sec_); config_.vision_address = node.get_parameter("vision_address").get_value(); config_.vision_port = node.get_parameter("vision_port").get_value(); config_.confidence_threshold = node.get_parameter("confidence_threshold").get_value(); use_udp_detection_ = node.get_parameter("use_udp_detection").get_value(); feedback_stale_timeout_ms_ = node.get_parameter("feedback_stale_timeout_ms").get_value(); + robot_vision_hold_sec_ = node.get_parameter("robot_vision_hold_sec").get_value(); // Initialize UDP receivers @@ -850,11 +852,16 @@ auto WorldModelDataProvider::processTrackedFrame( integrateBallInfo(); // ロボット情報の処理 - // 全チーム・全ロボットIDをリセット（trackerフレーム毎にvision/tracker両フラグをクリア） + // available_visionが継続してfalseになると、一時的なトラッカー欠落でもロボットがlost扱いになる。 + // robot_vision_hold_sec_ 以内に検出されていたロボットは状態を維持し、超過後のみリセットする。 for (int team_idx = 0; team_idx < 2; ++team_idx) { for (auto & robot : robot_info_[team_idx]) { - robot.available_vision = false; - robot.available_tracker = false; + if (!robot.available_vision) continue; + const double age = (now - rclcpp::Time(robot.vision.stamp)).seconds(); + if (age < 0.0 || age > robot_vision_hold_sec_) { + robot.available_vision = false; + robot.available_tracker = false; + } } } From 4c9a25e4b99c46df59713af0ebf6b11d25660cad Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 12:45:17 +0900 Subject: [PATCH 02/12] =?UTF-8?q?=E6=97=A7Kick=E5=AE=9F=E8=A3=85=E3=81=AE?= =?UTF-8?q?=E5=BE=A9=E6=97=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/kick_old.hpp | 53 ++++ crane_robot_skills/src/kick_old.cpp | 272 ++++++++++++++++++ 2 files changed, 325 insertions(+) create mode 100644 crane_robot_skills/include/crane_robot_skills/kick_old.hpp create mode 100644 crane_robot_skills/src/kick_old.cpp diff --git a/crane_robot_skills/include/crane_robot_skills/kick_old.hpp b/crane_robot_skills/include/crane_robot_skills/kick_old.hpp new file mode 100644 index 000000000..0d0921f66 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/kick_old.hpp @@ -0,0 +1,53 @@ +// Copyright (c) 2023 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_ROBOT_SKILLS__KICK_OLD_HPP_ +#define CRANE_ROBOT_SKILLS__KICK_OLD_HPP_ + +#include +#include +#include +#include + +namespace crane::skills +{ +// 983f026a（2025-09-14）以前の回り込みアルゴリズム（move_direction ベース 2 フェーズ） +enum class KickOldState { + ENTRY_POINT, + AROUND_BALL_AND_KICK, + REDIRECT_KICK, + POSITIVE_REDIRECT_KICK, +}; + +class KickOld : public SkillBaseWithState +{ +private: + static std::string getStateName(int s); + + Receive receive_skill; + +public: + template + explicit KickOld(Args &&... args) + : SkillBaseWithState( + static_cast(KickOldState::ENTRY_POINT), &KickOld::getStateName, "kick_old", + std::forward(args)...), + receive_skill(command) + { + initialize(); + } + + auto getBallExitPointFromField(const double offset = 0.3) -> Point; + +private: + void initialize(); + + void kickWithChip(); + + void kickStraight(); +}; +} // namespace crane::skills +#endif // CRANE_ROBOT_SKILLS__KICK_OLD_HPP_ diff --git a/crane_robot_skills/src/kick_old.cpp b/crane_robot_skills/src/kick_old.cpp new file mode 100644 index 000000000..efa5ee9d3 --- /dev/null +++ b/crane_robot_skills/src/kick_old.cpp @@ -0,0 +1,272 @@ +// Copyright (c) 2024 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 +#include + +#include "../include/crane_robot_skills/single_ball_placement.hpp" + +namespace crane::skills +{ +std::string KickOld::getStateName(int s) +{ + return std::string(magic_enum::enum_name(static_cast(s))); +} + +void KickOld::initialize() +{ + using S = KickOldState; + auto s = [](S state) { return static_cast(state); }; + + command->usePositionMode(); + setParameter("target", Point(0, 0)); + setParameter("kick_power", 0.7f); + setParameter("use_target_kick_speed", false); + setParameter("target_kick_speed", 2.0); + setParameter("use_target_chip_distance", false); + setParameter("target_chip_distance", 2.0); + setParameter("chip_kick", false); + setParameter("with_dribble", false); + setParameter("dribble_power", 0.3f); + setParameter("angle_threshold_deg", 15.0f); + setParameter("around_interval", 0.15f); + setParameter("go_around_ball", true); + setParameter("moving_speed_threshold", 0.2); + setParameter("kicked_speed_threshold", 1.5); + + receive_skill.setParameter("dribble_power", 0.3); + receive_skill.setParameter("enable_software_bumper", false); + receive_skill.setParameter("policy", std::string("min_slack")); + receive_skill.setParameter("enable_active_receive", true); + receive_skill.setParameter("enable_redirect", true); + receive_skill.setParameter("redirect_target", Point(0, 0)); + receive_skill.setParameter("redirect_kick_power", 0.3); + + addStateFunction(s(S::ENTRY_POINT), [this]() { + visualizer->text() + .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("KickOld::ENTRY_POINT") + .fill("white") + .fontSize(100) + .build(); + return Status::RUNNING; + }); + + addTransition(s(S::ENTRY_POINT), s(S::AROUND_BALL_AND_KICK), [this]() { return true; }); + + addStateFunction(s(S::POSITIVE_REDIRECT_KICK), [this]() { + visualizer->text() + .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("KickOld::POSITIVE_REDIRECT_KICK") + .fill("white") + .fontSize(100) + .build(); + const auto & ball_pos = world_model()->ball().pos; + command->lookAtFrom(getParameter("target"), ball_pos); + + const auto & ball_vel_normed = world_model()->ball().vel.normalized(); + Segment ball_line = world_model()->ball().getTrajectorySegmentByDistance(10.); + auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line); + if ((ball_pos - closest_point).dot(ball_vel_normed) > 0) { + auto target_pos = [&]() -> Point { + if (distance < 0.1) { + return ball_pos + ball_vel_normed; + } else { + return closest_point + ball_vel_normed * distance; + } + }(); + command->setDribblerTargetPosition(target_pos); + command->kickStraight(0.3); + command->disableBallAvoidance(); + } else { + command->setTargetPosition( + closest_point + (robot()->pose.pos - closest_point).normalized() * 0.3); + } + + return Status::RUNNING; + }); + + addTransition(s(S::POSITIVE_REDIRECT_KICK), s(S::ENTRY_POINT), [this]() { + return !world_model()->ball().isMovingAwayFrom(robot()->pose.pos, 10.0) || + !world_model()->ball().isMovingTowards(getParameter("target"), 30.0); + }); + + addStateFunction(s(S::REDIRECT_KICK), [this]() { + visualizer->text() + .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("KickOld::REDIRECT_KICK") + .fill("white") + .fontSize(100) + .build(); + receive_skill.setParameter("target", getParameter("target")); + if (robot()->getDistance(world_model()->ball().pos) < 0.5) { + receive_skill.setParameter("policy", std::string("closest")); + } else { + receive_skill.setParameter("policy", std::string("min_slack")); + } + command->disableBallAvoidance(); + return receive_skill.update(); + }); + + addTransition(s(S::REDIRECT_KICK), s(S::AROUND_BALL_AND_KICK), [this]() { + return !world_model()->ball().isMoving(getParameter("moving_speed_threshold")); + }); + + addTransition(s(S::REDIRECT_KICK), s(S::ENTRY_POINT), [this]() { + return world_model()->ball().isMoving(getParameter("kicked_speed_threshold")) && + world_model()->ball().isMovingAwayFrom(robot()->pose.pos, 30.); + }); + + addStateFunction(s(S::AROUND_BALL_AND_KICK), [this]() { + auto target = getParameter("target"); + Point ball_pos = world_model()->ball().pos; + visualizer->line() + .start(ball_pos) + .end(ball_pos + (target - ball_pos).normalized() * 1.0) + .stroke("blue") + .strokeWidth(10) + .build(); + + constexpr double SWITCH_DISTANCE = 0.5; + visualizer->circle() + .center(ball_pos) + .radius(SWITCH_DISTANCE) + .stroke("yellow") + .strokeWidth(10) + .build(); + + if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) { + visualizer->text() + .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("KickOld::AROUND_BALL(遠い)") + .fill("white") + .fontSize(100) + .build(); + // 遠距離フェーズ: ボール後方の固定点へ向かいつつ角度を合わせる + command->setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) + .lookAtFrom(target, ball_pos); + return Status::RUNNING; + } else { + visualizer->text() + .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) + .text("KickOld::AROUND_BALL（近い）") + .fill("white") + .fontSize(100) + .build(); + + // 近距離フェーズ: ロボット相対の move_direction ベクトルで回り込む + auto calculateRatio = + [](const double distance, const double min_distance, const double max_distance) { + return (distance - min_distance) / (max_distance - min_distance); + }; + + using boost::math::constants::degree; + double ratio = + 1.5 + + std::clamp( + -calculateRatio(robot()->getDistance(world_model()->ball().pos), 0.2, 1.5), -0.5, 0.); + + double move_direction = getAngle(target - robot()->pose.pos) + + (getAngleDiff( + getAngle(world_model()->ball().pos - robot()->pose.pos), + getAngle(target - robot()->pose.pos))) * + ratio; + Vector2 move_vec = getNormVec(move_direction); + double move_vec_gain = [&]() { + if ( + getAngleDiff(getAngle(target - ball_pos), robot()->pose.theta) < 2.5 * degree()) { + return 0.4; + } else { + return 0.2; + } + }(); + + Vector2 ball_away_vec = (robot()->pose.pos - world_model()->ball().pos).normalized(); + double ball_away_gain = 0.0; + if ( + robot()->getDistance(world_model()->ball().pos) < 0.2 && + getAngleDiff( + getAngle(target - ball_pos), getAngle(world_model()->ball().pos - robot()->pose.pos)) > + 10. * degree()) { + ball_away_gain = 0.0; + } + + command->lookAtFrom(target, ball_pos) + .setDribblerTargetPosition( + robot()->pose.pos + move_vec * move_vec_gain + world_model()->ball().vel * 0.5 + + ball_away_vec * ball_away_gain) + .disableCollisionAvoidance() + .disableBallAvoidance(); + + if ( + std::abs( + getAngleDiff(getAngle(target - ball_pos), getAngle(ball_pos - robot()->pose.pos))) < + 20. * degree()) { + if (getParameter("chip_kick")) { + kickWithChip(); + } else { + kickStraight(); + } + } else { + command->kickStraight(0.0); + } + + if (getParameter("with_dribble")) { + command->withDribble(getParameter("dribble_power")); + } else { + command->withDribble(0.0); + } + return Status::RUNNING; + } + }); + + addTransition(s(S::AROUND_BALL_AND_KICK), s(S::ENTRY_POINT), [this]() { + return world_model()->ball().isMoving(getParameter("kicked_speed_threshold")) && + world_model()->ball().isMovingAwayFrom(robot()->pose.pos, 30.); + }); +} + +auto KickOld::getBallExitPointFromField(const double offset) -> Point +{ + Segment ball_line{ + world_model()->ball().pos, + world_model()->ball().pos + world_model()->ball().vel.normalized() * 10.0}; + + const double X = world_model()->fieldSize().x() / 2.0 - offset; + const double Y = world_model()->fieldSize().y() / 2.0 - offset; + + std::vector segments; + segments.emplace_back(Point(X, Y), Point(X, -Y)); + segments.emplace_back(Point(-X, Y), Point(-X, -Y)); + segments.emplace_back(Point(X, Y), Point(-X, Y)); + segments.emplace_back(Point(X, -Y), Point(-X, -Y)); + + for (const auto & seg : segments) { + if (auto intersections = getIntersections(ball_line, seg); not intersections.empty()) { + return intersections.front(); + } + } + return world_model()->ball().pos; +} + +auto KickOld::kickWithChip() -> void +{ + if (getParameter("use_target_chip_distance")) { + command->setKickWithChipTargetDistance(getParameter("target_chip_distance")); + } else { + command->kickWithChip(getParameter("kick_power")); + } +} + +auto KickOld::kickStraight() -> void +{ + if (getParameter("use_target_kick_speed")) { + command->setKickStraightTargetSpeed(getParameter("target_kick_speed")); + } else { + command->kickStraight(getParameter("kick_power")); + } +} +} // namespace crane::skills From d46d78b406d02c3e966e5ce1c429c469e46d9a57 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 12:51:03 +0900 Subject: [PATCH 03/12] =?UTF-8?q?=E6=97=A7Kick=E5=AE=9F=E8=A3=85=E3=81=AE?= =?UTF-8?q?=E5=BE=A9=E6=97=A72?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/include/crane_robot_skills/attacker.hpp | 6 +++--- crane_robot_skills/include/crane_robot_skills/goal_kick.hpp | 4 ++-- crane_robot_skills/include/crane_robot_skills/goalie.hpp | 4 ++-- .../include/crane_robot_skills/penalty_kick.hpp | 4 ++-- .../include/crane_robot_skills/simple_kickoff.hpp | 4 ++-- crane_robot_skills/include/crane_robot_skills/skills.hpp | 1 + crane_robot_skills/src/attacker.cpp | 2 +- 7 files changed, 13 insertions(+), 12 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/attacker.hpp b/crane_robot_skills/include/crane_robot_skills/attacker.hpp index 04984377c..d13758862 100644 --- a/crane_robot_skills/include/crane_robot_skills/attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/attacker.hpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include #include #include @@ -64,7 +64,7 @@ class Attacker : public SkillBaseWithState int forced_pass_receiver_id = -1; - Kick kick_skill; + KickOld kick_skill; GoalKick goal_kick_skill; @@ -74,7 +74,7 @@ class Attacker : public SkillBaseWithState void onPostUpdate() override; private: - void configurePassKick(const Point & target, Kick & kick_skill); + void configurePassKick(const Point & target, KickOld & kick_skill); // KICK状態の進捗タイムアウト用 std::chrono::steady_clock::time_point kick_state_entry_time{}; diff --git a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp index ba71b0572..e7c0036db 100644 --- a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp @@ -8,7 +8,7 @@ #define CRANE_ROBOT_SKILLS__GOAL_KICK_HPP_ #include -#include +#include #include #include @@ -26,7 +26,7 @@ class GoalKick : public SkillBase Status update() override; - Kick kick_skill; + KickOld kick_skill; static double getBestAngleToShootFromPoint( double minimum_angle_accuracy, const Point from_point, diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 85d5407bb..4679a3047 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -35,7 +35,7 @@ class Goalie : public SkillBase std::string phase; - Kick kick_skill; + KickOld kick_skill; std::optional prev_wait_point; diff --git a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp index 229039163..9a6d77675 100644 --- a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -45,7 +45,7 @@ class PenaltyKick : public SkillBaseWithState return static_cast(SkillBaseWithState::getCurrentState()); } - Kick kick_skill; + KickOld kick_skill; std::optional start_ball_point = std::nullopt; }; diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index 6308d93ee..02fbed430 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -33,7 +33,7 @@ class SimpleKickOff : public SkillBase Status update() override; - Kick kick_skill; + KickOld kick_skill; private: bool kicked = false; diff --git a/crane_robot_skills/include/crane_robot_skills/skills.hpp b/crane_robot_skills/include/crane_robot_skills/skills.hpp index 9365813f1..dd546eae4 100644 --- a/crane_robot_skills/include/crane_robot_skills/skills.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skills.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 155f9a613..b6d36099e 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -358,7 +358,7 @@ void Attacker::onPostUpdate() } } -void Attacker::configurePassKick(const Point & target, Kick & kick_skill) +void Attacker::configurePassKick(const Point & target, KickOld & kick_skill) { auto pass_analysis = getPassAnalysis( world_model()->ball().pos, target, world_model()->theirs().robotsWhere().available().get()); From ff7da8833a0b4e42bcf8444165c4fd3e36f845ec Mon Sep 17 00:00:00 2001 From: ssaattww Date: Sat, 25 Apr 2026 09:56:57 +0900 Subject: [PATCH 04/12] =?UTF-8?q?sim/real=E3=81=A9=E3=81=A1=E3=82=89?= =?UTF-8?q?=E3=81=A7=E3=82=82=E8=87=AA=E5=89=8D=E3=81=AE=E3=83=88=E3=83=A9?= =?UTF-8?q?=E3=83=83=E3=82=AB=E3=83=BC=E3=82=92=E8=A6=8B=E3=82=8B=E3=82=88?= =?UTF-8?q?=E3=81=86=E3=81=AB=E5=A4=89=E6=9B=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml index e9d06b249..ae607da2e 100644 --- a/crane_bringup/launch/crane.launch.xml +++ b/crane_bringup/launch/crane.launch.xml @@ -47,7 +47,7 @@ limitations under the License. - + From bfec71eef8cdf24f6bd9de18519a2e0704af71c4 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 10:16:10 +0900 Subject: [PATCH 05/12] =?UTF-8?q?refactor(simple=5Fkickoff):=20=E3=83=81?= =?UTF-8?q?=E3=83=83=E3=83=97=E3=82=AD=E3=83=83=E3=82=AF=E3=82=92GoalKick?= =?UTF-8?q?=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AB=E7=BD=AE=E3=81=8D=E6=8F=9B?= =?UTF-8?q?=E3=81=88=E3=81=A6=E3=82=B4=E3=83=BC=E3=83=AB=E3=82=92=E7=8B=99?= =?UTF-8?q?=E3=81=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit キックオフ時にチップキックで固定距離を蹴る実装から、GoalKickスキルを 活用した実装に変更する。 変更内容: - chip_kick=true / target_chip_distance=2.0 の設定を削除 - Kick スキルの代わりに GoalKick スキルを保持するよう変更 - ターゲットをゴールセンター固定から GoalKick が計算する最適角度に変更 これにより、敵ロボット配置を考慮してゴール枠内の隙間を狙うストレート キック（target_kick_speed=6.0）でシュートするようになる。 --- .../include/crane_robot_skills/simple_kickoff.hpp | 14 +++----------- crane_robot_skills/src/simple_kickoff.cpp | 10 +--------- 2 files changed, 4 insertions(+), 20 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index 02fbed430..a01e9bc97 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -7,15 +7,10 @@ #ifndef CRANE_ROBOT_SKILLS__SIMPLE_KICKOFF_HPP_ #define CRANE_ROBOT_SKILLS__SIMPLE_KICKOFF_HPP_ -#include -#include -#include +#include #include #include -#include -#include #include -#include namespace crane::skills { @@ -24,16 +19,13 @@ class SimpleKickOff : public SkillBase public: template explicit SimpleKickOff(Args &&... args) - : SkillBase("simple_kickoff", std::forward(args)...), kick_skill(command) + : SkillBase("simple_kickoff", std::forward(args)...), goal_kick_skill(command) { - initializeParameters(); } - void initializeParameters(); - Status update() override; - KickOld kick_skill; + GoalKick goal_kick_skill; private: bool kicked = false; diff --git a/crane_robot_skills/src/simple_kickoff.cpp b/crane_robot_skills/src/simple_kickoff.cpp index 499c3cf26..65cba3ad8 100644 --- a/crane_robot_skills/src/simple_kickoff.cpp +++ b/crane_robot_skills/src/simple_kickoff.cpp @@ -8,13 +8,6 @@ namespace crane::skills { -void SimpleKickOff::initializeParameters() -{ - kick_skill.setParameter("chip_kick", true); - kick_skill.setParameter("use_target_chip_distance", true); - kick_skill.setParameter("target_chip_distance", 2.0); -} - Status SimpleKickOff::update() { if (!kicked && world_model()->ball().isMoving(0.5)) { @@ -26,7 +19,6 @@ Status SimpleKickOff::update() return Status::RUNNING; } - kick_skill.setParameter("target", world_model()->getAttackGoalCenter()); - return kick_skill.run(); + return goal_kick_skill.run(); } } // namespace crane::skills From a1ec7951df4601def6743004682425b74c51d2d3 Mon Sep 17 00:00:00 2001 From: ssaattww Date: Sat, 25 Apr 2026 11:05:49 +0900 Subject: [PATCH 06/12] =?UTF-8?q?zunoh=E6=88=A6=E3=81=AE=E8=A8=AD=E5=AE=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.xml | 4 ++-- .../config/unified_session_config.yaml | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/crane_bringup/launch/crane.launch.xml b/crane_bringup/launch/crane.launch.xml index ae607da2e..0a46b7fda 100644 --- a/crane_bringup/launch/crane.launch.xml +++ b/crane_bringup/launch/crane.launch.xml @@ -60,7 +60,7 @@ limitations under the License. - + @@ -81,7 +81,7 @@ limitations under the License. - + diff --git a/crane_session_coordinator/config/unified_session_config.yaml b/crane_session_coordinator/config/unified_session_config.yaml index de3e12aa4..b0f476fbf 100644 --- a/crane_session_coordinator/config/unified_session_config.yaml +++ b/crane_session_coordinator/config/unified_session_config.yaml @@ -14,9 +14,9 @@ situations: - name: attacker_skill max_robots: 1 - name: defender - max_robots: 3 + max_robots: 4 params: - reduced_robots: 1 + reduced_robots: 2 - name: pass_receive max_robots: 1 - name: sub_attacker_skill @@ -25,7 +25,7 @@ situations: max_robots: 1 - name: marker max_robots: 2 - - name: forward + - name: waiter max_robots: 20 OUR_BALL_PLACEMENT: @@ -195,6 +195,7 @@ situations: kick_allowed_skills: - goalie_skill - defender + - attacker_skill sessions: - name: goalie_skill max_robots: 1 From 07ae0545c3940aa5603ca5627a3d1be782418167 Mon Sep 17 00:00:00 2001 From: ssaattww Date: Sat, 25 Apr 2026 11:31:17 +0900 Subject: [PATCH 07/12] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=81=AE=E5=8B=95?= =?UTF-8?q?=E4=BD=9C=E3=81=8C=E4=B8=8D=E5=AE=89=E5=AE=9A=E3=81=AA=E3=81=AE?= =?UTF-8?q?=E3=81=A7=E5=89=8A=E9=99=A4=20=E5=9B=9E=E3=82=8A=E8=BE=BC?= =?UTF-8?q?=E3=81=BF=E9=80=9F=E5=BA=A6=E3=82=92=E4=BD=8E=E3=81=8F=E8=A8=AD?= =?UTF-8?q?=E5=AE=9A(zonoh=E7=B7=9A=E3=83=8F=E3=83=BC=E3=83=95=E3=82=BF?= =?UTF-8?q?=E3=82=A4=E3=83=A0=E5=BE=8C=E6=9C=89=E5=8A=B9=E5=8C=96)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 18 ++++++++++-------- crane_robot_skills/src/kick.cpp | 2 +- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index b6d36099e..70af56f78 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -314,14 +314,16 @@ void Attacker::initialize() goal_kick_skill.setParameter("target_kick_speed", 6.0); goal_kick_skill.setParameter("dribble_power", 0.2); return goal_kick_skill.run(); - } else if (pass_receiver_id.has_value()) { - // STANDARD_PASS - printTextOnRobot("KICK::STANDARD_PASS"); - kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; - kick_skill.setParameter("target", kick_target); - configurePassKick(kick_target, kick_skill); - return kick_skill.run(); - } else if (goal_angle_width > LOW_CHANCE_GOAL_ANGLE_THRESHOLD_DEG * M_PI / 180.0) { + } + // else if (pass_receiver_id.has_value()) { + // // STANDARD_PASS + // printTextOnRobot("KICK::STANDARD_PASS"); + // kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; + // kick_skill.setParameter("target", kick_target); + // configurePassKick(kick_target, kick_skill); + // return kick_skill.run(); + // } + else if (goal_angle_width > LOW_CHANCE_GOAL_ANGLE_THRESHOLD_DEG * M_PI / 180.0) { // LOW_CHANCE_GOAL_KICK printTextOnRobot("KICK::LOW_CHANCE_GOAL_KICK"); return goal_kick_skill.run(); diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index e02e598eb..e756f30d9 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -107,7 +107,7 @@ void Kick::initialize() // 角度が合っている場合のみキックゾーンに入ったと判断し、ボールに向かって押し込む if (angle_ok && bg::distance(ball_kick_zone, robot()->pose.pos) < 0.15) { command->disableCollisionAvoidance(); - return 0.5; + return 0.3; } else { return 0.0; } From cb32f420e190f39a54c9359c4fb63285c0112368 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 12:51:03 +0900 Subject: [PATCH 08/12] =?UTF-8?q?=E6=97=A7Kick=E5=AE=9F=E8=A3=85=E3=81=AE?= =?UTF-8?q?=E5=BE=A9=E6=97=A72?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/simple_kickoff.hpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index a01e9bc97..5b8709454 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -7,6 +7,9 @@ #ifndef CRANE_ROBOT_SKILLS__SIMPLE_KICKOFF_HPP_ #define CRANE_ROBOT_SKILLS__SIMPLE_KICKOFF_HPP_ +#include +#include +#include #include #include #include @@ -25,6 +28,7 @@ class SimpleKickOff : public SkillBase Status update() override; + KickOld kick_skill; GoalKick goal_kick_skill; private: From 555811ccc370eaf71878b245f4855ba7c9dcf361 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 13:08:56 +0900 Subject: [PATCH 09/12] =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_robot_skills/kick_old.hpp | 8 +-- .../crane_robot_skills/simple_kickoff.hpp | 6 +- crane_robot_skills/src/kick_old.cpp | 71 ------------------- 3 files changed, 5 insertions(+), 80 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/kick_old.hpp b/crane_robot_skills/include/crane_robot_skills/kick_old.hpp index 0d0921f66..7cdcd09d3 100644 --- a/crane_robot_skills/include/crane_robot_skills/kick_old.hpp +++ b/crane_robot_skills/include/crane_robot_skills/kick_old.hpp @@ -8,7 +8,6 @@ #define CRANE_ROBOT_SKILLS__KICK_OLD_HPP_ #include -#include #include #include @@ -18,8 +17,6 @@ namespace crane::skills enum class KickOldState { ENTRY_POINT, AROUND_BALL_AND_KICK, - REDIRECT_KICK, - POSITIVE_REDIRECT_KICK, }; class KickOld : public SkillBaseWithState @@ -27,15 +24,12 @@ class KickOld : public SkillBaseWithState private: static std::string getStateName(int s); - Receive receive_skill; - public: template explicit KickOld(Args &&... args) : SkillBaseWithState( static_cast(KickOldState::ENTRY_POINT), &KickOld::getStateName, "kick_old", - std::forward(args)...), - receive_skill(command) + std::forward(args)...) { initialize(); } diff --git a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp index 5b8709454..87802b8a8 100644 --- a/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp +++ b/crane_robot_skills/include/crane_robot_skills/simple_kickoff.hpp @@ -9,8 +9,8 @@ #include #include -#include #include +#include #include #include #include @@ -22,7 +22,9 @@ class SimpleKickOff : public SkillBase public: template explicit SimpleKickOff(Args &&... args) - : SkillBase("simple_kickoff", std::forward(args)...), goal_kick_skill(command) + : SkillBase("simple_kickoff", std::forward(args)...), + kick_skill(command), + goal_kick_skill(command) { } diff --git a/crane_robot_skills/src/kick_old.cpp b/crane_robot_skills/src/kick_old.cpp index efa5ee9d3..4a4bfcfb9 100644 --- a/crane_robot_skills/src/kick_old.cpp +++ b/crane_robot_skills/src/kick_old.cpp @@ -37,14 +37,6 @@ void KickOld::initialize() setParameter("moving_speed_threshold", 0.2); setParameter("kicked_speed_threshold", 1.5); - receive_skill.setParameter("dribble_power", 0.3); - receive_skill.setParameter("enable_software_bumper", false); - receive_skill.setParameter("policy", std::string("min_slack")); - receive_skill.setParameter("enable_active_receive", true); - receive_skill.setParameter("enable_redirect", true); - receive_skill.setParameter("redirect_target", Point(0, 0)); - receive_skill.setParameter("redirect_kick_power", 0.3); - addStateFunction(s(S::ENTRY_POINT), [this]() { visualizer->text() .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) @@ -57,69 +49,6 @@ void KickOld::initialize() addTransition(s(S::ENTRY_POINT), s(S::AROUND_BALL_AND_KICK), [this]() { return true; }); - addStateFunction(s(S::POSITIVE_REDIRECT_KICK), [this]() { - visualizer->text() - .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) - .text("KickOld::POSITIVE_REDIRECT_KICK") - .fill("white") - .fontSize(100) - .build(); - const auto & ball_pos = world_model()->ball().pos; - command->lookAtFrom(getParameter("target"), ball_pos); - - const auto & ball_vel_normed = world_model()->ball().vel.normalized(); - Segment ball_line = world_model()->ball().getTrajectorySegmentByDistance(10.); - auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line); - if ((ball_pos - closest_point).dot(ball_vel_normed) > 0) { - auto target_pos = [&]() -> Point { - if (distance < 0.1) { - return ball_pos + ball_vel_normed; - } else { - return closest_point + ball_vel_normed * distance; - } - }(); - command->setDribblerTargetPosition(target_pos); - command->kickStraight(0.3); - command->disableBallAvoidance(); - } else { - command->setTargetPosition( - closest_point + (robot()->pose.pos - closest_point).normalized() * 0.3); - } - - return Status::RUNNING; - }); - - addTransition(s(S::POSITIVE_REDIRECT_KICK), s(S::ENTRY_POINT), [this]() { - return !world_model()->ball().isMovingAwayFrom(robot()->pose.pos, 10.0) || - !world_model()->ball().isMovingTowards(getParameter("target"), 30.0); - }); - - addStateFunction(s(S::REDIRECT_KICK), [this]() { - visualizer->text() - .position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5) - .text("KickOld::REDIRECT_KICK") - .fill("white") - .fontSize(100) - .build(); - receive_skill.setParameter("target", getParameter("target")); - if (robot()->getDistance(world_model()->ball().pos) < 0.5) { - receive_skill.setParameter("policy", std::string("closest")); - } else { - receive_skill.setParameter("policy", std::string("min_slack")); - } - command->disableBallAvoidance(); - return receive_skill.update(); - }); - - addTransition(s(S::REDIRECT_KICK), s(S::AROUND_BALL_AND_KICK), [this]() { - return !world_model()->ball().isMoving(getParameter("moving_speed_threshold")); - }); - - addTransition(s(S::REDIRECT_KICK), s(S::ENTRY_POINT), [this]() { - return world_model()->ball().isMoving(getParameter("kicked_speed_threshold")) && - world_model()->ball().isMovingAwayFrom(robot()->pose.pos, 30.); - }); - addStateFunction(s(S::AROUND_BALL_AND_KICK), [this]() { auto target = getParameter("target"); Point ball_pos = world_model()->ball().pos; From f85f6ae8143a66d9d41c5432ef464307d4ce6e5b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 13:49:06 +0900 Subject: [PATCH 10/12] =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_web_debugger/src/websocket_server.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/crane_web_debugger/src/websocket_server.cpp b/crane_web_debugger/src/websocket_server.cpp index cb63bff11..90dff73a0 100644 --- a/crane_web_debugger/src/websocket_server.cpp +++ b/crane_web_debugger/src/websocket_server.cpp @@ -1456,8 +1456,7 @@ class WebSocketDebugServer : public rclcpp::Node connection->sendMessage(result.dump()); } - void handleSetPlannerParam( - std::shared_ptr connection, const json & request) + void handleSetPlannerParam(std::shared_ptr connection, const json & request) { std::vector params; if (request.contains("velocity_damping_gain")) { @@ -1477,13 +1476,12 @@ class WebSocketDebugServer : public rclcpp::Node return; } local_planner_params_client_->set_parameters( - params, [this](std::shared_future> - future) { + params, + [this](std::shared_future> future) { try { for (const auto & r : future.get()) { if (!r.successful) { - RCLCPP_WARN( - this->get_logger(), "set param failed: %s", r.reason.c_str()); + RCLCPP_WARN(this->get_logger(), "set param failed: %s", r.reason.c_str()); } } } catch (const std::exception & e) { From 2b665447e51b03eb1488bf536eddde4c11b422b3 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 13:49:19 +0900 Subject: [PATCH 11/12] =?UTF-8?q?STANDARD=5FPASSF=E5=BE=A9=E6=B4=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 18 ++++++++---------- 1 file changed, 8 insertions(+), 10 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 70af56f78..b6d36099e 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -314,16 +314,14 @@ void Attacker::initialize() goal_kick_skill.setParameter("target_kick_speed", 6.0); goal_kick_skill.setParameter("dribble_power", 0.2); return goal_kick_skill.run(); - } - // else if (pass_receiver_id.has_value()) { - // // STANDARD_PASS - // printTextOnRobot("KICK::STANDARD_PASS"); - // kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; - // kick_skill.setParameter("target", kick_target); - // configurePassKick(kick_target, kick_skill); - // return kick_skill.run(); - // } - else if (goal_angle_width > LOW_CHANCE_GOAL_ANGLE_THRESHOLD_DEG * M_PI / 180.0) { + } else if (pass_receiver_id.has_value()) { + // STANDARD_PASS + printTextOnRobot("KICK::STANDARD_PASS"); + kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; + kick_skill.setParameter("target", kick_target); + configurePassKick(kick_target, kick_skill); + return kick_skill.run(); + } else if (goal_angle_width > LOW_CHANCE_GOAL_ANGLE_THRESHOLD_DEG * M_PI / 180.0) { // LOW_CHANCE_GOAL_KICK printTextOnRobot("KICK::LOW_CHANCE_GOAL_KICK"); return goal_kick_skill.run(); From 99b6d8ebb1cba15955e49b7a8c07400f3c410b8d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 25 Apr 2026 13:49:36 +0900 Subject: [PATCH 12/12] =?UTF-8?q?=E6=9C=80=E5=88=9D=E3=81=AE=E9=85=8D?= =?UTF-8?q?=E7=BD=AE=E3=82=92=E9=81=A0=E3=81=96=E3=81=91=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_sessions/src/formation_session.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_sessions/src/formation_session.cpp b/crane_sessions/src/formation_session.cpp index 99f625c75..39a974365 100644 --- a/crane_sessions/src/formation_session.cpp +++ b/crane_sessions/src/formation_session.cpp @@ -12,7 +12,7 @@ std::vector FormationSession::getWingFormationPoints(int robot_num) { std::vector formation_points; - formation_points.emplace_back(0.6, 0.0); + formation_points.emplace_back(0.7, 0.0); formation_points.emplace_back(1.5, 1.2); formation_points.emplace_back(1.5, -1.2); formation_points.emplace_back(0.6, 2.4);