From 156728b88c2fd1d9dbf3072b08d7ad4b9de1911c Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 8 Oct 2023 20:36:56 +0900 Subject: [PATCH 1/7] =?UTF-8?q?=E3=82=A2=E3=82=BF=E3=83=83=E3=82=AB?= =?UTF-8?q?=E3=83=BC=EF=BC=9A=E3=82=B7=E3=83=A5=E3=83=BC=E3=83=88=E3=81=8C?= =?UTF-8?q?=E5=8E=B3=E3=81=97=E3=81=84=E3=81=A8=E3=81=8D=E3=81=AF=E8=BF=91?= =?UTF-8?q?=E3=81=8F=E3=81=AE=E5=91=B3=E6=96=B9=E3=81=AB=E3=83=91=E3=82=B9?= =?UTF-8?q?=E3=81=99=E3=82=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/attacker_planner.hpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 1928f85aa..8a39dd42f 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -40,7 +40,17 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase crane::RobotCommandWrapper target(robot_id.robot_id, world_model); auto robot = world_model->getRobot(robot_id); - auto best_target = getBestShootTarget(); + auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); + + // シュートの隙がないときは仲間へパス + if(goal_angle_width < 0.07){ + auto our_robots = world_model->ours.getAvailableRobots(); + our_robots.erase(std::remove_if(our_robots.begin(), our_robots.end(), [&](const auto &robot){ + return robot->id == robot_id.robot_id; + }), our_robots.end()); + auto nearest_robot = world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); + best_target = nearest_robot.first->pose.pos; + } // 経由ポイント From bd0693ca298fc10d1fe030a4a0ea29cc7407bccd Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 8 Oct 2023 20:37:12 +0900 Subject: [PATCH 2/7] =?UTF-8?q?=E3=82=B3=E3=83=9F=E3=83=83=E3=83=88?= =?UTF-8?q?=E6=BC=8F=E3=82=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/attacker_planner.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 8a39dd42f..c81b25785 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -98,7 +98,7 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase }); } - auto getBestShootTarget() -> Point + auto getBestShootTargetWithWidth() -> std::pair { const auto & ball = world_model->ball.pos; @@ -118,12 +118,12 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); } - auto largetst_interval = goal_range.getLargestInterval(); - std::cout << "interval width: " << largetst_interval.second - largetst_interval.first + auto largest_interval = goal_range.getLargestInterval(); + std::cout << "interval width: " << largest_interval.second - largest_interval.first << std::endl; - double target_angle = (largetst_interval.first + largetst_interval.second) / 2.0; + double target_angle = (largest_interval.first + largest_interval.second) / 2.0; - return ball + Point(cos(target_angle), sin(target_angle)) * 0.5; + return {ball + Point(cos(target_angle), sin(target_angle)) * 0.5, largest_interval.second - largest_interval.first}; } }; From e9eaaa4bfeb30ebdf0e6d954a05410860b94983e Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 8 Oct 2023 20:37:40 +0900 Subject: [PATCH 3/7] =?UTF-8?q?Receiver=E5=BE=A9=E6=B4=BB=EF=BC=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_bringup/launch/play_switcher.launch.py | 5 + .../launch/play_switcher_y.launch.py | 5 + session/crane_planner_plugins/CMakeLists.txt | 9 +- .../crane_planner_plugins/receive_planner.hpp | 101 +++++++++++++++++- .../src/receive/receive_node.cpp | 2 +- .../src/receive/test.cpp | 3 +- .../config/play_situation/SIMPLE_ATTACK.yaml | 2 + .../src/crane_session_controller.cpp | 1 + 8 files changed, 119 insertions(+), 9 deletions(-) diff --git a/crane_bringup/launch/play_switcher.launch.py b/crane_bringup/launch/play_switcher.launch.py index 6a56fe3f9..b8b906ddc 100755 --- a/crane_bringup/launch/play_switcher.launch.py +++ b/crane_bringup/launch/play_switcher.launch.py @@ -176,6 +176,10 @@ def generate_launch_description(): # output="screen" ) + receive = Node(package="crane_planner_plugins", executable="receive_node", + # output="screen" + ) + play_switcher = Node( package="crane_play_switcher", executable="play_switcher_node", @@ -226,6 +230,7 @@ def generate_launch_description(): kickoff, marker, attacker, + receive, world_model_publisher, play_switcher, visualizer, diff --git a/crane_bringup/launch/play_switcher_y.launch.py b/crane_bringup/launch/play_switcher_y.launch.py index 91fcc7868..a2c8b8dbd 100755 --- a/crane_bringup/launch/play_switcher_y.launch.py +++ b/crane_bringup/launch/play_switcher_y.launch.py @@ -178,6 +178,10 @@ def generate_launch_description(): # output="screen" ) + receive = Node(package="crane_planner_plugins", executable="receive_node", + # output="screen" + ) + play_switcher = Node( package="crane_play_switcher", executable="play_switcher_node", output="screen" ) @@ -226,6 +230,7 @@ def generate_launch_description(): goalie, kickoff, attacker, + receive, world_model_publisher, play_switcher, visualizer, diff --git a/session/crane_planner_plugins/CMakeLists.txt b/session/crane_planner_plugins/CMakeLists.txt index 520e379b2..bdd3f362e 100755 --- a/session/crane_planner_plugins/CMakeLists.txt +++ b/session/crane_planner_plugins/CMakeLists.txt @@ -44,10 +44,11 @@ add_planner_plugin( CLASS_NAME "crane::BallPlacementPlanner" ) -#add_planner_plugin( -# NAME "receive" -# CLASS_NAME "crane::ReceivePlanner" -#) +add_planner_plugin( + NAME "receive" + CLASS_NAME "crane::ReceivePlanner" +) + 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/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 2668935d4..15b66d926 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -13,7 +13,9 @@ #include "crane_game_analyzer/evaluations/evaluations.hpp" #include "crane_geometry/eigen_adapter.hpp" +#include "crane_geometry/interval.hpp" #include "crane_msg_wrappers/world_model_wrapper.hpp" +#include "crane_msg_wrappers/robot_command_wrapper.hpp" #include "crane_msgs/msg/pass_info.hpp" #include "crane_msgs/msg/receiver_plan.hpp" #include "crane_msgs/msg/world_model.hpp" @@ -27,8 +29,12 @@ namespace crane * ReceivePlannerは現在進行形でパスされているボールに対して， * ボールを受けるロボット(passer),その次にボールを受けるロボット(receiver)を指定するだけで * 最適なパス地点を計算し，その2台に対する指令を生成するプランナーです + * + * 何もないとき：パスの通る隙の多い地点に陣取る + * ボールが向かってきているとき：最適なパス地点を計算し，その地点に向かう + * */ -class ReceivePlanner : public rclcpp::Node +class ReceivePlanner : public rclcpp::Node, public PlannerBase { public: enum class ReceivePhase { @@ -53,8 +59,9 @@ class ReceivePlanner : public rclcpp::Node COMPOSITION_PUBLIC explicit ReceivePlanner(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : rclcpp::Node("receive_planner", options) + : rclcpp::Node("receive_planner", options), PlannerBase("receive", *this) { + RCLCPP_INFO(get_logger(), "initializing"); pass_info_pub = create_publisher("path_info", 1); using namespace std::placeholders; pass_req_service = create_service( @@ -64,6 +71,58 @@ class ReceivePlanner : public rclcpp::Node [this](void) -> void { pass_info.world_model = world_model->getMsg(); }); } + std::vector calculateControlTarget( + const std::vector & robots) override +{ + auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.5, 8); + + Point best_position; + double best_score = 0.0; + for (const auto & dpps_point : dpps_points) { + Segment line{world_model->ball.pos, dpps_point}; + double closest_distance = [&]() -> double{ + double closest_distance = std::numeric_limits::max(); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + ClosestPoint result; + bg::closest_point(robot->pose.pos, line, result); + if (result.distance < closest_distance) { + closest_distance = result.distance; + } + } + return closest_distance; + }(); + + if(closest_distance < 0.4) { + continue; + } + + const double score = getLargestGoalAngleWidthFromPosition(dpps_point); + if (score > best_score) { + best_score = score; + best_position = dpps_point; + } + } + + std::vector commands; + for(const auto & robot: robots){ + crane::RobotCommandWrapper target(robot.robot_id, world_model); + target.setTargetPosition(best_position); + target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); + commands.push_back(target.getMsg()); + } + + return commands; +} +auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override +{ + return this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { + return 100. / world_model->getSquareDistanceFromRobotToBall({true, robot->id}); + }); +} + void passRequestHandle( const std::shared_ptr request_header, const std::shared_ptr request, @@ -146,6 +205,29 @@ class ReceivePlanner : public rclcpp::Node return ret; } + + auto getLargestGoalAngleWidthFromPosition(const Point point) -> double + { + Interval goal_range; + + auto goal_posts = world_model->getTheirGoalPosts(); + goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point)); + + for (auto & enemy : world_model->theirs.robots) { + double distance = (point - enemy->pose.pos).norm(); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = getAngle(enemy->pose.pos - point); + double diff_angle = + atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); + + goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); + } + + auto largest_interval = goal_range.getLargestInterval(); + return largest_interval.second - largest_interval.first; + } + std::vector> getPositionsWithScore(Segment ball_line, Point next_target) { auto points = getPoints(ball_line, 0.05); @@ -181,6 +263,21 @@ class ReceivePlanner : public rclcpp::Node return points; } + std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num){ + std::vector points; + for (int theta_index = 0; theta_index < theta_div_num; theta_index++){ + double theta = 2.0 * M_PI * theta_index / theta_div_num; + for (double r = r_resolution; r <= 10.0; r += r_resolution){ + points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta))); + } + } + points.erase(std::remove_if(points.begin(), points.end(), [&](const auto & point){ + return not world_model->isFieldInside(point); + }), points.end()); + + return points; + } + double getPointScore(Point p, Point next_target) { double nearest_dist; diff --git a/session/crane_planner_plugins/src/receive/receive_node.cpp b/session/crane_planner_plugins/src/receive/receive_node.cpp index 2c9dab150..1a000aac9 100755 --- a/session/crane_planner_plugins/src/receive/receive_node.cpp +++ b/session/crane_planner_plugins/src/receive/receive_node.cpp @@ -6,7 +6,7 @@ #include -#include "crane_planner{plugins/receive_planner.hpp" +#include "crane_planner_plugins/receive_planner.hpp" int main(int argc, char * argv[]) { diff --git a/session/crane_planner_plugins/src/receive/test.cpp b/session/crane_planner_plugins/src/receive/test.cpp index 36ffc0edc..7b415b94d 100644 --- a/session/crane_planner_plugins/src/receive/test.cpp +++ b/session/crane_planner_plugins/src/receive/test.cpp @@ -14,8 +14,7 @@ int main(int argc, char * argv[]) { rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - crane::ReceivePlanner receive_planner(options); + crane::ReceivePlanner receive_planner; receive_planner.session_info.receiver_id = 2; auto world_model = std::make_shared(); // ball diff --git a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml index 223bf0c77..826770650 100644 --- a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml +++ b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml @@ -6,6 +6,8 @@ sessions: capacity: 1 - name: "attacker" capacity: 1 + - name: "receive" + capacity: 1 - name: "defender" capacity: 4 - name: "waiter" diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp index 72739f778..1ad865189 100644 --- a/session/crane_session_controller/src/crane_session_controller.cpp +++ b/session/crane_session_controller/src/crane_session_controller.cpp @@ -25,6 +25,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions session_planners["kickoff"] = std::make_shared("kickoff"); session_planners["attacker"] = std::make_shared("attacker"); session_planners["marker"] = std::make_shared("marker"); + session_planners["receive"] = std::make_shared("receive"); for (auto & planner : session_planners) { planner.second->construct(*this); } From 88eeb86f6468734bdc1d5103dd2fd689ef41eef5 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 8 Oct 2023 23:03:45 +0900 Subject: [PATCH 4/7] =?UTF-8?q?=E3=82=A2=E3=82=BF=E3=83=83=E3=82=AB?= =?UTF-8?q?=E3=83=BC=E3=81=AE=E3=82=AD=E3=83=83=E3=82=AF=E5=BC=B7=E3=81=99?= =?UTF-8?q?=E3=81=8Ew?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/attacker_planner.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index c81b25785..0c1e09c20 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -67,7 +67,7 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase target.enableCollisionAvoidance(); } else { target.setTargetPosition(world_model->ball.pos); - target.kickStraight(1.0).disableCollisionAvoidance(); + target.kickStraight(0.7).disableCollisionAvoidance(); target.enableCollisionAvoidance(); } From 317ad2a1b3216791b20946ad14d6d27473a63970 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Sun, 8 Oct 2023 23:04:22 +0900 Subject: [PATCH 5/7] =?UTF-8?q?=E3=83=AC=E3=82=B7=E3=83=BC=E3=83=90?= =?UTF-8?q?=E3=83=BC=E3=81=8C=E3=83=AF=E3=83=B3=E3=82=BF=E3=83=83=E3=83=81?= =?UTF-8?q?=E3=82=B7=E3=83=A5=E3=83=BC=E3=83=88=E3=82=92=E8=A9=A6=E3=81=BF?= =?UTF-8?q?=E3=82=8B=E3=82=88=E3=81=86=E3=81=AB?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../crane_planner_plugins/receive_planner.hpp | 147 +++++++++++++----- 1 file changed, 104 insertions(+), 43 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 15b66d926..312ea871f 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -14,8 +14,8 @@ #include "crane_game_analyzer/evaluations/evaluations.hpp" #include "crane_geometry/eigen_adapter.hpp" #include "crane_geometry/interval.hpp" -#include "crane_msg_wrappers/world_model_wrapper.hpp" #include "crane_msg_wrappers/robot_command_wrapper.hpp" +#include "crane_msg_wrappers/world_model_wrapper.hpp" #include "crane_msgs/msg/pass_info.hpp" #include "crane_msgs/msg/receiver_plan.hpp" #include "crane_msgs/msg/world_model.hpp" @@ -72,56 +72,90 @@ class ReceivePlanner : public rclcpp::Node, public PlannerBase } std::vector calculateControlTarget( - const std::vector & robots) override -{ - auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.5, 8); - - Point best_position; - double best_score = 0.0; - for (const auto & dpps_point : dpps_points) { - Segment line{world_model->ball.pos, dpps_point}; - double closest_distance = [&]() -> double{ - double closest_distance = std::numeric_limits::max(); - for (const auto & robot : world_model->theirs.getAvailableRobots()) { + const std::vector & robots) override + { + auto dpps_points = getDPPSPoints(world_model->ball.pos, 0.25, 16); + + std::vector commands; + for (const auto & robot : robots) { + auto robot_info = world_model->getRobot(robot); + crane::RobotCommandWrapper target(robot.robot_id, world_model); + // モード判断 + // こちらへ向かう速度成分 + float ball_vel = + world_model->ball.vel.dot((robot_info->pose.pos - world_model->ball.pos).normalized()); + target.kickStraight(0.7); + if (ball_vel > 0.5) { + Segment ball_line(world_model->ball.pos, (world_model->ball.pos + world_model->ball.vel.normalized() * (world_model->ball.pos - robot_info->pose.pos).norm())); + // ボールの進路上に移動 ClosestPoint result; - bg::closest_point(robot->pose.pos, line, result); - if (result.distance < closest_distance) { - closest_distance = result.distance; + bg::closest_point(robot_info->pose.pos, ball_line, result); + + // ゴールとボールの中間方向を向く + auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point); + Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; + auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); + double intermediate_angle = getAngle(to_goal + to_ball); + target.setTargetTheta(intermediate_angle); + + // キッカーの中心のためのオフセット + target.setTargetPosition(result.closest_point - (to_goal + to_ball).normalized() * 0.055); + + } else { + Point best_position; + double best_score = 0.0; + for (const auto & dpps_point : dpps_points) { + Segment line{world_model->ball.pos, dpps_point}; + double closest_distance = [&]() -> double { + double closest_distance = std::numeric_limits::max(); + for (const auto & robot : world_model->theirs.getAvailableRobots()) { + ClosestPoint result; + bg::closest_point(robot->pose.pos, line, result); + if (result.distance < closest_distance) { + closest_distance = result.distance; + } + } + return closest_distance; + }(); + + if (closest_distance < 0.4) { + continue; } - } - return closest_distance; - }(); - if(closest_distance < 0.4) { - continue; - } + double score = getLargestGoalAngleWidthFromPosition(dpps_point); + const double dist = world_model->getDistanceFromRobot(robot, dpps_point); + score = score * (1.0 - dist / 10.0); - const double score = getLargestGoalAngleWidthFromPosition(dpps_point); - if (score > best_score) { - best_score = score; - best_position = dpps_point; + if (score > best_score) { + best_score = score; + best_position = dpps_point; + } + } + target.setTargetPosition(best_position); +// target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); } - } - std::vector commands; - for(const auto & robot: robots){ - crane::RobotCommandWrapper target(robot.robot_id, world_model); - target.setTargetPosition(best_position); - target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); + // ゴールとボールの中間方向を向く + Point target_pos{target.latest_msg.target_x.front(),target.latest_msg.target_y.front()}; + auto goal_angle = getLargestGoalAngleFromPosition(target_pos); + Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; + auto to_ball = (world_model->ball.pos - target_pos).normalized(); + target.setTargetTheta(getAngle(to_goal + to_ball)); + commands.push_back(target.getMsg()); } return commands; -} -auto getSelectedRobots( - uint8_t selectable_robots_num, const std::vector & selectable_robots) - -> std::vector override -{ + } + auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override + { return this->getSelectedRobotsByScore( selectable_robots_num, selectable_robots, [this](const std::shared_ptr & robot) { return 100. / world_model->getSquareDistanceFromRobotToBall({true, robot->id}); }); -} + } void passRequestHandle( const std::shared_ptr request_header, @@ -228,6 +262,28 @@ auto getSelectedRobots( return largest_interval.second - largest_interval.first; } + auto getLargestGoalAngleFromPosition(const Point point) -> double + { + Interval goal_range; + + auto goal_posts = world_model->getTheirGoalPosts(); + goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point)); + + for (auto & enemy : world_model->theirs.robots) { + double distance = (point - enemy->pose.pos).norm(); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = getAngle(enemy->pose.pos - point); + double diff_angle = + atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS)); + + goal_range.erase(center_angle - diff_angle, center_angle + diff_angle); + } + + auto largest_interval = goal_range.getLargestInterval(); + return (largest_interval.second + largest_interval.first) * 0.5; + } + std::vector> getPositionsWithScore(Segment ball_line, Point next_target) { auto points = getPoints(ball_line, 0.05); @@ -263,17 +319,22 @@ auto getSelectedRobots( return points; } - std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num){ + std::vector getDPPSPoints(Point center, double r_resolution, int theta_div_num) + { std::vector points; - for (int theta_index = 0; theta_index < theta_div_num; theta_index++){ + for (int theta_index = 0; theta_index < theta_div_num; theta_index++) { double theta = 2.0 * M_PI * theta_index / theta_div_num; - for (double r = r_resolution; r <= 10.0; r += r_resolution){ + for (double r = r_resolution; r <= 10.0; r += r_resolution) { points.emplace_back(Point(center.x() + r * cos(theta), center.y() + r * sin(theta))); } } - points.erase(std::remove_if(points.begin(), points.end(), [&](const auto & point){ - return not world_model->isFieldInside(point); - }), points.end()); + points.erase( + std::remove_if( + points.begin(), points.end(), + [&](const auto & point) { + return (not world_model->isFieldInside(point)) or world_model->isDefenseArea(point); + }), + points.end()); return points; } From 397d02acbc0a97f01610b1a7bba105234b9493f2 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 9 Oct 2023 00:50:50 +0900 Subject: [PATCH 6/7] =?UTF-8?q?=E3=81=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../include/crane_planner_plugins/receive_planner.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 312ea871f..01629ff17 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -95,11 +95,11 @@ class ReceivePlanner : public rclcpp::Node, public PlannerBase auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point); Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); - double intermediate_angle = getAngle(to_goal + to_ball); + double intermediate_angle = getAngle(2 * to_goal + to_ball); target.setTargetTheta(intermediate_angle); // キッカーの中心のためのオフセット - target.setTargetPosition(result.closest_point - (to_goal + to_ball).normalized() * 0.055); + target.setTargetPosition(result.closest_point - (2 * to_goal + to_ball).normalized() * 0.055); } else { Point best_position; From 4a90d647be102368e38bac25cab6312b43b52105 Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Mon, 9 Oct 2023 00:52:27 +0900 Subject: [PATCH 7/7] ament_clang_format --- .../attacker_planner.hpp | 17 ++++++--- .../crane_planner_plugins/receive_planner.hpp | 38 ++++++++++--------- 2 files changed, 32 insertions(+), 23 deletions(-) diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp index 0c1e09c20..3e9416ece 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp @@ -43,12 +43,15 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); // シュートの隙がないときは仲間へパス - if(goal_angle_width < 0.07){ + if (goal_angle_width < 0.07) { auto our_robots = world_model->ours.getAvailableRobots(); - our_robots.erase(std::remove_if(our_robots.begin(), our_robots.end(), [&](const auto &robot){ - return robot->id == robot_id.robot_id; - }), our_robots.end()); - auto nearest_robot = world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); + our_robots.erase( + std::remove_if( + our_robots.begin(), our_robots.end(), + [&](const auto & robot) { return robot->id == robot_id.robot_id; }), + our_robots.end()); + auto nearest_robot = + world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); best_target = nearest_robot.first->pose.pos; } @@ -123,7 +126,9 @@ class AttackerPlanner : public rclcpp::Node, public PlannerBase << std::endl; double target_angle = (largest_interval.first + largest_interval.second) / 2.0; - return {ball + Point(cos(target_angle), sin(target_angle)) * 0.5, largest_interval.second - largest_interval.first}; + return { + ball + Point(cos(target_angle), sin(target_angle)) * 0.5, + largest_interval.second - largest_interval.first}; } }; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp index 01629ff17..8f5250ff3 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp @@ -86,20 +86,24 @@ class ReceivePlanner : public rclcpp::Node, public PlannerBase world_model->ball.vel.dot((robot_info->pose.pos - world_model->ball.pos).normalized()); target.kickStraight(0.7); if (ball_vel > 0.5) { - Segment ball_line(world_model->ball.pos, (world_model->ball.pos + world_model->ball.vel.normalized() * (world_model->ball.pos - robot_info->pose.pos).norm())); - // ボールの進路上に移動 - ClosestPoint result; - bg::closest_point(robot_info->pose.pos, ball_line, result); - - // ゴールとボールの中間方向を向く - auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point); - Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; - auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); - double intermediate_angle = getAngle(2 * to_goal + to_ball); - target.setTargetTheta(intermediate_angle); - - // キッカーの中心のためのオフセット - target.setTargetPosition(result.closest_point - (2 * to_goal + to_ball).normalized() * 0.055); + Segment ball_line( + world_model->ball.pos, + (world_model->ball.pos + world_model->ball.vel.normalized() * + (world_model->ball.pos - robot_info->pose.pos).norm())); + // ボールの進路上に移動 + ClosestPoint result; + bg::closest_point(robot_info->pose.pos, ball_line, result); + + // ゴールとボールの中間方向を向く + auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point); + Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; + auto to_ball = (world_model->ball.pos - result.closest_point).normalized(); + double intermediate_angle = getAngle(2 * to_goal + to_ball); + target.setTargetTheta(intermediate_angle); + + // キッカーの中心のためのオフセット + target.setTargetPosition( + result.closest_point - (2 * to_goal + to_ball).normalized() * 0.055); } else { Point best_position; @@ -132,14 +136,14 @@ class ReceivePlanner : public rclcpp::Node, public PlannerBase } } target.setTargetPosition(best_position); -// target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); + // target.setTargetTheta(getAngle(world_model->ball.pos - best_position)); } // ゴールとボールの中間方向を向く - Point target_pos{target.latest_msg.target_x.front(),target.latest_msg.target_y.front()}; + Point target_pos{target.latest_msg.target_x.front(), target.latest_msg.target_y.front()}; auto goal_angle = getLargestGoalAngleFromPosition(target_pos); Vector2 to_goal{cos(goal_angle), sin(goal_angle)}; - auto to_ball = (world_model->ball.pos - target_pos).normalized(); + auto to_ball = (world_model->ball.pos - target_pos).normalized(); target.setTargetTheta(getAngle(to_goal + to_ball)); commands.push_back(target.getMsg());