SVG対応

ibis-ssl · Jan 19, 2025 · 0440aa3 · 0440aa3
1 parent 43477d5
commit 0440aa3
Show file tree

Hide file tree

Showing 18 changed files with 419 additions and 93 deletions.
diff --git a/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp b/crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp
@@ -59,14 +59,28 @@ class KickEventDetector
     std::optional<KickOrigin> kick_event_origin = std::nullopt;
     for (const auto & id : detected_bots.friends) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected friend: " << static_cast<int>(id));
-      visualizer->addCircle(
-        world_model.getOurRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
+      SvgCircleBuilder circle_builder;
+      circle_builder.center(world_model.getOurRobot(id)->pose.pos)
+        .radius(0.5)
+        .stroke("blue")
+        .fill("blue")
+        .strokeWidth(2);
+      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) {
       RCLCPP_INFO_STREAM(rclcpp::get_logger("aaaa"), "Detected enemy: " << static_cast<int>(id));
-      visualizer->addCircle(
-        world_model.getTheirRobot(id)->pose.pos, 0.5, 2, "blue", "blue", 1.0, "KICK");
+      SvgCircleBuilder circle_builder;
+      circle_builder.center(world_model.getTheirRobot(id)->pose.pos)
+        .radius(0.5)
+        .stroke("blue")
+        .fill("blue")
+        .strokeWidth(2);
+      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});
     }
 
@@ -78,14 +92,28 @@ class KickEventDetector
         // キック中断判定
         kick_history.emplace_back(ongoing_kick_origin.value(), world_model.ball.pos);
         ongoing_kick_origin = std::nullopt;
-        visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT");
+        SvgCircleBuilder circle_builder;
+        circle_builder.center(world_model.ball.pos)
+          .radius(3.5)
+          .stroke("green")
+          .fill("black")
+          .strokeWidth(2);
+        visualizer->add(circle_builder.getSvgString());
+        // visualizer->addCircle(world_model.ball.pos, 3.5, 2, "green", "black", 1.0, "EVENT");
       }
     }
 
     // 進行中のキックを可視化
     if (ongoing_kick_origin.has_value()) {
-      visualizer->addTube(
-        world_model.ball.pos, ongoing_kick_origin.value().position, 0.2, 2, "red", "", 1.0, "KICK");
+      SvgLineBuilder line_builder;
+      line_builder.start(ongoing_kick_origin.value().position)
+        .end(world_model.ball.pos)
+        .stroke("red")
+        .strokeWidth(2);
+      visualizer->add(line_builder.getSvgString());
+      // visualizer->addTube(
+      //   world_model.ball.pos, ongoing_kick_origin.value().position,
+      //   0.2, 2, "red", "", 1.0, "KICK");
     }
 
     // キックの履歴を可視化

diff --git a/crane_local_planner/include/crane_local_planner/simple_planner.hpp b/crane_local_planner/include/crane_local_planner/simple_planner.hpp
@@ -40,9 +40,18 @@ 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);
+        // 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);
+        visualizer->add(line_builder.getSvgString());
       }
       if (command.local_planner_config.max_velocity > MAX_VEL) {
         command.local_planner_config.max_velocity = MAX_VEL;

diff --git a/crane_local_planner/src/crane_local_planner.cpp b/crane_local_planner/src/crane_local_planner.cpp
@@ -56,10 +56,18 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
                << "\" skill , but no position_target_mode is set.";
           RCLCPP_ERROR(get_logger(), what.str().c_str());
         } else {
-          planner->visualizer->addLine(
-            raw_command.current_pose.x, raw_command.current_pose.y,
-            raw_command.position_target_mode.front().target_x,
-            raw_command.position_target_mode.front().target_y, 1, "yellow", 0.3);
+          //          planner->visualizer->addLine(
+          //            raw_command.current_pose.x, raw_command.current_pose.y,
+          //            raw_command.position_target_mode.front().target_x,
+          //            raw_command.position_target_mode.front().target_y, 1, "yellow", 0.3);
+          SvgLineBuilder line_builder;
+          line_builder.start(raw_command.current_pose.x, raw_command.current_pose.y)
+            .end(
+              raw_command.position_target_mode.front().target_x,
+              raw_command.position_target_mode.front().target_y)
+            .stroke("yellow", 0.3)
+            .strokeWidth(2);
+          planner->visualizer->add(line_builder.getSvgString());
         }
         break;
       case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE:
