Skip to content

Commit

Permalink
ペナルティエリア回避の修正 (#578)
Browse files Browse the repository at this point in the history
  • Loading branch information
@HansRobo
HansRobo authored Oct 9, 2024
1 parent 20b925c commit 49cb314
Show file tree
Hide file tree
Showing 3 changed files with 103 additions and 21 deletions.
101 changes: 83 additions & 18 deletions crane_local_planner/src/rvo2_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -222,40 +222,105 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
Point target_pos;
target_pos << command.position_target_mode.front().target_x,
command.position_target_mode.front().target_y;

bool is_near_our_penalty_area =
(std::signbit(world_model->getOurGoalCenter().x()) == std::signbit(command.current_pose.x));
Box penalty_area = [&]() {
if (is_near_our_penalty_area) {
return world_model->getOurPenaltyArea();
} else {
return world_model->getTheirPenaltyArea();
}
}();
const Point goal_pos = [&]() {
if (is_near_our_penalty_area) {
return world_model->getOurGoalCenter();
} else {
return world_model->getTheirGoalCenter();
}
}();
if (not command.local_planner_config.disable_goal_area_avoidance) {
bool is_in_our_penalty_area = isInBox(world_model->getOurPenaltyArea(), target_pos);
bool is_in_their_penalty_area = isInBox(world_model->getTheirPenaltyArea(), target_pos);
if (is_in_our_penalty_area or is_in_their_penalty_area) {
bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2);
constexpr double SURROUNDING_OFFSET = 0.3;
constexpr double PENALTY_AREA_OFFSET = 0.1;
if (isInBox(
penalty_area, Point(command.current_pose.x, command.current_pose.y),
PENALTY_AREA_OFFSET)) {
// 目標点をペナルティエリアの外に出るようにする
target_pos = Point(command.current_pose.x, command.current_pose.y);
while (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
target_pos +=
(target_pos - goal_pos).normalized() * 0.05; // ゴールから5cmずつ離れていく
}
} else if (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
// ペナルティエリア内にいる場合は、ペナルティエリアの外に出るようにする
const Point goal_pos = [&]() {
if (is_in_our_penalty_area) {
return world_model->getOurGoalCenter();
} else {
return world_model->getTheirGoalCenter();
}
}();

// ゴールの後ろに回り込んだ場合は、ゴールの前に出るようにする
if (std::abs(target_pos.x()) > world_model->field_size.x() / 2.0) {
target_pos.x() = std::copysign(world_model->field_size.x() / 2.0, target_pos.x());
}

// 目標点をペナルティエリアの外に出るようにする
while ([&]() {
if (is_in_our_penalty_area) {
return isInBox(world_model->getOurPenaltyArea(), target_pos);
} else {
return isInBox(world_model->getTheirPenaltyArea(), target_pos);
}
}()) {
while (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
target_pos +=
(target_pos - goal_pos).normalized() * 0.05; // ゴールから5cmずつ離れていく
}
}
// ペナルティエリアを通り抜ける場合は、一旦角に
const Point current_pos = Point(command.current_pose.x, command.current_pose.y);
Segment move_line(current_pos, target_pos);
if (bg::intersects(move_line, penalty_area)) {
const auto penalty_area_size = world_model->penalty_area_size;
Point corner_1 = goal_pos + Point(
std::copysign(penalty_area_size.x(), -goal_pos.x()),
world_model->penalty_area_size.y() * 0.5);
Point around_corner_1 =
goal_pos + Point(
std::copysign(penalty_area_size.x() + SURROUNDING_OFFSET, -goal_pos.x()),
world_model->penalty_area_size.y() * 0.5 + SURROUNDING_OFFSET);

Point corner_2 = goal_pos + Point(
std::copysign(penalty_area_size.x(), -goal_pos.x()),
-world_model->penalty_area_size.y() * 0.5);
Point around_corner_2 =
goal_pos + Point(
std::copysign(penalty_area_size.x() + SURROUNDING_OFFSET, -goal_pos.x()),
-world_model->penalty_area_size.y() * 0.5 - SURROUNDING_OFFSET);

auto [distance_1, closest_point_1] = getClosestPointAndDistance(corner_1, move_line);
auto [distance_2, closest_point_2] = getClosestPointAndDistance(corner_2, move_line);

const double penalty_area_min_x = world_model->field_size.x() * 0.5 -
world_model->penalty_area_size.x() -
PENALTY_AREA_OFFSET;
if (
std::abs(closest_point_1.x()) > penalty_area_min_x &&
std::abs(closest_point_2.x()) > penalty_area_min_x) {
// 横切る場合は、近い方の角に向かう
if (bg::distance(corner_1, current_pos) < bg::distance(corner_2, current_pos)) {
target_pos = around_corner_1;
} else {
target_pos = around_corner_2;
}
} else if (isInBox(penalty_area, closest_point_1, PENALTY_AREA_OFFSET)) {
target_pos = around_corner_1;
} else if (isInBox(penalty_area, closest_point_2, PENALTY_AREA_OFFSET)) {
target_pos = around_corner_2;
} else {
std::stringstream what;
what << "Failed to find a target position outside the penalty area.";
what << " current_pos: " << current_pos.x() << ", " << current_pos.y();
what << " target_pos: " << target_pos.x() << ", " << target_pos.y();
what << " closest_point_1: " << closest_point_1.x() << ", " << closest_point_1.y();
what << " closest_point_2: " << closest_point_2.x() << ", " << closest_point_2.y();
throw std::runtime_error(what.str());
}
}
}

command.position_target_mode.front().target_x = target_pos.x();
command.position_target_mode.front().target_y = target_pos.y();
visualizer->addLine(
target_pos, Point(command.current_pose.x, command.current_pose.y), 1, "yellow");
}
}
}
Expand Down
17 changes: 17 additions & 0 deletions docs/rvo2_local_planner.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,3 +25,20 @@ SSLでは単に障害物を回避するだけではなく、ルールによる
よって、ボールプレイスメントエリアの回避は専用のPlanner作成するのが良い。
幸い、ボールプレイスメント中はエリアを避けることしか考えなくて良いので、非常にシンプルなPlannerを書くことができる。
（厳密には、ボールプレイスメント後の敵のフリーキックのことを考える必要はあるが...）

