Merge branch 'main' into feat/navigation-message-mappers

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+    "configurations": [
+        {
+            "name": "Linux",
+            "includePath": [
+                "${workspaceFolder}/**"
+            ],
+            "defines": [],
+            "compilerPath": "/usr/bin/gcc",
+            "cStandard": "c17",
+            "cppStandard": "gnu++17",
+            "intelliSenseMode": "linux-gcc-x64"
+        }
+    ],
+    "version": 4
+}

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,12 @@ namespace behavior {
 constinit const auto pBehaviorPollerTimeoutMs
     = ::robocin::parameters::View<1>::asInt32(10 /*ms ~= 100Hz*/);
 
+constinit const auto pAllyColor = Color::COLOR_BLUE;
+
+constinit const auto isAttackingToRight = ::robocin::parameters::View<1>::asBool(true);
+
+constinit const auto pBallIsMovingVelocity = ::robocin::parameters::View<1>::asInt32(300);
+constinit const auto pBallIsMovingFastVelocity = ::robocin::parameters::View<1>::asInt32(850);
 // NOLINTEND(*comment*)
 } // namespace behavior
 

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

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

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

@@ -0,0 +1,8 @@
+robocin_cpp_library(
+  NAME analyzers
+  HDRS field_analyzer.h
+       ball_analyzer.h
+  SRCS field_analyzer.cpp
+       ball_analyzer.cpp
+  DEPS common::geometry
+)

behavior-ms/behavior-bruxo/behavior/processing/analyzer/ball_analyzer.cpp