@@ -114,33 +122,62 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
     auto robot = world_model->getOurRobot(command.robot_id);
     switch (command.control_mode) {
       case crane_msgs::msg::RobotCommand::POSITION_TARGET_MODE: {
-        planner->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, "yellow", 0.5);
+        // planner->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, "yellow", 0.5);
+        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("yellow", 0.5)
+          .strokeWidth(1);
+        planner->visualizer->add(line_builder.getSvgString());
       } break;
       case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE: {
-        planner->visualizer->addLine(
-          robot->pose.pos.x(), robot->pose.pos.y(),
-          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, 1,
-          "white", 0.5);
+        // planner->visualizer->addLine(
+        //   robot->pose.pos.x(), robot->pose.pos.y(),
+        //   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, 1,
+        //   "white", 0.5);
+        SvgLineBuilder line_builder;
+        line_builder.start(robot->pose.pos)
+          .end(
+            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);
+        planner->visualizer->add(line_builder.getSvgString());
       } break;
       case crane_msgs::msg::RobotCommand::POLAR_VELOCITY_TARGET_MODE: {
-        planner->visualizer->addLine(
-          robot->pose.pos.x(), robot->pose.pos.y(),
-          robot->pose.pos.x() +
-            0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
-              std::cos(command.polar_velocity_target_mode.front().target_velocity_theta),
-          robot->pose.pos.y() +
-            0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
-              std::sin(command.polar_velocity_target_mode.front().target_velocity_theta),
-          1, "white", 0.5);
+        // planner->visualizer->addLine(
+        //   robot->pose.pos.x(), robot->pose.pos.y(),
+        //   robot->pose.pos.x() +
+        //     0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
+        //   std::cos(command.polar_velocity_target_mode.front().target_velocity_theta),
+        //   robot->pose.pos.y() +
+        //     0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
+        //   std::sin(command.polar_velocity_target_mode.front().target_velocity_theta),
+        //   1, "white", 0.5);
+        SvgLineBuilder line_builder;
+        line_builder.start(robot->pose.pos)
+          .end(
+            robot->pose.pos.x() +
+              0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
+                std::cos(command.polar_velocity_target_mode.front().target_velocity_theta),
+            robot->pose.pos.y() +
+              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);
+        planner->visualizer->add(line_builder.getSvgString());
       } break;
     }
   }
 
   planner->visualizer->flush();
-  crane::ConsaiVisualizerBuffer::publish();
+  crane::CraneVisualizerBuffer::publish();
 }
 }  // namespace crane
 

diff --git a/crane_robot_skills/src/attacker.cpp b/crane_robot_skills/src/attacker.cpp
@@ -110,7 +110,15 @@ Attacker::Attacker(RobotCommandWrapperBase::SharedPtr & base)
   });
 
   addStateFunction(AttackerState::STEAL_BALL, [this]() -> Status {
-    visualizer->addCircle(robot()->pose.pos, 0.25, 1, "blue", "white", 1.0);
+    // 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);
+    visualizer->add(circle_builder.getSvgString());
+
     return steal_ball_skill.run();
   });
 
@@ -270,7 +278,10 @@ 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");
+    // 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);
+    visualizer->add(line_builder.getSvgString());
 
     kick_skill.setParameter("target", kick_target);
     Segment ball_to_target{world_model()->ball.pos, kick_target};

diff --git a/crane_robot_skills/src/goal_kick.cpp b/crane_robot_skills/src/goal_kick.cpp
@@ -27,7 +27,10 @@ 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");
+  //  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);
+  visualizer->add(line_builder.getSvgString());
   kick_skill.setParameter("target", target);
   kick_skill.setParameter("dot_threshold", getParameter<double>("dot_threshold"));
   return kick_skill.run();

diff --git a/crane_robot_skills/src/goalie.cpp b/crane_robot_skills/src/goalie.cpp
@@ -49,7 +49,13 @@ Status Goalie::update()
     }
   }
 
-  visualizer->addPoint(robot()->pose.pos.x(), robot()->pose.pos.y(), 0, "white", 1., phase);
+  // 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)
+    .stroke("white")
+    .fontSize(1);
+  visualizer->add(text_builder.getSvgString());
   return Status::RUNNING;
 }
 
@@ -81,7 +87,10 @@ void Goalie::emitBallFromPenaltyArea()
     }
   }();
 
-  visualizer->addLine(ball, pass_target, 1, "blue");
+  // visualizer->addLine(ball, pass_target, 1, "blue");
+  SvgLineBuilder line_builder;
+  line_builder.start(ball).end(pass_target).stroke("blue").strokeWidth(1);
+  visualizer->add(line_builder.getSvgString());
 
   Point intermediate_point = ball + (ball - pass_target).normalized() * 0.2f;
   kick_skill.setParameter("target", pass_target);

diff --git a/crane_robot_skills/src/kick.cpp b/crane_robot_skills/src/kick.cpp
@@ -35,14 +35,26 @@ 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");
+    //  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")
+      .stroke("white")
+      .fontSize(1);
+    visualizer->add(text_builder.getSvgString());
     return Status::RUNNING;
   });
 
   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");
+    //  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")
+      .stroke("white")
+      .fontSize(1);
+    visualizer->add(text_builder.getSvgString());
     // ボールラインに沿って追いかけつつ、角度はtargetへ向ける
     const auto & ball_pos = world_model()->ball.pos;
     command.lookAtFrom(getParameter<Point>("target"), ball_pos);
@@ -77,7 +89,13 @@ Kick::Kick(RobotCommandWrapperBase::SharedPtr & base)
   });
 
   addStateFunction(KickState::REDIRECT_KICK, [this]() {
-    visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::REDIRECT_KICK");
+    // 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")
+      .stroke("white")
+      .fontSize(1);
+    visualizer->add(text_builder.getSvgString());
     receive_skill->setParameter("target", getParameter<Point>("target"));
     if (robot()->getDistance(world_model()->ball.pos) < 0.5) {
       receive_skill->setParameter("policy", std::string("closest"));
@@ -102,17 +120,44 @@ 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");
+    // 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);
+    visualizer->add(line_builder.getSvgString());
     constexpr double SWITCH_DISTANCE = 1.0;
-    visualizer->addCircle(ball_pos, SWITCH_DISTANCE, 1, "yellow", "yellow", 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);
+      visualizer->add(circle_builder.getSvgString());
+    }
     if (robot()->getDistance(ball_pos) > SWITCH_DISTANCE) {
-      visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL(遠い)");
+      // 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)
+          .text("Kick::AROUND_BALL(遠い)")
+          .stroke("white")
+          .fontSize(1);
+        visualizer->add(text_builder.getSvgString());
+      }
       command.setTargetPosition(ball_pos + (ball_pos - target).normalized() * 0.3)
         .lookAtFrom(target, ball_pos)
         .setTerminalVelocity(0.3);
       return Status::RUNNING;
     } else {
-      visualizer->addPoint(robot()->pose.pos, 0, "", 1., "Kick::AROUND_BALL（近い）");
+      // 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)
+          .text("Kick::AROUND_BALL（近い）")
+          .stroke("white")
+          .fontSize(1);
+        visualizer->add(text_builder.getSvgString());
+      }
       auto calculateRatio =
         [](const double distance, const double min_distance, const double max_distance) {
           return (distance - min_distance) / (max_distance - min_distance);

diff --git a/crane_robot_skills/src/marker.cpp b/crane_robot_skills/src/marker.cpp
@@ -38,9 +38,18 @@ Status Marker::update()
   }
   command.setTargetPosition(marking_point, 0.1).setTargetTheta(target_theta);
 
-  visualizer->addCircle(enemy_pos, 0.3, 1, "black", "");
-  visualizer->addLine(
-    robot()->pose.pos, enemy_pos + (enemy_pos - robot()->pose.pos).normalized() * 0.3, 2, "black");
+  //  visualizer->addCircle(enemy_pos, 0.3, 1, "black", "");
+  SvgCircleBuilder circle_builder;
+  circle_builder.center(enemy_pos).radius(0.3).stroke("black").strokeWidth(1);
+  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);
+  visualizer->add(line_builder.getSvgString());
   return Status::RUNNING;
 }
 }  // namespace crane::skills
diff --git a/crane_robot_skills/src/penalty_kick.cpp b/crane_robot_skills/src/penalty_kick.cpp
@@ -46,7 +46,9 @@ 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");
+    //    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);
 
     kick_skill.setParameter("target", best_target);