Merge pull request #163 from ibis-ssl/test_autoref

AutoRefのお試しをやりながらバグを潰していく
ibis-ssl · Feb 23, 2024 · 6dd9fce · 6dd9fce
2 parents 5e0db97 + 2ecfcbd
commit 6dd9fce
Show file tree

Hide file tree

Showing 11 changed files with 49 additions and 33 deletions.
diff --git a/crane_msgs/msg/control/RobotCommands.msg b/crane_msgs/msg/control/RobotCommands.msg
@@ -1,3 +1,4 @@
 std_msgs/Header header
+bool on_positive_half
 bool is_yellow
 RobotCommand[] robot_commands
diff --git a/crane_msgs/msg/world_model/WorldModel.msg b/crane_msgs/msg/world_model/WorldModel.msg
@@ -1,5 +1,6 @@
 std_msgs/Header header
 
+bool on_positive_half
 bool is_yellow
 
 uint8 our_goalie_id
@@ -14,6 +15,4 @@ BallInfo ball_info
 RobotInfoOurs[] robot_info_ours
 RobotInfoTheirs[] robot_info_theirs
 
-geometry_msgs/Point ball_placement_target
-
 PlaySituation play_situation
diff --git a/crane_play_switcher/src/play_switcher.cpp b/crane_play_switcher/src/play_switcher.cpp
@@ -187,6 +187,11 @@ void PlaySwitcher::referee_callback(const robocup_ssl_msgs::msg::Referee & msg)
     last_command_changed_state.stamp = now();
     last_command_changed_state.ball_position = world_model->ball.pos;
 
+    if (msg.designated_position.size() > 0) {
+      play_situation_msg.placement_position.x = msg.designated_position[0].x;
+      play_situation_msg.placement_position.y = msg.designated_position[0].y;
+    }
+
     // パブリッシュはコマンド更新時のみ
     play_situation_pub->publish(play_situation_msg);
   }

