From 3f1c6736fd4d373cf8ebbe9fe1fa62f21f066ad5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 17 Dec 2023 23:40:01 +0900 Subject: [PATCH 01/14] =?UTF-8?q?=E3=82=B4=E3=83=BC=E3=83=AB=E3=82=AD?= =?UTF-8?q?=E3=83=BC=E3=83=91=E3=83=BC=E3=81=8C=E3=83=9A=E3=83=8A=E3=83=AB?= =?UTF-8?q?=E3=83=86=E3=82=A3=E3=82=A8=E3=83=AA=E3=82=A2=E3=81=8B=E3=82=89?= =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=82=92=E6=8E=92=E5=87=BA=E3=81=99?= =?UTF-8?q?=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- session/crane_planner_plugins/CMakeLists.txt | 2 +- .../crane_planner_plugins/goalie_planner.hpp | 100 +++++++++++++----- 2 files changed, 75 insertions(+), 27 deletions(-) diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index b25c695bf..83d389116 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -7,7 +7,7 @@ if (NOT CMAKE_CXX_STANDARD) endif () if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wall -Wextra -Wpedantic -g) endif () add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS") diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 6cfa2253a..785e1e6b2 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -37,44 +37,92 @@ class GoaliePlanner : public PlannerBase for (auto robot_id : robots) { crane::RobotCommandWrapper target(robot_id.robot_id, world_model); auto robot = world_model->getRobot(robot_id); - auto ball = world_model->ball.pos; - auto goals = world_model->getOurGoalPosts(); + // シュートチェック Segment goal_line(goals.first, goals.second); - - Point goal_center; - goal_center << goals.first.x(), 0.0f; - Segment ball_line(ball, ball + world_model->ball.vel * 20000000.f); + Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f); std::vector intersections; + bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections); + + std::cout << "goalie setup" << std::endl; - // check shoot - bg::intersection(ball_line, goal_line, intersections); - Point target_point; - float target_theta; - if (not intersections.empty()) { - // std::cout << "Shoot block mode" << std::endl; + if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) { + // シュートブロック + std::cout << "shoot block mode" << std::endl; ClosestPoint result; bg::closest_point(ball_line, robot->pose.pos, result); - target_point << result.closest_point.x(), result.closest_point.y(); - // position control - target_theta = getAngle(-world_model->ball.vel); + target.setTargetPosition(result.closest_point); + target.setTargetTheta(getAngle(-world_model->ball.vel)); } else { - // go blocking point - // std::cout << "Normal blocking mode" << std::endl; - const double BLOCK_DIST = 0.15; + if (world_model->ball.isStopped() && world_model->isDefenseArea(ball)) { + std::cout << "ball discharge mode" << std::endl; + // パスできるロボットのリストアップ + auto passable_robot_list = world_model->ours.getAvailableRobots(); + passable_robot_list.erase( + std::remove_if( + passable_robot_list.begin(), passable_robot_list.end(), + [&](const RobotInfo::SharedPtr & r) { + // 自分以外 + if (robot->id != r->id) { + return true; + } + // 敵に塞がれていたら除外 + Segment ball_to_robot_line(ball, r->pose.pos); + for (const auto & enemy : world_model->theirs.getAvailableRobots()) { + auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); + if (dist < 0.2) { + return true; + } + } + return false; + }), + passable_robot_list.end()); - // 範囲外のときは正面に構える - if (not world_model->isFieldInside(world_model->ball.pos)) { - ball << 0, 0; - } - target_point = goal_center + (ball - goal_center).normalized() * BLOCK_DIST; - target_theta = getAngle(ball - target_point); - } + Point pass_target = [&]() { + if (not passable_robot_list.empty()) { + std::cout << "pass target is " << passable_robot_list.front()->pose.pos << std::endl; + return passable_robot_list.front()->pose.pos; + } else { + std::cout << "no pass target" << std::endl; + return Point{0, 0}; + } + }(); - target.setTargetPosition(target_point, target_theta); + std::cout << "list up passable robots" << std::endl; + Point intermediate_point = ball + (pass_target - ball).normalized() * 0.2f; + double angle_ball_to_target = getAngle(ball - pass_target); + double dot = (world_model->ball.pos - robot->pose.pos) + .normalized() + .dot((world_model->ball.pos - pass_target).normalized()); + // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 + if ( + dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, robot->pose.theta)) > 0.05) { + std::cout << "go to intermediate point" << std::endl; + target.setTargetPosition(intermediate_point); + target.enableCollisionAvoidance(); + } else { + std::cout << "go to pass target" << std::endl; + target.setTargetPosition(world_model->ball.pos); + target.kickStraight(0.7).disableCollisionAvoidance(); + target.enableCollisionAvoidance(); + } + target.setTargetTheta(getAngle(ball - pass_target)); + } else { + std::cout << "defense mode" << std::endl; + const double BLOCK_DIST = 0.15; + // 範囲外のときは正面に構える + if (not world_model->isFieldInside(world_model->ball.pos)) { + ball << 0, 0; + } + Point goal_center = world_model->getOurGoalCenter(); + goal_center << goals.first.x(), 0.0f; + target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); + target.setTargetTheta(getAngle(ball - robot->pose.pos)); + } + } control_targets.emplace_back(target.getMsg()); } return control_targets; From 7906decf00a91562a0c8deb4f92a0912dc63ef61 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 20 Dec 2023 23:02:30 +0900 Subject: [PATCH 02/14] =?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=88=A4=E5=AE=9A=E3=81=AE?= =?UTF-8?q?=E6=95=B5=E5=91=B3=E6=96=B9=E3=81=8C=E3=81=B2=E3=81=A3=E3=81=8F?= =?UTF-8?q?=E3=82=8A=E8=BF=94=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 --- .../include/crane_planner_plugins/goalie_planner.hpp | 2 +- .../include/crane_msg_wrappers/world_model_wrapper.hpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 785e1e6b2..9416e0626 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -56,7 +56,7 @@ class GoaliePlanner : public PlannerBase target.setTargetPosition(result.closest_point); target.setTargetTheta(getAngle(-world_model->ball.vel)); } else { - if (world_model->ball.isStopped() && world_model->isDefenseArea(ball)) { + if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball)) { std::cout << "ball discharge mode" << std::endl; // パスできるロボットのリストアップ auto passable_robot_list = world_model->ours.getAvailableRobots(); diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index bd9f3796a..5f43677d5 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -355,9 +355,9 @@ struct WorldModelWrapper return {nearest_robot, std::sqrt(min_sq_distance)}; } - bool isEnemyDefenseArea(const Point & p) const { return isInRect(ours.defense_area, p); } + bool isEnemyDefenseArea(const Point & p) const { return isInRect(theirs.defense_area, p); } - bool isFriendDefenseArea(const Point & p) const { return isInRect(theirs.defense_area, p); } + bool isFriendDefenseArea(const Point & p) const { return isInRect(ours.defense_area, p); } bool isDefenseArea(Point p) const { return isFriendDefenseArea(p) || isEnemyDefenseArea(p); } From a9bfb6609a38b46765409eededdc70c80a8cc29e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 20 Dec 2023 23:31:19 +0900 Subject: [PATCH 03/14] =?UTF-8?q?=E3=82=AD=E3=83=BC=E3=83=91=E3=83=BC?= =?UTF-8?q?=E3=81=AE=E3=83=9A=E3=83=8A=E3=83=AB=E3=83=86=E3=82=A3=E3=82=A8?= =?UTF-8?q?=E3=83=AA=E3=82=A2=E3=81=8B=E3=82=89=E3=81=AE=E3=83=9C=E3=83=BC?= =?UTF-8?q?=E3=83=AB=E6=8E=92=E5=87=BA=E3=81=8C=E3=81=86=E3=81=BE=E3=81=8F?= =?UTF-8?q?=E3=81=84=E3=81=8B=E3=81=AA=E3=81=8B=E3=81=A3=E3=81=9F=E5=95=8F?= =?UTF-8?q?=E9=A1=8C=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_planner_plugins/goalie_planner.hpp | 23 +++++++------------ 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 9416e0626..68c899c33 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -50,14 +50,12 @@ class GoaliePlanner : public PlannerBase if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) { // シュートブロック - std::cout << "shoot block mode" << std::endl; ClosestPoint result; bg::closest_point(ball_line, robot->pose.pos, result); target.setTargetPosition(result.closest_point); target.setTargetTheta(getAngle(-world_model->ball.vel)); } else { if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball)) { - std::cout << "ball discharge mode" << std::endl; // パスできるロボットのリストアップ auto passable_robot_list = world_model->ours.getAvailableRobots(); passable_robot_list.erase( @@ -65,7 +63,7 @@ class GoaliePlanner : public PlannerBase passable_robot_list.begin(), passable_robot_list.end(), [&](const RobotInfo::SharedPtr & r) { // 自分以外 - if (robot->id != r->id) { + if (robot->id == r->id) { return true; } // 敵に塞がれていたら除外 @@ -82,36 +80,31 @@ class GoaliePlanner : public PlannerBase Point pass_target = [&]() { if (not passable_robot_list.empty()) { - std::cout << "pass target is " << passable_robot_list.front()->pose.pos << std::endl; + // TODO: いい感じのロボットを選ぶようにする return passable_robot_list.front()->pose.pos; } else { - std::cout << "no pass target" << std::endl; return Point{0, 0}; } }(); - std::cout << "list up passable robots" << std::endl; - - Point intermediate_point = ball + (pass_target - ball).normalized() * 0.2f; - double angle_ball_to_target = getAngle(ball - pass_target); + Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; + double angle_ball_to_target = getAngle(pass_target - ball); double dot = (world_model->ball.pos - robot->pose.pos) .normalized() - .dot((world_model->ball.pos - pass_target).normalized()); + .dot((pass_target - world_model->ball.pos).normalized()); + std::cout << "dot: " << dot << std::endl; // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 if ( dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, robot->pose.theta)) > 0.05) { - std::cout << "go to intermediate point" << std::endl; target.setTargetPosition(intermediate_point); target.enableCollisionAvoidance(); } else { - std::cout << "go to pass target" << std::endl; target.setTargetPosition(world_model->ball.pos); - target.kickStraight(0.7).disableCollisionAvoidance(); + target.kickStraight(0.4).disableCollisionAvoidance(); target.enableCollisionAvoidance(); } - target.setTargetTheta(getAngle(ball - pass_target)); + target.setTargetTheta(getAngle(pass_target - ball)); } else { - std::cout << "defense mode" << std::endl; const double BLOCK_DIST = 0.15; // 範囲外のときは正面に構える if (not world_model->isFieldInside(world_model->ball.pos)) { From c463212d8f7a984dfa9b5a8709854fb0b5b6a254 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 20 Dec 2023 23:48:10 +0900 Subject: [PATCH 04/14] =?UTF-8?q?=E3=82=AD=E3=83=BC=E3=83=91=E3=83=BC?= =?UTF-8?q?=E3=81=AE=E3=83=9C=E3=83=BC=E3=83=AB=E6=8E=92=E5=87=BA=E3=81=AE?= =?UTF-8?q?=E5=8B=95=E3=81=8D=E3=82=92=E9=96=A2=E6=95=B0=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/goalie_planner.hpp | 104 +++++++++--------- 1 file changed, 55 insertions(+), 49 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 68c899c33..c13b43326 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -30,6 +30,58 @@ class GoaliePlanner : public PlannerBase { } + void emmitBallFromPenaltyArea(crane::RobotCommandWrapper & target) + { + auto ball = world_model->ball.pos; + // パスできるロボットのリストアップ + auto passable_robot_list = world_model->ours.getAvailableRobots(); + passable_robot_list.erase( + std::remove_if( + passable_robot_list.begin(), passable_robot_list.end(), + [&](const RobotInfo::SharedPtr & r) { + // 自分以外 + if (target.robot->id == r->id) { + return true; + } + // 敵に塞がれていたら除外 + Segment ball_to_robot_line(ball, r->pose.pos); + for (const auto & enemy : world_model->theirs.getAvailableRobots()) { + auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); + if (dist < 0.2) { + return true; + } + } + return false; + }), + passable_robot_list.end()); + + Point pass_target = [&]() { + if (not passable_robot_list.empty()) { + // TODO: いい感じのロボットを選ぶようにする + return passable_robot_list.front()->pose.pos; + } else { + return Point{0, 0}; + } + }(); + + Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; + double angle_ball_to_target = getAngle(pass_target - ball); + double dot = (world_model->ball.pos - target.robot->pose.pos) + .normalized() + .dot((pass_target - world_model->ball.pos).normalized()); + // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 + if ( + dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, target.robot->pose.theta)) > 0.05) { + target.setTargetPosition(intermediate_point); + target.enableCollisionAvoidance(); + } else { + target.setTargetPosition(world_model->ball.pos); + target.kickStraight(0.4).disableCollisionAvoidance(); + target.enableCollisionAvoidance(); + } + target.setTargetTheta(getAngle(pass_target - ball)); + } + std::vector calculateControlTarget( const std::vector & robots) override { @@ -56,58 +108,12 @@ class GoaliePlanner : public PlannerBase target.setTargetTheta(getAngle(-world_model->ball.vel)); } else { if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball)) { - // パスできるロボットのリストアップ - auto passable_robot_list = world_model->ours.getAvailableRobots(); - passable_robot_list.erase( - std::remove_if( - passable_robot_list.begin(), passable_robot_list.end(), - [&](const RobotInfo::SharedPtr & r) { - // 自分以外 - if (robot->id == r->id) { - return true; - } - // 敵に塞がれていたら除外 - Segment ball_to_robot_line(ball, r->pose.pos); - for (const auto & enemy : world_model->theirs.getAvailableRobots()) { - auto dist = bg::distance(ball_to_robot_line, enemy->pose.pos); - if (dist < 0.2) { - return true; - } - } - return false; - }), - passable_robot_list.end()); - - Point pass_target = [&]() { - if (not passable_robot_list.empty()) { - // TODO: いい感じのロボットを選ぶようにする - return passable_robot_list.front()->pose.pos; - } else { - return Point{0, 0}; - } - }(); - - Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f; - double angle_ball_to_target = getAngle(pass_target - ball); - double dot = (world_model->ball.pos - robot->pose.pos) - .normalized() - .dot((pass_target - world_model->ball.pos).normalized()); - std::cout << "dot: " << dot << std::endl; - // ボールと目標の延長線上にいない && 角度があってないときは，中間ポイントを経由 - if ( - dot < 0.95 || std::abs(getAngleDiff(angle_ball_to_target, robot->pose.theta)) > 0.05) { - target.setTargetPosition(intermediate_point); - target.enableCollisionAvoidance(); - } else { - target.setTargetPosition(world_model->ball.pos); - target.kickStraight(0.4).disableCollisionAvoidance(); - target.enableCollisionAvoidance(); - } - target.setTargetTheta(getAngle(pass_target - ball)); + // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す + emmitBallFromPenaltyArea(target); } else { const double BLOCK_DIST = 0.15; // 範囲外のときは正面に構える - if (not world_model->isFieldInside(world_model->ball.pos)) { + if (not world_model->isFieldInside(ball)) { ball << 0, 0; } Point goal_center = world_model->getOurGoalCenter(); From e20bdd5eb0c4ca8ad96525b7dd29ce11a9c1bdbf Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Wed, 20 Dec 2023 23:55:41 +0900 Subject: [PATCH 05/14] =?UTF-8?q?=E3=82=B2=E3=82=A4=E3=83=B3=E3=83=81?= =?UTF-8?q?=E3=83=A5=E3=83=BC=E3=83=8B=E3=83=B3=E3=82=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/play_switcher.launch.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index bdbd8827c..4425146af 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/crane_bringup/launch/play_switcher.launch.py @@ -113,7 +113,8 @@ def generate_launch_description(): { "planner": "simple", # "non_rvo_gain": 2.15, - "non_rvo_p_gain": 2.9, + "non_rvo_p_gain": 3.9, + "non_rvo_i_gain": 0.1, "non_rvo_d_gain": 1.0, } ], @@ -174,7 +175,7 @@ def generate_launch_description(): parameters=[ { "no_movement": False, - "theta_kp": 12.0, + "theta_kp": 5.0, "theta_ki": 0.0, "theta_kd": 1.0, } From 7e1248c2d1135aed7875b3d9760e18387d1bdca8 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Thu, 21 Dec 2023 00:04:31 +0900 Subject: [PATCH 06/14] =?UTF-8?q?=E3=83=9C=E3=83=BC=E3=83=AB=E3=81=AE?= =?UTF-8?q?=E6=96=B9=E5=90=91=E3=82=92=E5=90=91=E3=81=8F=E9=96=A2=E6=95=B0?= =?UTF-8?q?=E3=82=92=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/goalie_planner.hpp | 2 +- .../crane_msg_wrappers/robot_command_wrapper.hpp | 12 ++++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index c13b43326..4c7e35e40 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -119,7 +119,7 @@ class GoaliePlanner : public PlannerBase Point goal_center = world_model->getOurGoalCenter(); goal_center << goals.first.x(), 0.0f; target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); - target.setTargetTheta(getAngle(ball - robot->pose.pos)); + target.lookAtBall(); } } control_targets.emplace_back(target.getMsg()); diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index 0ded75089..0c82d2d7f 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -241,6 +241,18 @@ struct RobotCommandWrapper return *this; } + RobotCommandWrapper & lookAt(Position pos) + { + return setTargetTheta(getAngle(pos - robot->pose.pos)); + } + + RobotCommandWrapper & lookAtBall() { return lookAt(world_model->ball.pos); } + + RobotCommandWrapper & lookAtBallFrom(Position from) + { + return setTargetTheta(getAngle(world_model->ball.pos - from)); + } + const crane_msgs::msg::RobotCommand & getMsg() const { return latest_msg; } crane_msgs::msg::RobotCommand & getEditableMsg() { return latest_msg; } From 72a4856ae0a229ce1852f90f7ca79f5662c835ac Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 22:49:04 +0900 Subject: [PATCH 07/14] =?UTF-8?q?typo=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/goalie_planner.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 4c7e35e40..716897791 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -30,7 +30,7 @@ class GoaliePlanner : public PlannerBase { } - void emmitBallFromPenaltyArea(crane::RobotCommandWrapper & target) + void emitBallFromPenaltyArea(crane::RobotCommandWrapper & target) { auto ball = world_model->ball.pos; // パスできるロボットのリストアップ @@ -109,7 +109,7 @@ class GoaliePlanner : public PlannerBase } else { if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball)) { // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す - emmitBallFromPenaltyArea(target); + emitBallFromPenaltyArea(target); } else { const double BLOCK_DIST = 0.15; // 範囲外のときは正面に構える From c23f4be2a7a5936c68d6b2ecf8f5fec1c8604813 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 22:58:58 +0900 Subject: [PATCH 08/14] =?UTF-8?q?=E3=83=95=E3=82=A1=E3=82=A4=E3=83=AB?= =?UTF-8?q?=E3=81=AE=E7=A7=BB=E5=8B=95?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- session/crane_planner_plugins/CMakeLists.txt | 2 +- .../ball_placement_with_skill.cpp | 0 .../ball_placement_with_skill_node.cpp | 0 .../src/{receive/test.cpp => receive_test.cpp} | 0 4 files changed, 1 insertion(+), 1 deletion(-) rename session/crane_planner_plugins/src/{ball_placement_with_skill => }/ball_placement_with_skill.cpp (100%) rename session/crane_planner_plugins/src/{ball_placement_with_skill => }/ball_placement_with_skill_node.cpp (100%) rename session/crane_planner_plugins/src/{receive/test.cpp => receive_test.cpp} (100%) diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index 83d389116..25ec21e5d 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -52,7 +52,7 @@ add_planner_plugin( CLASS_NAME "crane::ReceivePlanner" ) -ament_auto_add_executable(receive_test_node src/receive/test.cpp) +ament_auto_add_executable(receive_test_node src/receive_test.cpp) ament_target_dependencies(receive_test_node "Python3") diff --git a/session/crane_planner_plugins/src/ball_placement_with_skill/ball_placement_with_skill.cpp b/session/crane_planner_plugins/src/ball_placement_with_skill.cpp similarity index 100% rename from session/crane_planner_plugins/src/ball_placement_with_skill/ball_placement_with_skill.cpp rename to session/crane_planner_plugins/src/ball_placement_with_skill.cpp diff --git a/session/crane_planner_plugins/src/ball_placement_with_skill/ball_placement_with_skill_node.cpp b/session/crane_planner_plugins/src/ball_placement_with_skill_node.cpp similarity index 100% rename from session/crane_planner_plugins/src/ball_placement_with_skill/ball_placement_with_skill_node.cpp rename to session/crane_planner_plugins/src/ball_placement_with_skill_node.cpp diff --git a/session/crane_planner_plugins/src/receive/test.cpp b/session/crane_planner_plugins/src/receive_test.cpp similarity index 100% rename from session/crane_planner_plugins/src/receive/test.cpp rename to session/crane_planner_plugins/src/receive_test.cpp From 235ef92c30471672569c8fecbad4868f1f4f9bf9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:07:22 +0900 Subject: [PATCH 09/14] =?UTF-8?q?WorldModel=E3=81=ABPlaySituation=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_msgs/msg/world_model/WorldModel.msg | 2 ++ .../include/crane_msg_wrappers/world_model_wrapper.hpp | 6 ++++++ 2 files changed, 8 insertions(+) diff --git a/crane_msgs/msg/world_model/WorldModel.msg b/crane_msgs/msg/world_model/WorldModel.msg index 6508e426d..f28c702ca 100755 --- a/crane_msgs/msg/world_model/WorldModel.msg +++ b/crane_msgs/msg/world_model/WorldModel.msg @@ -22,3 +22,5 @@ RobotInfoOurs[] robot_info_ours RobotInfoTheirs[] robot_info_theirs geometry_msgs/Point ball_placement_target + +PlaySituation play_situation diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 5f43677d5..145349c9a 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -16,6 +16,8 @@ #include #include +#include "play_situation_wrapper.hpp" + struct BallContact { std::chrono::system_clock::time_point last_contact_end_time; @@ -191,6 +193,8 @@ struct WorldModelWrapper void update(const crane_msgs::msg::WorldModel & world_model) { + play_situation.update(world_model.play_situation); + for (auto & our_robot : ours.robots) { our_robot->available = false; } @@ -417,6 +421,8 @@ struct WorldModelWrapper Ball ball; + PlaySituationWrapper play_situation; + private: rclcpp::Subscription::SharedPtr subscriber; From 4fe4215726c220a4a8968fee21b8ba7f7e0bedea Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:42:23 +0900 Subject: [PATCH 10/14] =?UTF-8?q?RobotCommandWrapper=E3=81=AE=E3=83=93?= =?UTF-8?q?=E3=83=AB=E3=83=89=E3=82=A8=E3=83=A9=E3=83=BC=E3=82=92=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/robot_command_wrapper.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp index 0c82d2d7f..67ba45818 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp @@ -7,7 +7,6 @@ #ifndef CRANE_MSG_WRAPPERS__ROBOT_COMMAND_WRAPPER_HPP_ #define CRANE_MSG_WRAPPERS__ROBOT_COMMAND_WRAPPER_HPP_ -#include #include #include #include @@ -25,7 +24,7 @@ struct RobotCommandWrapper typedef std::shared_ptr SharedPtr; RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper) - : robot(world_model_wrapper->getOurRobot(id)) + : robot(world_model_wrapper->getOurRobot(id)), world_model(world_model_wrapper) { latest_msg.robot_id = id; @@ -241,14 +240,14 @@ struct RobotCommandWrapper return *this; } - RobotCommandWrapper & lookAt(Position pos) + RobotCommandWrapper & lookAt(Point pos) { return setTargetTheta(getAngle(pos - robot->pose.pos)); } RobotCommandWrapper & lookAtBall() { return lookAt(world_model->ball.pos); } - RobotCommandWrapper & lookAtBallFrom(Position from) + RobotCommandWrapper & lookAtBallFrom(Point from) { return setTargetTheta(getAngle(world_model->ball.pos - from)); } @@ -260,6 +259,8 @@ struct RobotCommandWrapper crane_msgs::msg::RobotCommand latest_msg; const std::shared_ptr robot; + + WorldModelWrapper::SharedPtr world_model; }; } // namespace crane From 9841b9b2d2e7235737fa50c01b803aa1de04848f Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:43:01 +0900 Subject: [PATCH 11/14] =?UTF-8?q?skill=5Fbase.hpp=E3=81=A7DEFAULT=E3=81=8C?= =?UTF-8?q?=E5=A4=96=E9=83=A8=E5=AE=9A=E7=BE=A9=E3=81=A8=E3=81=8B=E3=81=B6?= =?UTF-8?q?=E3=82=8B=E3=81=AE=E3=81=A7undef=E3=81=97=E3=81=9F?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_robot_skills/include/crane_robot_skills/skill_base.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index 62ddedd78..80562e7fd 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -14,6 +14,8 @@ #include #include +#undef DEFAULT + namespace crane { From ef783593f64760a1936fe60b79eee67242db60f9 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:43:39 +0900 Subject: [PATCH 12/14] =?UTF-8?q?BallContact=E3=82=92crane=E5=90=8D?= =?UTF-8?q?=E5=89=8D=E7=A9=BA=E9=96=93=E3=81=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/world_model_wrapper.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp index 145349c9a..24c69bcfd 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp @@ -18,6 +18,8 @@ #include "play_situation_wrapper.hpp" +namespace crane +{ struct BallContact { std::chrono::system_clock::time_point last_contact_end_time; @@ -49,8 +51,6 @@ struct BallContact bool is_contacted_pre_frame = false; }; -namespace crane -{ struct RobotIdentifier { bool is_ours; From f5864b4066f978a953da3d9cdf653d4102f3c4bd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:44:07 +0900 Subject: [PATCH 13/14] =?UTF-8?q?PlaySituationWrapper=E3=81=AE=E3=82=A8?= =?UTF-8?q?=E3=83=A9=E3=83=BC=E3=82=92=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_msg_wrappers/play_situation_wrapper.hpp | 4 ++-- utility/crane_msg_wrappers/src/play_situation_wrapper.cpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp index 9e96d2361..aeeb70daa 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp @@ -7,8 +7,8 @@ #ifndef CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_ #define CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_ +#include #include -#include #include #include #include @@ -29,7 +29,7 @@ struct PlaySituationWrapper return situation_command.id == crane_msgs::msg::PlaySituation::INPLAY; } - Eigen::Vector2d placement_position; + Point placement_position; auto update(const crane_msgs::msg::PlaySituation & msg) -> void; diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp index 4b53a30be..e450f06a5 100644 --- a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp @@ -9,6 +9,7 @@ #include #include +#include "crane_msg_wrappers/world_model_wrapper.hpp" #include "robocup_ssl_msgs/msg/referee.hpp" namespace crane From ed9bf6186e69d4acbe2f32644499f989922ed7e1 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Fri, 22 Dec 2023 23:44:33 +0900 Subject: [PATCH 14/14] =?UTF-8?q?GaolPlanner=E3=81=AE=E5=A0=B4=E5=90=88?= =?UTF-8?q?=E5=88=86=E3=81=91=E5=AE=9F=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/goalie_planner.hpp | 76 +++++++++++-------- 1 file changed, 44 insertions(+), 32 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp index 716897791..8bd6cdb3a 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp @@ -82,46 +82,58 @@ class GoaliePlanner : public PlannerBase target.setTargetTheta(getAngle(pass_target - ball)); } + void inplay(crane::RobotCommandWrapper & target, bool enable_emit = true) + { + auto goals = world_model->getOurGoalPosts(); + auto ball = world_model->ball.pos; + // シュートチェック + Segment goal_line(goals.first, goals.second); + Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f); + std::vector intersections; + bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections); + if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) { + // シュートブロック + ClosestPoint result; + bg::closest_point(ball_line, target.robot->pose.pos, result); + target.setTargetPosition(result.closest_point); + target.setTargetTheta(getAngle(-world_model->ball.vel)); + } else { + if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball) && enable_emit) { + // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す + emitBallFromPenaltyArea(target); + } else { + const double BLOCK_DIST = 0.15; + // 範囲外のときは正面に構える + if (not world_model->isFieldInside(ball)) { + ball << 0, 0; + } + Point goal_center = world_model->getOurGoalCenter(); + goal_center << goals.first.x(), 0.0f; + target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); + target.lookAtBall(); + } + } + } + std::vector calculateControlTarget( const std::vector & robots) override { std::vector control_targets; for (auto robot_id : robots) { crane::RobotCommandWrapper target(robot_id.robot_id, world_model); - auto robot = world_model->getRobot(robot_id); - auto ball = world_model->ball.pos; - auto goals = world_model->getOurGoalPosts(); - - // シュートチェック - Segment goal_line(goals.first, goals.second); - Segment ball_line(ball, ball + world_model->ball.vel.normalized() * 20.f); - std::vector intersections; - bg::intersection(ball_line, Segment{goals.first, goals.second}, intersections); - std::cout << "goalie setup" << std::endl; - - if (not intersections.empty() && world_model->ball.vel.norm() > 0.5f) { - // シュートブロック - ClosestPoint result; - bg::closest_point(ball_line, robot->pose.pos, result); - target.setTargetPosition(result.closest_point); - target.setTargetTheta(getAngle(-world_model->ball.vel)); - } else { - if (world_model->ball.isStopped() && world_model->isFriendDefenseArea(ball)) { - // ボールが止まっていて，味方ペナルティエリア内にあるときは，ペナルティエリア外に出す - emitBallFromPenaltyArea(target); - } else { - const double BLOCK_DIST = 0.15; - // 範囲外のときは正面に構える - if (not world_model->isFieldInside(ball)) { - ball << 0, 0; - } - Point goal_center = world_model->getOurGoalCenter(); - goal_center << goals.first.x(), 0.0f; - target.setTargetPosition(goal_center + (ball - goal_center).normalized() * BLOCK_DIST); - target.lookAtBall(); - } + switch (world_model->play_situation.getSituationCommandID()) { + case crane_msgs::msg::PlaySituation::HALT: + target.stopHere(); + break; + case crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION: + [[fallthrough]]; + case crane_msgs::msg::PlaySituation::THEIR_PENALTY_START: + inplay(target, false); + default: + inplay(target, true); } + control_targets.emplace_back(target.getMsg()); } return control_targets;