Merge pull request #77 from ibis-ssl/update-goalie

ゴールキーパーアップデート

crane_bringup/launch/play_switcher.launch.py

@@ -113,7 +113,8 @@ def generate_launch_description():
             {
                 "planner": "simple",
                 # "non_rvo_gain": 2.15,
-                "non_rvo_p_gain": 2.9,
+                "non_rvo_p_gain": 3.9,
+                "non_rvo_i_gain": 0.1,
                 "non_rvo_d_gain": 1.0,
             }
         ],
@@ -174,7 +175,7 @@ def generate_launch_description():
         parameters=[
             {
                 "no_movement": False,
-                "theta_kp": 12.0,
+                "theta_kp": 5.0,
                 "theta_ki": 0.0,
                 "theta_kd": 1.0,
             }

crane_msgs/msg/world_model/WorldModel.msg

@@ -22,3 +22,5 @@ RobotInfoOurs[] robot_info_ours
 RobotInfoTheirs[] robot_info_theirs
 
 geometry_msgs/Point ball_placement_target
+
+PlaySituation play_situation

crane_robot_skills/include/crane_robot_skills/skill_base.hpp

@@ -14,6 +14,8 @@
 #include <string>
 #include <vector>
 
+#undef DEFAULT
+
 namespace crane
 {
 

session/crane_planner_plugins/CMakeLists.txt

@@ -7,7 +7,7 @@ if (NOT CMAKE_CXX_STANDARD)
 endif ()
 
 if (CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
+  add_compile_options(-Wall -Wextra -Wpedantic -g)
 endif ()
 
 add_definitions("-DBOOST_ALLOW_DEPRECATED_HEADERS")
@@ -52,7 +52,7 @@ add_planner_plugin(
   CLASS_NAME "crane::ReceivePlanner"
 )
 
-ament_auto_add_executable(receive_test_node src/receive/test.cpp)
+ament_auto_add_executable(receive_test_node src/receive_test.cpp)
 ament_target_dependencies(receive_test_node "Python3")
 
 

session/crane_planner_plugins/include/crane_planner_plugins/goalie_planner.hpp

@@ -30,50 +30,109 @@ class GoaliePlanner : public PlannerBase
   {
   }
 
-  std::vector<crane_msgs::msg::RobotCommand> calculateControlTarget(
-    const std::vector<RobotIdentifier> & robots) override
+  void emitBallFromPenaltyArea(crane::RobotCommandWrapper & target)
   {
-    std::vector<crane_msgs::msg::RobotCommand> control_targets;
-    for (auto robot_id : robots) {
-      crane::RobotCommandWrapper target(robot_id.robot_id, world_model);
-      auto robot = world_model->getRobot(robot_id);
-
-      auto ball = world_model->ball.pos;
+    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());
 
-      auto goals = world_model->getOurGoalPosts();
-
-      Segment goal_line(goals.first, goals.second);
+    Point pass_target = [&]() {
+      if (not passable_robot_list.empty()) {
+        // TODO: いい感じのロボットを選ぶようにする
+        return passable_robot_list.front()->pose.pos;
+      } else {
+        return Point{0, 0};
+      }
+    }();
 
-      Point goal_center;
-      goal_center << goals.first.x(), 0.0f;
-      Segment ball_line(ball, ball + world_model->ball.vel * 20000000.f);
-      std::vector<Point> intersections;
+    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.setTargetTheta(getAngle(pass_target - ball));
+  }
 
-      // check shoot
-      bg::intersection(ball_line, goal_line, intersections);
-      Point target_point;
-      float target_theta;
-      if (not intersections.empty()) {
-        //        std::cout << "Shoot block mode" << std::endl;
-        ClosestPoint result;
-        bg::closest_point(ball_line, robot->pose.pos, result);
-        target_point << result.closest_point.x(), result.closest_point.y();
-        // position control
-        target_theta = getAngle(-world_model->ball.vel);
+  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<Point> 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 {
-        // go blocking point
-        //        std::cout << "Normal blocking mode" << std::endl;
         const double BLOCK_DIST = 0.15;
-
         // 範囲外のときは正面に構える
-        if (not world_model->isFieldInside(world_model->ball.pos)) {
+        if (not world_model->isFieldInside(ball)) {
           ball << 0, 0;
         }
-        target_point = goal_center + (ball - goal_center).normalized() * BLOCK_DIST;
-        target_theta = getAngle(ball - target_point);
+        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();
       }
+    }
+  }
 
-      target.setTargetPosition(target_point, target_theta);
+  std::vector<crane_msgs::msg::RobotCommand> calculateControlTarget(
+    const std::vector<RobotIdentifier> & robots) override
+  {
+    std::vector<crane_msgs::msg::RobotCommand> control_targets;
+    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);
+        default:
+          inplay(target, true);
+      }
 
       control_targets.emplace_back(target.getMsg());
     }

utility/crane_msg_wrappers/include/crane_msg_wrappers/play_situation_wrapper.hpp

@@ -7,8 +7,8 @@
 #ifndef CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_
 #define CRANE_MSG_WRAPPERS__PLAY_SITUATION_WRAPPER_HPP_
 
+#include <crane_geometry/boost_geometry.hpp>
 #include <crane_msg_wrappers/geometry_wrapper.hpp>
-#include <crane_msg_wrappers/world_model_wrapper.hpp>
 #include <crane_msgs/msg/play_situation.hpp>
 #include <string>
 #include <vector>
@@ -29,7 +29,7 @@ struct PlaySituationWrapper
     return situation_command.id == crane_msgs::msg::PlaySituation::INPLAY;
   }
 