diff --git a/crane_sender/src/ibis_sender_node.cpp b/crane_sender/src/ibis_sender_node.cpp
@@ -113,7 +113,7 @@ class IbisSenderNode : public SenderBase
 
     for (auto command : msg.robot_commands) {
       //
-      if (msg.is_yellow) {
+      if (not msg.on_positive_half) {
         command.target_velocity.x *= -1;
         command.target_velocity.y *= -1;
         command.target_velocity.theta *= -1;

diff --git a/crane_simple_ai/include/crane_commander.hpp b/crane_simple_ai/include/crane_commander.hpp
@@ -110,6 +110,7 @@ class ROSNode : public rclcpp::Node
       crane_msgs::msg::RobotCommands msg;
       msg.header = world_model->getMsg().header;
       msg.is_yellow = world_model->isYellow();
+      msg.on_positive_half = world_model->onPositiveHalf();
       msg.robot_commands.push_back(latest_msg);
       publisher_robot_commands->publish(msg);
     });

diff --git a/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp b/crane_world_model_publisher/include/crane_world_model_publisher/world_model_publisher.hpp
@@ -91,6 +91,8 @@ class WorldModelPublisherComponent : public rclcpp::Node
 
   Color their_color;
 
+  bool on_positive_half;
+
   uint8_t our_goalie_id, their_goalie_id;
 
   uint8_t max_id;

diff --git a/crane_world_model_publisher/src/world_model_publisher.cpp b/crane_world_model_publisher/src/world_model_publisher.cpp
@@ -38,6 +38,16 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
   declare_parameter("team_name", "ibis-ssl");
   team_name = get_parameter("team_name").as_string();
 
+  declare_parameter("initial_team_color", "BLUE");
+  auto initial_team_color = get_parameter("initial_team_color").as_string();
+  if (initial_team_color == "BLUE") {
+    our_color = Color::BLUE;
+    their_color = Color::YELLOW;
+  } else {
+    our_color = Color::YELLOW;
+    their_color = Color::BLUE;
+  }
+
   sub_referee = this->create_subscription<robocup_ssl_msgs::msg::Referee>(
     "/referee", 1, [this](const robocup_ssl_msgs::msg::Referee & msg) {
       if (msg.yellow.name == team_name) {
@@ -46,12 +56,18 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
         their_color = Color::BLUE;
         our_goalie_id = msg.yellow.goalkeeper;
         their_goalie_id = msg.blue.goalkeeper;
+        if (not msg.blue_team_on_positive_half.empty()) {
+          on_positive_half = not msg.blue_team_on_positive_half[0];
+        }
       } else if (msg.blue.name == team_name) {
         // BLUE
         our_color = Color::BLUE;
         their_color = Color::YELLOW;
         our_goalie_id = msg.blue.goalkeeper;
         their_goalie_id = msg.yellow.goalkeeper;
+        if (not msg.blue_team_on_positive_half.empty()) {
+          on_positive_half = msg.blue_team_on_positive_half[0];
+        }
       } else {
         std::stringstream what;
         what << "Cannot find our team name, " << team_name << " in referee message. ";
@@ -64,17 +80,6 @@ WorldModelPublisherComponent::WorldModelPublisherComponent(const rclcpp::NodeOpt
         ball_placement_target_y = msg.designated_position.front().y / 1000.;
       }
     });
-
-  declare_parameter("initial_team_color", "BLUE");
-  auto initial_team_color = get_parameter("initial_team_color").as_string();
-  if (initial_team_color == "BLUE") {
-    our_color = Color::BLUE;
-    their_color = Color::YELLOW;
-
-  } else {
-    our_color = Color::YELLOW;
-    their_color = Color::BLUE;
-  }
 }
 
 void WorldModelPublisherComponent::visionDetectionsCallback(
@@ -164,6 +169,7 @@ void WorldModelPublisherComponent::publishWorldModel()
   crane_msgs::msg::WorldModel wm;
 
   wm.is_yellow = (our_color == Color::YELLOW);
+  wm.on_positive_half = on_positive_half;
   wm.ball_info = ball_info;
 
   updateBallContact();
@@ -196,9 +202,6 @@ void WorldModelPublisherComponent::publishWorldModel()
   wm.goal_size.x = goal_h;
   wm.goal_size.y = goal_w;
 
-  wm.ball_placement_target.x = ball_placement_target_x;
-  wm.ball_placement_target.y = ball_placement_target_y;
-
   wm.our_goalie_id = our_goalie_id;
   wm.their_goalie_id = their_goalie_id;
 

diff --git a/session/crane_planner_base/include/crane_planner_base/planner_base.hpp b/session/crane_planner_base/include/crane_planner_base/planner_base.hpp
@@ -69,7 +69,8 @@ class PlannerBase
     status = latest_status;
     crane_msgs::msg::RobotCommands msg;
     msg.is_yellow = world_model->isYellow();
-    for (auto command : robot_commands) {
+    msg.on_positive_half = world_model->onPositiveHalf();
+    for (const auto & command : robot_commands) {
       msg.robot_commands.emplace_back(command);
     }
     return msg;

diff --git a/session/crane_session_controller/src/crane_session_controller.cpp b/session/crane_session_controller/src/crane_session_controller.cpp
@@ -142,6 +142,7 @@ SessionControllerComponent::SessionControllerComponent(const rclcpp::NodeOptions
   world_model->addCallback([this]() {
     crane_msgs::msg::RobotCommands msg;
     msg.header = world_model->getMsg().header;
+    msg.on_positive_half = world_model->onPositiveHalf();
     msg.is_yellow = world_model->isYellow();
     for (const auto & planner : available_planners) {
       auto commands_msg = planner->getRobotCommands();

diff --git a/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp b/utility/crane_msg_wrappers/include/crane_msg_wrappers/world_model_wrapper.hpp
@@ -257,9 +257,9 @@ struct WorldModelWrapper
     defense_area_size << world_model.defense_area_size.x, world_model.defense_area_size.y;
 
     goal_size << world_model.goal_size.x, world_model.goal_size.y;
-    goal << (isYellow() ? field_size.x() * 0.5 : -field_size.x() * 0.5), 0.;
+    goal << getOurSideSign() * field_size.x() * 0.5, 0.;
 
-    if (goal.x() > 0) {
+    if (onPositiveHalf()) {
       ours.defense_area.max_corner() << goal.x(), goal.y() + world_model.defense_area_size.y / 2.;
       ours.defense_area.min_corner() << goal.x() - world_model.defense_area_size.x,
         goal.y() - world_model.defense_area_size.y / 2.;
@@ -274,19 +274,14 @@ struct WorldModelWrapper
     theirs.defense_area.min_corner()
       << std::min(-ours.defense_area.max_corner().x(), -ours.defense_area.min_corner().x()),
       ours.defense_area.min_corner().y();
-
-    if (
-      world_model.play_situation.command == crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or
-      world_model.play_situation.command == crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) {
-      *ball_placement_target << world_model.ball_placement_target.x,
-        world_model.ball_placement_target.y;
-    } else {
-      ball_placement_target = std::nullopt;
-    }
   }
 
   [[nodiscard]] const crane_msgs::msg::WorldModel & getMsg() const { return latest_msg; }
 
+  [[nodiscard]] bool onPositiveHalf() const { return (latest_msg.on_positive_half); }
+
+  [[nodiscard]] double getOurSideSign() const { return onPositiveHalf() ? 1.0 : -1.0; }
+
   [[nodiscard]] bool isYellow() const { return (latest_msg.is_yellow); }
 
   [[nodiscard]] bool hasUpdated() const { return has_updated; }
@@ -456,16 +451,24 @@ struct WorldModelWrapper
 
   [[nodiscard]] std::optional<Point> getBallPlacementTarget() const
   {
-    return ball_placement_target;
+    if (
+      play_situation.getSituationCommandID() ==
+        crane_msgs::msg::PlaySituation::OUR_BALL_PLACEMENT or
+      play_situation.getSituationCommandID() ==
+        crane_msgs::msg::PlaySituation::THEIR_BALL_PLACEMENT) {
+      return play_situation.placement_position;
+    } else {
+      return std::nullopt;
+    }
   }
 
   // rule 8.4.3
   [[nodiscard]] std::optional<Capsule> getBallPlacementArea() const
   {
-    if (ball_placement_target) {
+    if (auto target = getBallPlacementTarget()) {
       Capsule area;
       area.segment.first = ball.pos;
-      area.segment.second = ball_placement_target.value();
+      area.segment.second = target.value();
       area.radius = 0.5;
       return area;
     } else {

diff --git a/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp b/utility/crane_msg_wrappers/src/play_situation_wrapper.cpp
@@ -66,7 +66,7 @@ auto PlaySituationWrapper::update(const crane_msgs::msg::PlaySituation & msg) ->
   situation_command.id = msg.command;
   situation_command.text = situation_command_map[msg.command];
 
-  placement_position << msg.placement_position.x, msg.placement_position.y;
+  placement_position << msg.placement_position.x / 1000., msg.placement_position.y / 1000.;
 }
 
 auto PlaySituationWrapper::getRefereeCommandText(uint32_t id) -> std::string