Skip to content

Commit

Permalink
Merge branch 'develop' into fix/defense
Browse files Browse the repository at this point in the history
  • Loading branch information
@HansRobo
HansRobo authored Jan 16, 2025
2 parents 1e421b4 + e640773 commit 864f9b4
Show file tree
Hide file tree
Showing 6 changed files with 79 additions and 145 deletions.
1 change: 0 additions & 1 deletion crane_robot_skills/include/crane_robot_skills/attacker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@ namespace crane::skills
enum class AttackerState {
ENTRY_POINT,
FORCED_PASS,
CUT_THEIR_PASS,
STEAL_BALL,
REDIRECT_GOAL_KICK,
GOAL_KICK,
Expand Down
3 changes: 1 addition & 2 deletions crane_robot_skills/include/crane_robot_skills/kick.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,7 @@ namespace crane::skills
enum class KickState {
ENTRY_POINT,
CHASE_BALL,
AROUND_BALL,
KICK,
AROUND_BALL_AND_KICK,
REDIRECT_KICK,
POSITIVE_REDIRECT_KICK,
};
Expand Down
40 changes: 17 additions & 23 deletions crane_robot_skills/src/attacker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,22 +60,32 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
// パス
command.disableBallAvoidance();
kick_skill.setParameter("dot_threshold", 0.95);
kick_skill.setParameter("kick_power", 0.8);
int receiver_id = getParameter<int>("receiver_id");
if (receiver_id != -1) {
kick_target = world_model()->getOurRobot(receiver_id)->pose.pos;
}
kick_skill.setParameter("target", kick_target);
Segment kick_line{world_model()->ball.pos, kick_target};
// 近くに敵ロボットがいればチップキック
bool chip_kick = false;
if (const auto enemy_robots = world_model()->theirs.getAvailableRobots();
not enemy_robots.empty()) {
const auto & [nearest_enemy, enemy_distance] =
world_model()->getNearestRobotWithDistanceFromSegment(kick_line, enemy_robots);
if (enemy_distance < 0.4 && nearest_enemy->getDistance(world_model()->ball.pos) < 2.0) {
kick_skill.setParameter("kick_with_chip", true);
chip_kick = true;
}
}
if (chip_kick) {
kick_skill.setParameter("kick_with_chip", true);
kick_skill.setParameter("kick_power", 0.9);
kick_skill.setParameter("with_dribble", true);
kick_skill.setParameter("dribble_power", 0.7);
} else {
kick_skill.setParameter("kick_power", 0.2);
kick_skill.setParameter("kick_with_chip", false);
kick_skill.setParameter("dribble_power", 0.0);
}
kick_skill.run();
break;
}
Expand All @@ -85,23 +95,6 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
return Status::RUNNING;
});

addTransition(AttackerState::ENTRY_POINT, AttackerState::CUT_THEIR_PASS, [this]() -> bool {
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() or world_model()->ball.isStopped(0.2);
});

addStateFunction(AttackerState::CUT_THEIR_PASS, [this]() -> Status {
visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 0.5);
receive_skill.setParameter("enable_redirect", false);
receive_skill.setParameter("policy", std::string("min_slack"));
return receive_skill.run();
});

addTransition(AttackerState::ENTRY_POINT, AttackerState::STEAL_BALL, [this]() -> bool {
// 止まっているボールを相手が持っているとき
const auto enemy_robots = world_model()->theirs.getAvailableRobots();
Expand Down Expand Up @@ -168,10 +161,10 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
}
}();

receive_skill.setParameter("enable_redirect", false);
receive_skill.setParameter("enable_redirect", true);
receive_skill.setParameter("redirect_target", target);
receive_skill.setParameter("policy", std::string("closest"));
receive_skill.setParameter("redirect_kick_power", 0.8);
receive_skill.setParameter("redirect_kick_power", 0.2);
return receive_skill.run();
});

Expand Down Expand Up @@ -288,7 +281,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
kick_skill.setParameter("kick_with_chip", true);
}
}
kick_skill.setParameter("kick_power", 0.5);
kick_skill.setParameter("kick_power", 0.4);
kick_skill.setParameter("dot_threshold", 0.97);
return kick_skill.run();
});
Expand Down Expand Up @@ -361,7 +354,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
});

