Skip to content

Commit

Permalink
world_model->getLargestGoalAngleRangeFromPointを使えるところで全部使っていく
Browse files Browse the repository at this point in the history
  • Loading branch information
@HansRobo
HansRobo committed Feb 23, 2024
1 parent e7c4050 commit d0a532b
Show file tree
Hide file tree
Showing 4 changed files with 13 additions and 123 deletions.
44 changes: 0 additions & 44 deletions crane_robot_skills/include/crane_robot_skills/receiver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,50 +44,6 @@ class Receiver : public SkillBase<>

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

auto getLargestGoalAngleWidthFromPosition(const Point point) -> double
{
Interval goal_range;

auto goal_posts = world_model->getTheirGoalPosts();
goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));

for (auto & enemy : world_model->theirs.robots) {
double distance = (point - enemy->pose.pos).norm();
constexpr double MACHINE_RADIUS = 0.1;

double center_angle = getAngle(enemy->pose.pos - point);
double diff_angle =
atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));

goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
}

auto largest_interval = goal_range.getLargestInterval();
return largest_interval.second - largest_interval.first;
}

auto getLargestGoalAngleFromPosition(const Point point) -> double
{
Interval goal_range;

auto goal_posts = world_model->getTheirGoalPosts();
goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));

for (auto & enemy : world_model->theirs.robots) {
double distance = (point - enemy->pose.pos).norm();
constexpr double MACHINE_RADIUS = 0.1;

double center_angle = getAngle(enemy->pose.pos - point);
double diff_angle =
atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));

goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
}

auto largest_interval = goal_range.getLargestInterval();
return (largest_interval.second + largest_interval.first) * 0.5;
}

std::vector<std::pair<double, Point>> getPositionsWithScore(Segment ball_line, Point next_target)
{
auto points = getPoints(ball_line, 0.05);
Expand Down
8 changes: 5 additions & 3 deletions crane_robot_skills/src/receiver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,8 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)

// ゴールとボールの中間方向を向く
// TODO(Hansobo): ボールの速さ・キッカーの強さでボールの反射する角度が変わるため、要考慮
auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point);
auto [goal_angle, width] =
world_model->getLargestGoalAngleRangeFromPoint(result.closest_point);
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model->ball.pos - result.closest_point).normalized();
double intermediate_angle = getAngle(2 * to_goal + to_ball);
Expand Down Expand Up @@ -74,7 +75,8 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)
continue;
}

double score = getLargestGoalAngleWidthFromPosition(dpps_point);
auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(dpps_point);
double score = width;
const double dist = (robot->pose.pos - dpps_point).norm();
score = score * (1.0 - dist / 10.0);

Expand All @@ -89,7 +91,7 @@ Receiver::Receiver(uint8_t id, const std::shared_ptr<WorldModelWrapper> & wm)

// ゴールとボールの中間方向を向く
Point target_pos{command->latest_msg.target_x.front(), command->latest_msg.target_y.front()};
auto goal_angle = getLargestGoalAngleFromPosition(target_pos);
auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(target_pos);
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model->ball.pos - target_pos).normalized();
command->setTargetTheta(getAngle(to_goal + to_ball));
Expand Down
32 changes: 3 additions & 29 deletions session/crane_planner_plugins/include/crane_planner_plugins/attacker_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,9 @@ class AttackerPlanner : public PlannerBase
crane::RobotCommandWrapper target(robot_id.robot_id, world_model);
auto robot = world_model->getRobot(robot_id);

auto [best_target, goal_angle_width] = getBestShootTargetWithWidth();
auto [best_angle, goal_angle_width] =
world_model->getLargestGoalAngleRangeFromPoint(world_model->ball.pos);
Point best_target = world_model->ball.pos + getNormVec(best_angle) * 0.5;

// シュートの隙がないときは仲間へパス
if (goal_angle_width < 0.07) {
Expand Down Expand Up @@ -97,34 +99,6 @@ class AttackerPlanner : public PlannerBase
return 100.0 / std::max(world_model->getSquareDistanceFromRobotToBall(robot->id), 0.01);
});
}

