From 4b8472315f629f78e8e9321f0b6061b1c19699cb Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 6 Mar 2025 02:36:58 +0900 Subject: [PATCH 01/55] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=B9=E3=82=B3?= =?UTF-8?q?=E3=82=A2=E3=81=AE=E6=8F=8F=E7=94=BB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../world_model_publisher.hpp | 2 + .../src/world_model_publisher.cpp | 58 +++++++++++++++++++ .../include/crane_basics/ddps.hpp | 15 +++++ 3 files changed, 75 insertions(+) diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp index af9f4e7d..ba065aec 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp @@ -96,6 +96,8 @@ class WorldModelPublisherComponent : public rclcpp::Node CraneVisualizerBuffer::MessageBuilder::UniquePtr visualizer; + CraneVisualizerBuffer::MessageBuilder::UniquePtr pass_score_visualizer; + std::array, 20> friend_history; std::array, 20> enemy_history; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 1d20563e..efa1c0aa 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -4,6 +4,7 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. +#include #include #include #include @@ -21,6 +22,9 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt visualizer = std::make_unique("world_model/trajectory"); + pass_score_visualizer = + std::make_unique("world_model/pass_score"); + declare_parameter("position_history_size", 200); get_parameter("position_history_size", history_size); @@ -233,6 +237,60 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar } game_analysis_msg.their_slack.push_back(slack_msg); } + + auto our_robots = world_model->ours.getAvailableRobots(true); + const auto enemy_robots = world_model->theirs.getAvailableRobots(); + + auto calc_score = [&](Point p) { + Segment ball_to_target{world_model->ball.pos, p}; + double score = 1.0; + // 0~4mで遠くなるほどスコアが高い + score += std::clamp((p - world_model->ball.pos).norm() * 0.5, 0.0, 2.0); + // パス先のゴールチャンスが大きい場合はスコアを上げる(30度以上で最大0.5上昇) + auto [best_angle, goal_angle_width] = world_model->getLargestGoalAngleRangeFromPoint(p); + score += std::clamp(goal_angle_width / (M_PI / 12.), 0.0, 0.5); + // 敵ゴールに近いときはスコアを上げる + double normed_distance_to_their_goal = + ((p - world_model->getTheirGoalCenter()).norm() - (world_model->field_size.x() * 0.5)) / + (world_model->field_size.x() * 0.5); + // マイナスのときはゴールに近い + score *= (1.0 - normed_distance_to_their_goal * 0.5); + if (auto nearest_enemy = + world_model->getNearestRobotWithDistanceFromSegment(ball_to_target, enemy_robots); + nearest_enemy) { + // ボールから遠い敵がパスコースを塞いでいる場合は諦める + if ( + nearest_enemy->robot->getDistance(world_model->ball.pos) > 2.0 && + nearest_enemy->distance < 0.4) { + score = 0.0; + } + // パスラインに敵がいるときはスコアを下げる + score *= 1.0 / (1.0 + nearest_enemy->distance); + } + return score; + }; + + constexpr double UNIT = 0.2; + auto grid_points = getPoints( + Point(0, 0), UNIT, UNIT, world_model->field_size.x() / UNIT, + world_model->field_size.y() / UNIT); + auto score_grid = + grid_points | + ranges::views::transform([&](const auto & p) { return std::make_pair(p, calc_score(p)); }) | + ranges::to(); + + ranges::for_each(score_grid, [&](const auto & pair) { + SvgCircleBuilder circle; + circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); + pass_score_visualizer->add(circle.getSvgString()); + }); + pass_score_visualizer->flush(); + + auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { + return std::make_pair(robot, calc_score(robot->pose.pos)); + }) | + ranges::to(); + world_model->update(game_analysis_msg); } diff --git a/utility/crane_basics/include/crane_basics/ddps.hpp b/utility/crane_basics/include/crane_basics/ddps.hpp index f9e7c21d..b4704f65 100644 --- a/utility/crane_basics/include/crane_basics/ddps.hpp +++ b/utility/crane_basics/include/crane_basics/ddps.hpp @@ -51,5 +51,20 @@ inline auto getPoints(const Point & center, float unit, int unit_num) -> std::ve return points; } +inline auto getPoints( + const Point & center, float unit_x, float unit_y, int unit_num_x, int unit_num_y) + -> std::vector +{ + std::vector points; + for (float x = center.x() - unit_x * (unit_num_x / 2.f); + x <= center.x() + unit_x * (unit_num_x / 2.f); x += unit_x) { + for (float y = center.y() - unit_y * (unit_num_y / 2.f); + y <= center.y() + unit_y * (unit_num_y / 2.f); y += unit_y) { + points.emplace_back(Point(x, y)); + } + } + return points; +} + } // namespace crane #endif // CRANE_BASICS__DDPS_HPP_ From c805830f4a4f36e58487e8e8f64c333f49ad678b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 6 Mar 2025 02:39:04 +0900 Subject: [PATCH 02/55] =?UTF-8?q?launch=E3=81=AE=E6=95=B4=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.py | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/crane_bringup/launch/crane.launch.py b/crane_bringup/launch/crane.launch.py index 9cb5f0b4..20bdcdfe 100644 --- a/crane_bringup/launch/crane.launch.py +++ b/crane_bringup/launch/crane.launch.py @@ -47,11 +47,6 @@ def generate_launch_description(): DeclareLaunchArgument( "sim", default_value="true", description="シミュレータフラグ" ), - DeclareLaunchArgument( - "original_grsim", - default_value="false", - description="GrSimを使用する場合はtrueにする", - ), DeclareLaunchArgument( "simple_ai", default_value="false", description="SimpleAIモードのフラグ" ), @@ -107,13 +102,6 @@ def generate_launch_description(): ], on_exit=default_exit_behavior, ), - Node( - package="crane_clock_publisher", - executable="crane_clock_publisher_node", - output="screen", - parameters=[{"time_scale": 1.00}], - on_exit=default_exit_behavior, - ), Node( package="crane_sender", # executable="simulation_protocol_sender_node", From baea6295eb23960a115fb9c186f92e467b02fa75 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 6 Mar 2025 09:42:34 +0900 Subject: [PATCH 03/55] =?UTF-8?q?VS=20Code=E3=81=AE=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=81=AB=E3=82=B9=E3=83=9A=E3=83=AB=E3=83=81=E3=82=A7=E3=83=83?= =?UTF-8?q?=E3=82=AF=E3=81=AE=E8=BE=9E=E6=9B=B8=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 134 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 133 insertions(+), 1 deletion(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 51808ec9..891d31af 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -20,5 +20,137 @@ "chrono": "cpp", "*.ipp": "cpp", "functional": "cpp" - } + }, + "cSpell.words": [ + "grSim", + "robocup", + "robocupssl", + "teleop", + "inplay", + "matplotlibcpp", + "DPPS", + "consai", + "DBOOST", + "ZJUNlict", + "autoref", + "yellowteam", + "isteamyellow ", + "wheelsspeed", + "kickspeedx", + "kickspeedz", + "veltangent", + "velnormal", + "velangular", + "STRGREATER", + "bringup", + "gitflow", + "sint32", + "oneof", + "customwidget", + "customwidgets", + "Midlight", + "brushstyle", + "colorrole", + "statustip", + "Robotstatus", + "AIS", + "AMC", + "CMμs", + "ER-Force", + "erforce", + "ibis", + "ITAndroids", + "Immortals", + "KIKS", + "KgpKubs", + "MCT", + "Susano", + "Roboteam", + "MRL", + "NAELIC", + "NEUIslanders", + "OMID", + "OP-AmP", + "Parsian", + "RFC Cambridge", + "Ri-One", + "Ri-one", + "RoboDragons", + "RoboFEI", + "RoboIME", + "RoboJackets", + "Twente", + "RobôCin", + "SRC", + "SSH", + "STOx’s", + "Sysmic", + "TIGERs", + "Mannheim", + "Tritons", + "RCSC", + "Thunderbots", + "ULtron", + "UMass", + "Minutebots", + "URoboRus", + "Unknown", + "Warthog", + "ZJUNlict", + "luhbots", + "nAMeC", + "TRAPS", + "Shinobi", + "reRo", + "roboticserlangen", + "tigersmannheim", + "simulatorcli", + "sizepolicy", + "hsizetype", + "vsizetype", + "horstretch", + "verstretch", + "notr", + "rects", + "qcolor", + "frootspi", + "buttongroup", + "buttongroups", + "AUTOUIC", + "AUTORCC", + "FORCESTART", + "PETG", + "Waitables", + "zdixwfbhdipwhaaxtiqcxwywwkekaxog", + "callgrind", + "rowwise", + "colwise", + "MPPI", + "Kwargs", + "voicevox", + "FREEKICK", + "rcst", + "GLFW", + "GLEW", + "libglfw", + "libglew", + "opengl", + "imgui", + "Framebuffer", + "vsync", + "CHECKVERSION", + "BLAUE", + "Hackentrick", + "3rdparty", + "PROTOS", + "libprotobuf", + "RELWITHDEBINFO", + "NODEBUG", + "remmina", + "gltf", + "svgs", + "teleporting", + "DDPS", + "cooldown" + ] } From 4582d7d08605190b65574eb497ac6597c809f6f5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:58:44 +0900 Subject: [PATCH 04/55] =?UTF-8?q?=E3=83=91=E3=82=B9=E3=82=B9=E3=82=B3?= =?UTF-8?q?=E3=82=A2=E3=82=92GameAnalysis=E3=81=AB=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/CMakeLists.txt | 1 + crane_msgs/msg/analysis/FloatWithID.msg | 2 ++ crane_msgs/msg/analysis/GameAnalysis.msg | 3 +++ crane_world_model_publisher/src/world_model_publisher.cpp | 8 ++++++++ 4 files changed, 14 insertions(+) create mode 100755 crane_msgs/msg/analysis/FloatWithID.msg diff --git a/crane_msgs/CMakeLists.txt b/crane_msgs/CMakeLists.txt index f9888cb2..57a6e3a7 100755 --- a/crane_msgs/CMakeLists.txt +++ b/crane_msgs/CMakeLists.txt @@ -37,6 +37,7 @@ set(msg_files "msg/analysis/Kick.msg" "msg/analysis/Slack.msg" "msg/analysis/SlackWithPoint.msg" + "msg/analysis/FloatWithID.msg" "msg/planner/PassPlan.msg" "msg/planner/PassInfo.msg" diff --git a/crane_msgs/msg/analysis/FloatWithID.msg b/crane_msgs/msg/analysis/FloatWithID.msg new file mode 100755 index 00000000..e59b395d --- /dev/null +++ b/crane_msgs/msg/analysis/FloatWithID.msg @@ -0,0 +1,2 @@ +uint8 id +float32 value diff --git a/crane_msgs/msg/analysis/GameAnalysis.msg b/crane_msgs/msg/analysis/GameAnalysis.msg index 70ba9aa5..d4d67a36 100755 --- a/crane_msgs/msg/analysis/GameAnalysis.msg +++ b/crane_msgs/msg/analysis/GameAnalysis.msg @@ -5,3 +5,6 @@ Slack[] our_slack Slack[] their_slack float32 ball_horizon + +# sorted by score +FloatWithID[] pass_scores diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index efa1c0aa..05af6398 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -290,6 +290,14 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar return std::make_pair(robot, calc_score(robot->pose.pos)); }) | ranges::to(); + // larger score first + ranges::sort(score_with_bots, [](const auto & a, const auto & b) { return a.second > b.second; }); + + game_analysis_msg.pass_scores = score_with_bots | ranges::views::transform([](const auto & pair) { + crane_msgs::msg::FloatWithID msg; + return msg.set__id(pair.first->id).set__value(pair.second); + }) | + ranges::to(); world_model->update(game_analysis_msg); } From f308e06ce81997070fd238acd0001942edefc9a0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:59:05 +0900 Subject: [PATCH 05/55] =?UTF-8?q?=E3=83=9A=E3=83=8A=E3=83=AB=E3=83=86?= =?UTF-8?q?=E3=82=A3=E3=82=A8=E3=83=AA=E3=82=A2=E5=86=85=E3=81=AE=E3=83=91?= =?UTF-8?q?=E3=82=B9=E3=82=B9=E3=82=B3=E3=82=A2=E3=82=920=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 05af6398..1ab8b0fd 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -267,6 +267,10 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar // パスラインに敵がいるときはスコアを下げる score *= 1.0 / (1.0 + nearest_enemy->distance); } + + if (world_model->point_checker.isPenaltyArea(p)) { + score = 0.0; + } return score; }; From b2411ceb2c96efd7e63507be412504374d2c83bc Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 02:59:21 +0900 Subject: [PATCH 06/55] =?UTF-8?q?=E3=83=81=E3=83=83=E3=83=97=E3=82=AD?= =?UTF-8?q?=E3=83=83=E3=82=AF=E3=83=9C=E3=83=BC=E3=83=8A=E3=82=B9=E3=81=AF?= =?UTF-8?q?1m=E3=81=84=E3=81=AA=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 1ab8b0fd..88d4c506 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -260,7 +260,7 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar nearest_enemy) { // ボールから遠い敵がパスコースを塞いでいる場合は諦める if ( - nearest_enemy->robot->getDistance(world_model->ball.pos) > 2.0 && + nearest_enemy->robot->getDistance(world_model->ball.pos) > 1.0 && nearest_enemy->distance < 0.4) { score = 0.0; } From df6ab9543a809a1b51701b42d2ec5c21ee080806 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 03:27:07 +0900 Subject: [PATCH 07/55] =?UTF-8?q?attacker=E3=81=A7=E8=A8=88=E7=AE=97?= =?UTF-8?q?=E3=81=97=E3=81=9Fpass=20score=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 56 +++++------------------------ 1 file changed, 9 insertions(+), 47 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 24592d3b..e863fb91 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -217,58 +217,20 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - // TODO(HansRobo): しっかりパス先を選定する - // int receiver_id = getParameter("receiver_id"); double best_score = 0.0; Point best_target; - int best_id = -1; - for (auto & our_robot : our_robots) { - // ゴールに近い味方は対象外 - if (our_robot->getDistance(world_model()->getOurGoalCenter()) < 3.0) { + for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { + if (score_with_id.id != robot()->id) { continue; - } - Segment ball_to_target{world_model()->ball.pos, our_robot->pose.pos}; - auto target = our_robot->pose.pos; - double score = 1.0; - // パス先のゴールチャンスが大きい場合はスコアを上げる(30度以上で最大0.5上昇) - auto [best_angle, goal_angle_width] = - world_model()->getLargestGoalAngleRangeFromPoint(target); - score += std::clamp(goal_angle_width / (M_PI / 12.), 0.0, 0.5); - - // 敵ゴールに近いときはスコアを上げる - double normed_distance_to_their_goal = - ((target - world_model()->getTheirGoalCenter()).norm() - - (world_model()->field_size.x() * 0.5)) / - (world_model()->field_size.x() * 0.5); - // マイナスのときはゴールに近い - score *= (1.0 - normed_distance_to_their_goal); - - auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( - ball_to_target, world_model()->theirs.getAvailableRobots()); - // ボールから遠い敵がパスコースを塞いでいる場合は諦める - if (nearest_enemy.has_value()) { - if ( - nearest_enemy->robot->getDistance(world_model()->ball.pos) > 1.0 && - nearest_enemy->distance < 0.4) { - score = 0.0; - } - // パスラインに敵がいるときはスコアを下げる - score *= 1.0 / (1.0 + nearest_enemy->distance); - } - - if (score > best_score) { - best_score = score; - best_target = target; - best_id = our_robot->id; + } else if (score_with_id.id != world_model()->getOurGoalieId()) { + continue; + } else { + kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; + pass_receiver_id = score_with_id.id; + return true; } } - - auto ret = best_score > 0.5; - if (ret) { - kick_target = best_target; - pass_receiver_id = best_id; - } - return ret; + return false; }); addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { From 625e5b9b6e1dadce9865ffe52dd53231f1990976 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:08:35 +0900 Subject: [PATCH 08/55] =?UTF-8?q?emplace=5Frobot=5Fplanner=E3=81=A7?= =?UTF-8?q?=E6=9C=80=E5=A4=A7=E9=80=9F=E5=BA=A6=E3=82=92=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=81=A7=E3=81=8D=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E3=81=99?= =?UTF-8?q?=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/emplace_robot.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/emplace_robot.cpp b/crane_robot_skills/src/emplace_robot.cpp index 615dac6a..5fa8b10c 100644 --- a/crane_robot_skills/src/emplace_robot.cpp +++ b/crane_robot_skills/src/emplace_robot.cpp @@ -22,6 +22,7 @@ EmplaceRobot::EmplaceRobot(RobotCommandWrapperBase::SharedPtr & base) setParameter("robot_interval", 0.3); // ボールとの距離 setParameter("margin_distance", 0.5); + setParameter("max_speed", 1.5); } Status EmplaceRobot::update() @@ -36,7 +37,7 @@ Status EmplaceRobot::update() double position_y_side = getParameter("emplace_line_positive") ? 1.0 : -1.0; target_position.y() = position_y_side * world_model()->field_size.y() * 0.5; - command.setTargetPosition(target_position); + command.setTargetPosition(target_position).setMaxVelocity(getParameter("max_speed")); return Status::RUNNING; } } // namespace crane::skills From ef3d0287659e401b06362bf100d5c166e6eb236b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:09:50 +0900 Subject: [PATCH 09/55] =?UTF-8?q?=E3=81=86=E3=82=8B=E3=81=95=E3=81=84?= =?UTF-8?q?=E3=83=A1=E3=83=83=E3=82=BB=E3=83=BC=E3=82=B8=E3=82=92=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index fec6202f..5a2ca8bd 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -148,8 +148,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // ボールが角にある場合は引っ張る bool is_corner = std::abs(ball_pos.x()) > (field.x() - 0.05) && std::abs(ball_pos.y()) > (field.y() - 0.05); - std::cout << "is_corner: " << is_corner << ", contact duration: " - << robot()->ball_contact.getContactDuration().count() / 1e6 << std::endl; return is_corner && robot()->ball_contact.getContactDuration().count() / 1e6 > 500; }); From d5aeec0ca6e9866eddd32772bd764adafdefdd09 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:10:32 +0900 Subject: [PATCH 10/55] =?UTF-8?q?=E7=AF=84=E5=9B=B2=E5=86=85=E5=8F=82?= =?UTF-8?q?=E7=85=A7=E3=81=A7=E5=AE=9F=E8=A1=8C=E6=99=82=E3=82=A8=E3=83=A9?= =?UTF-8?q?=E3=83=BC=E3=81=99=E3=82=8B=E3=81=AE=E3=81=A7=E3=82=B3=E3=83=A1?= =?UTF-8?q?=E3=83=B3=E3=83=88=E3=82=A2=E3=82=A6=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_data_provider.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 deb9af94..381879c7 100644 --- a/crane_world_model_publisher/src/world_model_data_provider.cpp +++ b/crane_world_model_publisher/src/world_model_data_provider.cpp @@ -61,7 +61,8 @@ WorldModelDataProvider::WorldModelDataProvider(rclcpp::Node & node) if (feedback->ball_sensor) { contact.last_contacted_time = now; } - data.ball_sensor_detected[robot.id] = feedback->ball_sensor; + // 範囲内参照で実行時エラー + // data.ball_sensor_detected[robot.id] = feedback->ball_sensor; auto & robot_info = data.robot_info[static_cast(game_data.our_color)][robot.id]; robot_info.ball_sensor = feedback->ball_sensor; robot_info.last_ball_sensor_stamp = now; From 453259fb8915174242daf579e1137e1bae1e286e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:10:46 +0900 Subject: [PATCH 11/55] =?UTF-8?q?=E5=8F=AF=E8=A6=96=E5=8C=96=E3=81=8C?= =?UTF-8?q?=E9=87=8D=E3=81=9F=E3=81=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index 88d4c506..e2dcc9af 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -288,7 +288,7 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); pass_score_visualizer->add(circle.getSvgString()); }); - pass_score_visualizer->flush(); + // pass_score_visualizer->flush(); auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { return std::make_pair(robot, calc_score(robot->pose.pos)); From 4c4b6f2985427ccb6f79e1d757f9caaec0e4c303 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:11:10 +0900 Subject: [PATCH 12/55] =?UTF-8?q?=E5=AE=9F=E9=9A=9B=E3=81=AB=E4=BD=BF?= =?UTF-8?q?=E3=81=A3=E3=81=9F=E3=82=84=E3=81=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- docker/real/docker-compose.yaml | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/docker/real/docker-compose.yaml b/docker/real/docker-compose.yaml index d8e76ac8..b22279ae 100644 --- a/docker/real/docker-compose.yaml +++ b/docker/real/docker-compose.yaml @@ -1,4 +1,20 @@ services: + ssl-game-controller: + image: robocupssl/ssl-game-controller:3.11.2 + command: + - -visionAddress + - 224.5.23.2:10006 + - -trackerAddress + - 224.5.23.2:11010 # not yet overridable by autoRefs + - -publishAddress + - 224.5.23.1:11003 + - -address + - :8081 + volumes: + - ./config:/config:rw + network_mode: host + ports: + - 8081:8081/tcp ssl-vision-client: image: robocupssl/ssl-vision-client:1.6.0 command: @@ -18,7 +34,7 @@ services: - --visionAddress - 224.5.23.2:10006 - --refereeAddress - - 224.5.23.1:10003 + - 224.5.23.1:11003 - --trackerAddress - 224.5.23.2:11010 network_mode: host From aa83d0464a3b06f8b3d16e1bcb5eba83d002b20a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:08 +0900 Subject: [PATCH 13/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E5=8A=A0=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E8=90=BD=E3=81=A8=E3=81=99=E4=BB=A3=E3=82=8F?= =?UTF-8?q?=E3=82=8A=E3=81=AB=E6=9C=80=E9=AB=98=E9=80=9F=E5=BA=A6=E3=82=92?= =?UTF-8?q?=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 5a2ca8bd..6432fd42 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -290,8 +290,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(0.5); - command.setMaxAcceleration(1.0); + command.setMaxVelocity(2.0); + command.setMaxAcceleration(0.5); return Status::RUNNING; }); From 29d6f847f20e785323010e2c17da9f35c5cbd46c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:29 +0900 Subject: [PATCH 14/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E3=83=89=E3=83=AA?= =?UTF-8?q?=E3=83=96=E3=83=AB=E3=83=91=E3=83=AF=E3=83=BC=E3=82=92=E8=90=BD?= =?UTF-8?q?=E3=81=A8=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 6432fd42..bc4a2f45 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -281,8 +281,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.3); - move_with_ball->setParameter("ball_stabilizing_time", 3.); + move_with_ball->setParameter("dribble_power", 0.15); + move_with_ball->setParameter("ball_stabilizing_time", 2.); move_with_ball->setParameter("reach_threshold", 0.2); } From 6cf877fbf0caef2f12c549aade639795a95716e3 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:51 +0900 Subject: [PATCH 15/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E9=9B=A2=E8=84=B1=E3=81=AE=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index bc4a2f45..2daa04c5 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -356,6 +356,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.setTargetTheta(pull_back_angle); command.setOmegaLimit(0.0); + command.setMaxVelocity(1.0); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.disableGoalAreaAvoidance(); From d00c340b689b43edc19c14afb95d4b6850d96fae Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:30:10 +0900 Subject: [PATCH 16/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E7=B5=82=E4=BA=86=E3=81=AE=E6=9D=A1?= =?UTF-8?q?=E4=BB=B6=E3=82=92=E3=83=AB=E3=83=BC=E3=83=AB=E3=81=AB=E5=9F=BA?= =?UTF-8?q?=E3=81=A5=E3=81=84=E3=81=A6=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 2daa04c5..324fa669 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -367,7 +367,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba addTransition( SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 + return (world_model()->ball.pos - placement_target).norm() > 0.15; + }); } void SingleBallPlacement::print(std::ostream & os) const From 5903bc60b826c699526c731ca1b23ae98245081f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Mar 2025 06:31:29 +0000 Subject: [PATCH 17/55] style(pre-commit): autofix --- crane_robot_skills/src/single_ball_placement.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 324fa669..e959796a 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -366,8 +366,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { + SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, [this]() { Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 From 51c091ec237a1f0971327a86810be8fb7df4940e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:10:51 +0900 Subject: [PATCH 18/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E3=81=AEGetContactBall?= =?UTF-8?q?=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AE=E4=BD=BF=E7=94=A8=E3=82=92?= =?UTF-8?q?=E5=8F=96=E3=82=8A=E3=82=84=E3=82=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index e959796a..bcd549d7 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -254,21 +254,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .fill("white") .fontSize(100); visualizer->add(text_builder.getSvgString()); - if (not get_ball_contact) { - get_ball_contact = std::make_shared(command_base); - } - - skill_status = get_ball_contact->run(); + // if (not get_ball_contact) { + // get_ball_contact = std::make_shared(command_base); + // } + // + // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); + command.disableBallAvoidance(); command.setMaxVelocity(0.5); command.setMaxAcceleration(1.0); + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + command.lookAtFrom(placement_target, world_model()->ball.pos); + command.setTargetPosition(world_model()->ball.pos); return Status::RUNNING; }); addTransition( SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From d49f63420cf40c7e980f9bdec32c1e422eb930fa Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:28 +0900 Subject: [PATCH 19/55] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=AE=E9=80=9F=E5=BA=A6=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index bcd549d7..37a1632b 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -286,8 +286,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.15); - move_with_ball->setParameter("ball_stabilizing_time", 2.); + move_with_ball->setParameter("dribble_power", 0.25); + move_with_ball->setParameter("ball_stabilizing_time", 1.); move_with_ball->setParameter("reach_threshold", 0.2); } From 21ee6c5969bc9fb76c02fea90c1b7d190fa2288a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:50 +0900 Subject: [PATCH 20/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E9=80=9F=E5=BA=A6=E3=82=92=E4=B8=8B=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 37a1632b..13c90a15 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -261,7 +261,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); command.disableBallAvoidance(); - command.setMaxVelocity(0.5); + command.setMaxVelocity(0.2); command.setMaxAcceleration(1.0); Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); From 30534c81b7428a031c44d46e5a39d4fab78313d5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:13:19 +0900 Subject: [PATCH 21/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=8C=E6=97=A9=E3=81=99=E3=81=8E=E3=82=8B=E3=81=AE=E3=81=A7?= =?UTF-8?q?=E9=81=85=E3=81=8F=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 13c90a15..8030a5d1 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -295,8 +295,9 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(2.0); - command.setMaxAcceleration(0.5); + command.setMaxVelocity(1.2); + command.setMaxAcceleration(1.0); + command.setOmegaLimit(0.2); return Status::RUNNING; }); From 78ce5b080a5c0f266d2ae57a5afe04a00337879d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:13:48 +0900 Subject: [PATCH 22/55] =?UTF-8?q?INPLAY=E3=81=A7=E3=83=87=E3=82=A3?= =?UTF-8?q?=E3=83=95=E3=82=A7=E3=83=B3=E3=82=B9=E3=82=92=E6=B8=9B=E3=82=89?= =?UTF-8?q?=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_session_controller/config/play_situation/INPLAY.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/session/crane_session_controller/config/play_situation/INPLAY.yaml b/session/crane_session_controller/config/play_situation/INPLAY.yaml index ba95d0e0..6d9d5f54 100644 --- a/session/crane_session_controller/config/play_situation/INPLAY.yaml +++ b/session/crane_session_controller/config/play_situation/INPLAY.yaml @@ -4,7 +4,7 @@ sessions: - name: emplace_robot capacity: 1 - name: total_defense - capacity: 4 + capacity: 2 - name: attacker_skill capacity: 1 - name: pass_receive From 0d1bc8f6d63666573659454f0efd1da5e3a1970a Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 00:22:31 +0900 Subject: [PATCH 23/55] =?UTF-8?q?=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88?= =?UTF-8?q?=E3=82=A2=E3=82=A6=E3=83=88=E3=81=AE=E4=BB=95=E6=96=B9=E3=81=8C?= =?UTF-8?q?=E6=82=AA=E3=81=8F=E3=80=81=E5=A4=A7=E9=87=8F=E3=81=AE=E3=83=A1?= =?UTF-8?q?=E3=83=A2=E3=83=AA=E3=82=92=E6=B6=88=E8=B2=BB=E3=81=97=E3=81=A6?= =?UTF-8?q?=E3=81=97=E3=81=BE=E3=81=A3=E3=81=A6=E3=81=84=E3=81=9F=E3=81=AE?= =?UTF-8?q?=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_publisher.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index e2dcc9af..02808575 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -286,9 +286,9 @@ void WorldModelPublisherComponent::postProcessWorldModel(WorldModelWrapper::Shar ranges::for_each(score_grid, [&](const auto & pair) { SvgCircleBuilder circle; circle.center(pair.first).radius(pair.second * 0.05).stroke("red").strokeWidth(2.); - pass_score_visualizer->add(circle.getSvgString()); + // pass_score_visualizer->add(circle.getSvgString()); }); - // pass_score_visualizer->flush(); + pass_score_visualizer->flush(); auto score_with_bots = our_robots | ranges::views::transform([&](const auto & robot) { return std::make_pair(robot, calc_score(robot->pose.pos)); From 4c62eec2b89f9c0fd09fb646e7a0c0214e9a28dd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:31:00 +0900 Subject: [PATCH 24/55] =?UTF-8?q?rclcpp::Time=E3=82=92RCL=5FROS=5FTIME?= =?UTF-8?q?=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/visualization_data_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index 92b7601c..2af2f020 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -169,7 +169,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar .strokeWidth(20); visualizer_tracked->add(line_builder.getSvgString()); - auto now = rclcpp::Clock().now(); + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); const double corner_angle = std::acos(0.055 / 0.085); for (const auto & robot : world_model->ours.getAvailableRobots()) { SvgRobotBuilder builder; @@ -192,7 +192,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar // ボールセンサ if ( now.get_clock_type() == robot->ball_sensor_stamp.get_clock_type() && - (now - robot->ball_sensor_stamp).seconds() < 1.0 && robot->ball_sensor) { + std::abs((now - robot->ball_sensor_stamp).seconds()) < 0.01 && robot->ball_sensor) { SvgLineBuilder ball_sensor_line; ball_sensor_line .start(robot->kicker_center() + getVerticalVec(getNormVec(robot->pose.theta)) * 0.055) From 8176b140427025af689ff5be10c7ac82f6f6768f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:34:09 +0900 Subject: [PATCH 25/55] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=A7=E3=81=AE=E8=A7=92=E9=80=9F=E5=BA=A6=E5=88=B6?= =?UTF-8?q?=E9=99=90=E3=81=8C=E5=BC=B7=E3=81=99=E3=81=8E=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 8030a5d1..d64ceb93 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -297,7 +297,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disableRuleAreaAvoidance(); command.setMaxVelocity(1.2); command.setMaxAcceleration(1.0); - command.setOmegaLimit(0.2); + command.setOmegaLimit(1.0); return Status::RUNNING; }); From b095ce23d5d60d4034dfa155b0fab672bfc5e0e1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 15:03:31 +0900 Subject: [PATCH 26/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E7=B5=82=E4=BA=86=E6=9D=A1=E4=BB=B6=E3=81=AB=E3=83=9C?= =?UTF-8?q?=E3=83=BC=E3=83=AB=E3=82=BB=E3=83=B3=E3=82=B5=E3=82=92=E7=B5=84?= =?UTF-8?q?=E3=81=BF=E8=BE=BC=E3=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index d64ceb93..e615dae3 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -272,8 +272,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); + SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); + static int count = 0; + if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { + if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { + if (++count > 10) { + count = 0; + return true; + } else { + return false; + } + } else { + count = 0; + return false; + } + } else { + // ボールセンサが動いていないとき + return robot()->getDistance(world_model()->ball.pos) < 0.15; + } + }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From 88f4d4713eff64a29323e0483858aa870f6a4fe7 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 20:04:25 +0900 Subject: [PATCH 27/55] =?UTF-8?q?launch=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/crane_bringup/launch/crane.launch.py b/crane_bringup/launch/crane.launch.py index 20bdcdfe..c460399a 100644 --- a/crane_bringup/launch/crane.launch.py +++ b/crane_bringup/launch/crane.launch.py @@ -51,10 +51,10 @@ def generate_launch_description(): "simple_ai", default_value="false", description="SimpleAIモードのフラグ" ), DeclareLaunchArgument( - "max_vel", default_value="3.0", description="ロボットの最大速度" + "max_vel", default_value="7.0", description="ロボットの最大速度" ), DeclareLaunchArgument( - "speak", default_value="true", description="音声ノードの起動フラグ" + "speak", default_value="false", description="音声ノードの起動フラグ" ), DeclareLaunchArgument( "is_emplace_positive_side", @@ -62,7 +62,7 @@ def generate_launch_description(): description="ロボットの退場する方向", ), DeclareLaunchArgument( - "record", default_value="false", description="rosbag記録フラグ" + "record", default_value="true", description="rosbag記録フラグ" ), Node( package="crane_session_controller", @@ -91,13 +91,13 @@ def generate_launch_description(): output="screen", parameters=[ {"planner": "rvo2"}, - {"p_gain": 5.0}, + {"p_gain": 3.0}, {"i_gain": 0.00}, {"i_saturation": 0.00}, {"d_gain": 1.0}, {"max_vel": LaunchConfiguration("max_vel")}, - {"max_acc": 3.0}, - {"deceleration_factor": 1.5}, + {"max_acc": 2.0}, + {"deceleration_factor": 1.0}, {"rvo_radius": 0.15}, ], on_exit=default_exit_behavior, @@ -137,7 +137,7 @@ def generate_launch_description(): {"i_saturation": 0.0}, {"d_gain": 4.0}, {"max_vel": LaunchConfiguration("max_vel")}, - {"max_acc": 4.0}, + {"max_acc": 2.5}, {"deceleration_factor": 1.5}, ], on_exit=default_exit_behavior, @@ -147,9 +147,9 @@ def generate_launch_description(): executable="ibis_sender_node", parameters=[ {"no_movement": False}, - {"latency_ms": 0.0}, + {"latency_ms": 100.0}, {"sim_mode": LaunchConfiguration("sim")}, - {"kick_power_limit_straight": 0.30}, + {"kick_power_limit_straight": 0.50}, {"kick_power_limit_chip": 1.0}, { "use_simple_velocity": False From 831c7325cd1f3fe9dab24a2434f6f02a609f370d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 20:05:10 +0900 Subject: [PATCH 28/55] =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index e615dae3..d7539eb5 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -277,7 +277,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba static int count = 0; if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { - if (++count > 10) { + if (++count > 2) { count = 0; return true; } else { From 41efea0f9385e7bdd1c6b730025a28d30b290da8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:08:35 +0900 Subject: [PATCH 29/55] =?UTF-8?q?emplace=5Frobot=5Fplanner=E3=81=A7?= =?UTF-8?q?=E6=9C=80=E5=A4=A7=E9=80=9F=E5=BA=A6=E3=82=92=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=81=A7=E3=81=8D=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB=E3=81=99?= =?UTF-8?q?=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/emplace_robot.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/emplace_robot.cpp b/crane_robot_skills/src/emplace_robot.cpp index 615dac6a..5fa8b10c 100644 --- a/crane_robot_skills/src/emplace_robot.cpp +++ b/crane_robot_skills/src/emplace_robot.cpp @@ -22,6 +22,7 @@ EmplaceRobot::EmplaceRobot(RobotCommandWrapperBase::SharedPtr & base) setParameter("robot_interval", 0.3); // ボールとの距離 setParameter("margin_distance", 0.5); + setParameter("max_speed", 1.5); } Status EmplaceRobot::update() @@ -36,7 +37,7 @@ Status EmplaceRobot::update() double position_y_side = getParameter("emplace_line_positive") ? 1.0 : -1.0; target_position.y() = position_y_side * world_model()->field_size.y() * 0.5; - command.setTargetPosition(target_position); + command.setTargetPosition(target_position).setMaxVelocity(getParameter("max_speed")); return Status::RUNNING; } } // namespace crane::skills From 3152a8aac9174f72e12a4f96122c37963e97a874 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:10:32 +0900 Subject: [PATCH 30/55] =?UTF-8?q?=E7=AF=84=E5=9B=B2=E5=86=85=E5=8F=82?= =?UTF-8?q?=E7=85=A7=E3=81=A7=E5=AE=9F=E8=A1=8C=E6=99=82=E3=82=A8=E3=83=A9?= =?UTF-8?q?=E3=83=BC=E3=81=99=E3=82=8B=E3=81=AE=E3=81=A7=E3=82=B3=E3=83=A1?= =?UTF-8?q?=E3=83=B3=E3=83=88=E3=82=A2=E3=82=A6=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_world_model_publisher/src/world_model_data_provider.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 deb9af94..381879c7 100644 --- a/crane_world_model_publisher/src/world_model_data_provider.cpp +++ b/crane_world_model_publisher/src/world_model_data_provider.cpp @@ -61,7 +61,8 @@ WorldModelDataProvider::WorldModelDataProvider(rclcpp::Node & node) if (feedback->ball_sensor) { contact.last_contacted_time = now; } - data.ball_sensor_detected[robot.id] = feedback->ball_sensor; + // 範囲内参照で実行時エラー + // data.ball_sensor_detected[robot.id] = feedback->ball_sensor; auto & robot_info = data.robot_info[static_cast(game_data.our_color)][robot.id]; robot_info.ball_sensor = feedback->ball_sensor; robot_info.last_ball_sensor_stamp = now; From 70b84599d3709be0621bed6bd2a903fecaa82ec8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:08 +0900 Subject: [PATCH 31/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E5=8A=A0=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E8=90=BD=E3=81=A8=E3=81=99=E4=BB=A3=E3=82=8F?= =?UTF-8?q?=E3=82=8A=E3=81=AB=E6=9C=80=E9=AB=98=E9=80=9F=E5=BA=A6=E3=82=92?= =?UTF-8?q?=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index fec6202f..479943a5 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -292,8 +292,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(0.5); - command.setMaxAcceleration(1.0); + command.setMaxVelocity(2.0); + command.setMaxAcceleration(0.5); return Status::RUNNING; }); From 52497f818a4aa33b1258d679e24f784af8a35356 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:29 +0900 Subject: [PATCH 32/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E6=99=82=E3=81=AE=E3=83=89=E3=83=AA?= =?UTF-8?q?=E3=83=96=E3=83=AB=E3=83=91=E3=83=AF=E3=83=BC=E3=82=92=E8=90=BD?= =?UTF-8?q?=E3=81=A8=E3=81=99?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 479943a5..9fb95fb3 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -283,8 +283,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.3); - move_with_ball->setParameter("ball_stabilizing_time", 3.); + move_with_ball->setParameter("dribble_power", 0.15); + move_with_ball->setParameter("ball_stabilizing_time", 2.); move_with_ball->setParameter("reach_threshold", 0.2); } From 8cb334b0ba8f6822f0aea3f4e1e45cff1bdb78b0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:29:51 +0900 Subject: [PATCH 33/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E9=9B=A2=E8=84=B1=E3=81=AE=E9=80=9F?= =?UTF-8?q?=E5=BA=A6=E3=82=92=E4=B8=8A=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 9fb95fb3..d6fc8ee3 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -358,6 +358,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.setTargetTheta(pull_back_angle); command.setOmegaLimit(0.0); + command.setMaxVelocity(1.0); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.disableGoalAreaAvoidance(); From 1b3b8f1ce123c8ed5e12a9c30d9ba49d9bb88747 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 15:30:10 +0900 Subject: [PATCH 34/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E7=B5=82=E4=BA=86=E3=81=AE=E6=9D=A1?= =?UTF-8?q?=E4=BB=B6=E3=82=92=E3=83=AB=E3=83=BC=E3=83=AB=E3=81=AB=E5=9F=BA?= =?UTF-8?q?=E3=81=A5=E3=81=84=E3=81=A6=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index d6fc8ee3..c99bebe3 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -369,7 +369,12 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba addTransition( SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 + return (world_model()->ball.pos - placement_target).norm() > 0.15; + }); } void SingleBallPlacement::print(std::ostream & os) const From 456e38d2b9afc62863d2fd611c27f6c8612b6f2d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 7 Mar 2025 06:31:29 +0000 Subject: [PATCH 35/55] style(pre-commit): autofix --- crane_robot_skills/src/single_ball_placement.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index c99bebe3..901dbaf4 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -368,8 +368,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, - [this]() { + SingleBallPlacementStates::LEAVE_BALL, SingleBallPlacementStates::ENTRY_POINT, [this]() { Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); // ルール 5.2 0.15m以内で認められる。再配置が必要場合のみ、 ENTRY_POINTへ移動 From c1f2174c7440e85620fddc51cbfd0845c60175af Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:10:51 +0900 Subject: [PATCH 36/55] =?UTF-8?q?=E3=83=97=E3=83=AC=E3=82=A4=E3=82=B9?= =?UTF-8?q?=E3=83=A1=E3=83=B3=E3=83=88=E3=81=A7=E3=81=AEGetContactBall?= =?UTF-8?q?=E3=82=B9=E3=82=AD=E3=83=AB=E3=81=AE=E4=BD=BF=E7=94=A8=E3=82=92?= =?UTF-8?q?=E5=8F=96=E3=82=8A=E3=82=84=E3=82=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 901dbaf4..464b152e 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -256,21 +256,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .fill("white") .fontSize(100); visualizer->add(text_builder.getSvgString()); - if (not get_ball_contact) { - get_ball_contact = std::make_shared(command_base); - } - - skill_status = get_ball_contact->run(); + // if (not get_ball_contact) { + // get_ball_contact = std::make_shared(command_base); + // } + // + // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); + command.disableBallAvoidance(); command.setMaxVelocity(0.5); command.setMaxAcceleration(1.0); + Point placement_target; + placement_target << getParameter("placement_x"), getParameter("placement_y"); + command.lookAtFrom(placement_target, world_model()->ball.pos); + command.setTargetPosition(world_model()->ball.pos); return Status::RUNNING; }); addTransition( SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return skill_status == Status::SUCCESS; }); + [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From 42186769942f548d7980cf1733a1cef9c5ced142 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:28 +0900 Subject: [PATCH 37/55] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=AE=E9=80=9F=E5=BA=A6=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 464b152e..4c20247b 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -288,8 +288,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba move_with_ball = std::make_shared(command_base); move_with_ball->setParameter("target_x", getParameter("placement_x")); move_with_ball->setParameter("target_y", getParameter("placement_y")); - move_with_ball->setParameter("dribble_power", 0.15); - move_with_ball->setParameter("ball_stabilizing_time", 2.); + move_with_ball->setParameter("dribble_power", 0.25); + move_with_ball->setParameter("ball_stabilizing_time", 1.); move_with_ball->setParameter("reach_threshold", 0.2); } From 756bb7980dff2c4b7b02a98eb223fa2657471a11 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:12:50 +0900 Subject: [PATCH 38/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E9=80=9F=E5=BA=A6=E3=82=92=E4=B8=8B=E3=81=92=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 4c20247b..3827e2fd 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -263,7 +263,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); command.disableBallAvoidance(); - command.setMaxVelocity(0.5); + command.setMaxVelocity(0.2); command.setMaxAcceleration(1.0); Point placement_target; placement_target << getParameter("placement_x"), getParameter("placement_y"); From cd8d09dda246836b5a5207cec63c7806b4836787 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 7 Mar 2025 19:13:19 +0900 Subject: [PATCH 39/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=8C=E6=97=A9=E3=81=99=E3=81=8E=E3=82=8B=E3=81=AE=E3=81=A7?= =?UTF-8?q?=E9=81=85=E3=81=8F=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 3827e2fd..19396202 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -297,8 +297,9 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disablePlacementAvoidance(); command.disableGoalAreaAvoidance(); command.disableRuleAreaAvoidance(); - command.setMaxVelocity(2.0); - command.setMaxAcceleration(0.5); + command.setMaxVelocity(1.2); + command.setMaxAcceleration(1.0); + command.setOmegaLimit(0.2); return Status::RUNNING; }); From b64fdb5222b767a29ffb7572e56e7081fb08197b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:31:00 +0900 Subject: [PATCH 40/55] =?UTF-8?q?rclcpp::Time=E3=82=92RCL=5FROS=5FTIME?= =?UTF-8?q?=E3=82=92=E4=BD=BF=E7=94=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/visualization_data_handler.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_world_model_publisher/src/visualization_data_handler.cpp b/crane_world_model_publisher/src/visualization_data_handler.cpp index 92b7601c..2af2f020 100644 --- a/crane_world_model_publisher/src/visualization_data_handler.cpp +++ b/crane_world_model_publisher/src/visualization_data_handler.cpp @@ -169,7 +169,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar .strokeWidth(20); visualizer_tracked->add(line_builder.getSvgString()); - auto now = rclcpp::Clock().now(); + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); const double corner_angle = std::acos(0.055 / 0.085); for (const auto & robot : world_model->ours.getAvailableRobots()) { SvgRobotBuilder builder; @@ -192,7 +192,7 @@ void VisualizationDataHandler::publish_vis_tracked(const WorldModelWrapper::Shar // ボールセンサ if ( now.get_clock_type() == robot->ball_sensor_stamp.get_clock_type() && - (now - robot->ball_sensor_stamp).seconds() < 1.0 && robot->ball_sensor) { + std::abs((now - robot->ball_sensor_stamp).seconds()) < 0.01 && robot->ball_sensor) { SvgLineBuilder ball_sensor_line; ball_sensor_line .start(robot->kicker_center() + getVerticalVec(getNormVec(robot->pose.theta)) * 0.055) From c2a9b9ddc039c04fe3070e20b21fb418e7897509 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 14:34:09 +0900 Subject: [PATCH 41/55] =?UTF-8?q?SingleBallPlacementStates::MOVE=5FTO=5FTA?= =?UTF-8?q?RGET=E3=81=A7=E3=81=AE=E8=A7=92=E9=80=9F=E5=BA=A6=E5=88=B6?= =?UTF-8?q?=E9=99=90=E3=81=8C=E5=BC=B7=E3=81=99=E3=81=8E=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 19396202..5c476501 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -299,7 +299,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba command.disableRuleAreaAvoidance(); command.setMaxVelocity(1.2); command.setMaxAcceleration(1.0); - command.setOmegaLimit(0.2); + command.setOmegaLimit(1.0); return Status::RUNNING; }); From 1333223e269b10bc9cfefc2eff7a349845210176 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 15:03:31 +0900 Subject: [PATCH 42/55] =?UTF-8?q?SingleBallPlacementStates::CONTACT=5FBALL?= =?UTF-8?q?=E3=81=AE=E7=B5=82=E4=BA=86=E6=9D=A1=E4=BB=B6=E3=81=AB=E3=83=9C?= =?UTF-8?q?=E3=83=BC=E3=83=AB=E3=82=BB=E3=83=B3=E3=82=B5=E3=82=92=E7=B5=84?= =?UTF-8?q?=E3=81=BF=E8=BE=BC=E3=81=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../src/single_ball_placement.cpp | 22 +++++++++++++++++-- 1 file changed, 20 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 5c476501..4c6229b4 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -274,8 +274,26 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba }); addTransition( - SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, - [this]() { return robot()->getDistance(world_model()->ball.pos) < 0.15; }); + SingleBallPlacementStates::CONTACT_BALL, SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); + static int count = 0; + if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { + if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { + if (++count > 10) { + count = 0; + return true; + } else { + return false; + } + } else { + count = 0; + return false; + } + } else { + // ボールセンサが動いていないとき + return robot()->getDistance(world_model()->ball.pos) < 0.15; + } + }); addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() { SvgTextBuilder text_builder; From e087fb10a56c5028818a603f75fed4def96e713d Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sat, 8 Mar 2025 20:05:10 +0900 Subject: [PATCH 43/55] =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/single_ball_placement.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 4c6229b4..46c78692 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -279,7 +279,7 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba static int count = 0; if (now.get_clock_type() == robot()->ball_sensor_stamp.get_clock_type()) { if (std::abs((now - robot()->ball_sensor_stamp).seconds()) < 0.01 && robot()->ball_sensor) { - if (++count > 10) { + if (++count > 2) { count = 0; return true; } else { From 85e81890e67aa1615097c283378582776c0c6cd0 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 01:15:18 +0900 Subject: [PATCH 44/55] =?UTF-8?q?GetBallContact=E3=82=B9=E3=82=AD=E3=83=AB?= =?UTF-8?q?=E3=82=92=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_robot_skills/get_ball_contact.hpp | 38 ---------- .../single_ball_placement.hpp | 3 - .../include/crane_robot_skills/skills.hpp | 1 - crane_robot_skills/src/get_ball_contact.cpp | 72 ------------------- .../src/single_ball_placement.cpp | 16 +---- crane_simple_ai/src/crane_commander.cpp | 1 - .../src/simple_ai_planner.cpp | 1 - 7 files changed, 3 insertions(+), 129 deletions(-) delete mode 100644 crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp delete mode 100644 crane_robot_skills/src/get_ball_contact.cpp diff --git a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp b/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp deleted file mode 100644 index a7b209dd..00000000 --- a/crane_robot_skills/include/crane_robot_skills/get_ball_contact.hpp +++ /dev/null @@ -1,38 +0,0 @@ -// 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__GET_BALL_CONTACT_HPP_ -#define CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ - -#include -#include -#include - -namespace crane::skills -{ -class GetBallContact : public SkillBase -{ -public: - explicit GetBallContact(RobotCommandWrapperBase::SharedPtr & base); - - Status update() override; - - void print(std::ostream & out) const override; - -private: - Vector2 getApproachNormVec(); - - std::optional last_contact_start_time; - - builtin_interfaces::msg::Time last_contact_time; - - Point & last_contact_point; - - // double target_distance = 0.0; -}; - -} // namespace crane::skills -#endif // CRANE_ROBOT_SKILLS__GET_BALL_CONTACT_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp index aa7ea90e..e3f83937 100644 --- a/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp +++ b/crane_robot_skills/include/crane_robot_skills/single_ball_placement.hpp @@ -11,7 +11,6 @@ #include #include -#include "get_ball_contact.hpp" #include "go_over_ball.hpp" #include "move_with_ball.hpp" #include "robot_command_as_skill.hpp" @@ -38,8 +37,6 @@ class SingleBallPlacement private: std::shared_ptr go_over_ball; - std::shared_ptr get_ball_contact; - std::shared_ptr move_with_ball; std::shared_ptr sleep = nullptr; diff --git a/crane_robot_skills/include/crane_robot_skills/skills.hpp b/crane_robot_skills/include/crane_robot_skills/skills.hpp index 50885d1a..26b0b5d5 100644 --- a/crane_robot_skills/include/crane_robot_skills/skills.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skills.hpp @@ -11,7 +11,6 @@ #include #include #include -#include #include #include #include diff --git a/crane_robot_skills/src/get_ball_contact.cpp b/crane_robot_skills/src/get_ball_contact.cpp deleted file mode 100644 index 5e554909..00000000 --- a/crane_robot_skills/src/get_ball_contact.cpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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 - -namespace crane::skills -{ -GetBallContact::GetBallContact(RobotCommandWrapperBase::SharedPtr & base) -: SkillBase("GetBallContact", base), - last_contact_point(getContextReference("last_contact_point")) -{ - setParameter("min_contact_duration", 0.5); - setParameter("dribble_power", 0.5); -} - -Status GetBallContact::update() -{ - if ( - robot()->ball_contact.getContactDuration() > - std::chrono::duration(getParameter("min_contact_duration"))) { - return Status::SUCCESS; - } else { - auto approach_vec = getApproachNormVec(); - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); - command.setDribblerTargetPosition(world_model()->ball.pos + approach_vec * 0.05); - command.dribble(getParameter("dribble_power")); - command.disableBallAvoidance(); - return Status::RUNNING; - } -} - -void GetBallContact::print(std::ostream & out) const -{ - out << "[GetBallContact] "; - auto contact_duration = std::chrono::duration_cast( - robot()->ball_contact.getContactDuration()) - .count(); - if (contact_duration > 0) { - out << "contacted: " << contact_duration << "ms"; - } else { - out << "ball distance: " << (robot()->pose.pos - world_model()->ball.pos).norm(); - } -} - -Vector2 GetBallContact::getApproachNormVec() -{ - // if robot is far away from ball, the approach angle is the angle to the ball from robot - // if robot is close to ball, the approach angle is robot angle - // and, the approach angle is interpolated between these two cases - constexpr double FAR_THRESHOLD = 3.5; - constexpr double NEAR_THRESHOLD = 0.5; - - Vector2 far_vec{(robot()->pose.pos - world_model()->ball.pos).normalized()}; - Vector2 near_vec{cos(robot()->pose.theta), sin(robot()->pose.theta)}; - - double distance = (robot()->pose.pos - world_model()->ball.pos).norm(); - - return [&]() { - if (distance > FAR_THRESHOLD) { - return far_vec; - } else if (distance < NEAR_THRESHOLD) { - return near_vec; - } else { - double ratio = (distance - NEAR_THRESHOLD) / (FAR_THRESHOLD - NEAR_THRESHOLD); - return (far_vec * ratio + near_vec * (1.0 - ratio)).normalized(); - } - }(); -} -} // namespace crane::skills diff --git a/crane_robot_skills/src/single_ball_placement.cpp b/crane_robot_skills/src/single_ball_placement.cpp index 46c78692..f4611233 100644 --- a/crane_robot_skills/src/single_ball_placement.cpp +++ b/crane_robot_skills/src/single_ball_placement.cpp @@ -156,13 +156,8 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba // 失敗の場合は最初に戻る addTransition( SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, - SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() { - if (not get_ball_contact) { - return false; - } else { - return skill_status == Status::FAILURE; - } - }); + SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, + [this]() { return skill_status == Status::FAILURE; }); // PULL_BACK_FROM_EDGE_PULL addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() { @@ -256,11 +251,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba .fill("white") .fontSize(100); visualizer->add(text_builder.getSvgString()); - // if (not get_ball_contact) { - // get_ball_contact = std::make_shared(command_base); - // } - // - // skill_status = get_ball_contact->run(); command.disablePlacementAvoidance(); command.disableBallAvoidance(); command.setMaxVelocity(0.2); @@ -410,7 +400,7 @@ void SingleBallPlacement::print(std::ostream & os) const go_over_ball->print(os); break; case CONTACT_BALL: - get_ball_contact->print(os); + os << " CONTACT_BALL"; break; case MOVE_TO_TARGET: move_with_ball->print(os); diff --git a/crane_simple_ai/src/crane_commander.cpp b/crane_simple_ai/src/crane_commander.cpp index 79b3d1d4..083ea25a 100644 --- a/crane_simple_ai/src/crane_commander.cpp +++ b/crane_simple_ai/src/crane_commander.cpp @@ -70,7 +70,6 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); - setUpSkillDictionary(); // setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); diff --git a/session/crane_planner_plugins/src/simple_ai_planner.cpp b/session/crane_planner_plugins/src/simple_ai_planner.cpp index ed97d73b..7bee0d6b 100644 --- a/session/crane_planner_plugins/src/simple_ai_planner.cpp +++ b/session/crane_planner_plugins/src/simple_ai_planner.cpp @@ -43,7 +43,6 @@ SimpleAIPlanner::SimpleAIPlanner(WorldModelWrapper::SharedPtr & world_model, rcl setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); - setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); setUpSkillDictionary(); From bd0e56bc03877bddce37913d428c519487cb95f1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 02:55:14 +0900 Subject: [PATCH 45/55] =?UTF-8?q?RVO2Planner=E3=81=AE=E4=B8=8D=E8=A6=81?= =?UTF-8?q?=E3=81=AA=E3=83=91=E3=83=A9=E3=83=A1=E3=83=BC=E3=82=BF=E3=82=92?= =?UTF-8?q?=E5=89=8A=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/rvo2_planner.hpp | 2 -- crane_local_planner/src/rvo2_planner.cpp | 4 ---- 2 files changed, 6 deletions(-) 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 ad29f550..6ef88454 100644 --- a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp @@ -53,9 +53,7 @@ class RVO2Planner : public LocalPlannerBase float RVO_RADIUS = 0.09f; float RVO_MAX_SPEED = 10.0f; - float RVO_TRAPEZOIDAL_MAX_ACC = 8.0; float RVO_TRAPEZOIDAL_FRAME_RATE = 60; - float RVO_TRAPEZOIDAL_MAX_SPEED = 4.0; double MAX_VEL = 4.0; double ACCELERATION = 4.0; diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index a408168a..4bd752df 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -31,12 +31,8 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node) RVO_RADIUS = node.get_parameter("rvo_radius").as_double(); node.declare_parameter("rvo_max_speed", RVO_MAX_SPEED); RVO_MAX_SPEED = node.get_parameter("rvo_max_speed").as_double(); - node.declare_parameter("rvo_trapezoidal_max_acc", RVO_TRAPEZOIDAL_MAX_ACC); - RVO_TRAPEZOIDAL_MAX_ACC = node.get_parameter("rvo_trapezoidal_max_acc").as_double(); node.declare_parameter("rvo_trapezoidal_frame_rate", RVO_TRAPEZOIDAL_FRAME_RATE); RVO_TRAPEZOIDAL_FRAME_RATE = node.get_parameter("rvo_trapezoidal_frame_rate").as_double(); - node.declare_parameter("rvo_trapezoidal_max_speed", RVO_TRAPEZOIDAL_MAX_SPEED); - RVO_TRAPEZOIDAL_MAX_SPEED = node.get_parameter("rvo_trapezoidal_max_speed").as_double(); node.declare_parameter("max_vel", MAX_VEL); MAX_VEL = node.get_parameter("max_vel").as_double(); From 9356030f674fcd9dd507ad8a59d1c97b6fc7762f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 03:23:40 +0900 Subject: [PATCH 46/55] =?UTF-8?q?RVO2Planner=E3=81=A7=E3=81=AE=E3=83=97?= =?UTF-8?q?=E3=83=AD=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB=E3=81=AE=E5=80=A4?= =?UTF-8?q?=E3=82=92=E5=87=BA=E5=8A=9B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_local_planner/rvo2_planner.hpp | 2 +- crane_local_planner/src/rvo2_planner.cpp | 7 +++++-- crane_msgs/msg/control/LocalPlannerConfig.msg | 3 +++ 3 files changed, 9 insertions(+), 3 deletions(-) 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 6ef88454..74702ed9 100644 --- a/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/rvo2_planner.hpp @@ -26,7 +26,7 @@ class RVO2Planner : public LocalPlannerBase public: explicit RVO2Planner(rclcpp::Node & node); - void reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg); + void reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg); crane_msgs::msg::RobotCommands extractRobotCommandsFromRVOSim( const crane_msgs::msg::RobotCommands & msg); diff --git a/crane_local_planner/src/rvo2_planner.cpp b/crane_local_planner/src/rvo2_planner.cpp index 4bd752df..ccbd78b7 100644 --- a/crane_local_planner/src/rvo2_planner.cpp +++ b/crane_local_planner/src/rvo2_planner.cpp @@ -55,7 +55,7 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node) [this](const crane_msgs::msg::RobotFeedbackArray & msg) { latest_feedback = msg; }); } -void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg) +void RVO2Planner::reflectWorldToRVOSim(crane_msgs::msg::RobotCommands & msg) { if ( world_model->getMsg().play_situation.command_raw.value == @@ -70,7 +70,7 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms } } // 味方ロボット：RVO内の位置・速度（＝進みたい方向）の更新 - for (const auto & command : msg.robot_commands) { + for (auto & command : msg.robot_commands) { rvo_sim->setAgentPosition( command.robot_id, RVO::Vector2(command.current_pose.x, command.current_pose.y)); rvo_sim->setAgentPrefVelocity(command.robot_id, RVO::Vector2(0.f, 0.f)); @@ -151,6 +151,9 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms max_vel = std::min(max_vel, 1.0); } + 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; if (target_vel.norm() < command.local_planner_config.terminal_velocity) { diff --git a/crane_msgs/msg/control/LocalPlannerConfig.msg b/crane_msgs/msg/control/LocalPlannerConfig.msg index 36f05bcf..536938fb 100644 --- a/crane_msgs/msg/control/LocalPlannerConfig.msg +++ b/crane_msgs/msg/control/LocalPlannerConfig.msg @@ -10,3 +10,6 @@ float32 terminal_velocity 0 uint8 priority 0 float32 theta_tolerance 0.0 + +float32 final_planned_max_acceleration -1.0 +float32 final_planned_max_velocity -1.0 From 488a877cb654ba191d155111ce30fbdb70257905 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 03:47:15 +0900 Subject: [PATCH 47/55] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E5=9B=9E?= =?UTF-8?q?=E3=82=8A=E8=BE=BC=E3=81=BF=E3=81=AE=E3=81=A8=E3=81=8D=E3=81=AB?= =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=AE=E6=96=B9=E5=90=91=E3=82=92?= =?UTF-8?q?=E5=90=91=E3=81=8F=E3=82=88=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_robot_skills/src/kick.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index 74b5b2d3..e8c17d6f 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -139,7 +139,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) visualizer->add(text_builder.getSvgString()); } command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) - .lookAtFrom(target, ball_pos) + .lookAt(ball_pos) .setTerminalVelocity(0.3); return Status::RUNNING; } else { @@ -168,13 +168,22 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) getAngle(target - robot()->pose.pos))) * ratio; Vector2 move_vec = getNormVec(move_direction); - command.lookAtFrom(target, ball_pos) + command.lookAt(ball_pos) .setDribblerTargetPosition( robot()->pose.pos + move_vec * 0.3 + world_model()->ball.vel * 0.4) // .setTerminalVelocity(world_model()->ball.vel.norm()) .disableCollisionAvoidance() .disableBallAvoidance(); + double current_target_angle = getAngle(ball_pos - robot()->pose.pos); + double final_target_angle = getAngle(target - ball_pos); + using boost::math::constants::degree; + // しきい値以下ならキック方向の精度を高めるために最終方向を向く + if ( + std::abs(getAngleDiff(current_target_angle, final_target_angle)) < 10. * degree()) { + command.setTargetTheta(final_target_angle); + } + if (getParameter("chip_kick")) { command.kickWithChip(getParameter("kick_power")); } else { From d5097e256c1e3ceb5a1c7f6509603bcf96e356d9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 04:08:59 +0900 Subject: [PATCH 48/55] =?UTF-8?q?robot=5Freceiver=5Fnode=E3=81=AE=E8=87=AA?= =?UTF-8?q?=E5=8B=95=E5=86=8D=E8=B5=B7=E5=8B=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/crane.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/crane_bringup/launch/crane.launch.py b/crane_bringup/launch/crane.launch.py index c460399a..bcc505bc 100644 --- a/crane_bringup/launch/crane.launch.py +++ b/crane_bringup/launch/crane.launch.py @@ -177,6 +177,7 @@ def generate_launch_description(): package="crane_robot_receiver", executable="robot_receiver_node", output="screen", + respawn=True, # on_exit=default_exit_behavior, ), Node( From c23ba8aa2764e1d5c29ee6065fe364e0347b41f9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 10:14:00 +0900 Subject: [PATCH 49/55] =?UTF-8?q?=E9=80=9F=E5=BA=A6=E5=90=91=E4=B8=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kick.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index e8c17d6f..a2a288aa 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -140,7 +140,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) } command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) .lookAt(ball_pos) - .setTerminalVelocity(0.3); + .setTerminalVelocity(1.0); return Status::RUNNING; } else { { From a2cc08e8fb3bb65089683a20851a3992328164db Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 10:14:28 +0900 Subject: [PATCH 50/55] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E6=96=B9?= =?UTF-8?q?=E5=90=91=E3=82=92=E8=A6=8B=E3=82=8B=E3=82=84=E3=81=A4=E3=82=92?= =?UTF-8?q?=E8=AA=BF=E6=95=B4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/kick.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp index a2a288aa..f2c84208 100644 --- a/crane_robot_skills/src/kick.cpp +++ b/crane_robot_skills/src/kick.cpp @@ -139,8 +139,8 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) visualizer->add(text_builder.getSvgString()); } command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3) - .lookAt(ball_pos) - .setTerminalVelocity(1.0); + .lookAtFrom(target, ball_pos) + .setTerminalVelocity(0.3); return Status::RUNNING; } else { { @@ -168,27 +168,28 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base) getAngle(target - robot()->pose.pos))) * ratio; Vector2 move_vec = getNormVec(move_direction); - command.lookAt(ball_pos) + command.lookAtFrom(target, ball_pos) .setDribblerTargetPosition( - robot()->pose.pos + move_vec * 0.3 + world_model()->ball.vel * 0.4) + robot()->pose.pos + move_vec * 0.4 + world_model()->ball.vel * 0.3) // .setTerminalVelocity(world_model()->ball.vel.norm()) .disableCollisionAvoidance() .disableBallAvoidance(); - double current_target_angle = getAngle(ball_pos - robot()->pose.pos); - double final_target_angle = getAngle(target - ball_pos); using boost::math::constants::degree; - // しきい値以下ならキック方向の精度を高めるために最終方向を向く - if ( - std::abs(getAngleDiff(current_target_angle, final_target_angle)) < 10. * degree()) { - command.setTargetTheta(final_target_angle); - } - if (getParameter("chip_kick")) { - command.kickWithChip(getParameter("kick_power")); + if ( + std::abs( + getAngleDiff(getAngle(target - ball_pos), getAngle(ball_pos - robot()->pose.pos))) < + 10. * degree()) { + if (getParameter("chip_kick")) { + command.kickWithChip(getParameter("kick_power")); + } else { + command.kickStraight(getParameter("kick_power")); + } } else { - command.kickStraight(getParameter("kick_power")); + command.kickStraight(0.0); } + if (getParameter("with_dribble")) { command.withDribble(getParameter("dribble_power")); } else { From f36751fead7c357cbaf403012ebbc547915d4f95 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 10:32:56 +0900 Subject: [PATCH 51/55] AttackerState::GOAL_FRONT_DANCE --- .../include/crane_robot_skills/attacker.hpp | 3 + crane_robot_skills/src/attacker.cpp | 349 +++++++++++------- 2 files changed, 217 insertions(+), 135 deletions(-) diff --git a/crane_robot_skills/include/crane_robot_skills/attacker.hpp b/crane_robot_skills/include/crane_robot_skills/attacker.hpp index 5dcf5313..479f0628 100644 --- a/crane_robot_skills/include/crane_robot_skills/attacker.hpp +++ b/crane_robot_skills/include/crane_robot_skills/attacker.hpp @@ -34,6 +34,7 @@ enum class AttackerState { THROUGH, KICK_TO_GOAL, STOP, + GOAL_FRONT_DANCE, }; class Attacker : public SkillBaseWithState { @@ -59,6 +60,8 @@ class Attacker : public SkillBaseWithState goal_front_dance_target = std::nullopt; }; } // namespace crane::skills #endif // CRANE_ROBOT_SKILLS__ATTACKER_HPP_ diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index e863fb91..81d29018 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -4,6 +4,7 @@ // license that can be found in the LICENSE file or at // https://opensource.org/licenses/MIT. +#include #include namespace crane::skills @@ -210,161 +211,239 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return kick_skill.run(); }); - addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { - if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isMoving(1.0)) { - return false; - } - - auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); - const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - double best_score = 0.0; - Point best_target; - for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { - if (score_with_id.id != robot()->id) { - continue; - } else if (score_with_id.id != world_model()->getOurGoalieId()) { - continue; - } else { - kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; - pass_receiver_id = score_with_id.id; - return true; - } - } - return false; + addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_FRONT_DANCE, [this]() -> bool { + goal_front_dance_target = std::nullopt; + return world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); }); - addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { - // ボールが早い - if (world_model()->ball.isMoving(1.0)) { - pass_receiver_id = std::nullopt; - return true; - } - return false; + addTransition(AttackerState::GOAL_FRONT_DANCE, AttackerState::ENTRY_POINT, [this]() -> bool { + return not world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); }); - addStateFunction(AttackerState::STANDARD_PASS, [this]() -> Status { - if (pass_receiver_id) { - kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; + addStateFunction(AttackerState::GOAL_FRONT_DANCE, [this]() -> Status { + auto ball = world_model()->ball.pos; + auto near_enemy_robots = world_model()->theirs.getAvailableRobots(); + near_enemy_robots = + near_enemy_robots | + ranges::views::filter([&](const auto & r) { return r->getDistance(ball) < 1.5; }) | + ranges::to(); + auto points = getPoints(world_model()->ball.pos, 0.1, 5); + auto points_with_score = + points | ranges::views::transform([&](const auto & p) { + if ((p - ball).norm() < 0.2) { + return std::make_pair(p, 0.0); + } else { + // ゴールへの見通し + + // 現在位置からの見通し + Segment segment{ball + (p - ball).normalized() * 0.15, p}; + if (auto dist = world_model() + ->getNearestRobotWithDistanceFromSegment(segment, near_enemy_robots) + ->distance; + dist < 0.10) { + return std::make_pair(p, 0.0); + } else { + double score = world_model()->getLargestGoalAngleRangeFromPoint(p).second * dist * + (1. / (world_model()->getTheirGoalCenter() - p).norm()); + return std::make_pair(p, score); + } + } + }) | + ranges::to(); + + // スコアが一番高い点を選ぶ + if (auto best_point = ranges::max_element( + points_with_score, [](const auto & a, const auto & b) { return a.second < b.second; }); + not goal_front_dance_target.has_value() && best_point != points_with_score.end()) { + goal_front_dance_target = best_point->first; + ranges::for_each(points_with_score, [&](const auto & p) { + SvgCircleBuilder circle; + circle.center(p.first).radius(p.second).stroke("red"); + visualizer->add(circle.getSvgString()); + }); + SvgCircleBuilder best_circle; + best_circle.center(best_point->first).radius(best_point->second + 0.1).stroke("black"); + visualizer->add(best_circle.getSvgString()); } - auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); - const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - - SvgLineBuilder line_builder; - line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(10); - visualizer->add(line_builder.getSvgString()); - - kick_skill.setParameter("target", kick_target); - Segment ball_to_target{world_model()->ball.pos, kick_target}; - if (auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( - ball_to_target, world_model()->theirs.getAvailableRobots()); - nearest_enemy.has_value()) { - if (nearest_enemy->robot->getDistance(world_model()->ball.pos) < 2.0) { - kick_skill.setParameter("chip_kick", true); + const bool ball_touch = [&]() { + auto now = rclcpp::Clock(RCL_ROS_TIME).now(); + // ドリブルしていなかったらまずドリブラでボールにタッチ、その後best_pointへ + if (robot()->getBallSensorAvailable(now, rclcpp::Duration::from_seconds(0.01))) { + return robot()->ball_sensor; + } else { + return robot()->getDistance(ball) < 0.07; } - } - kick_skill.setParameter("kick_power", 0.4); - kick_skill.setParameter("dot_threshold", 0.97); - return kick_skill.run(); - }); - - addTransition(AttackerState::ENTRY_POINT, AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> bool { - // ボールが近く、相手コートにいるとき（本当にチャンスが無いとき(隙間が1deg以下)は除外） - double x_diff_with_their_goal = - std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); - auto [best_angle, goal_angle_width] = - world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); - return robot()->getDistance(world_model()->ball.pos) < 1.0 && - x_diff_with_their_goal < world_model()->field_size.x() * 0.5 && - goal_angle_width * 180.0 / M_PI > 1. && not world_model()->ball.isMoving(1.0); - }); - - addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { - return world_model()->ball.isMoving(1.0); - }); - - addStateFunction( - AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> Status { return goal_kick_skill.run(); }); + }(); - addTransition( - AttackerState::ENTRY_POINT, AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> bool { - // ボールに近く、自コートにいるとき - double x_diff_with_their_goal = - std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); - return robot()->getDistance(world_model()->ball.pos) < 1.0 && - x_diff_with_their_goal >= world_model()->field_size.x() * 0.5 && - not world_model()->ball.isMoving(1.0); - }); + kick_skill.setParameter("target", best_point.first); + kick_skill.setParameter("kick_power", 0.); + kick_skill.setParameter("with_dribble", true); + kick_skill.setParameter("dribble_power", 0.3); + kick_skill.run(); + return Status::RUNNING; +}); - addTransition( - AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, - [this]() -> bool { return world_model()->ball.isMoving(1.0); }); +addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_FRONT_DANCE, [this]() -> bool { + return world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); +}); - addStateFunction(AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.8); - kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("chip_kick", true); - command.disableBallAvoidance(); - return kick_skill.run(); - }); +addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { + if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isMoving(1.0)) { + return false; + } - addTransition(AttackerState::ENTRY_POINT, AttackerState::RECEIVE_BALL, [this]() -> bool { - if ( - world_model()->ball.vel.norm() < 1.0 or - world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { - // ボールが止まっているとき/ボールが自分から離れていっているときはは受け取らない - return false; + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); + const auto enemy_robots = world_model()->theirs.getAvailableRobots(); + double best_score = 0.0; + Point best_target; + for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { + if (score_with_id.id != robot()->id) { + continue; + } else if (score_with_id.id != world_model()->getOurGoalieId()) { + continue; } else { + kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; + pass_receiver_id = score_with_id.id; return true; } - }); - - addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { - // ボールが止まっている - if (world_model()->ball.vel.norm() < 1.0) { - return true; - } else if (world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { - // ボールが自分から離れていっている（多分受取に失敗した） - return true; - } else { - return false; - } - }); + } + return false; +}); - addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { - receive_skill.setParameter("enable_redirect", false); - receive_skill.setParameter("policy", std::string("min_slack")); - receive_skill.setParameter("dribble_power", 0.0); - receive_skill.setParameter("enable_software_bumper", false); - return receive_skill.run(); - }); +addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { + // ボールが早い + if (world_model()->ball.isMoving(1.0)) { + pass_receiver_id = std::nullopt; + return true; + } + return false; +}); - addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { - // 一定以上ボールに触れたら終了 - using std::chrono_literals::operator""s; - return robot()->ball_contact.getContactDuration() > 0.2s; - }); +addStateFunction(AttackerState::STANDARD_PASS, [this]() -> Status { + if (pass_receiver_id) { + kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; + } - addStateFunction(AttackerState::KICK_TO_GOAL, [this]() -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.9); - // kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("chip_kick", false); - return kick_skill.run(); - }); + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); + const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - addTransition(AttackerState::ENTRY_POINT, AttackerState::KICK_TO_GOAL, [this]() -> bool { - // どこにも当てはまらないときはゴールに向かってシュート - static int count = 0; - // 10フレームに1回ENTRY_POINTに戻して様子を見る - if (count++ > 10) { - count = 0; - return true && world_model()->ball.isMoving(1.0); - } else { - return false; + SvgLineBuilder line_builder; + line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(10); + visualizer->add(line_builder.getSvgString()); + + kick_skill.setParameter("target", kick_target); + Segment ball_to_target{world_model()->ball.pos, kick_target}; + if (auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( + ball_to_target, world_model()->theirs.getAvailableRobots()); + nearest_enemy.has_value()) { + if (nearest_enemy->robot->getDistance(world_model()->ball.pos) < 2.0) { + kick_skill.setParameter("chip_kick", true); } + } + kick_skill.setParameter("kick_power", 0.4); + kick_skill.setParameter("dot_threshold", 0.97); + return kick_skill.run(); +}); + +addTransition(AttackerState::ENTRY_POINT, AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> bool { + // ボールが近く、相手コートにいるとき（本当にチャンスが無いとき(隙間が1deg以下)は除外） + double x_diff_with_their_goal = + std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); + auto [best_angle, goal_angle_width] = + world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); + return robot()->getDistance(world_model()->ball.pos) < 1.0 && + x_diff_with_their_goal < world_model()->field_size.x() * 0.5 && + goal_angle_width * 180.0 / M_PI > 1. && not world_model()->ball.isMoving(1.0); +}); + +addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { + return world_model()->ball.isMoving(1.0); +}); + +addStateFunction(AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> Status { + return goal_kick_skill.run(); +}); + +addTransition( + AttackerState::ENTRY_POINT, AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> bool { + // ボールに近く、自コートにいるとき + double x_diff_with_their_goal = + std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); + return robot()->getDistance(world_model()->ball.pos) < 1.0 && + x_diff_with_their_goal >= world_model()->field_size.x() * 0.5 && + not world_model()->ball.isMoving(1.0); }); + +addTransition( + AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, + [this]() -> bool { return world_model()->ball.isMoving(1.0); }); + +addStateFunction(AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.8); + kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("chip_kick", true); + command.disableBallAvoidance(); + return kick_skill.run(); +}); + +addTransition(AttackerState::ENTRY_POINT, AttackerState::RECEIVE_BALL, [this]() -> bool { + if ( + world_model()->ball.vel.norm() < 1.0 or + world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { + // ボールが止まっているとき/ボールが自分から離れていっているときはは受け取らない + return false; + } else { + return true; + } +}); + +addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { + // ボールが止まっている + if (world_model()->ball.vel.norm() < 1.0) { + return true; + } else if (world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { + // ボールが自分から離れていっている（多分受取に失敗した） + return true; + } else { + return false; + } +}); + +addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { + receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("policy", std::string("min_slack")); + receive_skill.setParameter("dribble_power", 0.0); + receive_skill.setParameter("enable_software_bumper", false); + return receive_skill.run(); +}); + +addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { + // 一定以上ボールに触れたら終了 + using std::chrono_literals::operator""s; + return robot()->ball_contact.getContactDuration() > 0.2s; +}); + +addStateFunction(AttackerState::KICK_TO_GOAL, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.9); + // kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("chip_kick", false); + return kick_skill.run(); +}); + +addTransition(AttackerState::ENTRY_POINT, AttackerState::KICK_TO_GOAL, [this]() -> bool { + // どこにも当てはまらないときはゴールに向かってシュート + static int count = 0; + // 10フレームに1回ENTRY_POINTに戻して様子を見る + if (count++ > 10) { + count = 0; + return true && world_model()->ball.isMoving(1.0); + } else { + return false; + } +}); } std::shared_ptr Attacker::selectPassReceiver() From a316f161ee4fd7c1cbc72041412cd6410781da0f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 14:16:46 +0900 Subject: [PATCH 52/55] =?UTF-8?q?StealBall=E3=82=92=E3=82=B3=E3=83=A1?= =?UTF-8?q?=E3=83=B3=E3=83=88=E3=82=A2=E3=82=A6=E3=83=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 81d29018..34bddbea 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -104,12 +104,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return Status::RUNNING; }); - addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { - // 止まっているボールを相手が持っているとき - auto nearest = world_model()->getNearestRobotWithDistanceFromPoint( - world_model()->ball.pos, world_model()->theirs.getAvailableRobots()); - return nearest.has_value() && world_model()->ball.isStopped(0.1) && nearest->distance < 0.5; - }); + // addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool { + // // 止まっているボールを相手が持っているとき + // auto nearest = world_model()->getNearestRobotWithDistanceFromPoint( + // world_model()->ball.pos, world_model()->theirs.getAvailableRobots()); + // return nearest.has_value() && world_model()->ball.isStopped(0.1) && nearest->distance < 0.5; + // }); addTransition(AttackerState::STEAL_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { return world_model()->ball.isMoving(1.0); From e5a1917a712dcfbd776cadb061fa8ce8be41faa5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 14:20:17 +0900 Subject: [PATCH 53/55] =?UTF-8?q?STOP=E6=99=82=E3=81=AE=E8=B7=9D=E9=9B=A2?= =?UTF-8?q?=E3=82=92=E9=81=A0=E3=81=8F=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/ball_nearby_positioner.cpp | 2 +- session/crane_planner_plugins/src/skill_planners.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/ball_nearby_positioner.cpp b/crane_robot_skills/src/ball_nearby_positioner.cpp index b6688818..269ef31a 100644 --- a/crane_robot_skills/src/ball_nearby_positioner.cpp +++ b/crane_robot_skills/src/ball_nearby_positioner.cpp @@ -20,7 +20,7 @@ BallNearByPositioner::BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & setParameter("positioning_policy", std::string("goal")); // 整列距離 setParameter("robot_interval", 0.3); - setParameter("margin_distance", 0.6); + setParameter("margin_distance", 0.8); } Status BallNearByPositioner::update() diff --git a/session/crane_planner_plugins/src/skill_planners.cpp b/session/crane_planner_plugins/src/skill_planners.cpp index d1e69f55..a39938e7 100644 --- a/session/crane_planner_plugins/src/skill_planners.cpp +++ b/session/crane_planner_plugins/src/skill_planners.cpp @@ -274,7 +274,7 @@ auto BallNearByPositionerSkillPlanner::getSelectedRobots( skills.back()->setParameter("line_policy", std::string("arc")); skills.back()->setParameter("positioning_policy", std::string("goal")); skills.back()->setParameter("robot_interval", 0.35); - skills.back()->setParameter("margin_distance", 0.6); + skills.back()->setParameter("margin_distance", 0.8); } return selected; From 0aac6a68adc80ab34f74bd957f3beaad0896f6ba Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 9 Mar 2025 14:20:37 +0900 Subject: [PATCH 54/55] =?UTF-8?q?AttackerState::GOAL=5FFRONT=5FDANCE?= =?UTF-8?q?=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/src/attacker.cpp | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 34bddbea..250fd0c6 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -217,9 +217,18 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addTransition(AttackerState::GOAL_FRONT_DANCE, AttackerState::ENTRY_POINT, [this]() -> bool { + kick_skill.setParameter("with_dribble", false); return not world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); }); + addTransition(AttackerState::GOAL_FRONT_DANCE, AttackerState::GOAL_KICK, [this]() -> bool { + kick_skill.setParameter("with_dribble", false); + auto [best_angle, goal_angle_width] = + world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); + return robot()->getDistance(world_model()->ball.pos) < 2.0 && + goal_angle_width * 180.0 / M_PI > 5.; + }); + addStateFunction(AttackerState::GOAL_FRONT_DANCE, [this]() -> Status { auto ball = world_model()->ball.pos; auto near_enemy_robots = world_model()->theirs.getAvailableRobots(); @@ -243,7 +252,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) dist < 0.10) { return std::make_pair(p, 0.0); } else { - double score = world_model()->getLargestGoalAngleRangeFromPoint(p).second * dist * + auto [best_angle, goal_angle_width] = world_model()->getLargestGoalAngleRangeFromPoint(p); + double score = goal_angle_width * dist * (1. / (world_model()->getTheirGoalCenter() - p).norm()); return std::make_pair(p, score); } @@ -276,7 +286,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) } }(); - kick_skill.setParameter("target", best_point.first); + if (goal_front_dance_target) { + kick_skill.setParameter("target", goal_front_dance_target.value()); + } kick_skill.setParameter("kick_power", 0.); kick_skill.setParameter("with_dribble", true); kick_skill.setParameter("dribble_power", 0.3); From 9ee92555eb1b4e46806e4966ddca9580dd9538f8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 11 Mar 2025 11:35:37 +0000 Subject: [PATCH 55/55] style(pre-commit): autofix --- crane_robot_skills/src/attacker.cpp | 300 ++++++++++++++-------------- 1 file changed, 150 insertions(+), 150 deletions(-) diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 250fd0c6..f29ff72a 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -224,7 +224,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addTransition(AttackerState::GOAL_FRONT_DANCE, AttackerState::GOAL_KICK, [this]() -> bool { kick_skill.setParameter("with_dribble", false); auto [best_angle, goal_angle_width] = - world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); + world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); return robot()->getDistance(world_model()->ball.pos) < 2.0 && goal_angle_width * 180.0 / M_PI > 5.; }); @@ -252,9 +252,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) dist < 0.10) { return std::make_pair(p, 0.0); } else { - auto [best_angle, goal_angle_width] = world_model()->getLargestGoalAngleRangeFromPoint(p); - double score = goal_angle_width * dist * - (1. / (world_model()->getTheirGoalCenter() - p).norm()); + auto [best_angle, goal_angle_width] = + world_model()->getLargestGoalAngleRangeFromPoint(p); + double score = + goal_angle_width * dist * (1. / (world_model()->getTheirGoalCenter() - p).norm()); return std::make_pair(p, score); } } @@ -294,168 +295,167 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) kick_skill.setParameter("dribble_power", 0.3); kick_skill.run(); return Status::RUNNING; -}); + }); -addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_FRONT_DANCE, [this]() -> bool { - return world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); -}); + addTransition(AttackerState::ENTRY_POINT, AttackerState::GOAL_FRONT_DANCE, [this]() -> bool { + return world_model()->point_checker.isEnemyPenaltyArea(world_model()->ball.pos, 1.0); + }); -addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { - if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isMoving(1.0)) { + addTransition(AttackerState::ENTRY_POINT, AttackerState::STANDARD_PASS, [this]() -> bool { + if (robot()->getDistance(world_model()->ball.pos) > 1.0 or world_model()->ball.isMoving(1.0)) { + return false; + } + + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); + const auto enemy_robots = world_model()->theirs.getAvailableRobots(); + double best_score = 0.0; + Point best_target; + for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { + if (score_with_id.id != robot()->id) { + continue; + } else if (score_with_id.id != world_model()->getOurGoalieId()) { + continue; + } else { + kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; + pass_receiver_id = score_with_id.id; + return true; + } + } return false; - } + }); - auto our_robots = world_model()->ours.getAvailableRobots(robot()->id, true); - const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - double best_score = 0.0; - Point best_target; - for (const auto score_with_id : world_model()->getMsg().game_analysis.pass_scores) { - if (score_with_id.id != robot()->id) { - continue; - } else if (score_with_id.id != world_model()->getOurGoalieId()) { - continue; - } else { - kick_target = world_model()->getOurRobot(score_with_id.id)->pose.pos; - pass_receiver_id = score_with_id.id; + addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { + // ボールが早い + if (world_model()->ball.isMoving(1.0)) { + pass_receiver_id = std::nullopt; return true; } - } - return false; -}); + return false; + }); -addTransition(AttackerState::STANDARD_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { - // ボールが早い - if (world_model()->ball.isMoving(1.0)) { - pass_receiver_id = std::nullopt; - return true; - } - return false; -}); + addStateFunction(AttackerState::STANDARD_PASS, [this]() -> Status { + if (pass_receiver_id) { + kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; + } -addStateFunction(AttackerState::STANDARD_PASS, [this]() -> Status { - if (pass_receiver_id) { - kick_target = world_model()->getOurRobot(pass_receiver_id.value())->pose.pos; - } + auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); + const auto enemy_robots = world_model()->theirs.getAvailableRobots(); - auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); - const auto enemy_robots = world_model()->theirs.getAvailableRobots(); + SvgLineBuilder line_builder; + line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(10); + visualizer->add(line_builder.getSvgString()); - SvgLineBuilder line_builder; - line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(10); - visualizer->add(line_builder.getSvgString()); - - kick_skill.setParameter("target", kick_target); - Segment ball_to_target{world_model()->ball.pos, kick_target}; - if (auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( - ball_to_target, world_model()->theirs.getAvailableRobots()); - nearest_enemy.has_value()) { - if (nearest_enemy->robot->getDistance(world_model()->ball.pos) < 2.0) { - kick_skill.setParameter("chip_kick", true); + kick_skill.setParameter("target", kick_target); + Segment ball_to_target{world_model()->ball.pos, kick_target}; + if (auto nearest_enemy = world_model()->getNearestRobotWithDistanceFromSegment( + ball_to_target, world_model()->theirs.getAvailableRobots()); + nearest_enemy.has_value()) { + if (nearest_enemy->robot->getDistance(world_model()->ball.pos) < 2.0) { + kick_skill.setParameter("chip_kick", true); + } } - } - kick_skill.setParameter("kick_power", 0.4); - kick_skill.setParameter("dot_threshold", 0.97); - return kick_skill.run(); -}); - -addTransition(AttackerState::ENTRY_POINT, AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> bool { - // ボールが近く、相手コートにいるとき（本当にチャンスが無いとき(隙間が1deg以下)は除外） - double x_diff_with_their_goal = - std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); - auto [best_angle, goal_angle_width] = - world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); - return robot()->getDistance(world_model()->ball.pos) < 1.0 && - x_diff_with_their_goal < world_model()->field_size.x() * 0.5 && - goal_angle_width * 180.0 / M_PI > 1. && not world_model()->ball.isMoving(1.0); -}); - -addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { - return world_model()->ball.isMoving(1.0); -}); - -addStateFunction(AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> Status { - return goal_kick_skill.run(); -}); - -addTransition( - AttackerState::ENTRY_POINT, AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> bool { - // ボールに近く、自コートにいるとき + kick_skill.setParameter("kick_power", 0.4); + kick_skill.setParameter("dot_threshold", 0.97); + return kick_skill.run(); + }); + + addTransition(AttackerState::ENTRY_POINT, AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> bool { + // ボールが近く、相手コートにいるとき（本当にチャンスが無いとき(隙間が1deg以下)は除外） double x_diff_with_their_goal = std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); + auto [best_angle, goal_angle_width] = + world_model()->getLargestGoalAngleRangeFromPoint(world_model()->ball.pos); return robot()->getDistance(world_model()->ball.pos) < 1.0 && - x_diff_with_their_goal >= world_model()->field_size.x() * 0.5 && - not world_model()->ball.isMoving(1.0); + x_diff_with_their_goal < world_model()->field_size.x() * 0.5 && + goal_angle_width * 180.0 / M_PI > 1. && not world_model()->ball.isMoving(1.0); }); -addTransition( - AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, - [this]() -> bool { return world_model()->ball.isMoving(1.0); }); - -addStateFunction(AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.8); - kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("chip_kick", true); - command.disableBallAvoidance(); - return kick_skill.run(); -}); - -addTransition(AttackerState::ENTRY_POINT, AttackerState::RECEIVE_BALL, [this]() -> bool { - if ( - world_model()->ball.vel.norm() < 1.0 or - world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { - // ボールが止まっているとき/ボールが自分から離れていっているときはは受け取らない - return false; - } else { - return true; - } -}); - -addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { - // ボールが止まっている - if (world_model()->ball.vel.norm() < 1.0) { - return true; - } else if (world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { - // ボールが自分から離れていっている（多分受取に失敗した） - return true; - } else { - return false; - } -}); - -addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { - receive_skill.setParameter("enable_redirect", false); - receive_skill.setParameter("policy", std::string("min_slack")); - receive_skill.setParameter("dribble_power", 0.0); - receive_skill.setParameter("enable_software_bumper", false); - return receive_skill.run(); -}); - -addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { - // 一定以上ボールに触れたら終了 - using std::chrono_literals::operator""s; - return robot()->ball_contact.getContactDuration() > 0.2s; -}); - -addStateFunction(AttackerState::KICK_TO_GOAL, [this]() -> Status { - kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); - kick_skill.setParameter("kick_power", 0.9); - // kick_skill.setParameter("dot_threshold", 0.95); - kick_skill.setParameter("chip_kick", false); - return kick_skill.run(); -}); - -addTransition(AttackerState::ENTRY_POINT, AttackerState::KICK_TO_GOAL, [this]() -> bool { - // どこにも当てはまらないときはゴールに向かってシュート - static int count = 0; - // 10フレームに1回ENTRY_POINTに戻して様子を見る - if (count++ > 10) { - count = 0; - return true && world_model()->ball.isMoving(1.0); - } else { - return false; - } -}); + addTransition(AttackerState::LOW_CHANCE_GOAL_KICK, AttackerState::ENTRY_POINT, [this]() -> bool { + return world_model()->ball.isMoving(1.0); + }); + + addStateFunction( + AttackerState::LOW_CHANCE_GOAL_KICK, [this]() -> Status { return goal_kick_skill.run(); }); + + addTransition( + AttackerState::ENTRY_POINT, AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> bool { + // ボールに近く、自コートにいるとき + double x_diff_with_their_goal = + std::abs(world_model()->getTheirGoalCenter().x() - world_model()->ball.pos.x()); + return robot()->getDistance(world_model()->ball.pos) < 1.0 && + x_diff_with_their_goal >= world_model()->field_size.x() * 0.5 && + not world_model()->ball.isMoving(1.0); + }); + + addTransition( + AttackerState::MOVE_BALL_TO_OPPONENT_HALF, AttackerState::ENTRY_POINT, + [this]() -> bool { return world_model()->ball.isMoving(1.0); }); + + addStateFunction(AttackerState::MOVE_BALL_TO_OPPONENT_HALF, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.8); + kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("chip_kick", true); + command.disableBallAvoidance(); + return kick_skill.run(); + }); + + addTransition(AttackerState::ENTRY_POINT, AttackerState::RECEIVE_BALL, [this]() -> bool { + if ( + world_model()->ball.vel.norm() < 1.0 or + world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { + // ボールが止まっているとき/ボールが自分から離れていっているときはは受け取らない + return false; + } else { + return true; + } + }); + + addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { + // ボールが止まっている + if (world_model()->ball.vel.norm() < 1.0) { + return true; + } else if (world_model()->ball.isMovingAwayFrom(robot()->pose.pos)) { + // ボールが自分から離れていっている（多分受取に失敗した） + return true; + } else { + return false; + } + }); + + addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status { + receive_skill.setParameter("enable_redirect", false); + receive_skill.setParameter("policy", std::string("min_slack")); + receive_skill.setParameter("dribble_power", 0.0); + receive_skill.setParameter("enable_software_bumper", false); + return receive_skill.run(); + }); + + addTransition(AttackerState::RECEIVE_BALL, AttackerState::ENTRY_POINT, [this]() -> bool { + // 一定以上ボールに触れたら終了 + using std::chrono_literals::operator""s; + return robot()->ball_contact.getContactDuration() > 0.2s; + }); + + addStateFunction(AttackerState::KICK_TO_GOAL, [this]() -> Status { + kick_skill.setParameter("target", world_model()->getTheirGoalCenter()); + kick_skill.setParameter("kick_power", 0.9); + // kick_skill.setParameter("dot_threshold", 0.95); + kick_skill.setParameter("chip_kick", false); + return kick_skill.run(); + }); + + addTransition(AttackerState::ENTRY_POINT, AttackerState::KICK_TO_GOAL, [this]() -> bool { + // どこにも当てはまらないときはゴールに向かってシュート + static int count = 0; + // 10フレームに1回ENTRY_POINTに戻して様子を見る + if (count++ > 10) { + count = 0; + return true && world_model()->ball.isMoving(1.0); + } else { + return false; + } + }); } std::shared_ptr Attacker::selectPassReceiver()