Skip to content

Commit

Permalink
実装を分離 (#607)
Browse files Browse the repository at this point in the history
  • Loading branch information
@HansRobo
HansRobo authored Oct 12, 2024
1 parent dd0e390 commit c6d7848
Show file tree
Hide file tree
Showing 9 changed files with 605 additions and 643 deletions.
92 changes: 2 additions & 90 deletions crane_robot_skills/include/crane_robot_skills/ball_nearby_positioner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,97 +16,9 @@ namespace crane::skills
class BallNearByPositioner : public SkillBase<RobotCommandWrapperPosition>
{
public:
explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base)
: SkillBase("BallNearByPositioner", base)
{
// このロボットのインデックス
setParameter("current_robot_index", 0);
setParameter("total_robot_number", 1);
// 整列ポリシー（arc/straight）
setParameter("line_policy", std::string("arc"));
// ボールの位置決めポリシー（goal/pass）
setParameter("positioning_policy", std::string("goal"));
// 整列距離
setParameter("robot_interval", 0.3);
setParameter("margin_distance", 0.3);
}
explicit BallNearByPositioner(RobotCommandWrapperBase::SharedPtr & base);

Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
{
auto situation = world_model()->play_situation.getSituationCommandID();
double distance_from_ball = [&]() {
switch (situation) {
case crane_msgs::msg::PlaySituation::THEIR_DIRECT_FREE:
return 0.5;
case crane_msgs::msg::PlaySituation::THEIR_INDIRECT_FREE:
return 0.5;
case crane_msgs::msg::PlaySituation::STOP:
return 0.5;
case crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT:
return 0.5;
default:
return 0.0;
}
}();

distance_from_ball += getParameter<double>("margin_distance");
double normalized_offset =
(getParameter<int>("total_robot_number") - getParameter<int>("current_robot_index") - 1) / 2.;
double offset = normalized_offset * getParameter<double>("robot_interval");
Point base_position =
world_model()->ball.pos +
[&](const std::string & policy) {
if (policy == "goal") {
return (world_model()->getOurGoalCenter() - world_model()->ball.pos).normalized();
} else if (policy == "pass") {
// 2番目に近いロボット
auto theirs = world_model()->theirs.getAvailableRobots();
if (theirs.size() > 2) {
auto nearest_robot =
world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
theirs.erase(
std::remove_if(
theirs.begin(), theirs.end(),
[&](const auto & r) { return r->id == nearest_robot.first->id; }),
theirs.end());
auto second_nearest_robot =
world_model()->getNearestRobotWithDistanceFromPoint(world_model()->ball.pos, theirs);
return (second_nearest_robot.first->pose.pos - world_model()->ball.pos).normalized();
} else {
throw std::runtime_error(
"[BallNearByPositioner] 「positioning policy: "
"pass」の計算には少なくとも2台の敵ロボットが必要です");
}
} else {
throw std::runtime_error(
"[BallNearByPositioner] "
"予期しないパラメータ「positioning_policy」が入力されています: " +
policy);
}
}(getParameter<std::string>("positioning_policy")) *
distance_from_ball;

Point target_position = [&](const std::string & policy) -> Point {
Vector2 ball_to_base = (base_position - world_model()->ball.pos);
if (policy == "arc") {
double base_angle = getAngle(ball_to_base);
// r x theta = interval
// theta = interval / r
double angle_interval = getParameter<double>("robot_interval") / distance_from_ball;
return world_model()->ball.pos +
getNormVec(base_angle + normalized_offset * angle_interval) * distance_from_ball;
} else if (policy == "straight") {
return ball_to_base + getVerticalVec(ball_to_base.normalized()) * offset;
} else {
throw std::runtime_error(
"[BallNearByPositioner] 予期しないパラメータ「line_policy」が入力されています: " +
policy);
}
}(getParameter<std::string>("line_policy"));

command.setTargetPosition(target_position).lookAtBall();
return Status::RUNNING;
}
Status update([[maybe_unused]] const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;

void print(std::ostream & os) const override { os << "[BallNearByPositioner]"; }
};
Expand Down
94 changes: 0 additions & 94 deletions crane_robot_skills/include/crane_robot_skills/cut_pass.hpp
View file

This file was deleted.

62 changes: 3 additions & 59 deletions crane_robot_skills/include/crane_robot_skills/goal_kick.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,73 +17,17 @@ namespace crane::skills
class GoalKick : public SkillBase<RobotCommandWrapperPosition>
{
public:
explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base)
: SkillBase("GoalKick", base), kick_skill(base)
{
setParameter("キック角度の最低要求精度[deg]", 1.0);
setParameter("dot_threshold", 0.95);
kick_skill.setParameter("kick_power", 0.8);
kick_skill.setParameter("chip_kick", false);
kick_skill.setParameter("with_dribble", false);
kick_skill.setParameter("dot_threshold", getParameter<double>("dot_threshold"));
}
explicit GoalKick(RobotCommandWrapperBase::SharedPtr & base);

Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override
{
double best_angle = getBestAngleToShootFromPoint(
getParameter<double>("キック角度の最低要求精度[deg]") * M_PI / 180., world_model()->ball.pos,
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);
}
Status update(const ConsaiVisualizerWrapper::SharedPtr & visualizer) override;

void print(std::ostream & os) const override { os << "[GoalKick] "; }

Kick kick_skill;

static double getBestAngleToShootFromPoint(
double minimum_angle_accuracy, const Point from_point,
const WorldModelWrapper::SharedPtr & world_model)
{
auto [best_angle, goal_angle_width] =
world_model->getLargestGoalAngleRangeFromPoint(from_point);
if (goal_angle_width < 0.) {
// ゴールが見えない場合はgoal_angle_widthが負になる
// その場合は相手ゴール中心を狙う
best_angle = getAngle(world_model->getTheirGoalCenter() - from_point);
}
// 隙間のなかで更に良い角度を計算する。
// キック角度の最低要求精度をオフセットとしてできるだけ端っこを狙う
if (goal_angle_width < minimum_angle_accuracy * 2.0) {
double best_angle1, best_angle2;
best_angle1 = best_angle - goal_angle_width / 2.0 + minimum_angle_accuracy;
best_angle2 = best_angle + goal_angle_width / 2.0 - minimum_angle_accuracy;
Point their_goalie_pos = [&]() -> Point {
const auto & enemy_robots = world_model->theirs.getAvailableRobots();
if (not enemy_robots.empty()) {
return world_model
->getNearestRobotWithDistanceFromPoint(world_model->getTheirGoalCenter(), enemy_robots)
.first->pose.pos;
} else {
return world_model->getTheirGoalCenter();
}
}();
double their_goalie_angle = getAngle(their_goalie_pos - from_point);
// 敵ゴールキーパーから角度差が大きい方を選択
if (
std::abs(getAngleDiff(their_goalie_angle, best_angle1)) <
std::abs(getAngleDiff(their_goalie_angle, best_angle2))) {
best_angle = best_angle2;
} else {
best_angle = best_angle1;
}
}
return best_angle;
}
const WorldModelWrapper::SharedPtr & world_model);
};
} // namespace crane::skills
#endif // CRANE_ROBOT_SKILLS__GOAL_KICK_HPP_
Loading

0 comments on commit c6d7848

Please sign in to comment.