diff --git a/crane_bringup/launch/play_switcher.launch.xml b/crane_bringup/launch/play_switcher.launch.xml index dbb8d6c3c..9586ebba6 100644 --- a/crane_bringup/launch/play_switcher.launch.xml +++ b/crane_bringup/launch/play_switcher.launch.xml @@ -4,6 +4,7 @@ + @@ -37,11 +38,13 @@ - + - + + + diff --git a/crane_play_switcher/include/crane_play_switcher/play_switcher.hpp b/crane_play_switcher/include/crane_play_switcher/play_switcher.hpp index b6a020b4a..72295dd47 100644 --- a/crane_play_switcher/include/crane_play_switcher/play_switcher.hpp +++ b/crane_play_switcher/include/crane_play_switcher/play_switcher.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include "visibility_control.h" @@ -90,6 +91,8 @@ class PlaySwitcher : public rclcpp::Node BallAnalyzer ball_analyzer; + std::string team_name = "ibis"; + struct LastCommandChangedState { rclcpp::Time stamp; diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp index b60a05173..372de6dc3 100644 --- a/crane_play_switcher/src/play_switcher.cpp +++ b/crane_play_switcher/src/play_switcher.cpp @@ -25,6 +25,9 @@ PlaySwitcher::PlaySwitcher(const rclcpp::NodeOptions & options) play_situation_pub = create_publisher("/play_situation", 10); + declare_parameter("team_name", "ibis"); + team_name = get_parameter("team_name").as_string(); + decoded_referee_sub = create_subscription( "/referee", 10, [this](const robocup_ssl_msgs::msg::Referee & msg) { referee_callback(msg); }); @@ -97,7 +100,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) //-----------------------------------// // raw command -> crane command std::map command_map; - bool is_yellow = msg.yellow.name == "ibis"; + bool is_yellow = msg.yellow.name == team_name; command_map[Referee::COMMAND_HALT] = PlaySituation::HALT; command_map[Referee::COMMAND_STOP] = PlaySituation::STOP; @@ -125,11 +128,13 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg) play_situation_msg.command == PlaySituation::THEIR_KICKOFF_START or play_situation_msg.command == PlaySituation::THEIR_DIRECT_FREE or play_situation_msg.command == PlaySituation::THEIR_INDIRECT_FREE or - play_situation_msg.command == PlaySituation::THEIR_PENALTY_START or + // 敵PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない + // play_situation_msg.command == PlaySituation::THEIR_PENALTY_START or play_situation_msg.command == PlaySituation::OUR_KICKOFF_START or play_situation_msg.command == PlaySituation::OUR_DIRECT_FREE or - play_situation_msg.command == PlaySituation::OUR_INDIRECT_FREE or - play_situation_msg.command == PlaySituation::OUR_PENALTY_START) { + // 味方PKのINPLAYはOUR_PENALTY_STARTとして実装しているのでINPLAY遷移はしない + // play_situation_msg.command == PlaySituation::OUR_PENALTY_START or + play_situation_msg.command == PlaySituation::OUR_INDIRECT_FREE) { if (0.05 <= (last_command_changed_state.ball_position - world_model->ball.pos).norm()) { next_play_situation = PlaySituation::INPLAY; inplay_command_info.reason = diff --git a/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp new file mode 100644 index 000000000..8078f1f27 --- /dev/null +++ b/crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp @@ -0,0 +1,130 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_ROBOT_SKILLS__PENALTY_KICK_HPP_ +#define CRANE_ROBOT_SKILLS__PENALTY_KICK_HPP_ + +#include +#include +#include +#include +#include + +namespace crane::skills +{ +enum class PenaltyKickState { + PREPARE, + KICK, + DONE, +}; + +class PenaltyKick : public SkillBase +{ +private: + std::optional start_ball_point = std::nullopt; + +public: + explicit PenaltyKick(uint8_t id, const std::shared_ptr & wm) + : SkillBase("PenaltyKick", id, wm, PenaltyKickState::PREPARE) + { + setParameter("start_from_kick", false); + setParameter("prepare_margin", 0.6); + addStateFunction( + PenaltyKickState::PREPARE, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + Point target = world_model->ball.pos; + auto margin = getParameter("prepare_margin"); + target.x() += world_model->getOurGoalCenter().x() > 0 ? margin : -margin; + command->setTargetPosition(target); + command->lookAtBall(); + return Status::RUNNING; + }); + + addTransition(PenaltyKickState::PREPARE, PenaltyKickState::KICK, [this]() { + if (getParameter("start_from_kick")) { + return true; + } else { + return world_model->play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::OUR_PENALTY_START; + } + }); + addStateFunction( + PenaltyKickState::KICK, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + if (not start_ball_point) { + start_ball_point = world_model->ball.pos; + } + + auto [best_target, goal_angle_width] = getBestShootTargetWithWidth(); + + // 経由ポイント + Point intermediate_point = + world_model->ball.pos + (world_model->ball.pos - best_target).normalized() * 0.2; + + double dot = (robot->pose.pos - world_model->ball.pos) + .normalized() + .dot((world_model->ball.pos - best_target).normalized()); + double target_theta = getAngle(best_target - world_model->ball.pos); + // ボールと敵ゴールの延長線上にいない && 角度があってないときは,中間ポイントを経由 + if (dot < 0.95 || std::abs(getAngleDiff(target_theta, robot->pose.theta)) > 0.05) { + command->setTargetPosition(intermediate_point); + command->enableCollisionAvoidance(); + } else { + command->setTargetPosition(world_model->ball.pos); + command->kickStraight(0.7).disableCollisionAvoidance(); + command->enableCollisionAvoidance(); + command->disableBallAvoidance(); + } + + command->setTargetTheta(getAngle(best_target - world_model->ball.pos)); + return Status::RUNNING; + }); + + addTransition(PenaltyKickState::KICK, PenaltyKickState::DONE, [this]() { + return world_model->isDefenseArea(world_model->ball.pos); + }); + + addStateFunction( + PenaltyKickState::DONE, [this](ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status { + command->stopHere(); + return Status::RUNNING; + }); + } + + // SimpleAttackerからコピー + auto getBestShootTargetWithWidth() -> std::pair + { + const auto & ball = world_model->ball.pos; + + Interval goal_range; + + auto goal_posts = world_model->getTheirGoalPosts(); + goal_range.append(getAngle(goal_posts.first - ball), getAngle(goal_posts.second - ball)); + + for (auto & enemy : world_model->theirs.getAvailableRobots()) { + double distance = enemy->getDistance(ball); + constexpr double MACHINE_RADIUS = 0.1; + + double center_angle = getAngle(enemy->pose.pos - ball); + 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(); + + double target_angle = (largest_interval.first + largest_interval.second) / 2.0; + + return { + ball + getNormVec(target_angle) * 0.5, largest_interval.second - largest_interval.first}; + } + + void print(std::ostream & os) const override + { + os << "[Idle] stop_by_position: " << getParameter("stop_by_position") ? "true" : "false"; + } +}; +} // namespace crane::skills +#endif // CRANE_ROBOT_SKILLS__PENALTY_KICK_HPP_ diff --git a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp index c71dafba3..22abf2ef0 100644 --- a/crane_robot_skills/include/crane_robot_skills/skill_base.hpp +++ b/crane_robot_skills/include/crane_robot_skills/skill_base.hpp @@ -192,7 +192,7 @@ class SkillBase : public SkillInterface crane_msgs::msg::RobotCommand getRobotCommand() override { return command->getMsg(); } - // std::shared_ptr commander() const { return command; } + std::shared_ptr commander() const { return command; } void addStateFunction(const StatesType & state, StateFunctionType function) { 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 487921daf..9393990cc 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 @@ -50,6 +50,7 @@ extern "C" { #include #include #include +#include #include #include #include @@ -114,6 +115,10 @@ class WorldModelPublisherComponent : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_referee; + rclcpp::Subscription::SharedPtr sub_play_situation; + + crane_msgs::msg::PlaySituation latest_play_situation; + rclcpp::Publisher::SharedPtr pub_world_model; rclcpp::TimerBase::SharedPtr timer; diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp index af088eb8a..fef61db67 100644 --- a/crane_world_model_publisher/src/world_model_publisher.cpp +++ b/crane_world_model_publisher/src/world_model_publisher.cpp @@ -22,6 +22,10 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt visionGeometryCallback(msg); }); + sub_play_situation = create_subscription( + "/play_situation", 1, + [this](const crane_msgs::msg::PlaySituation::SharedPtr msg) { latest_play_situation = *msg; }); + pub_world_model = create_publisher("/world_model", 1); using std::chrono::operator""ms; @@ -70,6 +74,7 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt their_color = Color::BLUE; } } + void WorldModelPublisherComponent::visionDetectionsCallback( const robocup_ssl_msgs::msg::TrackedFrame::SharedPtr msg) { @@ -196,6 +201,8 @@ void WorldModelPublisherComponent::publishWorldModel() wm.our_goalie_id = our_goalie_id; wm.their_goalie_id = their_goalie_id; + wm.play_situation = latest_play_situation; + pub_world_model->publish(wm); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp new file mode 100644 index 000000000..998451368 --- /dev/null +++ b/session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp @@ -0,0 +1,86 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_PLANNER_PLUGINS__OUR_PENALTY_KICK_PLANNER_HPP_ +#define CRANE_PLANNER_PLUGINS__OUR_PENALTY_KICK_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "visibility_control.h" + +namespace crane +{ +class OurPenaltyKickPlanner : public PlannerBase +{ +private: + std::shared_ptr kicker = nullptr; + + std::vector> other_robots; + +public: + COMPOSITION_PUBLIC + explicit OurPenaltyKickPlanner( + WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) + : PlannerBase("OurPenaltyKickPlanner", world_model, visualizer) + { + } + + std::pair> calculateRobotCommand( + const std::vector & robots) override + { + std::vector robot_commands; + + for (auto & robot_command : other_robots) { + // 関係ないロボットはボールより1m以上下がる(ルール5.3.5.3) + Point target{}; + target << (world_model->getOurGoalCenter().x() + world_model->ball.pos.x()) / 2, + robot_command->robot->pose.pos.y(); + robot_command->setTargetPosition(target); + robot_command->setMaxVelocity(0.5); + robot_commands.push_back(robot_command->getMsg()); + } + if (kicker) { + auto status = kicker->run(visualizer); + robot_commands.emplace_back(kicker->getRobotCommand()); + if (status == skills::Status::SUCCESS) { + return {PlannerBase::Status::SUCCESS, robot_commands}; + } + } + return {PlannerBase::Status::RUNNING, robot_commands}; + } + + auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override + { + auto robots_sorted = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [&](const std::shared_ptr & robot) { + // ボールに近いほうが先頭 + return 100. / robot->getDistance(world_model->ball.pos); + }); + if (robots_sorted.size() > 0) { + // 一番ボールに近いロボットがキッカー + kicker = std::make_shared(robots_sorted.front(), world_model); + } + if (robots_sorted.size() > 1) { + for (auto it = robots_sorted.begin() + 1; it != robots_sorted.end(); it++) { + other_robots.emplace_back(std::make_shared(*it, world_model)); + } + } + return robots_sorted; + } +}; +} // namespace crane +#endif // CRANE_PLANNER_PLUGINS__OUR_PENALTY_KICK_PLANNER_HPP_ 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 9ef7ec6a6..dea649dd3 100644 --- a/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp +++ b/session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp @@ -16,9 +16,11 @@ #include "formation_planner.hpp" #include "marker_planner.hpp" #include "our_kickoff_planner.hpp" +#include "our_penalty_kick_planner.hpp" #include "receive_planner.hpp" #include "skill_planner.hpp" #include "temporary/ball_placement_planner.hpp" +#include "their_penalty_kick_planner.hpp" #include "tigers_goalie_planner.hpp" #include "waiter_planner.hpp" @@ -49,6 +51,10 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase: return std::make_unique(ts...); } else if (planner_name == "our_kickoff") { return std::make_unique(ts...); + } else if (planner_name == "our_penalty_kick") { + return std::make_unique(ts...); + } else if (planner_name == "their_penalty_kick") { + return std::make_unique(ts...); } else { throw std::runtime_error("Unknown planner name: " + planner_name); } diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp new file mode 100644 index 000000000..45b3693cb --- /dev/null +++ b/session/crane_planner_plugins/include/crane_planner_plugins/their_penalty_kick_planner.hpp @@ -0,0 +1,89 @@ +// Copyright (c) 2024 ibis-ssl +// +// Use of this source code is governed by an MIT-style +// license that can be found in the LICENSE file or at +// https://opensource.org/licenses/MIT. + +#ifndef CRANE_PLANNER_PLUGINS__THEIR_PENALTY_KICK_PLANNER_HPP_ +#define CRANE_PLANNER_PLUGINS__THEIR_PENALTY_KICK_PLANNER_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "visibility_control.h" + +namespace crane +{ +class TheirPenaltyKickPlanner : public PlannerBase +{ +private: + std::shared_ptr goalie = nullptr; + + std::vector> other_robots; + +public: + COMPOSITION_PUBLIC + explicit TheirPenaltyKickPlanner( + WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) + : PlannerBase("TheirPenaltyKickPlanner", world_model, visualizer) + { + } + + std::pair> calculateRobotCommand( + const std::vector & robots) override + { + std::vector robot_commands; + + for (auto & robot_command : other_robots) { + // 関係ないロボットはボールより1m以上下がる(ルール5.3.5.3) + Point target{}; + target << (world_model->getTheirGoalCenter().x() + world_model->ball.pos.x()) / 2, + robot_command->robot->pose.pos.y(); + robot_command->setTargetPosition(target); + robot_command->setMaxVelocity(0.5); + robot_commands.push_back(robot_command->getMsg()); + } + if (goalie) { + if ( + world_model->play_situation.getSituationCommandID() == + crane_msgs::msg::PlaySituation::THEIR_PENALTY_PREPARATION) { + goalie->commander()->setTargetPosition(world_model->getOurGoalCenter()); + goalie->commander()->lookAtBall(); + goalie->commander()->setMaxVelocity(0.5); + } else { + auto status = goalie->run(visualizer); + } + robot_commands.emplace_back(goalie->getRobotCommand()); + } + return {PlannerBase::Status::RUNNING, robot_commands}; + } + + auto getSelectedRobots( + uint8_t selectable_robots_num, const std::vector & selectable_robots) + -> std::vector override + { + goalie = std ::make_shared(world_model->getOurGoalieId(), world_model); + auto robots_sorted = this->getSelectedRobotsByScore( + selectable_robots_num, selectable_robots, [&](const std::shared_ptr & robot) { + // ボールに近いほうが先頭 + return 100. / robot->getDistance(world_model->ball.pos); + }); + for (auto it = robots_sorted.begin(); it != robots_sorted.end(); it++) { + if (*it != world_model->getOurGoalieId()) { + other_robots.emplace_back(std::make_shared(*it, world_model)); + } + } + return robots_sorted; + } +}; +} // namespace crane +#endif // CRANE_PLANNER_PLUGINS__THEIR_PENALTY_KICK_PLANNER_HPP_ diff --git a/session/crane_session_controller/config/normal.yaml b/session/crane_session_controller/config/normal.yaml index adb70be73..2b1ed8140 100644 --- a/session/crane_session_controller/config/normal.yaml +++ b/session/crane_session_controller/config/normal.yaml @@ -15,3 +15,11 @@ events: session: SIMPLE_ATTACK - event: OUR_BALL_PLACEMENT session: OUR_BALL_PLACEMENT + - event: OUR_PENALTY_PREPARATION + session: OUR_PENALTY_KICK + - event: OUR_PENALTY_START + session: OUR_PENALTY_KICK + - event: THEIR_PENALTY_PREPARATION + session: THEIR_PENALTY_KICK + - event: THEIR_PENALTY_START + session: THEIR_PENALTY_KICK diff --git a/session/crane_session_controller/config/play_situation/OUR_PENALTY_KICK.yaml b/session/crane_session_controller/config/play_situation/OUR_PENALTY_KICK.yaml new file mode 100644 index 000000000..7c5875dc8 --- /dev/null +++ b/session/crane_session_controller/config/play_situation/OUR_PENALTY_KICK.yaml @@ -0,0 +1,5 @@ +name: OUR_PENALTY_KICK +description: OUR_PENALTY_KICK +sessions: + - name: our_penalty_kick + capacity: 20 diff --git a/session/crane_session_controller/config/play_situation/THEIR_PENALTY_KICK.yaml b/session/crane_session_controller/config/play_situation/THEIR_PENALTY_KICK.yaml new file mode 100644 index 000000000..32d9409b6 --- /dev/null +++ b/session/crane_session_controller/config/play_situation/THEIR_PENALTY_KICK.yaml @@ -0,0 +1,5 @@ +name: THEIR_PENALTY_KICK +description: THEIR_PENALTY_KICK +sessions: + - name: their_penalty_kick + capacity: 20