可視化の改良

consai_ros2/consai_visualizer/src/consai_visualizer/field_widget.py

@@ -359,6 +359,7 @@ def _draw_visualizer_info_on_window_area(self, painter: QPainter):
         annotation.normalized_width = 0.1
         annotation.normalized_height = 0.05
         annotation.color.name = "white"
+        annotation.color.alpha = 1.0
         self._draw_shape_annotation(painter, annotation)
 
         # カーソル位置を描画

crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp

@@ -88,10 +88,10 @@ class KickEventDetector
         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");
-    }
+    // キックの履歴を可視化
+    //    for (const auto & [kick_origin, kick_end] : kick_history) {
+    //      visualizer->addLine(kick_origin.position, kick_end, 2, "red", 0.5, "KICK");
+    //    }
 
     for (const auto & record : records) {
       visualizer->addCircle(record.position, 0.1, 2, "red", "", 1.0, "ball");

crane_local_planner/src/crane_local_planner.cpp

@@ -44,7 +44,7 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
           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);
+            raw_command.position_target_mode.front().target_y, 1, "yellow", 0.3);
         }
         break;
       case crane_msgs::msg::RobotCommand::SIMPLE_VELOCITY_TARGET_MODE:
@@ -101,24 +101,25 @@ void LocalPlannerComponent::callbackRobotCommands(const crane_msgs::msg::RobotCo
       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);
+          command.position_target_mode.front().target_y, 1, "yellow", 0.5);
       } 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,
-          robot->pose.pos.y() + command.simple_velocity_target_mode.front().target_vy, 1);
+          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);
       } 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() +
-            command.polar_velocity_target_mode.front().target_velocity_r *
+            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() +
-            command.polar_velocity_target_mode.front().target_velocity_r *
+            0.5 * command.polar_velocity_target_mode.front().target_velocity_r *
               std::sin(command.polar_velocity_target_mode.front().target_velocity_theta),
-          1);
+          1, "white", 0.5);
       } break;
     }
   }

crane_local_planner/src/rvo2_planner.cpp

@@ -399,8 +399,6 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
 
       command.position_target_mode.front().target_x = target_pos.x();
       command.position_target_mode.front().target_y = target_pos.y();
-      visualizer->addLine(
-        target_pos, Point(command.current_pose.x, command.current_pose.y), 1, "yellow");
     }
   }
 }

crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp

@@ -178,6 +178,7 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   std::array<std::deque<geometry_msgs::msg::Pose2D>, 20> friend_history;
   std::array<std::deque<geometry_msgs::msg::Pose2D>, 20> enemy_history;
+  std::deque<Point> ball_history;
 
   int history_size;
 

crane_world_model_publisher/src/visualization_data_handler.cpp

@@ -148,9 +148,9 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
     // ボールは小さいのでボールの周りを大きな円で囲う
     vis_ball.line_color.name = "crimson";
     vis_ball.fill_color.alpha = 0.0;
-    vis_ball.line_color.alpha = 1.0;
-    vis_ball.line_size = 2;
-    vis_ball.radius = 0.8;
+    vis_ball.line_color.alpha = 0.7;
+    vis_ball.line_size = 1;
+    vis_ball.radius = 0.5;
     vis_ball.caption = "ball is here";
     vis_objects.circles.push_back(vis_ball);
 
@@ -358,7 +358,7 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg)
   const double CARDS_X = BOTS_X + BOTS_WIDTH + MARGIN_X;
   const double YELLOW_CARD_TIMES_WIDTH = 0.1;
   const double YELLOW_CARD_TIMES_X = CARDS_X + CARDS_WIDTH + MARGIN_X;
-  const double TIMEOUT_WIDTH = 0.1;
+  const double TIMEOUT_WIDTH = 0.2;
   const double TIMEOUT_X = YELLOW_CARD_TIMES_X + YELLOW_CARD_TIMES_WIDTH + MARGIN_X;
   const std::string COLOR_TEXT_BLUE = "deepskyblue";
   const std::string COLOR_TEXT_YELLOW = "yellow";

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -308,6 +308,10 @@ void WorldModelPublisherComponent::publishWorldModel()
   wm.their_max_allowed_bots = their_max_allowed_bots;
 
   updateBallContact();
+  ball_history.push_back(Point(wm.ball_info.pose.x, wm.ball_info.pose.y));
+  if (ball_history.size() > history_size) {
+    ball_history.pop_front();
+  }
 
   wm.ball_info.state_changed = false;
   if (ball_event_detected) {
@@ -412,17 +416,36 @@ void WorldModelPublisherComponent::publishWorldModel()
 
   pub_world_model->publish(wm);
 
+  constexpr int SAMPLING_NUM = 2;
   for (const auto & history : friend_history) {
-    for (int index = 0; const auto & pose : history) {
-      visualizer->addPoint(
-        pose.x, pose.y, 2, "yellow", index++ / static_cast<double>(history.size()));
+    if (history.size() > SAMPLING_NUM + 1) {
+      for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
+        Point p1;
+        Point p2;
+        p1 << history.at(index).x, history.at(index).y;
+        p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y;
+        visualizer->addLine(p1, p2, 1, "yellow", index / static_cast<double>(history.size()));
+      }
     }
   }
 
   for (const auto & history : enemy_history) {
-    for (int index = 0; const auto & pose : history) {
-      visualizer->addPoint(
-        pose.x, pose.y, 2, "blue", index++ / static_cast<double>(history.size()));
+    if (history.size() > SAMPLING_NUM + 1) {
+      for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
+        Point p1;
+        Point p2;
+        p1 << history.at(index).x, history.at(index).y;
+        p2 << history.at(index + SAMPLING_NUM).x, history.at(index + SAMPLING_NUM).y;
+        visualizer->addLine(p1, p2, 1, "blue", index / static_cast<double>(history.size()));
+      }
+    }
+  }
+
+  if (ball_history.size() > SAMPLING_NUM + 1) {
+    for (int index = 0; index < ball_history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {
+      visualizer->addLine(
+        ball_history.at(index), ball_history.at(index + SAMPLING_NUM), 1, "orange",
+        index / static_cast<double>(ball_history.size()));
     }
   }
   visualizer->flush();

session/crane_planner_plugins/include/crane_planner_plugins/attacker_skill_planner.hpp

@@ -46,11 +46,13 @@ class AttackerSkillPlanner : public PlannerBase
       std::string state_name(magic_enum::enum_name(skill->getCurrentState()));
       visualizer->addCircle(
         skill->commander().getRobot()->pose.pos, 0.3, 2, "red", "", 1.0, state_name);
-      visualizer->addLine(
-        world_model->ball.pos,
-        world_model->ball.pos +
-          world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(),
-        3, "red", 0.5, "");
+      if (world_model->ball.isMoving()) {
+        visualizer->addLine(
+          world_model->ball.pos,
+          world_model->ball.pos +
+            world_model->ball.vel.normalized() * world_model->getBallDistanceHorizon(),
+          3, "red", 0.5, "");
+      }
       auto status = skill->run();
       return {static_cast<PlannerBase::Status>(status), {skill->getRobotCommand()}};
     }