Skip to content

Commit 49cb314

Browse files
HansRoboHansRobo
authored
ペナルティエリア回避の修正 (#578)
1 parent 20b925c commit 49cb314

File tree

3 files changed

+103
-21
lines changed

3 files changed

+103
-21
lines changed

‎crane_local_planner/src/rvo2_planner.cpp

Lines changed: 83 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -222,40 +222,105 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
222222
Point target_pos;
223223
target_pos << command.position_target_mode.front().target_x,
224224
command.position_target_mode.front().target_y;
225+
226+
bool is_near_our_penalty_area =
227+
(std::signbit(world_model->getOurGoalCenter().x()) == std::signbit(command.current_pose.x));
228+
Box penalty_area = [&]() {
229+
if (is_near_our_penalty_area) {
230+
return world_model->getOurPenaltyArea();
231+
} else {
232+
return world_model->getTheirPenaltyArea();
233+
}
234+
}();
235+
const Point goal_pos = [&]() {
236+
if (is_near_our_penalty_area) {
237+
return world_model->getOurGoalCenter();
238+
} else {
239+
return world_model->getTheirGoalCenter();
240+
}
241+
}();
225242
if (not command.local_planner_config.disable_goal_area_avoidance) {
226-
bool is_in_our_penalty_area = isInBox(world_model->getOurPenaltyArea(), target_pos);
227-
bool is_in_their_penalty_area = isInBox(world_model->getTheirPenaltyArea(), target_pos);
228-
if (is_in_our_penalty_area or is_in_their_penalty_area) {
243+
bool is_in_penalty_area = isInBox(penalty_area, target_pos, 0.2);
244+
constexpr double SURROUNDING_OFFSET = 0.3;
245+
constexpr double PENALTY_AREA_OFFSET = 0.1;
246+
if (isInBox(
247+
penalty_area, Point(command.current_pose.x, command.current_pose.y),
248+
PENALTY_AREA_OFFSET)) {
249+
// 目標点をペナルティエリアの外に出るようにする
250+
target_pos = Point(command.current_pose.x, command.current_pose.y);
251+
while (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
252+
target_pos +=
253+
(target_pos - goal_pos).normalized() * 0.05; // ゴールから5cmずつ離れていく
254+
}
255+
} else if (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
229256
// ペナルティエリア内にいる場合は、ペナルティエリアの外に出るようにする
230-
const Point goal_pos = [&]() {
231-
if (is_in_our_penalty_area) {
232-
return world_model->getOurGoalCenter();
233-
} else {
234-
return world_model->getTheirGoalCenter();
235-
}
236-
}();
237-
238257
// ゴールの後ろに回り込んだ場合は、ゴールの前に出るようにする
239258
if (std::abs(target_pos.x()) > world_model->field_size.x() / 2.0) {
240259
target_pos.x() = std::copysign(world_model->field_size.x() / 2.0, target_pos.x());
241260
}
242261

243262
// 目標点をペナルティエリアの外に出るようにする
244-
while ([&]() {
245-
if (is_in_our_penalty_area) {
246-
return isInBox(world_model->getOurPenaltyArea(), target_pos);
247-
} else {
248-
return isInBox(world_model->getTheirPenaltyArea(), target_pos);
249-
}
250-
}()) {
263+
while (isInBox(penalty_area, target_pos, PENALTY_AREA_OFFSET)) {
251264
target_pos +=
252265
(target_pos - goal_pos).normalized() * 0.05; // ゴールから5cmずつ離れていく
253266
}
254267
}
268+
// ペナルティエリアを通り抜ける場合は、一旦角に
269+
const Point current_pos = Point(command.current_pose.x, command.current_pose.y);
270+
Segment move_line(current_pos, target_pos);
271+
if (bg::intersects(move_line, penalty_area)) {
272+
const auto penalty_area_size = world_model->penalty_area_size;
273+
Point corner_1 = goal_pos + Point(
274+
std::copysign(penalty_area_size.x(), -goal_pos.x()),
275+
world_model->penalty_area_size.y() * 0.5);
276+
Point around_corner_1 =
277+
goal_pos + Point(
278+
std::copysign(penalty_area_size.x() + SURROUNDING_OFFSET, -goal_pos.x()),
279+
world_model->penalty_area_size.y() * 0.5 + SURROUNDING_OFFSET);
280+
281+
Point corner_2 = goal_pos + Point(
282+
std::copysign(penalty_area_size.x(), -goal_pos.x()),
283+
-world_model->penalty_area_size.y() * 0.5);
284+
Point around_corner_2 =
285+
goal_pos + Point(
286+
std::copysign(penalty_area_size.x() + SURROUNDING_OFFSET, -goal_pos.x()),
287+
-world_model->penalty_area_size.y() * 0.5 - SURROUNDING_OFFSET);
288+
289+
auto [distance_1, closest_point_1] = getClosestPointAndDistance(corner_1, move_line);
290+
auto [distance_2, closest_point_2] = getClosestPointAndDistance(corner_2, move_line);
291+
292+
const double penalty_area_min_x = world_model->field_size.x() * 0.5 -
293+
world_model->penalty_area_size.x() -
294+
PENALTY_AREA_OFFSET;
295+
if (
296+
std::abs(closest_point_1.x()) > penalty_area_min_x &&
297+
std::abs(closest_point_2.x()) > penalty_area_min_x) {
298+
// 横切る場合は、近い方の角に向かう
299+
if (bg::distance(corner_1, current_pos) < bg::distance(corner_2, current_pos)) {
300+
target_pos = around_corner_1;
301+
} else {
302+
target_pos = around_corner_2;
303+
}
304+
} else if (isInBox(penalty_area, closest_point_1, PENALTY_AREA_OFFSET)) {
305+
target_pos = around_corner_1;
306+
} else if (isInBox(penalty_area, closest_point_2, PENALTY_AREA_OFFSET)) {
307+
target_pos = around_corner_2;
308+
} else {
309+
std::stringstream what;
310+
what << "Failed to find a target position outside the penalty area.";
311+
what << " current_pos: " << current_pos.x() << ", " << current_pos.y();
312+
what << " target_pos: " << target_pos.x() << ", " << target_pos.y();
313+
what << " closest_point_1: " << closest_point_1.x() << ", " << closest_point_1.y();
314+
what << " closest_point_2: " << closest_point_2.x() << ", " << closest_point_2.y();
315+
throw std::runtime_error(what.str());
316+
}
317+
}
255318
}
256319

257320
command.position_target_mode.front().target_x = target_pos.x();
258321
command.position_target_mode.front().target_y = target_pos.y();
322+
visualizer->addLine(
323+
target_pos, Point(command.current_pose.x, command.current_pose.y), 1, "yellow");
259324
}
260325
}
261326
}