auto getBestShootTargetWithWidth() -> std::pair<Point, double>
{
const auto & ball = world_model->ball.pos;

Interval goal_range;

auto goal_posts = world_model->getTheirGoalPosts();
goal_range.append(getAngle(goal_posts.first - ball), getAngle(goal_posts.second - ball));

for (auto & enemy : world_model->theirs.robots) {
double distance = (ball - enemy->pose.pos).norm();
constexpr double MACHINE_RADIUS = 0.1;

double center_angle = getAngle(enemy->pose.pos - ball);
double diff_angle =
atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));

goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
}

auto largest_interval = goal_range.getLargestInterval();

double target_angle = (largest_interval.first + largest_interval.second) / 2.0;

return {
ball + getNormVec(target_angle) * 0.5, largest_interval.second - largest_interval.first};
}
};

} // namespace crane
Expand Down
52 changes: 5 additions & 47 deletions session/crane_planner_plugins/include/crane_planner_plugins/receive_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,8 @@ class ReceivePlanner : public PlannerBase
bg::closest_point(robot_info->pose.pos, ball_line, result);

// ゴールとボールの中間方向を向く
auto goal_angle = getLargestGoalAngleFromPosition(result.closest_point);
auto [goal_angle, width] =
world_model->getLargestGoalAngleRangeFromPoint(result.closest_point);
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model->ball.pos - result.closest_point).normalized();
double intermediate_angle = getAngle(2 * to_goal + to_ball);
Expand Down Expand Up @@ -130,7 +131,8 @@ class ReceivePlanner : public PlannerBase
continue;
}

double score = getLargestGoalAngleWidthFromPosition(dpps_point);
auto [angle, width] = world_model->getLargestGoalAngleRangeFromPoint(dpps_point);
double score = width;
const double dist = world_model->getDistanceFromRobot(robot, dpps_point);
score = score * (1.0 - dist / 10.0);

Expand All @@ -145,7 +147,7 @@ class ReceivePlanner : public PlannerBase

// ゴールとボールの中間方向を向く
Point target_pos{target.latest_msg.target_x.front(), target.latest_msg.target_y.front()};
auto goal_angle = getLargestGoalAngleFromPosition(target_pos);
auto [goal_angle, width] = world_model->getLargestGoalAngleRangeFromPoint(target_pos);
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model->ball.pos - target_pos).normalized();
target.setTargetTheta(getAngle(to_goal + to_ball));
Expand Down Expand Up @@ -250,50 +252,6 @@ class ReceivePlanner : public PlannerBase
return ret;
}

auto getLargestGoalAngleWidthFromPosition(const Point point) -> double
{
Interval goal_range;

auto goal_posts = world_model->getTheirGoalPosts();
goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));

for (auto & enemy : world_model->theirs.robots) {
double distance = (point - enemy->pose.pos).norm();
constexpr double MACHINE_RADIUS = 0.1;

double center_angle = getAngle(enemy->pose.pos - point);
double diff_angle =
atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));

goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
}

auto largest_interval = goal_range.getLargestInterval();
return largest_interval.second - largest_interval.first;
}

auto getLargestGoalAngleFromPosition(const Point point) -> double
{
Interval goal_range;

auto goal_posts = world_model->getTheirGoalPosts();
goal_range.append(getAngle(goal_posts.first - point), getAngle(goal_posts.second - point));

for (auto & enemy : world_model->theirs.robots) {
double distance = (point - enemy->pose.pos).norm();
constexpr double MACHINE_RADIUS = 0.1;

double center_angle = getAngle(enemy->pose.pos - point);
double diff_angle =
atan(MACHINE_RADIUS / std::sqrt(distance * distance - MACHINE_RADIUS * MACHINE_RADIUS));

goal_range.erase(center_angle - diff_angle, center_angle + diff_angle);
}

auto largest_interval = goal_range.getLargestInterval();
return (largest_interval.second + largest_interval.first) * 0.5;
}

std::vector<std::pair<double, Point>> getPositionsWithScore(Segment ball_line, Point next_target)
{
auto points = getPoints(ball_line, 0.05);
Expand Down

0 comments on commit d0a532b

Please sign in to comment.