Merge pull request #145 from ibis-ssl/feature/kickoff

Kickoff動作を作る

.github/workflows/code_amount.yaml

@@ -0,0 +1,18 @@
+name: code amount
+
+on:
+  workflow_dispatch:
+  pull_request:
+
+jobs:
+  spell-check:
+    runs-on: ubuntu-latest
+    steps:
+      - name: install cloc
+        run: sudo apt-get install -y cloc
+
+      - name: Check out repository
+        uses: actions/checkout@v4
+
+      - name: Run cloc
+        run: cloc . --vcs=git

crane_bringup/launch/play_switcher.launch.xml

@@ -45,14 +45,14 @@
 
     <node pkg="consai_visualizer" exec="consai_visualizer"/>
 
-<!--    <node pkg="crane_sender" exec="sim_sender_node" output="screen">-->
-<!--        <param name="no_movement" value="false"/>-->
-<!--        <param name="theta_kp" value="5.0"/>-->
-<!--        <param name="theta_ki" value="0.0"/>-->
-<!--        <param name="theta_kd" value="1.0"/>-->
-<!--    </node>-->
-
-    <node pkg="crane_sender" exec="ibis_sender_node" output="screen">
-        <param name="sim" value="true"/>
+    <node pkg="crane_sender" exec="sim_sender_node" output="screen">
+        <param name="no_movement" value="false"/>
+        <param name="theta_kp" value="5.0"/>
+        <param name="theta_ki" value="0.0"/>
+        <param name="theta_kd" value="1.0"/>
     </node>
+
+<!--    <node pkg="crane_sender" exec="ibis_sender_node" output="screen">-->
+<!--        <param name="sim" value="true"/>-->
+<!--    </node>-->
 </launch>

crane_robot_skills/include/crane_robot_skills/kickoff_attack.hpp

@@ -0,0 +1,76 @@
+// 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__KICKOFF_ATTACK_HPP_
+#define CRANE_ROBOT_SKILLS__KICKOFF_ATTACK_HPP_
+
+#include <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/go_over_ball.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane::skills
+{
+enum class KickoffAttackState {
+  PREPARE_KICKOFF,
+  KICKOFF,
+};
+class KickoffAttack : public SkillBase<KickoffAttackState>
+{
+public:
+  explicit KickoffAttack(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<KickoffAttackState>(
+      "KickoffAttack", id, world_model, KickoffAttackState::PREPARE_KICKOFF)
+  {
+    setParameter("target_x", 0.0f);
+    setParameter("target_y", 1.0f);
+    setParameter("kick_power", 0.5);
+    addStateFunction(
+      KickoffAttackState::PREPARE_KICKOFF,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
+        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+        if (not go_over_ball) {
+          go_over_ball = std::make_shared<GoOverBall>(robot->id, world_model);
+          go_over_ball->setParameter("next_target_x", getParameter<double>("target_x"));
+          go_over_ball->setParameter("next_target_y", getParameter<double>("target_y"));
+          go_over_ball->setParameter("margin", 0.3);
+          command.setMaxVelocity(0.5);
+        }
+        go_over_ball_status = go_over_ball->run(command, visualizer);
+        return Status::RUNNING;
+      });
+    addTransition(
+      KickoffAttackState::PREPARE_KICKOFF, KickoffAttackState::KICKOFF,
+      [this]() -> bool { return go_over_ball_status == Status::SUCCESS; });
+
+    addStateFunction(
+      KickoffAttackState::KICKOFF,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot, crane::RobotCommandWrapper & command,
+        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+        command.setMaxVelocity(0.5);
+        command.kickStraight(getParameter<double>("kick_power"));
+        command.setTargetPosition(world_model->ball.pos);
+        command.setTerminalVelocity(0.5);
+        if (world_model->ball.vel.norm() > 0.3) {
+          return Status::SUCCESS;
+        } else {
+          return Status::RUNNING;
+        }
+      });
+  }
+
+  void print(std::ostream & os) const override { os << "[KickoffAttack]"; }
+
+private:
+  std::shared_ptr<GoOverBall> go_over_ball = nullptr;
+
+  Status go_over_ball_status = Status::RUNNING;
+};
+}  // namespace crane::skills
+#endif  // CRANE_ROBOT_SKILLS__KICKOFF_ATTACK_HPP_

