diff --git a/crane_robot_skills/include/crane_robot_skills/kick.hpp b/crane_robot_skills/include/crane_robot_skills/kick.hpp index b1c6bf5d..33cdfd4f 100644 --- 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 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("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("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("target"), 30.0); + }); + addStateFunction( KickState::REDIRECT_KICK, [this](const ConsaiVisualizerWrapper::SharedPtr & visualizer) { visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK");