Skip to content

Commit

Permalink
太く見やすく
Browse files Browse the repository at this point in the history
  • Loading branch information
HansRobo committed Jan 30, 2025
1 parent b2692d8 commit 397ad24
Show file tree
Hide file tree
Showing 17 changed files with 37 additions and 37 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ class KickEventDetector
.radius(0.5)
.stroke("blue")
.fill("blue")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{true, id});
}
Expand All @@ -73,7 +73,7 @@ class KickEventDetector
.radius(0.5)
.stroke("blue")
.fill("blue")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
kick_event_origin.emplace(ros_clock.now(), world_model.ball.pos, RobotIdentifier{false, id});
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ class SimplePlanner : public LocalPlannerBase
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
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
4 changes: 2 additions & 2 deletions crane_robot_skills/src/attacker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
.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,7 +278,7 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
const auto enemy_robots = world_model()->theirs.getAvailableRobots();

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
2 changes: 1 addition & 1 deletion crane_robot_skills/src/goal_kick.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -28,7 +28,7 @@ Status GoalKick::update()

Point target = world_model()->ball.pos + getNormVec(best_angle) * 0.5;
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
2 changes: 1 addition & 1 deletion crane_robot_skills/src/goalie.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,7 @@ void Goalie::emitBallFromPenaltyArea()
}();

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
4 changes: 2 additions & 2 deletions crane_robot_skills/src/kick.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -121,12 +121,12 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
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;
{
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) {
Expand Down
4 changes: 2 additions & 2 deletions crane_robot_skills/src/marker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,13 +39,13 @@ Status Marker::update()
command.setTargetPosition(marking_point, 0.1).setTargetTheta(target_theta);

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());
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
2 changes: 1 addition & 1 deletion crane_robot_skills/src/penalty_kick.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ PenaltyKick::PenaltyKick(RobotCommandWrapperBase::SharedPtr & base)
minimum_angle_accuracy, world_model()->ball.pos, world_model());
Point best_target = world_model()->ball.pos + getNormVec(best_angle) * 0.5;
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
4 changes: 2 additions & 2 deletions crane_robot_skills/src/receive.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ Status Receive::update()
Point interception_point = getInterceptionPoint() + offset;

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,7 +88,7 @@ Point Receive::getInterceptionPoint() const
world_model()->ball.pos,
(world_model()->ball.pos + world_model()->ball.vel.normalized() * 10.0));
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
2 changes: 1 addition & 1 deletion crane_robot_skills/src/sub_attacker.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ Status SubAttacker::update()
auto to_ball = (world_model()->ball.pos - target_pos).normalized();
{
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
16 changes: 8 additions & 8 deletions crane_world_model_publisher/src/visualization_data_handler.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
builder.start(field_line.p1().x() * 0.001, field_line.p1().y() * 0.001)
.end(field_line.p2().x() * 0.001, field_line.p2().y() * 0.001)
.stroke("white")
.strokeWidth(2);
.strokeWidth(20);
visualizer_geometry->add(builder.getSvgString());
}

Expand All @@ -115,7 +115,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
builder.center(field_arc.center().x() * 0.001, field_arc.center().y() * 0.001)
.radius(field_arc.radius() * 0.001)
.stroke("white")
.strokeWidth(2);
.strokeWidth(20);
visualizer_geometry->add(builder.getSvgString());
}

Expand Down Expand Up @@ -144,7 +144,7 @@ void VisualizationDataHandler::publish_vis_geometry(const SSL_GeometryData & geo
(geometry_data.field().field_length() + geometry_data.field().boundary_width() * 2) * 0.001,
(geometry_data.field().field_width() + geometry_data.field().boundary_width() * 2) * 0.001)
.stroke("black")
.strokeWidth(3);
.strokeWidth(30);
visualizer_geometry->add(rect_builder.getSvgString());
visualizer_geometry->flush();
CraneVisualizerBuffer::publish();
Expand All @@ -164,15 +164,15 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
.radius(0.0215)
.stroke("black")
.fill("orange")
.strokeWidth(1);
.strokeWidth(10);
visualizer_tracked->add(builder.getSvgString());

// ボールは小さいのでボールの周りを大きな円で囲う
builder.center(ball.pos().x(), ball.pos().y())
.radius(0.5)
.stroke("crimson", 0.7)
.fill("none")
.strokeWidth(1);
.strokeWidth(10);
visualizer_tracked->add(builder.getSvgString());

