円弧デフェンス (#516)

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
ibis-ssl · Aug 5, 2024 · b4104fd · b4104fd
1 parent 7126fad
commit b4104fd
Show file tree

Hide file tree

Showing 18 changed files with 336 additions and 153 deletions.
diff --git a/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp b/consai_ros2/consai_vision_tracker/src/visualization_data_handler.cpp
@@ -57,7 +57,7 @@ void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::Share
   vis_ball.fill_color.alpha = 0.7;
   vis_ball.line_size = 1;
   vis_ball.radius = 0.0215;
-  vis_ball.caption = cam_id;
+  //  vis_ball.caption = cam_id;
 
   for (const auto & ball : msg->balls) {
     vis_ball.center.x = ball.x * 0.001;
@@ -71,7 +71,7 @@ void VisualizationDataHandler::publish_vis_detection(const DetectionFrame::Share
   vis_robot.fill_color.name = "dodgerblue";
   vis_robot.fill_color.alpha = 0.7;
   vis_robot.line_size = 1;
-  vis_robot.caption = cam_id;
+  //  vis_robot.caption = cam_id;
   for (const auto & robot : msg->robots_blue) {
     if (robot.robot_id.size() <= 0) {
       continue;
@@ -126,7 +126,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt
     line.p1.y = field_line.p1.y * 0.001;
     line.p2.x = field_line.p2.x * 0.001;
     line.p2.y = field_line.p2.y * 0.001;
-    line.caption = field_line.name;
+    //    line.caption = field_line.name;
 
     vis_objects->lines.push_back(line);
   }
@@ -142,7 +142,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt
     arc.radius = field_arc.radius * 0.001;
     arc.start_angle = field_arc.a1;
     arc.end_angle = field_arc.a2;
-    arc.caption = field_arc.name;
+    //    arc.caption = field_arc.name;
 
     vis_objects->arcs.push_back(arc);
   }
@@ -154,11 +154,11 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt
   point.size = 6;
   point.x = -msg->field.field_length * 0.001 / 2.0 + 8.0;
   point.y = 0.0;
-  point.caption = "penalty_mark_positive";
+  //  point.caption = "penalty_mark_positive";
   vis_objects->points.push_back(point);
 
   point.x = -point.x;
-  point.caption = "penalty_mark_negative";
+  //  point.caption = "penalty_mark_negative";
   vis_objects->points.push_back(point);
 
   // フィールドの枠
@@ -170,7 +170,7 @@ void VisualizationDataHandler::publish_vis_geometry(const GeometryData::SharedPt
   rect.center.y = 0.0;
   rect.width = (msg->field.field_length + msg->field.boundary_width * 2) * 0.001;
   rect.height = (msg->field.field_width + msg->field.boundary_width * 2) * 0.001;
-  rect.caption = "wall";
+  //  rect.caption = "wall";
   vis_objects->rects.push_back(rect);
 
   pub_vis_objects_->publish(std::move(vis_objects));
@@ -217,7 +217,7 @@ TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFra
       ball_vel.p1.y = ball.pos.y;
       ball_vel.p2.x = ball.pos.x + ball.vel[0].x;
       ball_vel.p2.y = ball.pos.y + ball.vel[0].y;
-      ball_vel.caption = std::to_string(vel_norm);
+      //      ball_vel.caption = std::to_string(vel_norm);
       vis_objects->lines.push_back(ball_vel);
     }
   }
@@ -243,31 +243,31 @@ TrackedFrame::UniquePtr VisualizationDataHandler::publish_vis_tracked(TrackedFra
     vis_objects->robots.push_back(vis_robot);
 
     // 速度を描画
-    if (robot.vel.size() > 0 && robot.vel_angular.size() > 0) {
-      const double vel_norm = std::hypot(robot.vel[0].x, robot.vel[0].y);
-      VisLine robot_vel;
-      // 直進速度
-      robot_vel.color.name = "gold";
-      robot_vel.color.alpha = VELOCITY_ALPHA;
-      robot_vel.size = 2;
-      robot_vel.p1.x = robot.pos.x;
-      robot_vel.p1.y = robot.pos.y;
-      robot_vel.p2.x = robot.pos.x + robot.vel[0].x;
-      robot_vel.p2.y = robot.pos.y + robot.vel[0].y;
-      robot_vel.caption = std::to_string(vel_norm);
-      vis_objects->lines.push_back(robot_vel);
-
-      // 角速度
-      const double vel_angular_norm = std::fabs(robot.vel_angular[0]);
-      robot_vel.color.name = "crimson";
-      robot_vel.color.alpha = VELOCITY_ALPHA;
-      robot_vel.p1.x = robot.pos.x;
-      robot_vel.p1.y = robot.pos.y;
-      robot_vel.p2.x = robot.pos.x + robot.vel_angular[0];
-      robot_vel.p2.y = robot.pos.y;
-      robot_vel.caption = std::to_string(vel_angular_norm);
-      vis_objects->lines.push_back(robot_vel);
-    }
+    //    if (robot.vel.size() > 0 && robot.vel_angular.size() > 0) {
+    //      const double vel_norm = std::hypot(robot.vel[0].x, robot.vel[0].y);
+    //      VisLine robot_vel;
+    //      // 直進速度
+    //      robot_vel.color.name = "gold";
+    //      robot_vel.color.alpha = VELOCITY_ALPHA;
+    //      robot_vel.size = 2;
+    //      robot_vel.p1.x = robot.pos.x;
+    //      robot_vel.p1.y = robot.pos.y;
+    //      robot_vel.p2.x = robot.pos.x + robot.vel[0].x;
+    //      robot_vel.p2.y = robot.pos.y + robot.vel[0].y;
+    //      robot_vel.caption = std::to_string(vel_norm);
+    //      vis_objects->lines.push_back(robot_vel);
+    //
+    //      // 角速度
+    //      const double vel_angular_norm = std::fabs(robot.vel_angular[0]);
+    //      robot_vel.color.name = "crimson";
+    //      robot_vel.color.alpha = VELOCITY_ALPHA;
+    //      robot_vel.p1.x = robot.pos.x;
+    //      robot_vel.p1.y = robot.pos.y;
+    //      robot_vel.p2.x = robot.pos.x + robot.vel_angular[0];
+    //      robot_vel.p2.y = robot.pos.y;
+    //      robot_vel.caption = std::to_string(vel_angular_norm);
+    //      vis_objects->lines.push_back(robot_vel);
+    //    }
   }
 
   pub_vis_objects_->publish(std::move(vis_objects));

diff --git a/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp b/crane_robot_skills/include/crane_robot_skills/goal_kick.hpp
@@ -35,6 +35,7 @@ class GoalKick : public SkillBase<RobotCommandWrapperPosition>
       world_model());
 
     Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5;
+    visualizer->addLine(world_model()->ball.pos, target, 2, "red");
     kick_skill.setParameter("target", target);
     kick_skill.setParameter("dot_threshold", getParameter<double>("dot_threshold"));
     return kick_skill.run(visualizer);

diff --git a/crane_robot_skills/include/crane_robot_skills/redirect.hpp b/crane_robot_skills/include/crane_robot_skills/redirect.hpp
@@ -25,7 +25,7 @@ class Redirect : public SkillBase<RobotCommandWrapperPosition>
     setParameter("policy", std::string("closest"));
   }
 