addStateFunction(AttackerState::RECEIVE_BALL, [this]() -> Status {
receive_skill.setParameter("enable_redirect", true);
receive_skill.setParameter("enable_redirect", false);
receive_skill.setParameter("policy", std::string("closest"));
receive_skill.setParameter("dribble_power", 0.0);
receive_skill.setParameter("enable_software_bumper", false);
return receive_skill.run();
Expand Down
13 changes: 11 additions & 2 deletions crane_robot_skills/src/goalie.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
// https://opensource.org/licenses/MIT.

#include <crane_robot_skills/goalie.hpp>
#include <robocup_ssl_msgs/msg/referee.hpp>

namespace crane::skills
{
Expand Down Expand Up @@ -35,9 +36,17 @@ Status Goalie::update()
phase = "ペナルティキック";
inplay(false);
break;
default:
inplay(true);
default: {
if (
world_model()->play_situation.getRefereeCommandID() ==
robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
// STOPのときにはボールを排出しない
inplay(false);
} else {
inplay(true);
}
break;
}
}

visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase);
Expand Down
166 changes: 50 additions & 116 deletions crane_robot_skills/src/kick.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
phase(getContextReference<std::string>("phase"))
{
setParameter("target", Point(0, 0));
setParameter("kick_power", 0.5f);
setParameter("kick_power", 0.7f);
setParameter("chip_kick", false);
setParameter("with_dribble", false);
setParameter("dribble_power", 0.3f);
Expand All @@ -39,51 +39,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
return Status::RUNNING;
});

addTransition(KickState::ENTRY_POINT, KickState::CHASE_BALL, [this]() {
return world_model()->ball.isMoving(getParameter<double>("moving_speed_threshold"));
});

addTransition(KickState::ENTRY_POINT, KickState::AROUND_BALL, [this]() { return true; });

addStateFunction(KickState::CHASE_BALL, [this]() {
std::stringstream state;
state << "Kick::CHASE_BALL::";
// メモ：ボールが近い時はボールから少しずらした位置を目指したほうがいいかも
auto [min_slack_pos, max_slack_pos] =
world_model()->getMinMaxSlackInterceptPoint({robot()}, 5.0, 0.1, -0.3, 1., 2.0);
if (min_slack_pos) {
state << "min_slack: " << min_slack_pos.value().x() << ", " << min_slack_pos.value().y();
command.setTargetPosition(min_slack_pos.value()).lookAtBallFrom(min_slack_pos.value());
} else {
// ball_lineとフィールドラインの交点を目指す
Point ball_exit_point = getBallExitPointFromField(0.3);
command.setTargetPosition(ball_exit_point)
.lookAtFrom(world_model()->ball.pos, ball_exit_point);
state << "ball_exit: " << ball_exit_point.x() << ", " << ball_exit_point.y();
}
visualizer->addPoint(robot()->pose.pos, 0, "", 1., state.str());
return Status::RUNNING;
});

addTransition(KickState::CHASE_BALL, KickState::AROUND_BALL, [this]() {
// ボールが止まったら回り込みへ
command.disableBallAvoidance();
return not world_model()->ball.isMoving(getParameter<double>("moving_speed_threshold"));
});

addTransition(KickState::CHASE_BALL, KickState::REDIRECT_KICK, [this]() {
// ボールライン上に乗ったらリダイレクトキックへ
command.disableBallAvoidance();
return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0) &&
getAngleDiff(
getAngle(world_model()->ball.vel),
getAngle(getParameter<Point>("target") - robot()->pose.pos) < M_PI / 2.0);
});

addTransition(KickState::CHASE_BALL, KickState::POSITIVE_REDIRECT_KICK, [this]() {
command.disableBallAvoidance();
return world_model()->ball.isMovingTowards(robot()->pose.pos, 10.0);
});
addTransition(KickState::ENTRY_POINT, KickState::AROUND_BALL_AND_KICK, [this]() { return true; });

addStateFunction(KickState::POSITIVE_REDIRECT_KICK, [this]() {
visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::POSITIVE_REDIRECT_KICK");
Expand Down Expand Up @@ -132,7 +88,7 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
return receive_skill->update();
});

addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL, [this]() {
addTransition(KickState::REDIRECT_KICK, KickState::AROUND_BALL_AND_KICK, [this]() {
// ボールが止まったら回り込みへ
return not world_model()->ball.isMoving(getParameter<double>("moving_speed_threshold"));
});
Expand All @@ -143,86 +99,64 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.);
});

addStateFunction(KickState::AROUND_BALL, [this]() {
visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL");
addStateFunction(KickState::AROUND_BALL_AND_KICK, [this]() {
auto target = getParameter<Point>("target");
Point ball_pos = world_model()->ball.pos;

// 経由ポイント
Point intermediate_point =
ball_pos + (ball_pos - target).normalized() * getParameter<double>("around_interval");
command.setTargetPosition(intermediate_point)
.setTerminalVelocity(0.1)
.lookAtFrom(target, ball_pos)
.enableCollisionAvoidance();

// ボールを避けて回り込む
if (((robot()->pose.pos - ball_pos).normalized()).dot((target - ball_pos).normalized()) > 0.1) {
Point around_point = [&]() {
Vector2 vertical_vec = getVerticalVec((target - ball_pos).normalized()) *
getParameter<double>("around_interval");
Point around_point1 = ball_pos + vertical_vec;
Point around_point2 = ball_pos - vertical_vec;
if (robot()->getDistance(around_point1) < robot()->getDistance(around_point2)) {
return around_point1;
} else {
return around_point2;
}
}();
command.setTargetPosition(around_point).lookAtFrom(target, ball_pos);
} else {
// 経由ポイントへGO
command.setTargetPosition(intermediate_point)
visualizer->addLine(ball_pos, ball_pos + (target - ball_pos).normalized() * 1.0, 1, "blue");
constexpr double SWITCH_DISTANCE = 1.0;
visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 0.);
if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) {
visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)");
command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3)
.lookAtFrom(target, ball_pos)
.enableCollisionAvoidance();
}
return Status::RUNNING;
});