-  Eigen::Vector2d placement_position;
+  Point placement_position;
 
   auto update(const crane_msgs::msg::PlaySituation & msg) -> void;
 

utility/crane_msg_wrappers/include/crane_msg_wrappers/robot_command_wrapper.hpp

@@ -7,7 +7,6 @@
 #ifndef CRANE_MSG_WRAPPERS__ROBOT_COMMAND_WRAPPER_HPP_
 #define CRANE_MSG_WRAPPERS__ROBOT_COMMAND_WRAPPER_HPP_
 
-#include <Eigen/Core>
 #include <crane_geometry/boost_geometry.hpp>
 #include <crane_geometry/geometry_operations.hpp>
 #include <crane_msgs/msg/robot_command.hpp>
@@ -25,7 +24,7 @@ struct RobotCommandWrapper
   typedef std::shared_ptr<RobotCommandWrapper> SharedPtr;
 
   RobotCommandWrapper(uint8_t id, WorldModelWrapper::SharedPtr world_model_wrapper)
-  : robot(world_model_wrapper->getOurRobot(id))
+  : robot(world_model_wrapper->getOurRobot(id)), world_model(world_model_wrapper)
   {
     latest_msg.robot_id = id;
 
@@ -241,13 +240,27 @@ struct RobotCommandWrapper
     return *this;
   }
 
+  RobotCommandWrapper & lookAt(Point pos)
+  {
+    return setTargetTheta(getAngle(pos - robot->pose.pos));
+  }
+
+  RobotCommandWrapper & lookAtBall() { return lookAt(world_model->ball.pos); }
+
+  RobotCommandWrapper & lookAtBallFrom(Point from)
+  {
+    return setTargetTheta(getAngle(world_model->ball.pos - from));
+  }
+
   const crane_msgs::msg::RobotCommand & getMsg() const { return latest_msg; }
 
   crane_msgs::msg::RobotCommand & getEditableMsg() { return latest_msg; }
 
   crane_msgs::msg::RobotCommand latest_msg;
 
   const std::shared_ptr<RobotInfo> robot;
+
+  WorldModelWrapper::SharedPtr world_model;
 };
 }  // namespace crane
 

utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp

@@ -16,6 +16,10 @@
 #include <rclcpp/rclcpp.hpp>
 #include <vector>
 
+#include "play_situation_wrapper.hpp"
+
+namespace crane
+{
 struct BallContact
 {
   std::chrono::system_clock::time_point last_contact_end_time;
@@ -47,8 +51,6 @@ struct BallContact
   bool is_contacted_pre_frame = false;
 };
 
-namespace crane
-{
 struct RobotIdentifier
 {
   bool is_ours;
@@ -191,6 +193,8 @@ struct WorldModelWrapper
 
   void update(const crane_msgs::msg::WorldModel & world_model)
   {
+    play_situation.update(world_model.play_situation);
+
     for (auto & our_robot : ours.robots) {
       our_robot->available = false;
     }
@@ -355,9 +359,9 @@ struct WorldModelWrapper
     return {nearest_robot, std::sqrt(min_sq_distance)};
   }
 
-  bool isEnemyDefenseArea(const Point & p) const { return isInRect(ours.defense_area, p); }
+  bool isEnemyDefenseArea(const Point & p) const { return isInRect(theirs.defense_area, p); }
 
-  bool isFriendDefenseArea(const Point & p) const { return isInRect(theirs.defense_area, p); }
+  bool isFriendDefenseArea(const Point & p) const { return isInRect(ours.defense_area, p); }
 
   bool isDefenseArea(Point p) const { return isFriendDefenseArea(p) || isEnemyDefenseArea(p); }
 
@@ -417,6 +421,8 @@ struct WorldModelWrapper
 
   Ball ball;
 
+  PlaySituationWrapper play_situation;
+
 private:
   rclcpp::Subscription<crane_msgs::msg::WorldModel>::SharedPtr subscriber;
 

utility/crane_msg_wrappers/src/play_situation_wrapper.cpp

@@ -9,6 +9,7 @@
 #include <map>
 #include <string>
 
+#include "crane_msg_wrappers/world_model_wrapper.hpp"
 #include "robocup_ssl_msgs/msg/referee.hpp"
 
 namespace crane