-  Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
+  Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
   {
     auto interception_point = getInterceptionPoint();
     auto target_angle = getRobotAngle(
@@ -38,6 +38,8 @@ class Redirect : public SkillBase<RobotCommandWrapperPosition>
     } else {
       command.kickStraight(getParameter<double>("kick_power"));
     }
+
+    visualizer->addLine(robot()->pose.pos, getParameter<Point>("redirect_target"), 1, "red");
     return Status::RUNNING;
   }
 

diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp
@@ -20,6 +20,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   redirect_skill(base),
   steal_ball_skill(base)
 {
+  receive_skill.setParameter("policy", std::string("min_slack"));
   setParameter("receiver_id", 0);
   addStateFunction(
     AttackerState::ENTRY_POINT,
@@ -113,16 +114,19 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
     });
 
   addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool {
-    return not world_model()->isOurBallByBallOwnerCalculator() && world_model()->ball.isMoving(0.2);
+    return not world_model()->isOurBallByBallOwnerCalculator() &&
+           world_model()->ball.isMoving(0.2) &&
+           world_model()->ball.isMovingTowards(robot()->pose.pos);
   });
 
   addTransition(AttackerState::CUT_THEIR_PASS, AttackerState::ENTRY_POINT, [this]() -> bool {
-    return world_model()->isOurBallByBallOwnerCalculator();
+    return world_model()->isOurBallByBallOwnerCalculator() or world_model()->ball.isStopped(0.2);
   });
 
   addStateFunction(
     AttackerState::CUT_THEIR_PASS,
-    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+    [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5);
       return receive_skill.run(visualizer);
     });
 