addTransition(KickState::AROUND_BALL, KickState::KICK, [this]() {
// 中間地点に到達したらキックへ
Point intermediate_point =
world_model()->ball.pos +
(world_model()->ball.pos - getParameter<Point>("target")).normalized() *
getParameter<double>("around_interval");
return robot()->getDistance(intermediate_point) < 0.05 && robot()->vel.linear.norm() < 0.15;
});

addStateFunction(KickState::KICK, [this]() {
visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::KICK");
auto target = getParameter<Point>("target");
Point ball_pos = world_model()->ball.pos;
command.setTargetPosition(ball_pos + (target - ball_pos).normalized() * 0.1)
.setTerminalVelocity(0.5)
.disableCollisionAvoidance()
.disableBallAvoidance();
if (getParameter<bool>("chip_kick")) {
command.kickWithChip(getParameter<double>("kick_power"));
} else {
command.kickStraight(getParameter<double>("kick_power"));
}
if (getParameter<bool>("with_dribble")) {
command.dribble(getParameter<double>("dribble_power"));
.setTerminalVelocity(0.3);
return Status::RUNNING;
} else {
// ドリブラーを止める
command.withDribble(0.0);
visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）");
auto calculateRatio =
[](const double distance, const double min_distance, const double max_distance) {
return (distance - min_distance) / (max_distance - min_distance);
};

// ボールを避けて回り込む
using boost::math::constants::degree;
double ratio =
1.5 + std::clamp(
-calculateRatio(robot()->getDistance(world_model()->ball.pos), 0.2, 1.5), -0.5, 0.);

double move_direction = getAngle(target - robot()->pose.pos) +
(getAngleDiff(
getAngle(world_model()->ball.pos - robot()->pose.pos),
getAngle(target - robot()->pose.pos))) *
ratio;
Vector2 move_vec = getNormVec(move_direction);
command.lookAtFrom(target, ball_pos)
.setDribblerTargetPosition(
robot()->pose.pos + move_vec * 0.3 + world_model()->ball.vel * 0.4)
// .setTerminalVelocity(world_model()->ball.vel.norm())
.disableCollisionAvoidance()
.disableBallAvoidance();

if (getParameter<bool>("chip_kick")) {
command.kickWithChip(getParameter<double>("kick_power"));
} else {
command.kickStraight(getParameter<double>("kick_power"));
}
if (getParameter<bool>("with_dribble")) {
command.withDribble(getParameter<double>("dribble_power"));
} else {
// ドリブラーを止める
command.withDribble(0.0);
}
return Status::RUNNING;
}
return Status::RUNNING;
});

addTransition(KickState::KICK, KickState::ENTRY_POINT, [this]() {
addTransition(KickState::AROUND_BALL_AND_KICK, KickState::ENTRY_POINT, [this]() {
// 素早く遠ざかっていったら終了
return world_model()->ball.isMoving(getParameter<double>("kicked_speed_threshold")) &&
world_model()->ball.isMovingAwayFrom(robot()->pose.pos, 30.);
});

addTransition(KickState::KICK, KickState::ENTRY_POINT, [this]() -> bool {
// 素早く遠ざかっていったら終了
auto target = getParameter<Point>("target");
Point ball_pos = world_model()->ball.pos;
Point p = ball_pos + (target - ball_pos).normalized() * 0.3;
return robot()->getDistance(p) < 0.1;
});
}
auto Kick::getBallExitPointFromField(const double offset) -> Point
{
Expand Down
1 change: 0 additions & 1 deletion crane_world_model_publisher/src/world_model_publisher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -275,7 +275,6 @@ void WorldModelPublisherComponent::visionDetectionsCallback(const TrackedFrame &
int ball_info_history_size = ball_info_history.size();
if (1 > ball_info_history_size) {
auto last_ball_info_history = ball_info_history[ball_info_history_size - 1];
// double elapsed_time_since_last_detected = tracked_frame.timestamp() - last_ball_info_history.detection_time;
double elapsed_time_since_last_detected = (now() - last_ball_detect_time).seconds();

// 0.5secビジョンから見えていなければ見失った
Expand Down

0 comments on commit 864f9b4

Please sign in to comment.