-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge remote-tracking branch 'origin/develop' into feature/aggressive…
…_goalie
- Loading branch information
Showing
13 changed files
with
359 additions
and
7 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
130 changes: 130 additions & 0 deletions
130
crane_robot_skills/include/crane_robot_skills/penalty_kick.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <crane_geometry/eigen_adapter.hpp> | ||
| #include <crane_geometry/interval.hpp> | ||
| #include <crane_robot_skills/skill_base.hpp> | ||
| #include <memory> | ||
| #include <utility> | ||
|
|
||
| namespace crane::skills | ||
| { | ||
| enum class PenaltyKickState { | ||
| PREPARE, | ||
| KICK, | ||
| DONE, | ||
| }; | ||
|
|
||
| class PenaltyKick : public SkillBase<PenaltyKickState> | ||
| { | ||
| private: | ||
| std::optional<Point> start_ball_point = std::nullopt; | ||
|
|
||
| public: | ||
| explicit PenaltyKick(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm) | ||
| : SkillBase<PenaltyKickState>("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<double>("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<bool>("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<Point, double> | ||
| { | ||
| 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<bool>("stop_by_position") ? "true" : "false"; | ||
| } | ||
| }; | ||
| } // namespace crane::skills | ||
| #endif // CRANE_ROBOT_SKILLS__PENALTY_KICK_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
86 changes: 86 additions & 0 deletions
86
session/crane_planner_plugins/include/crane_planner_plugins/our_penalty_kick_planner.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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 <crane_geometry/boost_geometry.hpp> | ||
| #include <crane_msg_wrappers/world_model_wrapper.hpp> | ||
| #include <crane_msgs/srv/robot_select.hpp> | ||
| #include <crane_planner_base/planner_base.hpp> | ||
| #include <crane_robot_skills/penalty_kick.hpp> | ||
| #include <functional> | ||
| #include <memory> | ||
| #include <rclcpp/rclcpp.hpp> | ||
| #include <utility> | ||
| #include <vector> | ||
|
|
||
| #include "visibility_control.h" | ||
|
|
||
| namespace crane | ||
| { | ||
| class OurPenaltyKickPlanner : public PlannerBase | ||
| { | ||
| private: | ||
| std::shared_ptr<skills::PenaltyKick> kicker = nullptr; | ||
|
|
||
| std::vector<std::shared_ptr<RobotCommandWrapper>> other_robots; | ||
|
|
||
| public: | ||
| COMPOSITION_PUBLIC | ||
| explicit OurPenaltyKickPlanner( | ||
| WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer) | ||
| : PlannerBase("OurPenaltyKickPlanner", world_model, visualizer) | ||
| { | ||
| } | ||
|
|
||
| std::pair<Status, std::vector<crane_msgs::msg::RobotCommand>> calculateRobotCommand( | ||
| const std::vector<RobotIdentifier> & robots) override | ||
| { | ||
| std::vector<crane_msgs::msg::RobotCommand> 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<uint8_t> & selectable_robots) | ||
| -> std::vector<uint8_t> override | ||
| { | ||
| auto robots_sorted = this->getSelectedRobotsByScore( | ||
| selectable_robots_num, selectable_robots, [&](const std::shared_ptr<RobotInfo> & robot) { | ||
| // ボールに近いほうが先頭 | ||
| return 100. / robot->getDistance(world_model->ball.pos); | ||
| }); | ||
| if (robots_sorted.size() > 0) { | ||
| // 一番ボールに近いロボットがキッカー | ||
| kicker = std::make_shared<skills::PenaltyKick>(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<RobotCommandWrapper>(*it, world_model)); | ||
| } | ||
| } | ||
| return robots_sorted; | ||
| } | ||
| }; | ||
| } // namespace crane | ||
| #endif // CRANE_PLANNER_PLUGINS__OUR_PENALTY_KICK_PLANNER_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Oops, something went wrong.