@@ -0,0 +1,108 @@
+#include "behavior/processing/analyzer/ball_analyzer.h"
+
+#include "behavior/parameters/parameters.h"
+
+#include <robocin/geometry/mathematics.h>
+#include <robocin/geometry/point2d.h>
+
+namespace behavior {
+
+bool BallAnalyzer::isBallStopped(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() < pBallIsMovingVelocity();
+}
+
+bool BallAnalyzer::isBallMovingFast(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingFastVelocity();
+}
+
+bool BallAnalyzer::isBallMoving(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingVelocity();
+}
+
+bool BallAnalyzer::isBallMovingSlowly(BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > pBallIsMovingVelocity()
+         && ball_velocity.length() < pBallIsMovingFastVelocity();
+}
+
+bool BallAnalyzer::isBallMovingWithVelocity(double min_velocity, BallMessage& ball) {
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  return ball_velocity.length() > min_velocity;
+}
+
+bool BallAnalyzer::isBallMovingTowards(const robocin::Point2Df& target,
+                                       BallMessage& ball,
+                                       double max_angle_difference) {
+  if (!isBallMoving(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  robocin::Point2Df ball_to_target_vector_direction = (target - ball_position).normalized();
+
+  double angle_difference
+      = std::abs(ball_to_target_vector_direction.angleTo(ball_velocity.normalized()));
+  return angle_difference < max_angle_difference;
+}
+
+bool BallAnalyzer::isBallMovingAway(const robocin::Point2Df& target, BallMessage& ball) {
+  bool is_ball_moving_away_from_target = isBallMoving(ball) && !isBallMovingTowards(target, ball);
+  return is_ball_moving_away_from_target;
+}
+
+bool BallAnalyzer::isBallMovingAwayWithVelocity(const robocin::Point2Df& target,
+                                                double velocity,
+                                                BallMessage& ball) {
+  bool is_ball_moving_away_with_velocity
+      = isBallMovingWithVelocity(velocity, ball) && !isBallMovingTowards(target, ball);
+  return is_ball_moving_away_with_velocity;
+}
+
+bool BallAnalyzer::isBallMovingToOffensiveGoal(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  robocin::Point2Df offset_from_goal_center = robocin::Point2Df{0.0, *(field.width) / 2};
+
+  bool moving_to_field_bottom_line
+      = mathematics::segmentsIntersect(field.enemyGoalOutsideCenter() + offset_from_goal_center,
+                                       field.enemyGoalOutsideCenter() - offset_from_goal_center,
+                                       ball_position,
+                                       ball_position + ball_velocity.resized(*(field.length)));
+  return moving_to_field_bottom_line;
+}
+
+bool BallAnalyzer::isBallMovingToDefensiveGoal(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  robocin::Point2Df ball_position = robocin::Point2Df{ball.position->x, ball.position->y};
+  robocin::Point2Df ball_velocity = robocin::Point2Df{ball.velocity->x, ball.velocity->y};
+  float offset_y = (*field.penalty_area_width) * sqrt(2) / 2;
+  robocin::Point2Df offset_from_goal_center = {0, offset_y};
+  bool is_moving_to_defensive_goal
+      = mathematics::segmentsIntersect(field.allyGoalOutsideCenter() + offset_from_goal_center,
+                                       field.allyGoalOutsideCenter() - offset_from_goal_center,
+                                       ball_position,
+                                       ball_position + ball_velocity.resized(*(field.length)));
+  return is_moving_to_defensive_goal;
+}
+
+bool BallAnalyzer::isBallMovingToEnemySide(const FieldMessage& field, BallMessage& ball) {
+  if (isBallStopped(ball)) {
+    return false;
+  }
+
+  bool is_moving_to_enemy_side = (ball.velocity->x * field.attackDirection().x) > 0;
+  return is_moving_to_enemy_side;
+}
+
+} // namespace behavior

behavior-ms/behavior-bruxo/behavior/processing/analyzer/ball_analyzer.h

@@ -0,0 +1,33 @@
+#ifndef BEHAVIOR_PROCESSING_ANALYZER_BALL_ANALYZER_H
+#define BEHAVIOR_PROCESSING_ANALYZER_BALL_ANALYZER_H
+
+#include "behavior/processing/messages/perception/ball/ball_message.h"
+#include "behavior/processing/messages/perception/field/field_message.h"
+
+#include <robocin/geometry/point2d.h>
+
+namespace behavior {
+
+class BallAnalyzer {
+ public:
+  BallAnalyzer();
+
+  static bool isBallStopped(BallMessage& ball);
+  static bool isBallMovingFast(BallMessage& ball);
+  static bool isBallMoving(BallMessage& ball);
+  static bool isBallMovingSlowly(BallMessage& ball);
+  static bool isBallMovingWithVelocity(double min_velocity, BallMessage& ball);
+  static bool isBallMovingTowards(const robocin::Point2Df& target,
+                                  BallMessage& ball,
+                                  double max_angle_difference = 1.047);
+  static bool isBallMovingAway(const robocin::Point2Df& target, BallMessage& ball);
+  static bool
+  isBallMovingAwayWithVelocity(const robocin::Point2Df& target, double velocity, BallMessage& ball);
+  static bool isBallMovingToOffensiveGoal(const FieldMessage& field, BallMessage& ball);
+  static bool isBallMovingToDefensiveGoal(const FieldMessage& field, BallMessage& ball);
+  static bool isBallMovingToEnemySide(const FieldMessage& field, BallMessage& ball);
+};
+
+} // namespace behavior
+
+#endif /* BEHAVIOR_PROCESSING_ANALYZER_BALL_ANALYZER_H */

behavior-ms/behavior-bruxo/behavior/processing/analyzer/field_analyzer.cpp

@@ -0,0 +1 @@
+#include "behavior/processing/analyzer/field_analyzer.h"

behavior-ms/behavior-bruxo/behavior/processing/analyzer/field_analyzer.h

@@ -0,0 +1,163 @@
+#ifndef BEHAVIOR_PROCESSING_ANALYZER_FIELD_ANALYZER_H
+#define BEHAVIOR_PROCESSING_ANALYZER_FIELD_ANALYZER_H
+
+#include "behavior/processing/messages/perception/field/field_message.h"
+
+#include <robocin/geometry/point2d.h>
+
+namespace behavior {
+
+class FieldAnalyzer {
+ public:
+  FieldAnalyzer();
+
+  // goal contains:
+
+  static bool
+  leftGoalContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(field.leftGoalInsideBottom(), field.leftGoalOutsideTop(), point, radius);
+    } else {
+      return contains(field.leftGoalInsideBottom(), field.leftGoalOutsideTop(), point);
+    }
+  }
+
+  static bool
+  rightGoalContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(field.rightGoalOutsideBottom(), field.rightGoalInsideTop(), point, radius);
+    } else {
+      return contains(field.rightGoalOutsideBottom(), field.rightGoalInsideTop(), point);
+    }
+  }
+
+  static bool
+  allyGoalContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? leftGoalContains(point, field, radius) :
+                                  rightGoalContains(point, field, radius);
+  }
+
+  static bool
+  enemyGoalContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? rightGoalContains(point, field, radius) :
+                                  leftGoalContains(point, field, radius);
+  }
+
+  // without goals contains:
+
+  static bool
+  withoutGoalsContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(field.bottomLeft(), field.topRight(), point, radius);
+    } else {
+      return contains(field.bottomLeft(), field.topRight(), point);
+    }
+  }
+
+  // contains:
+
+  static bool contains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return leftGoalContains(point, field, radius) || withoutGoalsContains(point, field, radius)
+           || rightGoalContains(point, field, radius);
+  }
+
+  // side contains:
+
+  static bool
+  leftSideContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return leftGoalContains(point, field, radius)
+             || contains(field.bottomLeft(), field.topCenter(), point, radius);
+    } else {
+      return leftGoalContains(point, field, radius)
+             || contains(field.bottomLeft(), field.topCenter(), point);
+    }
+  }
+
+  static bool
+  rightSideContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(field.bottomCenter(), field.topRight(), point, radius)
+             || rightGoalContains(point, field, radius);
+    } else {
+      return contains(field.bottomCenter(), field.topRight(), point)
+             || rightGoalContains(point, field, radius);
+    }
+  }
+
+  static bool
+  allySideContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? leftSideContains(point, field, radius) :
+                                  rightSideContains(point, field, radius);
+  }
+
+  static bool
+  enemySideContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? rightSideContains(point, field, radius) :
+                                  leftSideContains(point, field, radius);
+  }
+
+  // penalty area contains:
+
+  static bool
+  leftPenaltyAreaContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(robocin::Point2Df(field.min().x, field.leftPenaltyAreaCornerBottom().y),
+                      field.leftPenaltyAreaCornerTop(),
+                      point,
+                      radius);
+    } else {
+      return contains(robocin::Point2Df(field.min().x, field.leftPenaltyAreaCornerBottom().y),
+                      field.leftPenaltyAreaCornerTop(),
+                      point);
+    }
+  }
+
+  static bool
+  rightPenaltyAreaContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    if (radius) {
+      return contains(field.rightPenaltyAreaCornerBottom(),
+                      robocin::Point2Df(field.max().x, field.rightPenaltyAreaCornerTop().y),
+                      point,
+                      radius);
+    } else {
+      return contains(field.rightPenaltyAreaCornerBottom(),
+                      robocin::Point2Df(field.max().x, field.rightPenaltyAreaCornerTop().y),
+                      point);
+    }
+  }
+
+  static bool
+  allyPenaltyAreaContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? leftPenaltyAreaContains(point, field, radius) :
+                                  rightPenaltyAreaContains(point, field, radius);
+  }
+
+  static bool
+  enemyPenaltyAreaContains(const robocin::Point2Df& point, FieldMessage& field, double radius = 0) {
+    return isAttackingToRight() ? rightPenaltyAreaContains(point, field, radius) :
+                                  leftPenaltyAreaContains(point, field, radius);
+  }
+
+ protected:
+  // strictly contains:
+  static bool contains(const robocin::Point2Df& bottomLeft,
+                       const robocin::Point2Df& topRight,
+                       const robocin::Point2Df& point) {
+    return (bottomLeft.x < point.x && point.x < topRight.x)
+           && (bottomLeft.y < point.y && point.y < topRight.y);
+  }
+
+  static bool contains(const robocin::Point2Df& a1,
+                       const robocin::Point2Df& a2,
+                       const robocin::Point2Df& point,
+                       double radius) {
+    const robocin::Point2Df& b1(point - robocin::Point2Df(radius / 2, radius / 2));
+    const robocin::Point2Df& b2(point + robocin::Point2Df(radius / 2, radius / 2));
+    return ((b1.x < a2.x) && (a1.x < b2.x)) && ((b1.y < a2.y) && (a1.y < b2.y));
+  }
+};
+
+} // namespace behavior
+
+#endif /* BEHAVIOR_PROCESSING_ANALYZER_FIELD_ANALYZER_H */

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

@@ -91,12 +91,14 @@ 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_detection.field(),
+                last_game_status_.value());
+
+  for (const auto& robot : world_.allies) {
     behavior_message.output.emplace_back(
         OutputMessage{RobotIdMessage{}, MotionMessage{}, PlanningMessage{}});
   }