Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

ヴィジュアライザ関連 #714

Merged
merged 2 commits into from
Jan 31, 2025
Merged

ヴィジュアライザ関連 #714

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b2692d8
旧ヴィジュアライザ用のコードを削除
HansRobo Jan 30, 2025
397ad24
太く見やすく
HansRobo Jan 30, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 2 additions & 14 deletions crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,10 +63,8 @@ class KickEventDetector
.radius(0.5)
.stroke("blue")
.fill("blue")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
// visualizer->addCircle(
// world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{true, id});
}
for (const auto & id : detected_bots.enemies) {
Expand All @@ -75,10 +73,8 @@ class KickEventDetector
.radius(0.5)
.stroke("blue")
.fill("blue")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
// visualizer->addCircle(
// world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{false, id});
}

Expand All @@ -101,15 +97,7 @@ class KickEventDetector
.stroke("red", 0.3)
.strokeWidth(200);
visualizer->add(line_builder.getSvgString());
// visualizer->addTube(
// world_model.ball.pos, ongoing_kick_origin.value().position,
// 0.2, 2, "red", "", 1.0, "KICK");
}

// キックの履歴を可視化
// for (const auto & [kick_origin, kick_end] : kick_history) {
// visualizer->addLine(kick_origin.position, kick_end, 2, "red", 0.5, "KICK");
// }
}

bool hasInterruptedOnGoingKick(const WorldModelWrapper & world_model) const
Expand Down
6 changes: 1 addition & 5 deletions crane_local_planner/include/crane_local_planner/simple_planner.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,17 +40,13 @@ class SimplePlanner : public LocalPlannerBase
for (auto & command : commands.robot_commands) {
auto robot = world_model->getOurRobot(command.robot_id);
if (not command.position_target_mode.empty()) {
// visualizer->addLine(
// robot->pose.pos.x(), robot->pose.pos.y(),
// command.position_target_mode.front().target_x,
// command.position_target_mode.front().target_y, 1);
SvgLineBuilder line_builder;
line_builder.start(robot->pose.pos)
.end(
command.position_target_mode.front().target_x,
command.position_target_mode.front().target_y)
.stroke("red")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(line_builder.getSvgString());
}
if (command.local_planner_config.max_velocity > MAX_VEL) {
Expand Down
8 changes: 4 additions & 4 deletions crane_local_planner/src/crane_local_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
raw_command.position_target_mode.front().target_x,
raw_command.position_target_mode.front().target_y)
.stroke("yellow", 0.3)
.strokeWidth(2);
.strokeWidth(20);
planner->visualizer->add(line_builder.getSvgString());
}
break;
Expand Down Expand Up @@ -132,7 +132,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
command.position_target_mode.front().target_x,
command.position_target_mode.front().target_y)
.stroke("yellow", 0.5)
.strokeWidth(1);
.strokeWidth(10);
planner->visualizer->add(line_builder.getSvgString());
} break;
case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
Expand All @@ -147,7 +147,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
robot->pose.pos.x() + command.simple_velocity_target_mode.front().target_vx * 0.5,
robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy * 0.5)
.stroke("white", 0.5)
.strokeWidth(1);
.strokeWidth(10);
planner->visualizer->add(line_builder.getSvgString());
} break;
case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
Expand All @@ -170,7 +170,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
std::sin(command.polar_velocity_target_mode.front().target_velocity_theta))
.stroke("white", 0.5)
.strokeWidth(1);
.strokeWidth(10);
planner->visualizer->add(line_builder.getSvgString());
} break;
}
Expand Down
6 changes: 2 additions & 4 deletions crane_robot_skills/src/attacker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -110,13 +110,12 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
});

addStateFunction(AttackerState::STEAL_BALL, [this]() -> Status {
// visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0);
SvgCircleBuilder circle_builder;
circle_builder.center(world_model()->ball.pos)
.radius(0.25)
.stroke("blue")
.fill("white")
.strokeWidth(1);
.strokeWidth(10);
visualizer->add(circle_builder.getSvgString());

return steal_ball_skill.run();
Expand Down Expand Up @@ -278,9 +277,8 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
auto our_robots = world_model()->ours.getAvailableRobots(robot()->id);
const auto enemy_robots = world_model()->theirs.getAvailableRobots();

// visualizer->addLine(world_model()->ball.pos, kick_target, 1, "red");
SvgLineBuilder line_builder;
line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(1);
line_builder.start(world_model()->ball.pos).end(kick_target).stroke("red").strokeWidth(10);
visualizer->add(line_builder.getSvgString());

kick_skill.setParameter("target", kick_target);
Expand Down
3 changes: 1 addition & 2 deletions crane_robot_skills/src/goal_kick.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,9 +27,8 @@ Status GoalKick::update()
world_model());

Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5;
// visualizer->addLine(world_model()->ball.pos, target, 2, "red");
SvgLineBuilder line_builder;
line_builder.start(world_model()->ball.pos).end(target).stroke("red").strokeWidth(2);
line_builder.start(world_model()->ball.pos).end(target).stroke("red").strokeWidth(20);
visualizer->add(line_builder.getSvgString());
kick_skill.setParameter("target", target);
kick_skill.setParameter("dot_threshold", getParameter<double>("dot_threshold"));
Expand Down
4 changes: 1 addition & 3 deletions crane_robot_skills/src/goalie.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,6 @@ Status Goalie::update()
}
}

// visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(phase)
Expand Down Expand Up @@ -87,9 +86,8 @@ void Goalie::emitBallFromPenaltyArea()
}
}();

// visualizer->addLine(ball, pass_target, 1, "blue");
SvgLineBuilder line_builder;
line_builder.start(ball).end(pass_target).stroke("blue").strokeWidth(1);
line_builder.start(ball).end(pass_target).stroke("blue").strokeWidth(10);
visualizer->add(line_builder.getSvgString());

Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f;
Expand Down
11 changes: 2 additions & 9 deletions crane_robot_skills/src/kick.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,6 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
receive_skill->setParameter("redirect_kick_power", 0.3);

addStateFunction(KickState::ENTRY_POINT, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::ENTRY_POINT");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("Kick::ENTRY_POINT")
Expand All @@ -48,7 +47,6 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
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");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("Kick::POSITIVE_REDIRECT_KICK")
Expand Down Expand Up @@ -89,7 +87,6 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
});

addStateFunction(KickState::REDIRECT_KICK, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("Kick::REDIRECT_KICK")
Expand Down Expand Up @@ -120,22 +117,19 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
addStateFunction(KickState::AROUND_BALL_AND_KICK, [this]() {
auto target = getParameter<Point>("target");
Point ball_pos = world_model()->ball.pos;
// visualizer->addLine(ball_pos, ball_pos + (target - ball_pos).normalized() * 1.0, 1, "blue");
SvgLineBuilder line_builder;
line_builder.start(ball_pos)
.end(ball_pos + (target - ball_pos).normalized() * 1.0)
.stroke("blue")
.strokeWidth(1);
.strokeWidth(10);
visualizer->add(line_builder.getSvgString());
constexpr double SWITCH_DISTANCE = 1.0;
// visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 0.);
{
SvgCircleBuilder circle_builder;
circle_builder.center(ball_pos).radius(SWITCH_DISTANCE).stroke("yellow").strokeWidth(1);
circle_builder.center(ball_pos).radius(SWITCH_DISTANCE).stroke("yellow").strokeWidth(10);
visualizer->add(circle_builder.getSvgString());
}
if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) {
// visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)");
{
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
Expand All @@ -149,7 +143,6 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
.setTerminalVelocity(0.3);
return Status::RUNNING;
} else {
// visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）");
{
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
Expand Down
7 changes: 2 additions & 5 deletions crane_robot_skills/src/marker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,17 +38,14 @@ Status Marker::update()
}
command.setTargetPosition(marking_point, 0.1).setTargetTheta(target_theta);

// visualizer->addCircle(enemy_pos, 0.3, 1, "black", "");
SvgCircleBuilder circle_builder;
circle_builder.center(enemy_pos).radius(0.3).stroke("black").strokeWidth(1);
circle_builder.center(enemy_pos).radius(0.3).stroke("black").strokeWidth(10);
visualizer->add(circle_builder.getSvgString());
// visualizer->addLine(
// robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black");
SvgLineBuilder line_builder;
line_builder.start(robot()->pose.pos)
.end(enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3)
.stroke("black")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(line_builder.getSvgString());
return Status::RUNNING;
}
Expand Down
3 changes: 1 addition & 2 deletions crane_robot_skills/src/penalty_kick.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,8 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base)
double best_angle = GoalKick::getBestAngleToShootFromPoint(
minimum_angle_accuracy, world_model()->ball.pos, world_model());
Point best_target = world_model()->ball.pos + getNormVec(best_angle) * 0.5;
// visualizer->addPoint(best_target.x(), best_target.y(), 1, "red", 1.0, "best_target");
SvgCircleBuilder circle_builder;
circle_builder.center(best_target).radius(0.1).stroke("red").strokeWidth(1);
circle_builder.center(best_target).radius(0.1).stroke("red").strokeWidth(10);

kick_skill.setParameter("target", best_target);

Expand Down
6 changes: 2 additions & 4 deletions crane_robot_skills/src/receive.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,8 @@ Status Receive::update()
}();
Point interception_point = getInterceptionPoint() + offset;

// visualizer->addLine(interception_point, robot()->pose.pos, 1, "red", 1., "intercept");
SvgLineBuilder line_builder;
line_builder.start(interception_point).end(robot()->pose.pos).stroke("red").strokeWidth(1);
line_builder.start(interception_point).end(robot()->pose.pos).stroke("red").strokeWidth(10);
visualizer->add(line_builder.getSvgString());

if (getParameter<bool>("enable_redirect")) {
Expand Down Expand Up @@ -88,9 +87,8 @@ Point Receive::getInterceptionPoint() const
Segment ball_line(
world_model()->ball.pos,
(world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0));
// visualizer->addLine(ball_line.first, ball_line.second, 1, "blue", 1., "ball_line");
SvgLineBuilder line_builder;
line_builder.start(ball_line.first).end(ball_line.second).stroke("blue").strokeWidth(1);
line_builder.start(ball_line.first).end(ball_line.second).stroke("blue").strokeWidth(10);
visualizer->add(line_builder.getSvgString());
auto result = getClosestPointAndDistance(robot()->pose.pos, ball_line);
return result.closest_point;
Expand Down
9 changes: 0 additions & 9 deletions crane_robot_skills/src/single_ball_placement.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
setParameter("コート端判定のオフセット", 0.0);

addStateFunction(SingleBallPlacementStates::ENTRY_POINT, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand All @@ -47,7 +46,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba

// 端にある場合、コート側からアプローチする
addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PREPARE, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down Expand Up @@ -107,7 +105,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba

// PULL_BACK_FROM_EDGE_TOUCH
addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_TOUCH, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down Expand Up @@ -169,7 +166,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba

// PULL_BACK_FROM_EDGE_PULL
addStateFunction(SingleBallPlacementStates::PULL_BACK_FROM_EDGE_PULL, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down Expand Up @@ -200,7 +196,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
[this]() { return robot()->getDistance(world_model()->ball.pos) > 0.15; });

addStateFunction(SingleBallPlacementStates::GO_OVER_BALL, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down Expand Up @@ -255,7 +250,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
[this]() { return skill_status == Status::SUCCESS; });

addStateFunction(SingleBallPlacementStates::CONTACT_BALL, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand All @@ -279,7 +273,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
[this]() { return skill_status == Status::SUCCESS; });

addStateFunction(SingleBallPlacementStates::MOVE_TO_TARGET, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down Expand Up @@ -319,7 +312,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
[this]() { return skill_status == Status::FAILURE; });

addStateFunction(SingleBallPlacementStates::SLEEP, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand All @@ -346,7 +338,6 @@ SingleBallPlacement::SingleBallPlacement(RobotCommandWrapperBase::SharedPtr & ba
});

addStateFunction(SingleBallPlacementStates::LEAVE_BALL, [this]() {
// visualizer->addPoint(robot()->pose.pos, 0, "white", 1.0, state_string);
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text(state_string)
Expand Down
14 changes: 1 addition & 13 deletions crane_robot_skills/src/sub_attacker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,9 +49,6 @@ Status SubAttacker::update()
result.closest_point + (robot()->pose.pos - result.closest_point).normalized() * 0.5);
command.enableBallAvoidance();
{
// visualizer->addPoint(
// robot()->pose.pos.x(), robot()->pose.pos.y(), 0,
// "red", 1., "ボールラインから一旦遠ざかる");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("ボールラインから一旦遠ざかる")
Expand All @@ -62,8 +59,6 @@ Status SubAttacker::update()
} else {
// ボールの進路上に移動
{
// visualizer->addPoint(
// robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ボールの進路上に移動");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("ボールの進路上に移動")
Expand All @@ -89,8 +84,6 @@ Status SubAttacker::update()
}
} else {
{
// visualizer->addPoint(
// robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "red", 1., "ベストポジションへ移動");
SvgTextBuilder text_builder;
text_builder.position(robot()->pose.pos.x() - 0.5, robot()->pose.pos.y() + 0.5)
.text("ベストポジションへ移動")
Expand All @@ -102,9 +95,6 @@ Status SubAttacker::update()
double best_score = 0.0;
for (const auto & dpps_point : dpps_points) {
double score = getPointScore(dpps_point, world_model()->ball.pos, world_model());
// visualizer->addPoint(
// dpps_point.x(), dpps_point.y(), std::clamp(static_cast<int>(score * 100), 0, 20), "blue",
// 1.);

if (score > best_score) {
best_score = score;
Expand All @@ -122,10 +112,8 @@ Status SubAttacker::update()
auto to_goal = getNormVec(goal_angle);
auto to_ball = (world_model()->ball.pos - target_pos).normalized();
{
// visualizer->addLine(
// target_pos, target_pos + to_goal * 3.0, 2, "yellow", 1.0, "Supporterシュートライン");
SvgLineBuilder line_builder;
line_builder.start(target_pos).end(target_pos + to_goal * 3.0).stroke("yellow").strokeWidth(2);
line_builder.start(target_pos).end(target_pos + to_goal * 3.0).stroke("yellow").strokeWidth(20);
visualizer->add(line_builder.getSvgString());
}
command.setTargetTheta(getAngle(to_goal + to_ball));
Expand Down
Loading