@@ -142,7 +146,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
   addStateFunction(
     AttackerState::STEAL_BALL,
-    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+    [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+      visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0);
       return steal_ball_skill.run(visualizer);
     });
 
@@ -192,8 +197,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
           return intersection_points.front();
         }
       }();
+
       redirect_skill.setParameter("redirect_target", target);
-      redirect_skill.setParameter("policy", std::string("closest"));
+      redirect_skill.setParameter("policy", std::string("max_slack"));
       redirect_skill.setParameter("kick_power", 0.8);
       return redirect_skill.run(visualizer);
     });
@@ -280,7 +286,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
   addStateFunction(
     AttackerState::STANDARD_PASS,
-    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+    [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
       // TODO(HansRobo): しっかりパス先を選定する
       //    int receiver_id = getParameter<int>("receiver_id");
@@ -318,6 +324,9 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
           best_target = target;
         }
       }
+
+      visualizer->addLine(world_model()->ball.pos, best_target, 1, "red");
+
       kick_skill.setParameter("target", best_target);
       Segment ball_to_target{world_model()->ball.pos, best_target};
       auto [nearest_enemy, enemy_distance] = world_model()->getNearestRobotWithDistanceFromSegment(
@@ -348,7 +357,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
 
   addStateFunction(
     AttackerState::LOW_CHANCE_GOAL_KICK,
-    [this]([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
+    [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) -> Status {
       return goal_kick_skill.run(visualizer);
     });
 

diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp
@@ -16,7 +16,7 @@ Marker::Marker(RobotCommandWrapperBase::SharedPtr & base)
   setParameter("mark_mode", std::string("save_goal"));
 }
 
-Status Marker::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer)
+Status Marker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer)
 {
   auto marked_robot = world_model()->getTheirRobot(getParameter<int>("marking_robot_id"));
   auto enemy_pos = marked_robot->pose.pos;
@@ -37,6 +37,10 @@ Status Marker::update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr
     throw std::runtime_error("unknown mark mode");
   }
   command.setTargetPosition(marking_point, target_theta);
+
+  visualizer->addCircle(enemy_pos, 0.3, 1, "black", "");
+  visualizer->addLine(
+    robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black");
   return Status::RUNNING;
 }
 }  // namespace crane::skills
diff --git a/crane_robot_skills/src/steal_ball.cpp b/crane_robot_skills/src/steal_ball.cpp
@@ -50,19 +50,29 @@ StealBall::StealBall(RobotCommandWrapperBase::SharedPtr & base)
       command.disableBallAvoidance();
       command.disableCollisionAvoidance();
       const auto method = getParameter<std::string>("steal_method");
+      auto their_frontier = world_model()->getNearestRobotWithDistanceFromPoint(
+        world_model()->ball.pos, world_model()->theirs.getAvailableRobots());
       if (method == "front") {
         command.setDribblerTargetPosition(
           world_model()->ball.pos, getAngle(world_model()->ball.pos - robot()->pose.pos));
         command.dribble(0.5);
       } else if (method == "side") {
+        command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos));
         if (robot()->getDistance(world_model()->ball.pos) < (0.085 - 0.030)) {
+          command.setDribblerTargetPosition(
+            world_model()->ball.pos +
+            getVerticalVec(world_model()->ball.pos - robot()->pose.pos) * 0.3);
           // ロボット半径より近くに来れば急回転して刈り取れる
-          command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
+          //          command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos) + M_PI / 2);
         } else {
-          command.setTargetTheta(getAngle(world_model()->ball.pos - robot()->pose.pos));
+          command.setDribblerTargetPosition(world_model()->ball.pos);
+        }
+        if (
+          world_model()->getTheirFrontier().has_value() &&
+          robot()->getDistance(world_model()->ball.pos) <
+            world_model()->getTheirFrontier()->robot->getDistance(world_model()->ball.pos)) {
+          command.kickWithChip(0.5);
         }