### ペナルティエリアの回避

#### 点の回避

目標位置、現在位置がペナルティエリアにはいったときに適用する。

1. 位置Aをゴール中心から遠ざかる方向に0.1m移動する
2. 位置Aがペナルティエリア内にある場合、1に戻る
3. 位置Aを最終目標位置として設定する

#### 経路の回避

目標位置・現在位置共にペナルティエリアに入っていない場合でも、経路がペナルティエリアを通過する場合がある。

1. 経路とペナルティエリアが衝突するか調べる。衝突しない場合、なにもしない
2. ま
6 changes: 3 additions & 3 deletions session/crane_planner_plugins/src/defender_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ DefenderPlanner::calculateRobotCommand(const std::vector<RobotIdentifier> & robo
}

auto ball = world_model->ball.pos;
[[maybe_unused]] const double OFFSET_X = 0.1;
[[maybe_unused]] const double OFFSET_Y = 0.1;
[[maybe_unused]] const double OFFSET_X = 0.2;
[[maybe_unused]] const double OFFSET_Y = 0.2;

//
// calc ball line
Expand Down Expand Up @@ -164,7 +164,7 @@ std::vector<Point> DefenderPlanner::getDefenseLinePoints(
double lower_parameter = upper_parameter;

auto add_parameter = [&](double parameter) -> bool {
const double OFFSET_X = 0.1, OFFSET_Y = 0.1;
const double OFFSET_X = 0.2, OFFSET_Y = 0.2;
auto [threshold1, threshold2, threshold3] =
getDefenseLinePointParameterThresholds(OFFSET_X, OFFSET_Y);
if (parameter < 0. || parameter > threshold3) {
Expand Down

0 comments on commit 49cb314

Please sign in to comment.