ball_x = ball.pos().x();
Expand All @@ -185,7 +185,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
line_builder.start(ball.pos().x(), ball.pos().y())
.end(ball.pos().x() + ball.vel().x(), ball.pos().y() + ball.vel().y())
.stroke("gold", VELOCITY_ALPHA)
.strokeWidth(2);
.strokeWidth(20);
visualizer_tracked->add(line_builder.getSvgString());
}
}
Expand All @@ -200,7 +200,7 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
double robot_theta = robot.orientation();
builder.position(robot.pos().x(), robot.pos().y(), robot.orientation())
.stroke("black")
.strokeWidth(1);
.strokeWidth(10);
if (robot.robot_id().team() == RobotId::TEAM_COLOR_BLUE) {
builder.fill("dodgerblue");
} else {
Expand Down Expand Up @@ -526,7 +526,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg)
msg->designated_position.front().x / 1000., msg->designated_position.front().y / 1000.)
.end(ball_x, ball_y)
.stroke("aquamarine")
.strokeWidth(1);
.strokeWidth(10);
visualizer_referee->add(line_builder.getSvgString());
}
}
Expand Down
6 changes: 3 additions & 3 deletions crane_world_model_publisher/src/world_model_publisher.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -451,7 +451,7 @@ void WorldModelPublisherComponent::publishWorldModel()
line_builder.start(p1)
.end(p2)
.stroke("yellow", index / static_cast<double>(history.size()))
.strokeWidth(3);
.strokeWidth(30);
visualizer->add(line_builder.getSvgString());
}
}
Expand All @@ -468,7 +468,7 @@ void WorldModelPublisherComponent::publishWorldModel()
line_builder.start(p1)
.end(p2)
.stroke("blue", index / static_cast<double>(history.size()))
.strokeWidth(3);
.strokeWidth(30);
visualizer->add(line_builder.getSvgString());
}
}
Expand All @@ -480,7 +480,7 @@ void WorldModelPublisherComponent::publishWorldModel()
line_builder.start(ball_history.at(index))
.end(ball_history.at(index + SAMPLING_NUM))
.stroke("orange", index / static_cast<double>(ball_history.size()))
.strokeWidth(3);
.strokeWidth(30);
visualizer->add(line_builder.getSvgString());
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,7 +50,7 @@ class AttackerSkillPlanner : public PlannerBase
circle_builder.center(skill->commander().getRobot()->pose.pos)
.radius(0.3)
.stroke("red")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
}
if (world_model->ball.isMoving()) {
Expand All @@ -61,7 +61,7 @@ class AttackerSkillPlanner : public PlannerBase
world_model->ball.pos +
world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon())
.stroke("red")
.strokeWidth(3);
.strokeWidth(30);
visualizer->add(line_builder.getSvgString());
}
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ class OffensivePlanner : public PlannerBase
circle_builder.center(attacker->commander().getRobot()->pose.pos)
.radius(0.3)
.stroke("red")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(circle_builder.getSvgString());
}
{
Expand All @@ -57,7 +57,7 @@ class OffensivePlanner : public PlannerBase
world_model->ball.pos +
world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon())
.stroke("red")
.strokeWidth(3);
.strokeWidth(30);
visualizer->add(line_builder.getSvgString());
}
auto status = attacker->run();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -99,7 +99,7 @@ class BallPlacementAvoidancePlanner : public PlannerBase
line_builder.start(command.original_position)
.end(target_position)
.stroke("yellow")
.strokeWidth(2);
.strokeWidth(20);
visualizer->add(line_builder.getSvgString());
} else {
command.command->setTargetPosition(command.original_position);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -190,7 +190,7 @@ class SimplePlacerPlanner : public PlannerBase
// visualize areas with info
for (const auto & area : areas_with_info) {
SvgRectBuilder rect_builder;
rect_builder.box(area.box).stroke("yellow").strokeWidth(1);
rect_builder.box(area.box).stroke("yellow").strokeWidth(10);
visualizer->add(rect_builder.getSvgString());

SvgTextBuilder text_builder;
Expand All @@ -206,7 +206,7 @@ class SimplePlacerPlanner : public PlannerBase
line_builder.start(cmd.current_pose.x, cmd.current_pose.y)
.end(cmd.position_target_mode.front().target_x, cmd.position_target_mode.front().target_y)
.stroke("blue")
.strokeWidth(1);
.strokeWidth(10);
visualizer->add(line_builder.getSvgString());
}
return {PlannerBase::Status::RUNNING, robot_commands};
Expand Down

0 comments on commit 397ad24

Please sign in to comment.