crane_robot_skills/include/crane_robot_skills/kickoff_support.hpp

@@ -0,0 +1,39 @@
+// 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__KICKOFF_SUPPORT_HPP_
+#define CRANE_ROBOT_SKILLS__KICKOFF_SUPPORT_HPP_
+
+#include <crane_geometry/eigen_adapter.hpp>
+#include <crane_robot_skills/skill_base.hpp>
+
+namespace crane::skills
+{
+class KickoffSupport : public SkillBase<>
+{
+public:
+  explicit KickoffSupport(uint8_t id, const std::shared_ptr<WorldModelWrapper> & world_model)
+  : SkillBase<>("KickoffSupport", id, world_model, DefaultStates::DEFAULT)
+  {
+    setParameter("target_x", 0.0f);
+    setParameter("target_y", 1.0f);
+    addStateFunction(
+      DefaultStates::DEFAULT,
+      [this](
+        const std::shared_ptr<WorldModelWrapper> & world_model,
+        const std::shared_ptr<RobotInfo> & robot, RobotCommandWrapper & command,
+        ConsaiVisualizerWrapper::SharedPtr visualizer) -> Status {
+        Point target(getParameter<double>("target_x"), getParameter<double>("target_y"));
+        command.setTargetPosition(target);
+        command.lookAtBallFrom(target);
+        return Status::RUNNING;
+      });
+  }
+
+  void print(std::ostream & os) const override { os << "[KickoffSupport]"; }
+};
+}  // namespace crane::skills
+#endif  // CRANE_ROBOT_SKILLS__KICKOFF_SUPPORT_HPP_

crane_robot_skills/include/crane_robot_skills/skills.hpp

@@ -11,8 +11,10 @@
 #include <crane_robot_skills/go_over_ball.hpp>
 #include <crane_robot_skills/goalie.hpp>
 #include <crane_robot_skills/idle.hpp>
+#include <crane_robot_skills/kickoff_attack.hpp>
+#include <crane_robot_skills/kickoff_support.hpp>
 #include <crane_robot_skills/marker.hpp>
-#include <crane_robot_skills/move_to_geometry.hpp>
+//#include <crane_robot_skills/move_to_geometry.hpp>
 #include <crane_robot_skills/move_with_ball.hpp>
 #include <crane_robot_skills/receiver.hpp>
 #include <crane_robot_skills/robot_command_as_skill.hpp>

crane_robot_skills/src/robot_skills.cpp

@@ -8,8 +8,8 @@
 
 namespace crane::skills
 {
-template class MoveToGeometry<Point>;
-template class MoveToGeometry<Segment>;
+//template class MoveToGeometry<Point>;
+//template class MoveToGeometry<Segment>;
 }  // namespace crane::skills
 
 //#include <rclcpp_components/register_node_macro.hpp>

crane_simple_ai/src/crane_commander.cpp

