STOP中の調整 (#679)

crane_game_analyzer/include/crane_game_analyzer/kick_event_detector.hpp

@@ -92,10 +92,6 @@ class KickEventDetector
     //    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");
-    }
   }
 
   bool hasInterruptedOnGoingKick(const WorldModelWrapper & world_model) const

crane_local_planner/src/rvo2_planner.cpp

@@ -7,6 +7,7 @@
 #include "crane_local_planner/rvo2_planner.hpp"
 
 #include <boost/stacktrace.hpp>
+#include <robocup_ssl_msgs/msg/referee.hpp>
 
 // cspell: ignore OBST
 
@@ -60,7 +61,9 @@ RVO2Planner::RVO2Planner(rclcpp::Node & node)
 
 void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & msg)
 {
-  if (world_model->play_situation.getSituationCommandID() == crane_msgs::msg::PlaySituation::STOP) {
+  if (
+    world_model->play_situation.getRefereeCommandID() ==
+    robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
     // 1.5m/sだとたまに超えるので1.0m/sにしておく
     for (int i = 0; i < 40; i++) {
       rvo_sim->setAgentMaxSpeed(i, 1.0f);
@@ -146,8 +149,8 @@ void RVO2Planner::reflectWorldToRVOSim(const crane_msgs::msg::RobotCommands & ms
         max_vel = std::min(max_vel, max_vel_by_decel);
         max_vel = std::min(max_vel, max_vel_by_acc);
         if (
-          world_model->play_situation.getSituationCommandID() ==
-          crane_msgs::msg::PlaySituation::STOP) {
+          world_model->play_situation.getRefereeCommandID() ==
+          robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
           // 1.5m/sだとたまに超えるので1.0m/sにしておく
           max_vel = std::min(max_vel, 1.0);
         }
@@ -291,8 +294,8 @@ void RVO2Planner::overrideTargetPosition(crane_msgs::msg::RobotCommands & msg)
         double SURROUNDING_OFFSET = 0.3;
         double PENALTY_AREA_OFFSET = 0.1;
         if (
-          world_model->play_situation.getSituationCommandID() ==
-          crane_msgs::msg::PlaySituation::STOP) {
+          world_model->play_situation.getRefereeCommandID() ==
+          robocup_ssl_msgs::msg::Referee::COMMAND_STOP) {
           PENALTY_AREA_OFFSET = 0.5;
           SURROUNDING_OFFSET = 0.6;
         }

crane_play_switcher/src/play_switcher.cpp

@@ -81,6 +81,7 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
 
   // TODO(HansRobo): robocup_ssl_msgs/msg/Refereeをもう少しわかりやすい形式にする必要あり
   play_situation_msg.stage = msg.stage;
+  play_situation_msg.command_raw = msg.command;
 
   if (latest_raw_referee_command != static_cast<int>(msg.command)) {
     //-----------------------------------//

crane_world_model_publisher/include/crane_world_model_publisher/visualization_data_handler.hpp

@@ -43,6 +43,10 @@ class VisualizationDataHandler
   rclcpp::Subscription<Referee>::SharedPtr sub_referee_;
 
   rclcpp::Publisher<VisualizerObjectsArray>::SharedPtr pub_vis_objects_;
+
+  double ball_x;
+
+  double ball_y;
 };
 
 }  // namespace crane

crane_world_model_publisher/src/visualization_data_handler.cpp

@@ -20,6 +20,7 @@
 #include <crane_visualization_interfaces/msg/shape_point.hpp>
 #include <crane_visualization_interfaces/msg/shape_rectangle.hpp>
 #include <crane_visualization_interfaces/msg/shape_robot.hpp>
+#include <crane_visualization_interfaces/msg/shape_tube.hpp>
 #include <robocup_ssl_msgs/msg/robot_id.hpp>
 
 namespace crane
@@ -33,6 +34,7 @@ using VisPoint = crane_visualization_interfaces::msg::ShapePoint;
 using VisRect = crane_visualization_interfaces::msg::ShapeRectangle;
 using VisRobot = crane_visualization_interfaces::msg::ShapeRobot;
 using VisText = crane_visualization_interfaces::msg::ShapeText;
+using VisTube = crane_visualization_interfaces::msg::ShapeTube;
 using RobotId = robocup_ssl_msgs::msg::RobotId;
 
 VisualizationDataHandler::VisualizationDataHandler(rclcpp::Node & node)
@@ -154,6 +156,9 @@ void VisualizationDataHandler::publish_vis_tracked(const TrackedFrame & tracked_
     vis_ball.caption = "ball is here";
     vis_objects.circles.push_back(vis_ball);
 
+    ball_x = ball.pos().x();
+    ball_y = ball.pos().y();
+
     // 速度を描画
     if (ball.has_vel()) {
       const double vel_norm = std::hypot(ball.vel().x(), ball.vel().y());
@@ -527,17 +532,19 @@ void VisualizationDataHandler::publish_vis_referee(const Referee::SharedPtr msg)
     msg->command == Referee::COMMAND_BALL_PLACEMENT_BLUE ||
     msg->command == Referee::COMMAND_BALL_PLACEMENT_YELLOW) {
     if (not msg->designated_position.empty()) {
-      VisCircle vis_circle;
-      vis_circle.center.x = msg->designated_position.front().x;
-      vis_circle.center.y = msg->designated_position.front().y;
-      vis_circle.radius = 0.15;
-      vis_circle.line_color.name = "aquamarine";
-      vis_circle.line_color.alpha = 1.0;
-      vis_circle.fill_color.name = "aquamarine";
-      vis_circle.fill_color.alpha = 1.0;
-      vis_circle.line_size = 1;
-      vis_circle.caption = "placement pos";
-      vis_objects.circles.push_back(vis_circle);
+      VisTube vis_tube;
+      vis_tube.p1.x = msg->designated_position.front().x / 1000.;
+      vis_tube.p1.y = msg->designated_position.front().y / 1000.;
+      vis_tube.p2.x = ball_x;
+      vis_tube.p2.y = ball_y;
+      vis_tube.radius = 0.5;
+      vis_tube.line_color.name = "aquamarine";
+      vis_tube.line_color.alpha = 1.0;
+      //      vis_tube.fill_color.name = "aquamarine";
+      vis_tube.fill_color.alpha = 0.0;
+      vis_tube.line_size = 1;
+      vis_tube.caption = "placement";
+      vis_objects.tubes.push_back(vis_tube);
     }
   }
 

crane_world_model_publisher/src/world_model_publisher.cpp

@@ -416,7 +416,7 @@ void WorldModelPublisherComponent::publishWorldModel()
 
   pub_world_model->publish(wm);
 
-  constexpr int SAMPLING_NUM = 2;
+  constexpr int SAMPLING_NUM = 4;
   for (const auto & history : friend_history) {
     if (history.size() > SAMPLING_NUM + 1) {
       for (int index = 0; index < history.size() - SAMPLING_NUM; index += SAMPLING_NUM) {

session/crane_planner_plugins/src/skill_planners.cpp

@@ -269,7 +269,7 @@ auto BallNearByPositionerSkillPlanner::getSelectedRobots(
     skills.back()->setParameter("line_policy", std::string("arc"));
     skills.back()->setParameter("positioning_policy", std::string("goal"));
     skills.back()->setParameter("robot_interval", 0.35);
-    skills.back()->setParameter("margin_distance", 0.35);
+    skills.back()->setParameter("margin_distance", 0.6);
   }
 
   return selected;