-        // thetaに依存するので後置
-        command.setDribblerTargetPosition(world_model()->ball.pos);
       }
       return Status::RUNNING;
     });

diff --git a/crane_robot_skills/src/sub_attacker.cpp b/crane_robot_skills/src/sub_attacker.cpp
@@ -77,9 +77,9 @@ Status SubAttacker::update(const ConsaiVisualizerWrapper::SharedPtr & visualizer
     double best_score = 0.0;
     for (const auto & dpps_point : dpps_points) {
       double score = getPointScore(dpps_point, world_model()->ball.pos, world_model());
-      visualizer->addPoint(
-        dpps_point.x(), dpps_point.y(), std::clamp(static_cast<int>(score * 100), 0, 20), "blue",
-        1.);
+      //      visualizer->addPoint(
+      //        dpps_point.x(), dpps_point.y(), std::clamp(static_cast<int>(score * 100), 0, 20), "blue",
+      //        1.);
 
       if (score > best_score) {
         best_score = score;

diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp
@@ -45,6 +45,9 @@ class AttackerSkillPlanner : public PlannerBase
     if (not skill) {
       return {PlannerBase::Status::RUNNING, {}};
     } else {
+      std::string state_name(magic_enum::enum_name(skill->getCurrentState()));
+      visualizer->addCircle(
+        skill->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name);
       auto status = skill->run(visualizer);
       return {static_cast<PlannerBase::Status>(status), {skill->getRobotCommand()}};
     }

diff --git a/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp b/session/crane_planner_plugins/include/crane_planner_plugins/defender_planner.hpp
@@ -39,57 +39,10 @@ class DefenderPlanner : public PlannerBase
   std::pair<Status, std::vector<crane_msgs::msg::RobotCommand>> calculateRobotCommand(
     const std::vector<RobotIdentifier> & robots) override;
 
-  // defense_pointを中心にrobot_num台のロボットをdefense_line上に等間隔に配置する
-  std::vector<Point> getDefensePoints(const int robot_num, const Segment & ball_line) const
-  {
-    const double DEFENSE_INTERVAL = 0.2;
-    std::vector<Point> defense_points;
-
-    if (auto defense_parameter = getDefenseLinePointParameter(ball_line)) {
-      double upper_parameter, lower_parameter;
-
-      auto add_parameter = [&](double parameter) -> bool {
-        const double OFFSET_X = 0.1, OFFSET_Y = 0.1;
-        auto [threshold1, threshold2, threshold3] =
-          getDefenseLinePointParameterThresholds(OFFSET_X, OFFSET_Y);
-        if (parameter < 0. || parameter > threshold3) {
-          return false;
-        } else {
-          if (upper_parameter < parameter) {
-            upper_parameter = parameter;
-          }
-          if (lower_parameter > parameter) {
-            lower_parameter = parameter;
-          }
-          defense_points.push_back(getDefenseLinePoint(parameter));
-          return true;
-        }
-      };
-      // 1台目
-      upper_parameter = *defense_parameter;
-      lower_parameter = *defense_parameter;
-      add_parameter(*defense_parameter);
-
-      // 2台目以降
-      for (int i = 0; i < robot_num - 1; i++) {
-        if (i % 2 == 0) {
-          // upper側に追加
-          if (not add_parameter(upper_parameter + DEFENSE_INTERVAL)) {
-            // だめならlower側
-            add_parameter(lower_parameter - DEFENSE_INTERVAL);
-          }
-        } else {
-          // lower側に追加
-          if (not add_parameter(lower_parameter - DEFENSE_INTERVAL)) {
-            // だめならupper側
-            add_parameter(upper_parameter + DEFENSE_INTERVAL);
-          }
-        }
-      }
-    }
+  std::vector<Point> getDefenseArcPoints(const int robot_num, const Segment & ball_line) const;
 
-    return defense_points;
-  }
+  // defense_pointを中心にrobot_num台のロボットをdefense_line上に等間隔に配置する
+  std::vector<Point> getDefenseLinePoints(const int robot_num, const Segment & ball_line) const;
 
   auto getDefenseLinePoint(double parameter) const -> Point
   {