[behavior-ms] Add behavior world class (#88)

This PR add a world to behavior service processing.

World contains:
- Allies (RobotMessage)
- Enemies (RobotMessage)
- Ball (BallMessage) -> get ball with highest confidence
- GameStatus (GameStatusMessage)

behavior-ms/behavior-bruxo/behavior/parameters/parameters.h

@@ -1,6 +1,8 @@
 #ifndef BEHAVIOR_PARAMETERS_PARAMETERS_H
 #define BEHAVIOR_PARAMETERS_PARAMETERS_H
 
+#include "behavior/processing/messages/common/robot_id/robot_id.h"
+
 #include <robocin/parameters/parameters.h>
 
 namespace behavior {
@@ -9,6 +11,8 @@ namespace behavior {
 constinit const auto pBehaviorPollerTimeoutMs
     = ::robocin::parameters::View<1>::asInt32(10 /*ms ~= 100Hz*/);
 
+constinit const auto pAllyColor = Color::COLOR_BLUE;
+
 // NOLINTEND(*comment*)
 } // namespace behavior
 

behavior-ms/behavior-bruxo/behavior/processing/CMakeLists.txt

@@ -4,5 +4,6 @@ robocin_cpp_library(
     SRCS behavior_processor.cpp
     DEPS common::output
          messages
+         entities
          state_machine
 )

behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.cpp

@@ -91,12 +91,13 @@ std::optional<rc::Behavior> BehaviorProcessor::process(std::span<const Payload>
   const rc::Detection last_detection = detection_messages.back();
 
   BehaviorMessage behavior_message;
-  // TODO: implement the logic to generate the behavior based on the last detection and the last
-  // decision
-  ///////////////////////////////////////////////////////////////////////////////////
-
-  for (const auto& robot : last_detection.robots()) {
 
+  world_.update(last_decision_.value(),
+                {last_detection.robots().begin(), last_detection.robots().end()},
+                {last_detection.balls().begin(), last_detection.balls().end()},
+                last_game_status_.value());
+
+  for (const auto& robot : world_.allies) {
     behavior_message.output.emplace_back(
         OutputMessage{RobotIdMessage{}, MotionMessage{}, PlanningMessage{}});
   }

behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.h

@@ -2,6 +2,7 @@
 #define BEHAVIOR_PROCESSING_BEHAVIOR_PROCESSOR_H
 
 #include "behavior/messaging/receiver/payload.h"
+#include "behavior/processing/entities/world.h"
 #include "behavior/processing/messages/behavior/behavior_message.h"
 #include "behavior/processing/messages/common/robot_id/robot_id.h"
 #include "behavior/processing/messages/motion/motion_message.h"
@@ -45,6 +46,7 @@ class BehaviorProcessor : public IBehaviorProcessor {
   process(std::span<const Payload> payloads) override;
 
  private:
+  World world_;
   std::unique_ptr<::robocin::parameters::IHandlerEngine> parameters_handler_engine_;
   std::unique_ptr<behavior::GoalkeeperStateMachine> goalkeeper_state_machine_;
   std::optional<::protocols::decision::Decision> last_decision_;

behavior-ms/behavior-bruxo/behavior/processing/entities/world.cpp

@@ -1,32 +1,65 @@
 #include "behavior/processing/entities/world.h"
 
-namespace behavior {
+#include "world.h"
 
-void World::update(std::optional<DecisionMessage>& decision,
-                   std::optional<std::span<RobotMessage>>& allies,
-                   std::optional<std::span<RobotMessage>>& enemies,
-                   std::optional<BallMessage>& ball,
-                   std::optional<GameStatusMessage>& game_status) {
+#include <protocols/common/robot_id.pb.h>
+#include <protocols/perception/detection.pb.h>
+#include <vector>
 
-  if (decision.has_value()) {
-    this->decision = std::move(decision.value());
-  }
+namespace behavior {
 
-  if (allies.has_value()) {
-    this->allies = allies.value();
+void World::takeBallHighConfidence(const std::vector<protocols::perception::Ball>& balls) {
+  if (balls.empty()) {
+    return;
   }
 
-  if (enemies.has_value()) {
-    this->enemies = enemies.value();
-  }
+  auto max_ball
+      = std::max_element(balls.begin(), balls.end(), [](const auto& ball, const auto& candidate) {
+          return ball.confidence() < candidate.confidence();
+        });
 
-  if (ball.has_value()) {
-    this->ball = std::move(ball.value());
+  this->ball.fromProto(*max_ball);
+}
+
+void World::takeAlliesAndEnemies(const std::vector<protocols::perception::Robot>& robots) {
+  if (robots.empty()) {
+    return;
   }
 
-  if (game_status.has_value()) {
-    this->game_status = std::move(game_status.value());
+  allies.clear();
+  enemies.clear();
+
+  for (const auto& robot : robots) {
+    RobotMessage robot_message;
+    robot_message.fromProto(robot);
+
+    if (isAlly(robot_message)) {
+      this->allies.emplace_back(std::move(robot_message));
+    } else {
+      this->enemies.emplace_back(std::move(robot_message));
+    }
   }
 }
 
+void World::takeDecision(const protocols::decision::Decision& decision) {
+  this->decision.fromProto(decision);
+}
+
+void World::takeGameStatus(const protocols::referee::GameStatus& game_status) {
+  this->game_status.fromProto(game_status);
+}
+
+bool World::isAlly(const RobotMessage& robot) const { return robot.robot_id->color == pAllyColor; }
+
+void World::update(const protocols::decision::Decision& decision,
+                   const std::vector<protocols::perception::Robot>& robots,
+                   const std::vector<protocols::perception::Ball>& balls,
+                   const protocols::referee::GameStatus& game_status) {
+
+  World::takeDecision(decision);
+  World::takeAlliesAndEnemies(robots);
+  World::takeBallHighConfidence(balls);
+  World::takeGameStatus(game_status);
+}
+
 } // namespace behavior

behavior-ms/behavior-bruxo/behavior/processing/entities/world.h

@@ -1,11 +1,20 @@
 #ifndef BEHAVIOR_PROCESSING_ENTITIES_WORLD_H
 #define BEHAVIOR_PROCESSING_ENTITIES_WORLD_H
 
+
+#include "behavior/parameters/parameters.h"
+
 #include "behavior/processing/messages/decision/decision_message.h"
 #include "behavior/processing/messages/perception/ball/ball_message.h"
 #include "behavior/processing/messages/perception/robot/robot_message.h"
 #include "behavior/processing/messages/referee/game_status_message.h"
 
+#include <protocols/behavior/behavior_unification.pb.h>
+#include <protocols/decision/decision.pb.h>
+#include <protocols/perception/detection.pb.h>
+#include <protocols/referee/game_status.pb.h>
+#include <vector>
+
 namespace behavior {
 
 class World {
@@ -23,14 +32,22 @@ class World {
   DecisionMessage decision;
   GameStatusMessage game_status;
 
-  std::span<RobotMessage> allies;
-  std::span<RobotMessage> enemies;
+  std::vector<RobotMessage> allies;
+  std::vector<RobotMessage> enemies;
+
+  void update(const protocols::decision::Decision& decision,
+              const std::vector<protocols::perception::Robot>& robots,
+              const std::vector<protocols::perception::Ball>& balls,
+              const protocols::referee::GameStatus& game_status);
+
+ private:
+  void takeBallHighConfidence(const std::vector<protocols::perception::Ball>& balls);
+  void takeAlliesAndEnemies(const std::vector<protocols::perception::Robot>& robots);
+  void takeDecision(const protocols::decision::Decision& decision);
+  void takeGameStatus(const protocols::referee::GameStatus& game_status);
+
+  [[nodiscard]] bool isAlly(const RobotMessage& robot) const;
 
-  void update(std::optional<DecisionMessage>& decision,
-              std::optional<std::span<RobotMessage>>& allies,
-              std::optional<std::span<RobotMessage>>& enemies,
-              std::optional<BallMessage>& ball,
-              std::optional<GameStatusMessage>& game_status);
 };
 
 } // namespace behavior