From b4104fd22d262765beff1f5e35b0280697d2ce4b Mon Sep 17 00:00:00 2001 From: Kotaro Yoshimoto Date: Tue, 6 Aug 2024 01:14:19 +0900 Subject: [PATCH] =?UTF-8?q?=E5=86=86=E5=BC=A7=E3=83=87=E3=83=95=E3=82=A7?= =?UTF-8?q?=E3=83=B3=E3=82=B9=20(#516)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../src/visualization_data_handler.cpp | 66 +++++----- .../include/crane_robot_skills/goal_kick.hpp | 1 + .../include/crane_robot_skills/redirect.hpp | 4 +- crane_robot_skills/src/attacker.cpp | 23 +++- crane_robot_skills/src/marker.cpp | 6 +- crane_robot_skills/src/steal_ball.cpp | 18 ++- crane_robot_skills/src/sub_attacker.cpp | 6 +- .../attacker_skill_planner.hpp | 3 + .../defender_planner.hpp | 53 +------- .../src/defender_planner.cpp | 121 +++++++++++++++++- .../src/marker_planner.cpp | 81 +++++++----- .../config/play_situation/ATTACK_TEST.yaml | 6 +- .../config/play_situation/SIMPLE_ATTACK.yaml | 4 +- .../config/play_situation/STOP.yaml | 2 +- .../crane_basics/geometry_operations.hpp | 52 +++++++- .../consai_visualizer_wrapper.hpp | 6 +- .../world_model_wrapper.hpp | 17 ++- .../src/world_model_wrapper.cpp | 20 +-- 18 files changed, 336 insertions(+), 153 deletions(-) diff --git a/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp b/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp index 0e9be1152..703b39309 100644 --- a/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp +++ b/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp @@ -57,7 +57,7 @@ void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::Share vis_ball.fill_color.alpha = 0.7; vis_ball.line_size = 1; vis_ball.radius = 0.0215; - vis_ball.caption = cam_id; + // vis_ball.caption = cam_id; for (const auto & ball : msg->balls) { vis_ball.center.x = ball.x * 0.001; @@ -71,7 +71,7 @@ void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::Share vis_robot.fill_color.name = "dodgerblue"; vis_robot.fill_color.alpha = 0.7; vis_robot.line_size = 1; - vis_robot.caption = cam_id; + // vis_robot.caption = cam_id; for (const auto & robot : msg->robots_blue) { if (robot.robot_id.size() <= 0) { continue; @@ -126,7 +126,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt line.p1.y = field_line.p1.y * 0.001; line.p2.x = field_line.p2.x * 0.001; line.p2.y = field_line.p2.y * 0.001; - line.caption = field_line.name; + // line.caption = field_line.name; vis_objects->lines.push_back(line); } @@ -142,7 +142,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt arc.radius = field_arc.radius * 0.001; arc.start_angle = field_arc.a1; arc.end_angle = field_arc.a2; - arc.caption = field_arc.name; + // arc.caption = field_arc.name; vis_objects->arcs.push_back(arc); } @@ -154,11 +154,11 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt point.size = 6; point.x = -msg->field.field_length * 0.001 / 2.0 + 8.0; point.y = 0.0; - point.caption = "penalty_mark_positive"; + // point.caption = "penalty_mark_positive"; vis_objects->points.push_back(point); point.x = -point.x; - point.caption = "penalty_mark_negative"; + // point.caption = "penalty_mark_negative"; vis_objects->points.push_back(point); // フィールドの枠 @@ -170,7 +170,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt rect.center.y = 0.0; rect.width = (msg->field.field_length + msg->field.boundary_width * 2) * 0.001; rect.height = (msg->field.field_width + msg->field.boundary_width * 2) * 0.001; - rect.caption = "wall"; + // rect.caption = "wall"; vis_objects->rects.push_back(rect); pub_vis_objects_->publish(std::move(vis_objects)); @@ -217,7 +217,7 @@ TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFra ball_vel.p1.y = ball.pos.y; ball_vel.p2.x = ball.pos.x + ball.vel[0].x; ball_vel.p2.y = ball.pos.y + ball.vel[0].y; - ball_vel.caption = std::to_string(vel_norm); + // ball_vel.caption = std::to_string(vel_norm); vis_objects->lines.push_back(ball_vel); } } @@ -243,31 +243,31 @@ TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFra vis_objects->robots.push_back(vis_robot); // 速度を描画 - if (robot.vel.size() > 0 && robot.vel_angular.size() > 0) { - const double vel_norm = std::hypot(robot.vel[0].x, robot.vel[0].y); - VisLine robot_vel; - // 直進速度 - robot_vel.color.name = "gold"; - robot_vel.color.alpha = VELOCITY_ALPHA; - robot_vel.size = 2; - robot_vel.p1.x = robot.pos.x; - robot_vel.p1.y = robot.pos.y; - robot_vel.p2.x = robot.pos.x + robot.vel[0].x; - robot_vel.p2.y = robot.pos.y + robot.vel[0].y; - robot_vel.caption = std::to_string(vel_norm); - vis_objects->lines.push_back(robot_vel); - - // 角速度 - const double vel_angular_norm = std::fabs(robot.vel_angular[0]); - robot_vel.color.name = "crimson"; - robot_vel.color.alpha = VELOCITY_ALPHA; - robot_vel.p1.x = robot.pos.x; - robot_vel.p1.y = robot.pos.y; - robot_vel.p2.x = robot.pos.x + robot.vel_angular[0]; - robot_vel.p2.y = robot.pos.y; - robot_vel.caption = std::to_string(vel_angular_norm); - vis_objects->lines.push_back(robot_vel); - } + // if (robot.vel.size() > 0 && robot.vel_angular.size() > 0) { + // const double vel_norm = std::hypot(robot.vel[0].x, robot.vel[0].y); + // VisLine robot_vel; + // // 直進速度 + // robot_vel.color.name = "gold"; + // robot_vel.color.alpha = VELOCITY_ALPHA; + // robot_vel.size = 2; + // robot_vel.p1.x = robot.pos.x; + // robot_vel.p1.y = robot.pos.y; + // robot_vel.p2.x = robot.pos.x + robot.vel[0].x; + // robot_vel.p2.y = robot.pos.y + robot.vel[0].y; + // robot_vel.caption = std::to_string(vel_norm); + // vis_objects->lines.push_back(robot_vel); + // + // // 角速度 + // const double vel_angular_norm = std::fabs(robot.vel_angular[0]); + // robot_vel.color.name = "crimson"; + // robot_vel.color.alpha = VELOCITY_ALPHA; + // robot_vel.p1.x = robot.pos.x; + // robot_vel.p1.y = robot.pos.y; + // robot_vel.p2.x = robot.pos.x + robot.vel_angular[0]; + // robot_vel.p2.y = robot.pos.y; + // robot_vel.caption = std::to_string(vel_angular_norm); + // vis_objects->lines.push_back(robot_vel); + // } } pub_vis_objects_->publish(std::move(vis_objects)); diff --git a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp index 7a11820dd..9a8f3d0d1 100644 --- a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp @@ -35,6 +35,7 @@ class GoalKick : public SkillBase world_model()); Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5; + visualizer->addLine(world_model()->ball.pos, target, 2, "red"); kick_skill.setParameter("target", target); kick_skill.setParameter("dot_threshold", getParameter("dot_threshold")); return kick_skill.run(visualizer); diff --git a/crane_robot_skills/include/crane_robot_skills/redirect.hpp b/crane_robot_skills/include/crane_robot_skills/redirect.hpp index 70754ea33..2ea197f30 100644 --- a/crane_robot_skills/include/crane_robot_skills/redirect.hpp +++ b/crane_robot_skills/include/crane_robot_skills/redirect.hpp @@ -25,7 +25,7 @@ class Redirect : public SkillBase setParameter("policy", std::string("closest")); } - Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override + Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override { auto interception_point = getInterceptionPoint(); auto target_angle = getRobotAngle( @@ -38,6 +38,8 @@ class Redirect : public SkillBase } else { command.kickStraight(getParameter("kick_power")); } + + visualizer->addLine(robot()->pose.pos, getParameter("redirect_target"), 1, "red"); return Status::RUNNING; } diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp index 74e3f70fd..1ee1add3b 100644 --- a/crane_robot_skills/src/attacker.cpp +++ b/crane_robot_skills/src/attacker.cpp @@ -20,6 +20,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) redirect_skill(base), steal_ball_skill(base) { + receive_skill.setParameter("policy", std::string("min_slack")); setParameter("receiver_id", 0); addStateFunction( AttackerState::ENTRY_POINT, @@ -113,16 +114,19 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) }); addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool { - return not world_model()->isOurBallByBallOwnerCalculator() && world_model()->ball.isMoving(0.2); + return not world_model()->isOurBallByBallOwnerCalculator() && + world_model()->ball.isMoving(0.2) && + world_model()->ball.isMovingTowards(robot()->pose.pos); }); addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool { - return world_model()->isOurBallByBallOwnerCalculator(); + return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2); }); addStateFunction( AttackerState::CUT_THEIR_PASS, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5); return receive_skill.run(visualizer); }); @@ -142,7 +146,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addStateFunction( AttackerState::STEAL_BALL, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0); return steal_ball_skill.run(visualizer); }); @@ -192,8 +197,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) return intersection_points.front(); } }(); + redirect_skill.setParameter("redirect_target", target); - redirect_skill.setParameter("policy", std::string("closest")); + redirect_skill.setParameter("policy", std::string("max_slack")); redirect_skill.setParameter("kick_power", 0.8); return redirect_skill.run(visualizer); }); @@ -280,7 +286,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addStateFunction( AttackerState::STANDARD_PASS, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { auto our_robots = world_model()->ours.getAvailableRobots(robot()->id); // TODO(HansRobo): しっかりパス先を選定する // int receiver_id = getParameter("receiver_id"); @@ -318,6 +324,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) best_target = target; } } + + visualizer->addLine(world_model()->ball.pos, best_target, 1, "red"); + kick_skill.setParameter("target", best_target); Segment ball_to_target{world_model()->ball.pos, best_target}; auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment( @@ -348,7 +357,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base) addStateFunction( AttackerState::LOW_CHANCE_GOAL_KICK, - [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { + [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status { return goal_kick_skill.run(visualizer); }); diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp index e94a70a89..2f035d2f8 100644 --- a/crane_robot_skills/src/marker.cpp +++ b/crane_robot_skills/src/marker.cpp @@ -16,7 +16,7 @@ Marker::Marker(RobotCommandWrapperBase::SharedPtr & base) setParameter("mark_mode", std::string("save_goal")); } -Status Marker::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) +Status Marker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) { auto marked_robot = world_model()->getTheirRobot(getParameter("marking_robot_id")); auto enemy_pos = marked_robot->pose.pos; @@ -37,6 +37,10 @@ Status Marker::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr throw std::runtime_error("unknown mark mode"); } command.setTargetPosition(marking_point, target_theta); + + visualizer->addCircle(enemy_pos, 0.3, 1, "black", ""); + visualizer->addLine( + robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black"); return Status::RUNNING; } } // namespace crane::skills diff --git a/crane_robot_skills/src/steal_ball.cpp b/crane_robot_skills/src/steal_ball.cpp index d6211677f..061297279 100644 --- a/crane_robot_skills/src/steal_ball.cpp +++ b/crane_robot_skills/src/steal_ball.cpp @@ -50,19 +50,29 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base) command.disableBallAvoidance(); command.disableCollisionAvoidance(); const auto method = getParameter("steal_method"); + auto their_frontier = world_model()->getNearestRobotWithDistanceFromPoint( + world_model()->ball.pos, world_model()->theirs.getAvailableRobots()); if (method == "front") { command.setDribblerTargetPosition( world_model()->ball.pos, getAngle(world_model()->ball.pos - robot()->pose.pos)); command.dribble(0.5); } else if (method == "side") { + command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) { + command.setDribblerTargetPosition( + world_model()->ball.pos + + getVerticalVec(world_model()->ball.pos - robot()->pose.pos) * 0.3); // ロボット半径より近くに来れば急回転して刈り取れる - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2); + // command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2); } else { - command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos)); + command.setDribblerTargetPosition(world_model()->ball.pos); + } + if ( + world_model()->getTheirFrontier().has_value() && + robot()->getDistance(world_model()->ball.pos) < + world_model()->getTheirFrontier()->robot->getDistance(world_model()->ball.pos)) { + command.kickWithChip(0.5); } - // thetaに依存するので後置 - command.setDribblerTargetPosition(world_model()->ball.pos); } return Status::RUNNING; }); diff --git a/crane_robot_skills/src/sub_attacker.cpp b/crane_robot_skills/src/sub_attacker.cpp index 2093d962c..9f4c8f7e8 100644 --- a/crane_robot_skills/src/sub_attacker.cpp +++ b/crane_robot_skills/src/sub_attacker.cpp @@ -77,9 +77,9 @@ Status SubAttacker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer double best_score = 0.0; for (const auto & dpps_point : dpps_points) { double score = getPointScore(dpps_point, world_model()->ball.pos, world_model()); - visualizer->addPoint( - dpps_point.x(), dpps_point.y(), std::clamp(static_cast(score * 100), 0, 20), "blue", - 1.); + // visualizer->addPoint( + // dpps_point.x(), dpps_point.y(), std::clamp(static_cast(score * 100), 0, 20), "blue", + // 1.); if (score > best_score) { best_score = score; diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp index 1bbb723ea..6c960306a 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp @@ -45,6 +45,9 @@ class AttackerSkillPlanner : public PlannerBase if (not skill) { return {PlannerBase::Status::RUNNING, {}}; } else { + std::string state_name(magic_enum::enum_name(skill->getCurrentState())); + visualizer->addCircle( + skill->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name); auto status = skill->run(visualizer); return {static_cast(status), {skill->getRobotCommand()}}; } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp index 92c532dac..16b8aa6ac 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp @@ -39,57 +39,10 @@ class DefenderPlanner : public PlannerBase std::pair> calculateRobotCommand( const std::vector & robots) override; - // defense_pointを中心にrobot_num台のロボットをdefense_line上に等間隔に配置する - std::vector getDefensePoints(const int robot_num, const Segment & ball_line) const - { - const double DEFENSE_INTERVAL = 0.2; - std::vector defense_points; - - if (auto defense_parameter = getDefenseLinePointParameter(ball_line)) { - double upper_parameter, lower_parameter; - - auto add_parameter = [&](double parameter) -> bool { - const double OFFSET_X = 0.1, OFFSET_Y = 0.1; - auto [threshold1, threshold2, threshold3] = - getDefenseLinePointParameterThresholds(OFFSET_X, OFFSET_Y); - if (parameter < 0. || parameter > threshold3) { - return false; - } else { - if (upper_parameter < parameter) { - upper_parameter = parameter; - } - if (lower_parameter > parameter) { - lower_parameter = parameter; - } - defense_points.push_back(getDefenseLinePoint(parameter)); - return true; - } - }; - // 1台目 - upper_parameter = *defense_parameter; - lower_parameter = *defense_parameter; - add_parameter(*defense_parameter); - - // 2台目以降 - for (int i = 0; i < robot_num - 1; i++) { - if (i % 2 == 0) { - // upper側に追加 - if (not add_parameter(upper_parameter + DEFENSE_INTERVAL)) { - // だめならlower側 - add_parameter(lower_parameter - DEFENSE_INTERVAL); - } - } else { - // lower側に追加 - if (not add_parameter(lower_parameter - DEFENSE_INTERVAL)) { - // だめならupper側 - add_parameter(upper_parameter + DEFENSE_INTERVAL); - } - } - } - } + std::vector getDefenseArcPoints(const int robot_num, const Segment & ball_line) const; - return defense_points; - } + // defense_pointを中心にrobot_num台のロボットをdefense_line上に等間隔に配置する + std::vector getDefenseLinePoints(const int robot_num, const Segment & ball_line) const; auto getDefenseLinePoint(double parameter) const -> Point { diff --git a/session/crane_planner_plugins/src/defender_planner.cpp b/session/crane_planner_plugins/src/defender_planner.cpp index ae2369eee..f08a1895b 100644 --- a/session/crane_planner_plugins/src/defender_planner.cpp +++ b/session/crane_planner_plugins/src/defender_planner.cpp @@ -30,12 +30,21 @@ DefenderPlanner::calculateRobotCommand(const std::vector & robo auto intersections = getIntersections(ball_line, goal_line); if (intersections.empty()) { // シュートがなければ通常の動き - ball_line.first = world_model->goal; + ball_line.first = world_model->getOurGoalCenter(); ball_line.second = ball; } } - std::vector defense_points = getDefensePoints(robots.size(), ball_line); + std::vector defense_points = [&]() { + // フィールド横幅の半分よりボールが遠ければ円弧守備に移行 + if ( + world_model->getDistanceFromBall(world_model->getOurGoalCenter()) < + world_model->field_size.y() * 0.5) { + return getDefenseLinePoints(robots.size(), ball_line); + } else { + return getDefenseArcPoints(robots.size(), ball_line); + } + }(); if (not defense_points.empty()) { std::vector robot_points; @@ -87,4 +96,112 @@ DefenderPlanner::calculateRobotCommand(const std::vector & robo return {PlannerBase::Status::RUNNING, robot_commands}; } } + +std::vector DefenderPlanner::getDefenseArcPoints( + const int robot_num, const Segment & ball_line) const +{ + const double DEFENSE_INTERVAL = 0.4; + const double RADIUS_OFFSET = 0.4; + std::vector defense_points; + // ペナルティエリアの一番遠い点を通る円の半径 + const double RADIUS = + std::hypot(world_model->penalty_area_size.x(), world_model->penalty_area_size.y() * 0.5) + + RADIUS_OFFSET; + // r * theta = interval + // theta = interval / e + const double ANGLE_INTERVAL = DEFENSE_INTERVAL / RADIUS; + + auto defense_point = [&]() -> Point { + Circle circle; + circle.center = world_model->getOurGoalCenter(); + circle.radius = RADIUS; + auto intersections = getIntersections(circle, ball_line); + switch (static_cast(intersections.size())) { + case 0: { + // ボールの進行方向がこちらを向いていないときは、中間地点に潜り込む + return world_model->getOurGoalCenter() + + (world_model->ball.pos - world_model->getOurGoalCenter()).normalized() * RADIUS; + } + case 1: { + return intersections[0]; + } + default: { + // ボールに一番近い交点を返す + double min_distance = std::numeric_limits::max(); + Point best_intersection = + world_model->getOurGoalCenter() + + (world_model->ball.pos - world_model->getOurGoalCenter()).normalized() * RADIUS; + for (auto & intersection : intersections) { + double distance = (world_model->ball.pos, intersection).norm(); + if (distance < min_distance) { + min_distance = distance; + best_intersection = intersection; + } + } + return best_intersection; + } + } + }(); + + double defense_angle = getAngle(defense_point - world_model->getOurGoalCenter()); + for (int i = 0; i < robot_num; i++) { + double normalized_angle_offset = (robot_num - i - 1) / 2.; + defense_points.emplace_back( + world_model->getOurGoalCenter() + + getNormVec(defense_angle + ANGLE_INTERVAL * normalized_angle_offset) * RADIUS); + } + return defense_points; +} + +std::vector DefenderPlanner::getDefenseLinePoints( + const int robot_num, const Segment & ball_line) const +{ + const double DEFENSE_INTERVAL = 0.2; + std::vector defense_points; + + if (auto defense_parameter = getDefenseLinePointParameter(ball_line)) { + double upper_parameter, lower_parameter; + + auto add_parameter = [&](double parameter) -> bool { + const double OFFSET_X = 0.1, OFFSET_Y = 0.1; + auto [threshold1, threshold2, threshold3] = + getDefenseLinePointParameterThresholds(OFFSET_X, OFFSET_Y); + if (parameter < 0. || parameter > threshold3) { + return false; + } else { + if (upper_parameter < parameter) { + upper_parameter = parameter; + } + if (lower_parameter > parameter) { + lower_parameter = parameter; + } + defense_points.push_back(getDefenseLinePoint(parameter)); + return true; + } + }; + // 1台目 + upper_parameter = *defense_parameter; + lower_parameter = *defense_parameter; + add_parameter(*defense_parameter); + + // 2台目以降 + for (int i = 0; i < robot_num - 1; i++) { + if (i % 2 == 0) { + // upper側に追加 + if (not add_parameter(upper_parameter + DEFENSE_INTERVAL)) { + // だめならlower側 + add_parameter(lower_parameter - DEFENSE_INTERVAL); + } + } else { + // lower側に追加 + if (not add_parameter(lower_parameter - DEFENSE_INTERVAL)) { + // だめならupper側 + add_parameter(upper_parameter + DEFENSE_INTERVAL); + } + } + } + } + + return defense_points; +} } // namespace crane diff --git a/session/crane_planner_plugins/src/marker_planner.cpp b/session/crane_planner_plugins/src/marker_planner.cpp index b31d10761..adf195871 100644 --- a/session/crane_planner_plugins/src/marker_planner.cpp +++ b/session/crane_planner_plugins/src/marker_planner.cpp @@ -32,44 +32,59 @@ auto MarkerPlanner::getSelectedRobots( skill_map.clear(); std::vector selected_robots; // 味方ゴールに近い敵ロボットをselectable_robots_num台選択 - std::vector, double>> robots_and_distances; - for (auto enemy_robot : world_model->theirs.getAvailableRobots()) { - robots_and_distances.emplace_back( - enemy_robot, std::abs(world_model->goal.x() - enemy_robot->pose.pos.x())); + std::vector, double>> robots_and_goal_angles; + for (const auto & enemy_robot : world_model->theirs.getAvailableRobots()) { + auto [best_angle, angle_width] = + world_model->getLargestOurGoalAngleRangeFromPoint(enemy_robot->pose.pos); + robots_and_goal_angles.emplace_back(enemy_robot, angle_width); } - std::sort(robots_and_distances.begin(), robots_and_distances.end(), [&](auto & a, auto & b) { - return a.second < b.second; + std::sort(robots_and_goal_angles.begin(), robots_and_goal_angles.end(), [&](auto & a, auto & b) { + // ゴールへの角度が大きいほど選択優先度が高い + return a.second > b.second; }); for (int i = 0; i < selectable_robots_num; i++) { - auto enemy_robot = robots_and_distances.at(i).first; - // マークする敵ロボットに一番近い味方ロボットを選択 - double min_distance = 1000000.0; - uint8_t min_index = 0; - for (size_t j = 0; j < selectable_robots.size(); j++) { - double distance = - world_model->getOurRobot(selectable_robots[j])->getDistance(enemy_robot->pose.pos); - if ( - distance < min_distance && - std::count(selected_robots.begin(), selected_robots.end(), selectable_robots[j]) == 0) { - min_distance = distance; - min_index = j; - } - } - marking_target_map[selectable_robots[min_index]] = enemy_robot->id; - selected_robots.push_back(selectable_robots[min_index]); - auto marker_base = std::make_shared( - "marker_planner", selectable_robots[min_index], world_model); - skill_map.emplace(selectable_robots[min_index], std::make_shared(marker_base)); - skill_map[selectable_robots[min_index]]->setParameter("marking_robot_id", enemy_robot->id); - if ((world_model->ball.pos - world_model->goal).norm() > 6.0) { - skill_map[selectable_robots[min_index]]->setParameter( - "mark_mode", std::string("intercept_pass")); - skill_map[selectable_robots[min_index]]->setParameter("mark_distance", 0.5); + auto enemy_robot = robots_and_goal_angles.at(i).first; + if (not world_model->point_checker.isInOurHalf(enemy_robot->pose.pos)) { + // 相手コートにいる敵ロボットはマークしない + continue; + } else if (robots_and_goal_angles.at(i).second < 3.0 * M_PI / 180.) { + // シュートコースが狭い場合はマークしない + continue; + } else if ((enemy_robot->pose.pos - world_model->ball.pos).norm() < 1.0) { + // ボールに近い敵ロボットはマークしない + continue; } else { - skill_map[selectable_robots[min_index]]->setParameter("mark_mode", std::string("save_goal")); - double distance = (world_model->goal - enemy_robot->pose.pos).norm() * 0.1 + 0.2; - skill_map[selectable_robots[min_index]]->setParameter("mark_distance", distance); + // マークする敵ロボットに一番近い味方ロボットを選択 + double min_distance = 1000000.0; + uint8_t min_index = 0; + for (size_t j = 0; j < selectable_robots.size(); j++) { + double distance = + world_model->getOurRobot(selectable_robots[j])->getDistance(enemy_robot->pose.pos); + if ( + distance < min_distance && + std::count(selected_robots.begin(), selected_robots.end(), selectable_robots[j]) == 0) { + min_distance = distance; + min_index = j; + } + } + marking_target_map[selectable_robots[min_index]] = enemy_robot->id; + selected_robots.push_back(selectable_robots[min_index]); + auto marker_base = std::make_shared( + "marker_planner", selectable_robots[min_index], world_model); + skill_map.emplace( + selectable_robots[min_index], std::make_shared(marker_base)); + skill_map[selectable_robots[min_index]]->setParameter("marking_robot_id", enemy_robot->id); + if ((world_model->ball.pos - enemy_robot->pose.pos).norm() > 3.0) { + skill_map[selectable_robots[min_index]]->setParameter( + "mark_mode", std::string("intercept_pass")); + skill_map[selectable_robots[min_index]]->setParameter("mark_distance", 0.5); + } else { + skill_map[selectable_robots[min_index]]->setParameter( + "mark_mode", std::string("save_goal")); + double distance = (world_model->goal - enemy_robot->pose.pos).norm() * 0.1 + 0.2; + skill_map[selectable_robots[min_index]]->setParameter("mark_distance", distance); + } } } diff --git a/session/crane_session_controller/config/play_situation/ATTACK_TEST.yaml b/session/crane_session_controller/config/play_situation/ATTACK_TEST.yaml index 49ff2b187..9bfc47f9b 100644 --- a/session/crane_session_controller/config/play_situation/ATTACK_TEST.yaml +++ b/session/crane_session_controller/config/play_situation/ATTACK_TEST.yaml @@ -5,9 +5,11 @@ sessions: capacity: 1 - name: attacker_skill capacity: 1 + - name: defender + capacity: 2 + - name: marker + capacity: 2 - name: sub_attacker_skill capacity: 1 - - name: defender - capacity: 4 - name: waiter capacity: 20 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 01f56c971..6a0ffc3a6 100644 --- a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml +++ b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml @@ -8,6 +8,8 @@ sessions: - name: sub_attacker_skill capacity: 1 - name: defender - capacity: 4 + capacity: 2 + - name: marker + capacity: 2 - name: waiter capacity: 20 diff --git a/session/crane_session_controller/config/play_situation/STOP.yaml b/session/crane_session_controller/config/play_situation/STOP.yaml index bd7a42633..49a907411 100644 --- a/session/crane_session_controller/config/play_situation/STOP.yaml +++ b/session/crane_session_controller/config/play_situation/STOP.yaml @@ -6,6 +6,6 @@ sessions: - name: ball_nearby_positioner_skill capacity: 2 - name: defender - capacity: 3 + capacity: 2 - name: formation capacity: 20 diff --git a/utility/crane_basics/include/crane_basics/geometry_operations.hpp b/utility/crane_basics/include/crane_basics/geometry_operations.hpp index 03ef38a25..c415a2e30 100644 --- a/utility/crane_basics/include/crane_basics/geometry_operations.hpp +++ b/utility/crane_basics/include/crane_basics/geometry_operations.hpp @@ -102,10 +102,58 @@ inline auto getReachTime(double distance, double v0, double acc, double max_vel) } } -inline auto getIntersections(const Segment & line1, const Segment & line2) -> std::vector +inline auto getIntersections(const Segment & segment1, const Segment & segment2) + -> std::vector { std::vector intersections; - bg::intersection(line1, line2, intersections); + bg::intersection(segment1, segment2, intersections); + return intersections; +} + +inline auto getIntersections(const Circle & circle, const Segment & segment) -> std::vector +{ + std::vector intersections; + double distance = bg::distance(circle, segment); + if (distance > circle.radius) { + // 交差しない + return intersections; + } else { + // 交差する + // 交点を求める + Vector2 norm_vec = getVerticalVec(segment.second - segment.first).normalized(); + if ( + ((circle.center + norm_vec) - segment.first).norm() > + ((circle.center - norm_vec) - segment.first).norm()) { + norm_vec = -norm_vec; + } + double d = sqrt(circle.radius * circle.radius - distance * distance); + Vector2 seg_norm = (segment.second - segment.first).normalized(); + Point p1 = circle.center + norm_vec * distance + seg_norm * d; + Point p2 = circle.center + norm_vec * distance - seg_norm * d; + + // 交点が線分上にあるか確認 + if ( + (p1 - segment.first).dot(segment.second - segment.first) > 0 && + (p1 - segment.second).dot(segment.first - segment.second) > 0) { + intersections.push_back(p1); + } + + if ( + (p2 - segment.first).dot(segment.second - segment.first) > 0 && + (p2 - segment.second).dot(segment.first - segment.second) > 0) { + intersections.push_back(p2); + } + + return intersections; + } +} + +template +inline auto getIntersections(const Geometry1 & geometry1, const Geometry2 & geometry2) + -> std::vector +{ + std::vector intersections; + bg::intersection(geometry1, geometry2, intersections); return intersections; } diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp index d71a7db16..92ba47c4b 100644 --- a/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp +++ b/utility/crane_msg_wrappers/include/crane_msg_wrappers/consai_visualizer_wrapper.hpp @@ -730,7 +730,11 @@ struct ConsaiVisualizerWrapper circle.line_color.name = line_color; circle.line_color.alpha = alpha; circle.fill_color.name = fill_color; - circle.fill_color.alpha = alpha; + if (fill_color == "") { + circle.fill_color.alpha = 0.0; + } else { + circle.fill_color.alpha = alpha; + } circle.line_size = line_size; circle.caption = caption; latest_msg.circles.push_back(circle); 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 3f54a3b4e..e6d7a3164 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 @@ -225,11 +225,12 @@ struct WorldModelWrapper -> std::optional; [[nodiscard]] auto getMinMaxSlackInterceptPoint( - std::vector> robots, double t_horizon = 5.0, double t_step = 0.1) - -> std::pair, std::optional>; + std::vector> robots, double t_horizon = 5.0, double t_step = 0.1, + double slack_time_offset = 0.0) -> std::pair, std::optional>; [[nodiscard]] auto getMinMaxSlackInterceptPointAndSlackTime( - std::vector> robots, double t_horizon = 5.0, double t_step = 0.1) + std::vector> robots, double t_horizon = 5.0, double t_step = 0.1, + double slack_time_offset = 0.0) -> std::pair>, std::optional>>; TeamInfo ours; @@ -413,6 +414,16 @@ struct WorldModelWrapper [this, offset](const Point & p) { return not isPenaltyArea(p, offset); }); } + [[nodiscard]] bool isInOurHalf(const Point & p, double offset = 0.) const + { + return p.x() * world_model->getOurSideSign() > offset; + } + + void addInOurHalfChecker(double offset = 0.) + { + checkers.emplace_back([this, offset](const Point & p) { return isInOurHalf(p, offset); }); + } + enum class Rule { EQUAL_TO, NOT_EQUAL_TO, diff --git a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp index 382dcf3d0..08a6c33e6 100644 --- a/utility/crane_msg_wrappers/src/world_model_wrapper.cpp +++ b/utility/crane_msg_wrappers/src/world_model_wrapper.cpp @@ -358,7 +358,8 @@ auto WorldModelWrapper::getLargestOurGoalAngleRangeFromPoint( } auto WorldModelWrapper::getMinMaxSlackInterceptPointAndSlackTime( - std::vector> robots, double t_horizon, double t_step) + std::vector> robots, double t_horizon, double t_step, + double slack_time_offset) -> std::pair>, std::optional>> { auto ball_sequence = getBallSequence(t_horizon, t_step, ball.pos, ball.vel); @@ -373,14 +374,14 @@ auto WorldModelWrapper::getMinMaxSlackInterceptPointAndSlackTime( } if (const auto slack = getBallSlackTime(t_ball, robots); slack.has_value()) { - auto slack_time = slack.value().slack_time; + auto slack_time = slack.value().slack_time + slack_time_offset; auto intercept_point = slack.value().intercept_point; - if (slack_time > max_slack_time) { + if (slack_time > max_slack_time && slack_time > 0.) { max_slack_time = slack_time; max_intercept_point_and_time = std::make_optional>({intercept_point, slack_time}); } - if (slack_time < min_slack_time) { + if (slack_time < min_slack_time && slack_time > 0.) { min_slack_time = slack_time; min_intercept_point_and_time = std::make_optional>({intercept_point, slack_time}); @@ -394,10 +395,11 @@ auto WorldModelWrapper::getMinMaxSlackInterceptPointAndSlackTime( } auto WorldModelWrapper::getMinMaxSlackInterceptPoint( - std::vector> robots, double t_horizon, double t_step) - -> std::pair, std::optional> + std::vector> robots, double t_horizon, double t_step, + double slack_time_offset) -> std::pair, std::optional> { - auto [min_slack, max_slack] = getMinMaxSlackInterceptPointAndSlackTime(robots, t_horizon, t_step); + auto [min_slack, max_slack] = + getMinMaxSlackInterceptPointAndSlackTime(robots, t_horizon, t_step, slack_time_offset); std::optional min_intercept_point = std::nullopt; std::optional max_intercept_point = std::nullopt; if (min_slack.has_value()) { @@ -493,8 +495,8 @@ auto WorldModelWrapper::BallOwnerCalculator::updateScore(bool our_team) -> void scores.begin(), scores.end(), [&](const auto & e) { return e.robot->id == previous_sorted_robots.front().robot->id; }); previous_best_bot != scores.end()) { - // Slackタイム0.5秒のアドバンテージ - previous_best_bot->score += 0.5; + // Slackタイム1.0秒のアドバンテージ + previous_best_bot->score += 1.0; } }