@@ -81,6 +81,8 @@ CraneCommander::CraneCommander(QWidget * parent) : QMainWindow(parent), ui(new U
   setUpSkillDictionary<skills::Receiver>();
   setUpSkillDictionary<skills::Marker>();
   setUpSkillDictionary<skills::SingleBallPlacement>();
+  setUpSkillDictionary<skills::KickoffAttack>();
+  setUpSkillDictionary<skills::KickoffSupport>();
 
   ui->commandComboBox->clear();
   for (const auto & task : default_task_dict) {

session/crane_planner_plugins/include/crane_planner_plugins/our_kickoff_planner.hpp

@@ -0,0 +1,93 @@
+// 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_KICKOFF_PLANNER_HPP_
+#define CRANE_PLANNER_PLUGINS__OUR_KICKOFF_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/kickoff_attack.hpp>
+#include <crane_robot_skills/kickoff_support.hpp>
+#include <functional>
+#include <memory>
+#include <rclcpp/rclcpp.hpp>
+
+#include "visibility_control.h"
+
+namespace crane
+{
+class OurKickOffPlanner : public PlannerBase
+{
+private:
+  std::shared_ptr<skills::KickoffAttack> kickoff_attack;
+  std::shared_ptr<RobotCommandWrapper> attacker_command;
+
+  std::shared_ptr<skills::KickoffSupport> kickoff_support;
+  std::shared_ptr<RobotCommandWrapper> supporter_command;
+
+  //  std::shared_ptr<ConsaiVisualizerWrapper> visualizer;
+
+public:
+  COMPOSITION_PUBLIC explicit OurKickOffPlanner(
+    WorldModelWrapper::SharedPtr & world_model, ConsaiVisualizerWrapper::SharedPtr visualizer)
+  : PlannerBase("our_kickoff_planner", 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;
+
+    kickoff_attack->run(*attacker_command, visualizer);
+    kickoff_support->run(*supporter_command, visualizer);
+
+    robot_commands.emplace_back(attacker_command->getMsg());
+    robot_commands.emplace_back(supporter_command->getMsg());
+
+    // いい感じにSUCCESSも返す
+    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
+  {
+    // 一番ボールに近いロボットをkickoff attack
+    auto best_attacker = std::max_element(
+      selectable_robots.begin(), selectable_robots.end(), [this](const auto & a, const auto & b) {
+        return world_model->getOurRobot(a)->getDistance(world_model->ball.pos) <
+               world_model->getOurRobot(b)->getDistance(world_model->ball.pos);
+      });
+    Point supporter_pos{0.0, 2.0};
+    auto best_supporter = std::max_element(
+      selectable_robots.begin(), selectable_robots.end(),
+      [this, supporter_pos, best_attacker](const auto & a, const auto & b) {
+        if (a == *best_attacker) {
+          // bの方大きい => best_attackerであるaが除外される
+          return true;
+        } else if (b == *best_attacker) {
+          // bの方大きくない => best_attackerであるbが除外される
+          return false;
+        } else {
+          return world_model->getOurRobot(a)->getDistance(supporter_pos) <
+                 world_model->getOurRobot(b)->getDistance(supporter_pos);
+        }
+      });
+
+    kickoff_attack = std::make_shared<skills::KickoffAttack>(*best_attacker, world_model);
+    attacker_command = std::make_shared<RobotCommandWrapper>(*best_attacker, world_model);
+    kickoff_support = std::make_shared<skills::KickoffSupport>(*best_supporter, world_model);
+    supporter_command = std::make_shared<RobotCommandWrapper>(*best_supporter, world_model);
+
+    return {*best_attacker, *best_supporter};
+  }
+};
+
+}  // namespace crane
+#endif  // CRANE_PLANNER_PLUGINS__OUR_KICKOFF_PLANNER_HPP_

session/crane_planner_plugins/include/crane_planner_plugins/planners.hpp

@@ -16,6 +16,7 @@
 #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 "tigers_goalie_planner.hpp"
@@ -48,6 +49,10 @@ auto generatePlanner(const std::string & planner_name, Ts... ts) -> PlannerBase:
     return std::make_unique<TigersGoaliePlanner>(ts...);
   } else if (planner_name == "waiter") {
     return std::make_unique<WaiterPlanner>(ts...);
+  } else if (planner_name == "our_kickoff") {
+    return std::make_unique<OurKickOffPlanner>(ts...);
+  } else {
+    throw std::runtime_error("Unknown planner name: " + planner_name);
   }
 }
 }  // namespace crane

session/crane_session_controller/config/normal.yaml

@@ -10,5 +10,7 @@ events:
     session: "formation"
   - event: "OUR_KICKOFF_PREPARATION"
     session: "formation"
+  - event: "OUR_KICKOFF_START"
+    session: "OUR_KICKOFF_START"
   - event: "INPLAY"
-    session: "mark"
+    session: "SIMPLE_ATTACK"

session/crane_session_controller/config/play_situation/OUR_KICKOFF_START.yaml

@@ -4,7 +4,7 @@ description: OUR_KICKOFF_START
 sessions:
   - name: "goalie"
     capacity: 1
-  - name: "attacker"
-    capacity: 1
+  - name: "our_kickoff"
+    capacity: 2
   - name: "waiter"
     capacity: 20