‎docs/rvo2_local_planner.md

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -25,3 +25,20 @@ SSLでは単に障害物を回避するだけではなく、ルールによる
2525
よって、ボールプレイスメントエリアの回避は専用のPlanner作成するのが良い。
2626
幸い、ボールプレイスメント中はエリアを避けることしか考えなくて良いので、非常にシンプルなPlannerを書くことができる。
2727
（厳密には、ボールプレイスメント後の敵のフリーキックのことを考える必要はあるが...）
28+
29+
### ペナルティエリアの回避
30+
31+
#### 点の回避
32+
33+
目標位置、現在位置がペナルティエリアにはいったときに適用する。
34+
35+
1. 位置Aをゴール中心から遠ざかる方向に0.1m移動する
36+
2. 位置Aがペナルティエリア内にある場合、1に戻る
37+
3. 位置Aを最終目標位置として設定する
38+
39+
#### 経路の回避
40+
41+
目標位置・現在位置共にペナルティエリアに入っていない場合でも、経路がペナルティエリアを通過する場合がある。
42+
43+
1. 経路とペナルティエリアが衝突するか調べる。衝突しない場合、なにもしない
44+
2. ま

‎session/crane_planner_plugins/src/defender_planner.cpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -16,8 +16,8 @@ DefenderPlanner::calculateRobotCommand(const std::vector<RobotIdentifier> & robo
1616
}
1717

1818
auto ball = world_model->ball.pos;
19-
[[maybe_unused]] const double OFFSET_X = 0.1;
20-
[[maybe_unused]] const double OFFSET_Y = 0.1;
19+
[[maybe_unused]] const double OFFSET_X = 0.2;
20+
[[maybe_unused]] const double OFFSET_Y = 0.2;
2121

2222
//
2323
// calc ball line
@@ -164,7 +164,7 @@ std::vector<Point> DefenderPlanner::getDefenseLinePoints(
164164
double lower_parameter = upper_parameter;
165165

166166
auto add_parameter = [&](double parameter) -> bool {
167-
const double OFFSET_X = 0.1, OFFSET_Y = 0.1;
167+
const double OFFSET_X = 0.2, OFFSET_Y = 0.2;
168168
auto [threshold1, threshold2, threshold3] =
169169
getDefenseLinePointParameterThresholds(OFFSET_X, OFFSET_Y);
170170
if (parameter < 0. || parameter > threshold3) {

0 commit comments

Comments
 (0)