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