KickState::POSITIVE_REDIRECT_KICK追加

ibis-ssl · Oct 12, 2024 · e4bbe60 · e4bbe60
1 parent 16bbf8f
commit e4bbe60
Showing 1 changed file with 34 additions and 0 deletions.
diff --git a/crane_robot_skills/include/crane_robot_skills/kick.hpp b/crane_robot_skills/include/crane_robot_skills/kick.hpp
@@ -99,6 +99,40 @@ class Kick : public SkillBaseWithState<KickState, RobotCommandWrapperPosition>
       return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0);
     });
 
+    addTransition(KickState::CHASE_BALL, KickState::POSITIVE_REDIRECT_KICK, [this]() {
+      return world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 10.0) &&
+             world_model()->ball.isMovingTowards(getParameter<Point>("target"), 30.0);
+    });
+
+    addStateFunction(
+      KickState::POSITIVE_REDIRECT_KICK,
+      [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) {
+        visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK");
+        // ボールラインに沿って追いかけつつ、角度はtargetへ向ける
+        const auto & ball_pos = world_model()->ball.pos;
+        command.lookAtFrom(getParameter<Point>("target"), ball_pos);
+
+        const auto & ball_vel_normed = world_model()->ball.vel.normalized();
+        Segment ball_line{ball_pos - ball_vel_normed * 10, ball_pos + ball_vel_normed * 10};
+        auto [distance, closest_point] = getClosestPointAndDistance(ball_pos, ball_line);
+        auto target_pos = [&]() -> Point {
+          if (distance < 0.1) {
+            return ball_pos + ball_vel_normed;
+          } else {
+            return closest_point + ball_vel_normed * distance;
+          }
+        }();
+        command.setDribblerTargetPosition(target_pos);
+        command.kickStraight(0.3);
+        command.disableBallAvoidance();
+        return Status::RUNNING;
+      });
+
+    addTransition(KickState::POSITIVE_REDIRECT_KICK, KickState::ENTRY_POINT, [this]() {
+      return !world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 10.0) or
+             !world_model()->ball.isMovingTowards(getParameter<Point>("target"), 30.0);
+    });
+
     addStateFunction(
       KickState::REDIRECT_KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) {
         visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK");