diff --git a/crane_local_planner/include/crane_local_planner/simple_avoid_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_avoid_planner.hpp index 697293267..2a0699fcf 100644 --- a/crane_local_planner/include/crane_local_planner/simple_avoid_planner.hpp +++ b/crane_local_planner/include/crane_local_planner/simple_avoid_planner.hpp @@ -125,9 +125,8 @@ class SimpleAvoidPlanner { // 味方ロボットを回避 - for (const auto & r : world_model->ours.getAvailableRobots()) { - if ( - robot->id != r->id && bg::distance(r->geometry(), latest_path) <= r->geometry().radius) { + for (const auto & r : world_model->ours.getAvailableRobots(robot->id)) { + if (bg::distance(r->geometry(), latest_path) <= r->geometry().radius) { auto avoidance_points_tmp = getAvoidancePoints(robot->pose.pos, r->geometry(), r->geometry().radius); avoidance_points.insert( @@ -245,10 +244,8 @@ class SimpleAvoidPlanner // ただし、ゴールからたどっていく。 { // 味方ロボットを回避 - for (const auto & r : world_model->ours.getAvailableRobots()) { - if ( - from_robot->id != r->id && - bg::distance(r->geometry(), latest_path) <= r->geometry().radius) { + for (const auto & r : world_model->ours.getAvailableRobots(from_robot->id)) { + if (bg::distance(r->geometry(), latest_path) <= r->geometry().radius) { auto avoidance_points_tmp = getAvoidancePoints(to, r->geometry(), r->geometry().radius + 0.2); avoidance_points.insert( @@ -433,8 +430,8 @@ class SimpleAvoidPlanner [&](Point p) { Segment path{from, p}; // 味方ロボットとの干渉をチェック - for (const auto & r : world_model->ours.getAvailableRobots()) { - if (r->id != robot->id && bg::distance(r->geometry(), path) <= 0.0) { + for (const auto & r : world_model->ours.getAvailableRobots(robot->id)) { + if (bg::distance(r->geometry(), path) <= 0.0) { return true; } } diff --git a/crane_robot_skills/include/crane_robot_skills/goalie.hpp b/crane_robot_skills/include/crane_robot_skills/goalie.hpp index 1982da8bb..3e0c4adf3 100644 --- a/crane_robot_skills/include/crane_robot_skills/goalie.hpp +++ b/crane_robot_skills/include/crane_robot_skills/goalie.hpp @@ -21,15 +21,11 @@ class Goalie : public SkillBase<> { auto ball = world_model->ball.pos; // パスできるロボットのリストアップ - auto passable_robot_list = world_model->ours.getAvailableRobots(); + auto passable_robot_list = world_model->ours.getAvailableRobots(target.robot->id); 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()) { diff --git a/crane_robot_skills/src/simple_attacker.cpp b/crane_robot_skills/src/simple_attacker.cpp index 8b6a307c4..a830f2f69 100644 --- a/crane_robot_skills/src/simple_attacker.cpp +++ b/crane_robot_skills/src/simple_attacker.cpp @@ -21,12 +21,7 @@ SimpleAttacker::SimpleAttacker(uint8_t id, const std::shared_ptrours.getAvailableRobots(); - our_robots.erase( - std::remove_if( - our_robots.begin(), our_robots.end(), - [&](const auto & our_robot) { return our_robot->id == robot->id; }), - our_robots.end()); + auto our_robots = world_model->ours.getAvailableRobots(robot->id); auto nearest_robot = world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); best_target = nearest_robot.first->pose.pos; diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp index de6e175a6..93da494db 100644 --- a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp +++ b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp @@ -89,6 +89,8 @@ class WorldModelPublisherComponent : public rclcpp::Node Color their_color; + uint8_t our_goalie_id, their_goalie_id; + uint8_t max_id; static constexpr float DISAPPEARED_TIME_THRESH = 3.0f; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index dffd19b9b..bd9ca4846 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -40,9 +40,13 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (msg.yellow.name == team_name) { our_color = Color::YELLOW; their_color = Color::BLUE; + our_goalie_id = msg.yellow.goalkeeper; + their_goalie_id = msg.blue.goalkeeper; } else if (msg.blue.name == team_name) { our_color = Color::BLUE; their_color = Color::YELLOW; + our_goalie_id = msg.blue.goalkeeper; + their_goalie_id = msg.yellow.goalkeeper; } else { std::stringstream what; what << "Cannot find our team name, " << team_name << " in referee message. "; @@ -61,6 +65,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt if (initial_team_color == "BLUE") { our_color = Color::BLUE; their_color = Color::YELLOW; + } else { our_color = Color::YELLOW; their_color = Color::BLUE; @@ -189,6 +194,9 @@ void WorldModelPublisherComponent::publishWorldModel() wm.ball_placement_target.x = ball_placement_target_x; wm.ball_placement_target.y = ball_placement_target_y; + wm.our_goalie_id = our_goalie_id; + wm.their_goalie_id = their_goalie_id; + pub_world_model->publish(wm); } 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 a9bdf10e0..f1c76b7e7 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,7 @@ class AttackerPlanner : public PlannerBase // シュートの隙がないときは仲間へパス 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 our_robots = world_model->ours.getAvailableRobots(robot_id.robot_id); auto nearest_robot = world_model->getNearestRobotsWithDistanceFromPoint(world_model->ball.pos, our_robots); best_target = nearest_robot.first->pose.pos; 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 deleted file mode 100644 index a4c5f9f87..000000000 --- a/session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp +++ /dev/null @@ -1,159 +0,0 @@ -// Copyright (c) 2021 ibis-ssl -// -// Use of this source code is governed by an MIT-style -// license that can be found in the LICENSE file or at -// https://opensource.org/licenses/MIT. - -#ifndef CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ -#define CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include "visibility_control.h" - -namespace crane -{ -class GoaliePlanner : public PlannerBase -{ -public: - COMPOSITION_PUBLIC - explicit GoaliePlanner( - WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) - : PlannerBase("goalie", world_model, visualizer) - { - } - - void emitBallFromPenaltyArea(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.disableBallAvoidance(); - } - target.setTargetTheta(getAngle(pass_target - ball)); - target.disableGoalAreaAvoidance(); - } - - 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::pair> calculateRobotCommand( - const std::vector & robots) override - { - std::vector robot_commands; - for (auto robot_id : robots) { - crane::RobotCommandWrapper target(robot_id.robot_id, world_model); - - 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); - break; - default: - inplay(target, true); - break; - } - - robot_commands.emplace_back(target.getMsg()); - } - return {PlannerBase::Status::RUNNING, robot_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) { - // choose id smaller first - return 15. - static_cast(-robot->id); - }); - } -}; - -} // namespace crane -#endif // CRANE_PLANNER_PLUGINS__GOALIE_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp index 400d11ee9..711ac63e7 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/marker_planner.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include @@ -33,21 +34,11 @@ class MarkerPlanner : public PlannerBase const std::vector & robots) override { std::vector robot_commands; - for (auto robot_id : robots) { - auto robot = world_model->getRobot(robot_id); - auto marked_robot = world_model->getTheirRobot(marking_target_map.at(robot_id.robot_id)); - auto enemy_pos = marked_robot->pose.pos; - constexpr double INTERVAL = 0.5; - - Point marking_point = - enemy_pos + (world_model->getOurGoalCenter() - enemy_pos).normalized() * INTERVAL; - double target_theta = getAngle(enemy_pos - world_model->getOurGoalCenter()); - - crane::RobotCommandWrapper target(robot_id.robot_id, world_model); - target.setTargetPosition(marking_point, target_theta); - - robot_commands.emplace_back(target.getMsg()); + for (auto & [id, value] : skill_map) { + auto & [command, skill] = value; + skill->run(*command, visualizer); + robot_commands.emplace_back(command->getMsg()); } return {PlannerBase::Status::RUNNING, robot_commands}; } @@ -61,6 +52,7 @@ class MarkerPlanner : public PlannerBase } marking_target_map.clear(); + skill_map.clear(); std::vector selected_robots; // 味方ゴールに近い敵ロボットをselectable_robots_num台選択 std::vector, double>> robots_and_distances; @@ -79,7 +71,7 @@ class MarkerPlanner : public PlannerBase uint8_t min_index = 0; for (int j = 0; j < selectable_robots.size(); j++) { double distance = - (enemy_robot->pose.pos - world_model->getOurRobot(selectable_robots[j])->pose.pos).norm(); + world_model->getOurRobot(selectable_robots[j])->getDistance(enemy_robot->pose.pos); if ( distance < min_distance && std::count(selected_robots.begin(), selected_robots.end(), j) == 0) { @@ -89,6 +81,14 @@ class MarkerPlanner : public PlannerBase } marking_target_map[selectable_robots[min_index]] = enemy_robot->id; selected_robots.push_back(selectable_robots[min_index]); + skill_map.emplace( + selectable_robots[min_index], + std::make_pair( + std::make_shared(selectable_robots[min_index], world_model), + std::make_shared(selectable_robots[min_index], world_model))); + skill_map[selectable_robots[min_index]].second->setParameter( + "marking_robot_id", enemy_robot->id); + skill_map[selectable_robots[min_index]].second->setParameter("mark_distance", 0.5); } return selected_robots; @@ -97,6 +97,10 @@ class MarkerPlanner : public PlannerBase private: // key: ID of our robot in charge, value: ID of the enemy marked robot std::unordered_map marking_target_map; + + std::unordered_map< + uint8_t, std::pair, std::shared_ptr>> + skill_map; }; } // namespace crane diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp index 41c52a5bc..396979d98 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp @@ -25,12 +25,12 @@ class OurKickOffPlanner : public PlannerBase { private: std::shared_ptr kickoff_attack; + std::shared_ptr attacker_command; std::shared_ptr kickoff_support; - std::shared_ptr supporter_command; - // std::shared_ptr visualizer; + std::shared_ptr supporter_command; public: COMPOSITION_PUBLIC explicit OurKickOffPlanner( diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp index 3d910d12b..df146e3b4 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -10,15 +10,14 @@ #include #include "attacker_planner.hpp" -#include "ball_placement_planner.hpp" -#include "ball_placement_with_skill_planner.hpp" #include "defender_planner.hpp" #include "formation_planner.hpp" -#include "goalie_planner.hpp" #include "marker_planner.hpp" #include "our_kickoff_planner.hpp" #include "receive_planner.hpp" #include "skill_planner.hpp" +#include "temporary/ball_placement_planner.hpp" +#include "temporary/ball_placement_with_skill_planner.hpp" #include "tigers_goalie_planner.hpp" #include "waiter_planner.hpp" @@ -33,12 +32,12 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase: return std::make_unique(ts...); } else if (planner_name == "ball_placement_with_skill") { return std::make_unique(ts...); + } else if (planner_name == "ball_placement_skill") { + return std::make_unique(ts...); } else if (planner_name == "defender") { return std::make_unique(ts...); } else if (planner_name == "formation") { return std::make_unique(ts...); - } else if (planner_name == "goalie") { - return std::make_unique(ts...); } else if (planner_name == "goalie_skill") { return std::make_unique(ts...); } else if (planner_name == "marker") { diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp index 3299e89a4..c4af29e69 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/skill_planner.hpp @@ -57,6 +57,91 @@ namespace crane } \ } -DEFINE_SKILL_PLANNER(Goalie); +// DEFINE_SKILL_PLANNER(Goalie); + +class GoalieSkillPlanner : public PlannerBase +{ +public: + std ::shared_ptr skill = nullptr; + + std ::shared_ptr robot_command_wrapper = nullptr; + + COMPOSITION_PUBLIC explicit GoalieSkillPlanner( + WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) + : PlannerBase("Goalie", world_model, visualizer) + { + } + + std ::pair> calculateRobotCommand( + const std ::vector & robots) override + { + if (not skill or not robot_command_wrapper) { + return {PlannerBase ::Status ::RUNNING, {}}; + } else { + std ::vector robot_commands; + auto status = skill->run(*robot_command_wrapper, visualizer); + return {static_cast(status), {robot_command_wrapper->getMsg()}}; + } + } + auto getSelectedRobots( + uint8_t selectable_robots_num, const std ::vector & selectable_robots) + -> std ::vector override + { + return {world_model->getOurGoalieId()}; + } +}; + +class BallPlacementSkillPlanner : public PlannerBase +{ +public: + std ::shared_ptr skill = nullptr; + + std ::shared_ptr robot_command_wrapper = nullptr; + + COMPOSITION_PUBLIC explicit BallPlacementSkillPlanner( + WorldModelWrapper ::SharedPtr & world_model, ConsaiVisualizerWrapper ::SharedPtr visualizer) + : PlannerBase("BallPlacement", world_model, visualizer) + { + } + + std ::pair> calculateRobotCommand( + const std ::vector & robots) override + { + if (not skill or not robot_command_wrapper) { + return {PlannerBase ::Status ::RUNNING, {}}; + } else { + if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { + skill->setParameter("placement_x", target->x()); + skill->setParameter("placement_y", target->y()); + } + std ::vector robot_commands; + auto status = skill->run(*robot_command_wrapper, visualizer); + return {static_cast(status), {robot_command_wrapper->getMsg()}}; + } + } + + auto getSelectedRobots( + uint8_t selectable_robots_num, const std ::vector & selectable_robots) + -> std ::vector override + { + // ボールに近いロボットを1台選択 + auto selected_robots = this->getSelectedRobotsByScore( + 1, selectable_robots, [this](const std::shared_ptr & robot) { + // ボールに近いほどスコアが高い + return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01); + }); + skill = std ::make_shared(selected_robots.front(), world_model); + + if (auto target = world_model->getBallPlacementTarget(); target.has_value()) { + skill->setParameter("placement_x", target->x()); + skill->setParameter("placement_y", target->y()); + } + + robot_command_wrapper = + std ::make_shared(selected_robots.front(), world_model); + return {selected_robots.front()}; + } +}; + } // namespace crane #endif // CRANE_PLANNER_PLUGINS__SKILL_PLANNER_HPP_ diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp similarity index 99% rename from session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp rename to session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp index 588c854ac..5d07212bc 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_planner.hpp @@ -17,7 +17,7 @@ #include #include -#include "visibility_control.h" +#include "../visibility_control.h" namespace crane { diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp similarity index 99% rename from session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp rename to session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp index 267ed8e9a..e216f7b83 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/ball_placement_with_skill_planner.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/temporary/ball_placement_with_skill_planner.hpp @@ -20,7 +20,7 @@ #include #include -#include "visibility_control.h" +#include "../visibility_control.h" namespace crane { diff --git a/session/crane_session_controller/config/attack.yaml b/session/crane_session_controller/config/attack.yaml index 484394c99..b4cbd654e 100644 --- a/session/crane_session_controller/config/attack.yaml +++ b/session/crane_session_controller/config/attack.yaml @@ -12,3 +12,5 @@ events: session: "formation" - event: "INPLAY" session: "SIMPLE_ATTACK" + - event: "OUR_BALL_PLACEMENT" + session: "OUR_BALL_PLACEMENT" diff --git a/session/crane_session_controller/config/defense.yaml b/session/crane_session_controller/config/defense.yaml index 1d902fec6..285a41147 100644 --- a/session/crane_session_controller/config/defense.yaml +++ b/session/crane_session_controller/config/defense.yaml @@ -15,4 +15,4 @@ events: - event: "INPLAY" session: "defense" - event: "OUR_BALL_PLACEMENT" - session: "ball_placement_with_skill" + session: "OUR_BALL_PLACEMENT" diff --git a/session/crane_session_controller/config/event_config.yaml b/session/crane_session_controller/config/event_config.yaml index 0d86aaa46..b4cbd654e 100644 --- a/session/crane_session_controller/config/event_config.yaml +++ b/session/crane_session_controller/config/event_config.yaml @@ -13,4 +13,4 @@ events: - event: "INPLAY" session: "SIMPLE_ATTACK" - event: "OUR_BALL_PLACEMENT" - session: "ball_placement_with_skill" + session: "OUR_BALL_PLACEMENT" diff --git a/session/crane_session_controller/config/normal.yaml b/session/crane_session_controller/config/normal.yaml index 1110ba54b..6cad54346 100644 --- a/session/crane_session_controller/config/normal.yaml +++ b/session/crane_session_controller/config/normal.yaml @@ -14,3 +14,5 @@ events: session: "OUR_KICKOFF_START" - event: "INPLAY" session: "SIMPLE_ATTACK" + - event: "OUR_BALL_PLACEMENT" + session: "OUR_BALL_PLACEMENT" diff --git a/session/crane_session_controller/config/play_situation/OUR_BALL_PLACEMENT.yaml b/session/crane_session_controller/config/play_situation/OUR_BALL_PLACEMENT.yaml new file mode 100644 index 000000000..f82a8ef17 --- /dev/null +++ b/session/crane_session_controller/config/play_situation/OUR_BALL_PLACEMENT.yaml @@ -0,0 +1,10 @@ +--- +name: OUR_BALL_PLACEMENT +description: OUR_BALL_PLACEMENT +sessions: + - name: "goalie_skill" + capacity: 1 + - name: "ball_placement_skill" + capacity: 1 + - name: "waiter" + capacity: 20 diff --git a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_PREPARATION.yaml b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_PREPARATION.yaml index a9e57110b..b34ec89e4 100644 --- a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_PREPARATION.yaml +++ b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_PREPARATION.yaml @@ -2,7 +2,7 @@ name: OUR_KICKOFF_PREPARATION description: OUR_KICKOFF_PREPARATION sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "formation" capacity: 20 diff --git a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml index ca2e4c93c..5db4d4c12 100644 --- a/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml +++ b/session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml @@ -2,7 +2,7 @@ name: OUR_KICKOFF_START description: OUR_KICKOFF_START sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "our_kickoff" capacity: 2 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 826770650..35e62e00a 100644 --- a/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml +++ b/session/crane_session_controller/config/play_situation/SIMPLE_ATTACK.yaml @@ -2,7 +2,7 @@ name: SIMPLE_ATTACK description: SIMPLE ATTACK sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "attacker" capacity: 1 diff --git a/session/crane_session_controller/config/play_situation/ball_placement_with_skill.yaml b/session/crane_session_controller/config/play_situation/ball_placement_with_skill.yaml index 18d11b789..f9b173d22 100644 --- a/session/crane_session_controller/config/play_situation/ball_placement_with_skill.yaml +++ b/session/crane_session_controller/config/play_situation/ball_placement_with_skill.yaml @@ -2,7 +2,7 @@ name: ball_placement_with_skill description: ball_placement_with_skill sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "ball_placement_with_skill" capacity: 1 diff --git a/session/crane_session_controller/config/play_situation/defense.yaml b/session/crane_session_controller/config/play_situation/defense.yaml index 359c998b1..7bf99f3d9 100644 --- a/session/crane_session_controller/config/play_situation/defense.yaml +++ b/session/crane_session_controller/config/play_situation/defense.yaml @@ -2,7 +2,7 @@ name: defense description: simple defense sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "defender" capacity: 10 diff --git a/session/crane_session_controller/config/play_situation/formation.yaml b/session/crane_session_controller/config/play_situation/formation.yaml index 4a1e912ec..634fd458e 100644 --- a/session/crane_session_controller/config/play_situation/formation.yaml +++ b/session/crane_session_controller/config/play_situation/formation.yaml @@ -2,7 +2,7 @@ name: formation description: formation sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "formation" capacity: 20 diff --git a/session/crane_session_controller/config/play_situation/goalie.yaml b/session/crane_session_controller/config/play_situation/goalie.yaml index ff88e51b1..a7afcd7fe 100644 --- a/session/crane_session_controller/config/play_situation/goalie.yaml +++ b/session/crane_session_controller/config/play_situation/goalie.yaml @@ -2,7 +2,7 @@ name: goalie description: for goalie sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "waiter" capacity: 10 diff --git a/session/crane_session_controller/config/play_situation/mark.yaml b/session/crane_session_controller/config/play_situation/mark.yaml index b977205bf..e2d254a5f 100644 --- a/session/crane_session_controller/config/play_situation/mark.yaml +++ b/session/crane_session_controller/config/play_situation/mark.yaml @@ -2,7 +2,7 @@ name: mark description: mark sessions: - - name: "goalie" + - name: "goalie_skill" capacity: 1 - name: "defender" capacity: 3 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 9b1bcf384..1899e84ce 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 @@ -94,22 +94,22 @@ struct TeamInfo std::vector> robots; - std::vector> getAvailableRobots() + std::vector> getAvailableRobots(uint8_t my_id = 255) { std::vector> available_robots; for (auto robot : robots) { - if (robot->available) { + if (robot->available && robot->id != my_id) { available_robots.emplace_back(robot); } } return available_robots; } - std::vector getAvailableRobotIds() + std::vector getAvailableRobotIds(uint8_t my_id = 255) { std::vector available_robot_ids; for (auto robot : robots) { - if (robot->available) { + if (robot->available && robot->id != my_id) { available_robot_ids.emplace_back(robot->id); } } @@ -469,6 +469,10 @@ struct WorldModelWrapper } } + [[nodiscard]] uint8_t getOurGoalieId() const { return latest_msg.our_goalie_id; } + + [[nodiscard]] uint8_t getTheirGoalieId() const { return latest_msg.their_goalie_id; } + TeamInfo ours; TeamInfo theirs;