From b91f4fd4a9c875e6f5cddc38bd04a9029b977626 Mon Sep 17 00:00:00 2001 From: Marcela <96249442+maa134340@users.noreply.github.com> Date: Sat, 26 Oct 2024 21:49:06 -0300 Subject: [PATCH 1/7] Adds new version of the angular library to robocin/ssl-core (#63) The library provides a handful of functions to manipulate angles in a practical way, which are used in navigation methods, such as go to point. --- .../processing/behavior_processor.cpp | 30 ++-- common/cpp/robocin/geometry/point2d.h | 95 ++++------- .../network/udp_multicast_socket_sender.cpp | 68 ++++---- .../network/udp_multicast_socket_sender.h | 51 +++--- common/cpp/robocin/utility/CMakeLists.txt | 14 ++ common/cpp/robocin/utility/angular.cpp | 1 + common/cpp/robocin/utility/angular.h | 63 +++++++ common/cpp/robocin/utility/angular_test.cpp | 156 ++++++++++++++++++ .../cpp/robocin/utility/iproto_convertible.h | 4 +- .../communication/communication_main.cpp | 29 ++-- .../controller/consumer_controller.cpp | 11 +- .../messaging/receiver/message_receiver.h | 2 +- .../messaging/receiver/payload.h | 3 +- .../messaging/receiver/payload_mapper.cpp | 2 +- .../messaging/sender/message_sender.h | 2 +- .../processing/communication_processor.cpp | 38 ++--- .../processing/communication_processor.h | 4 +- .../robot_command/robot_command_mapper.cpp | 108 ++++++------ .../robot_command/robot_command_mapper.h | 16 +- .../decision/decision_main.cpp | 11 +- .../processing/decision_processor.cpp | 23 +-- .../controller/consumer_controller.cpp | 6 +- .../controller/consumer_controller.h | 7 +- .../controller/producer_controller.h | 1 - .../messaging/receiver/message_receiver.cpp | 5 +- .../navigation/messaging/receiver/payload.cpp | 4 +- .../messaging/receiver/payload_mapper.cpp | 6 +- .../messaging/sender/message_sender.h | 3 +- .../navigation/navigation_main.cpp | 9 +- .../navigation/parameters/parameters.cpp | 2 +- .../motion_parser/motion_parser.cpp | 95 ++++++----- .../processing/motion_parser/motion_parser.h | 35 ++-- .../processing/navigation_processor.cpp | 57 +++---- .../processing/navigation_processor.h | 1 - .../processing/utils/robot_move.cpp | 2 +- .../navigation/processing/utils/robot_move.h | 17 +- 36 files changed, 576 insertions(+), 405 deletions(-) create mode 100644 common/cpp/robocin/utility/angular.cpp create mode 100644 common/cpp/robocin/utility/angular.h create mode 100644 common/cpp/robocin/utility/angular_test.cpp diff --git a/behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.cpp b/behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.cpp index 2ebf76b1..3f2feff1 100644 --- a/behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.cpp +++ b/behavior-ms/behavior-bruxo/behavior/processing/behavior_processor.cpp @@ -3,16 +3,13 @@ #include "behavior/messaging/receiver/payload.h" #include "behavior/parameters/parameters.h" -#include #include - +#include #include - #include - +#include #include #include -#include namespace behavior { @@ -25,16 +22,15 @@ using ::robocin::object_ptr; namespace rc { - using ::protocols::common::RobotId; +using ::protocols::decision::Decision; using ::protocols::perception::Ball; using ::protocols::perception::Detection; -using ::protocols::decision::Decision; using ::protocols::behavior::GoToPoint; +using ::protocols::behavior::unification::Behavior; using ::protocols::behavior::unification::Motion; using ::protocols::behavior::unification::Output; -using ::protocols::behavior::unification::Behavior; } // namespace rc @@ -57,30 +53,30 @@ BehaviorProcessor::BehaviorProcessor( std::optional BehaviorProcessor::process(std::span payloads) { rc::Behavior behavior_output; - if(std::vector decision_messages = decisionfromPayloads(payloads); - !decision_messages.empty()) { + if (std::vector decision_messages = decisionfromPayloads(payloads); + !decision_messages.empty()) { last_decision_ = decision_messages.back(); } - if(!last_decision_) { + if (!last_decision_) { return std::nullopt; } std::vector detection_messages = detectionFromPayloads(payloads); - if(detection_messages.empty()) { + if (detection_messages.empty()) { // a new package must be generated only when a new detection is received. return std::nullopt; } const rc::Detection last_detection = detection_messages.back(); - - // TODO: implement the logic to generate the behavior based on the last detection and the last decision + // TODO: implement the logic to generate the behavior based on the last detection and the last + // decision /////////////////////////////////////////////////////////////////////////////////// - for(const auto& behavior_ : last_decision_->behavior()) { + for (const auto& behavior_ : last_decision_->behavior()) { rc::Output output; output.mutable_robot_id()->CopyFrom(behavior_.robot_id()); - - if(behavior_.id() == 0) { + + if (behavior_.id() == 0) { rc::Ball ball = last_detection.balls(last_detection.balls_size() - 1); auto* go_to_point = output.mutable_motion()->mutable_go_to_point(); diff --git a/common/cpp/robocin/geometry/point2d.h b/common/cpp/robocin/geometry/point2d.h index 4da90677..decf862a 100644 --- a/common/cpp/robocin/geometry/point2d.h +++ b/common/cpp/robocin/geometry/point2d.h @@ -1,10 +1,10 @@ #ifndef ROBOCIN_GEOMETRY_POINT2D_H #define ROBOCIN_GEOMETRY_POINT2D_H -#include -#include #include #include +#include +#include namespace robocin { @@ -17,16 +17,14 @@ struct Point2D { // Constructors: inline constexpr Point2D() : x(0), y(0) {} inline constexpr Point2D(value_type x, value_type y) : x(x), y(y) {} - + template inline constexpr Point2D(const std::pair& pair) : - x(static_cast(pair.first)), - y(static_cast(pair.second)) {} + x(static_cast(pair.first)), + y(static_cast(pair.second)) {} // Static methods: - inline static constexpr Point2D null() { - return Point2D(0, 0); - } + inline static constexpr Point2D null() { return Point2D(0, 0); } inline static constexpr Point2D fromPolar(value_type angle) { return Point2D(std::cos(angle), std::sin(angle)); @@ -38,7 +36,7 @@ struct Point2D { // Floatint-point comparison: bool fuzzyIsNull(value_type value, value_type epsilon = static_cast(1e-9)) { - return std::abs(value) < epsilon; + return std::abs(value) < epsilon; } inline constexpr bool operator==(const Point2D& other) const { @@ -48,7 +46,7 @@ struct Point2D { // Arithmetic-assignment operators: inline constexpr Point2D& operator+=(const Point2D& other) { x += other.x, y += other.y; - return *this; + return *this; } inline constexpr Point2D& operator-=(const Point2D& other) { @@ -67,21 +65,13 @@ struct Point2D { } // arithmetic operators: - inline constexpr Point2D operator+(const Point2D& other) const { - return Point2D(*this) += other; - } + inline constexpr Point2D operator+(const Point2D& other) const { return Point2D(*this) += other; } - inline constexpr Point2D operator-(const Point2D& other) const { - return Point2D(*this) -= other; - } + inline constexpr Point2D operator-(const Point2D& other) const { return Point2D(*this) -= other; } - inline constexpr Point2D operator*(value_type factor) const { - return Point2D(*this) *= factor; - } + inline constexpr Point2D operator*(value_type factor) const { return Point2D(*this) *= factor; } - inline constexpr Point2D operator/(value_type factor) const { - return Point2D(*this) /= factor; - } + inline constexpr Point2D operator/(value_type factor) const { return Point2D(*this) /= factor; } // arithmetic friends: friend inline constexpr Point2D operator*(value_type factor, const Point2D& point) { @@ -89,47 +79,31 @@ struct Point2D { } // sign operators: - inline constexpr Point2D operator+() const { - return *this; - } + inline constexpr Point2D operator+() const { return *this; } - inline constexpr Point2D operator-() const { - return Point2D(-x, -y); - } + inline constexpr Point2D operator-() const { return Point2D(-x, -y); } // Custom - [[nodiscard]] inline constexpr bool isNull() const { - return fuzzyIsNull(x) and fuzzyIsNull(y); - } + [[nodiscard]] inline constexpr bool isNull() const { return fuzzyIsNull(x) and fuzzyIsNull(y); } - inline constexpr void transpose() { - std::swap(x, y); - } + inline constexpr void transpose() { std::swap(x, y); } - inline constexpr Point2D transposed() const { - return Point2D{y, x}; - } + inline constexpr Point2D transposed() const { return Point2D{y, x}; } inline constexpr void swap(Point2D& other) noexcept { std::swap(x, other.x); std::swap(y, other.y); } - inline constexpr std::pair toPair() const { - return std::make_pair(x, y); - } + inline constexpr std::pair toPair() const { return std::make_pair(x, y); } template inline constexpr std::pair toPair() const { return std::make_pair(static_cast(x), static_cast(y)); } - inline constexpr value_type manhattanLength() const { - return std::abs(x) + std::abs(y); - } + inline constexpr value_type manhattanLength() const { return std::abs(x) + std::abs(y); } - inline constexpr value_type dot(const Point2D& other) const { - return x * other.x + y * other.y; - } + inline constexpr value_type dot(const Point2D& other) const { return x * other.x + y * other.y; } inline constexpr value_type cross(const Point2D& other) const { return x * other.y - y * other.x; @@ -143,9 +117,7 @@ struct Point2D { return std::sqrt(distanceSquaredTo(other)); } - inline constexpr void rotateClockWise90() { - std::swap(x, y), y = -y; - } + inline constexpr void rotateClockWise90() { std::swap(x, y), y = -y; } inline constexpr Point2D rotatedClockWise90() const { Point2D result{*this}; @@ -153,9 +125,7 @@ struct Point2D { return result; } - inline constexpr void rotateCounterClockWise90() { - std::swap(x, y), x = -x; - } + inline constexpr void rotateCounterClockWise90() { std::swap(x, y), x = -x; } inline constexpr Point2D rotatedCounterClockWise90() const { Point2D result{*this}; @@ -195,25 +165,17 @@ struct Point2D { return result; } - inline constexpr auto angle() const { - return std::atan2(y, x); - } + inline constexpr auto angle() const { return std::atan2(y, x); } inline constexpr auto angleTo(const Point2D& other) const { return std::atan2(cross(other), dot(other)); } - inline constexpr value_type lengthSquared() const { - return dot(*this); - } + inline constexpr value_type lengthSquared() const { return dot(*this); } - inline constexpr auto length() const { - return std::sqrt(lengthSquared()); - } + inline constexpr auto length() const { return std::sqrt(lengthSquared()); } - inline constexpr auto norm() const { - return std::sqrt(lengthSquared()); - } + inline constexpr auto norm() const { return std::sqrt(lengthSquared()); } inline constexpr void resize(value_type t) { if (auto norm = this->norm(); not fuzzyIsNull(norm)) { @@ -298,10 +260,9 @@ struct Point2D { } friend inline std::ostream& operator<<(std::ostream& os, const Point2D& point) { - os << '(' << point.x << ", " << point.y << ')'; - return os; + os << '(' << point.x << ", " << point.y << ')'; + return os; } - }; Point2D() -> Point2D; diff --git a/common/cpp/robocin/network/udp_multicast_socket_sender.cpp b/common/cpp/robocin/network/udp_multicast_socket_sender.cpp index d473bbd7..f49aa34f 100644 --- a/common/cpp/robocin/network/udp_multicast_socket_sender.cpp +++ b/common/cpp/robocin/network/udp_multicast_socket_sender.cpp @@ -22,57 +22,53 @@ namespace robocin { UdpMulticastSocketSender::UdpMulticastSocketSender(size_t size) : fd_(socket(AF_INET, SOCK_DGRAM, 0)), size_(size) { - if (fd_ == -1) { - throw std::runtime_error("failed to create socket."); - } - } + if (fd_ == -1) { + throw std::runtime_error("failed to create socket."); + } +} void UdpMulticastSocketSender::connect(std::string_view ip_address, int port) const { - if (::setsockopt(fd_, - SOL_SOCKET, - SO_REUSEADDR, - &std::integral_constant::value, - sizeof(int)) - == -1) { - throw std::runtime_error("failed to set SO_REUSEADDR."); - } + if (::setsockopt(fd_, + SOL_SOCKET, + SO_REUSEADDR, + &std::integral_constant::value, + sizeof(int)) + == -1) { + throw std::runtime_error("failed to set SO_REUSEADDR."); + } - sockaddr_in udp_addr{}; - udp_addr.sin_family = AF_INET; - udp_addr.sin_port = ::htons(port); - udp_addr.sin_addr.s_addr = ::inet_addr(std::string{ip_address}.c_str()); + sockaddr_in udp_addr{}; + udp_addr.sin_family = AF_INET; + udp_addr.sin_port = ::htons(port); + udp_addr.sin_addr.s_addr = ::inet_addr(std::string{ip_address}.c_str()); - if (::connect(fd_, - reinterpret_cast(&udp_addr), - sizeof(udp_addr)) == -1) { - throw std::runtime_error("failed to connect to multicast group."); - } + if (::connect(fd_, reinterpret_cast(&udp_addr), sizeof(udp_addr)) == -1) { + throw std::runtime_error("failed to connect to multicast group."); + } } void UdpMulticastSocketSender::send(const send_type& message) const { - if (::send(fd_, message.data(), message.size(), 0) == -1) - throw std::runtime_error("failed to send message."); - else if (static_cast(::send(fd_, message.data(), message.size(), 0)) != message.size()) - throw std::runtime_error("failed to send all message."); + if (::send(fd_, message.data(), message.size(), 0) == -1) + throw std::runtime_error("failed to send message."); + else if (static_cast(::send(fd_, message.data(), message.size(), 0)) != message.size()) + throw std::runtime_error("failed to send all message."); } void UdpMulticastSocketSender::send(std::string_view message) const { - if (::send(fd_, message.data(), message.size(), 0) == -1) - throw std::runtime_error("failed to send message."); - else if (static_cast(::send(fd_, message.data(), message.size(), 0)) != message.size()) - throw std::runtime_error("failed to send all message."); + if (::send(fd_, message.data(), message.size(), 0) == -1) + throw std::runtime_error("failed to send message."); + else if (static_cast(::send(fd_, message.data(), message.size(), 0)) != message.size()) + throw std::runtime_error("failed to send all message."); } -int UdpMulticastSocketSender::fd() const { - return fd_; -} +int UdpMulticastSocketSender::fd() const { return fd_; } void UdpMulticastSocketSender::close() const { - if (::close(fd_) == -1) { - throw std::runtime_error("failed to close socket."); - } + if (::close(fd_) == -1) { + throw std::runtime_error("failed to close socket."); + } } } // namespace robocin -#endif \ No newline at end of file +#endif diff --git a/common/cpp/robocin/network/udp_multicast_socket_sender.h b/common/cpp/robocin/network/udp_multicast_socket_sender.h index 42144ccc..251186e2 100644 --- a/common/cpp/robocin/network/udp_multicast_socket_sender.h +++ b/common/cpp/robocin/network/udp_multicast_socket_sender.h @@ -9,35 +9,36 @@ namespace robocin { class IUdpMulticastSocketSender { - public: - using send_type = std::string; - IUdpMulticastSocketSender() = default; - IUdpMulticastSocketSender(const IUdpMulticastSocketSender&) = delete; - IUdpMulticastSocketSender& operator=(const IUdpMulticastSocketSender&) = delete; - IUdpMulticastSocketSender(IUdpMulticastSocketSender&&) = default; - IUdpMulticastSocketSender& operator=(IUdpMulticastSocketSender&&) = default; - virtual ~IUdpMulticastSocketSender() = default; - virtual void connect(std::string_view ip_address, int port) const = 0; - virtual void send(const send_type& message) const = 0; - virtual void send(std::string_view message) const = 0; - virtual void close() const = 0; - [[nodiscard]] virtual int fd() const = 0; + public: + using send_type = std::string; + IUdpMulticastSocketSender() = default; + IUdpMulticastSocketSender(const IUdpMulticastSocketSender&) = delete; + IUdpMulticastSocketSender& operator=(const IUdpMulticastSocketSender&) = delete; + IUdpMulticastSocketSender(IUdpMulticastSocketSender&&) = default; + IUdpMulticastSocketSender& operator=(IUdpMulticastSocketSender&&) = default; + virtual ~IUdpMulticastSocketSender() = default; + virtual void connect(std::string_view ip_address, int port) const = 0; + virtual void send(const send_type& message) const = 0; + virtual void send(std::string_view message) const = 0; + virtual void close() const = 0; + [[nodiscard]] virtual int fd() const = 0; }; class UdpMulticastSocketSender : public IUdpMulticastSocketSender { - public: - explicit UdpMulticastSocketSender(size_t size = 1024); - void connect(std::string_view ip_address, int port) const override; - void send(const send_type& message) const override; - void send(std::string_view message) const override; - void close() const override; - [[nodiscard]] int fd() const override; - private: - int fd_; - size_t size_; + public: + explicit UdpMulticastSocketSender(size_t size = 1024); + void connect(std::string_view ip_address, int port) const override; + void send(const send_type& message) const override; + void send(std::string_view message) const override; + void close() const override; + [[nodiscard]] int fd() const override; + + private: + int fd_; + size_t size_; }; -} +} // namespace robocin #endif -#endif // ROBOCIN_NETWORK_UDP_MULTICAST_SOCKET_SENDER_H \ No newline at end of file +#endif // ROBOCIN_NETWORK_UDP_MULTICAST_SOCKET_SENDER_H diff --git a/common/cpp/robocin/utility/CMakeLists.txt b/common/cpp/robocin/utility/CMakeLists.txt index ed32fb66..cf89d7c4 100644 --- a/common/cpp/robocin/utility/CMakeLists.txt +++ b/common/cpp/robocin/utility/CMakeLists.txt @@ -54,3 +54,17 @@ robocin_cpp_test( SRCS fuzzy_compare_test.cpp DEPS utility ) + +robocin_cpp_library( + NAME angular + HDRS angular.h + SRCS angular.cpp + DEPS utility +) + +robocin_cpp_test( + NAME angular_test + HDRS internal/test/epsilon_injector.h + SRCS angular_test.cpp + DEPS angular +) diff --git a/common/cpp/robocin/utility/angular.cpp b/common/cpp/robocin/utility/angular.cpp new file mode 100644 index 00000000..d17765b8 --- /dev/null +++ b/common/cpp/robocin/utility/angular.cpp @@ -0,0 +1 @@ +#include "robocin/utility/angular.h" diff --git a/common/cpp/robocin/utility/angular.h b/common/cpp/robocin/utility/angular.h new file mode 100644 index 00000000..5e42c3fc --- /dev/null +++ b/common/cpp/robocin/utility/angular.h @@ -0,0 +1,63 @@ +#ifndef ROBOCIN_UTILITY_ANGULAR_H +#define ROBOCIN_UTILITY_ANGULAR_H + +#include "robocin/utility/concepts.h" + +#include +#include + +namespace robocin { + +template +constexpr auto degreesToRadians(T degrees) { + using F = std::conditional_t, T, double>; + + constexpr F kDegreesToRadiansFactor = std::numbers::pi_v / 180; + + return degrees * kDegreesToRadiansFactor; +} + +template +constexpr auto radiansToDegrees(T radians) { + using F = std::conditional_t, T, double>; + + constexpr F kRadiansToDegreesFactor = 180 / std::numbers::pi_v; + + return radians * kRadiansToDegreesFactor; +} + +template +constexpr auto normalizeAngle(T angle) { + using F = std::conditional_t, T, double>; + + constexpr F kPi = std::numbers::pi_v; + constexpr F k2Pi = 2 * kPi; + + if (-kPi <= angle && angle <= kPi) { + return static_cast(angle); + } + + F result = std::fmod(static_cast(angle), k2Pi); + if (result < -kPi) { + result += k2Pi; + } else if (result > kPi) { + result -= k2Pi; + } + return result; +} + +template +constexpr auto smallestAngleDiff(T lhs, U rhs) { + using F = std::conditional_t>, T, double>; + + return normalizeAngle(rhs - lhs); +} + +template +constexpr auto absSmallestAngleDiff(T lhs, U rhs) { + return std::abs(smallestAngleDiff(lhs, rhs)); +} + +} // namespace robocin + +#endif // ROBOCIN_UTILITY_ANGULAR_H diff --git a/common/cpp/robocin/utility/angular_test.cpp b/common/cpp/robocin/utility/angular_test.cpp new file mode 100644 index 00000000..ef77635c --- /dev/null +++ b/common/cpp/robocin/utility/angular_test.cpp @@ -0,0 +1,156 @@ +#include "robocin/utility/angular.h" +#include "robocin/utility/internal/test/epsilon_injector.h" + +#include +#include +#include + +namespace robocin { + +using ::testing::Test; +using ::testing::Types; + +using FloatingPointTestTypes = Types; + +template +class FloatingPointTest : public Test {}; +TYPED_TEST_SUITE(FloatingPointTest, FloatingPointTestTypes); + +TYPED_TEST(FloatingPointTest, DegreesToRadiansGivenPiFractions) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + static constexpr T kPiInDegrees = 180; + + for (const T kRatio : std::views::iota(0, 7)) { + EXPECT_NEAR(degreesToRadians(kRatio * kPiInDegrees), kRatio * kPi, kEpsilon); + } +} + +TYPED_TEST(FloatingPointTest, RadiansToDegreesGivenPiFractions) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + static constexpr T kPiInDegrees = 180; + + for (const T kRatio : std::views::iota(0, 7)) { + EXPECT_NEAR(radiansToDegrees(kRatio * kPi), kRatio * kPiInDegrees, kEpsilon); + } +} + +TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesBetweenPiAndMinusPi) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + EXPECT_NEAR(normalizeAngle(0.0), 0.0, kEpsilon); // 0.0 degrees + EXPECT_NEAR(normalizeAngle(kPi / 2), kPi / 2, kEpsilon); // 90.0 degrees + EXPECT_NEAR(normalizeAngle(-kPi / 2), -kPi / 2, kEpsilon); // -90.0 degrees + EXPECT_NEAR(normalizeAngle(kPi), kPi, kEpsilon); // 180.0 degrees + EXPECT_NEAR(normalizeAngle(-kPi), -kPi, kEpsilon); // -180.0 degrees +} + +TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesGreaterThanPi) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + EXPECT_NEAR(normalizeAngle(5 * kPi / 4), -3 * kPi / 4, kEpsilon); // 225.0 degrees + EXPECT_NEAR(normalizeAngle(-5 * kPi / 4), 3 * kPi / 4, kEpsilon); // -225.0 degrees + EXPECT_NEAR(normalizeAngle(3 * kPi / 2), -kPi / 2, kEpsilon); // 270.0 degrees + EXPECT_NEAR(normalizeAngle(-3 * kPi / 2), kPi / 2, kEpsilon); // -270.0 degrees + EXPECT_NEAR(normalizeAngle(7 * kPi / 3), kPi / 3, kEpsilon); // 420.0 degrees + EXPECT_NEAR(normalizeAngle(-7 * kPi / 3), -kPi / 3, kEpsilon); // -420.0 degrees +} + +TYPED_TEST(FloatingPointTest, NormalizeAngleGivenAnglesLessThanMinusPi) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + EXPECT_NEAR(normalizeAngle(-9 * kPi / 4), -kPi / 4, kEpsilon); // -405.0 degrees + EXPECT_NEAR(normalizeAngle(9 * kPi / 4), kPi / 4, kEpsilon); // 405.0 degrees + EXPECT_NEAR(normalizeAngle(-5 * kPi / 2), -kPi / 2, kEpsilon); // -450.0 degrees + EXPECT_NEAR(normalizeAngle(5 * kPi / 2), kPi / 2, kEpsilon); // 450.0 degrees + EXPECT_NEAR(normalizeAngle(-11 * kPi / 3), kPi / 3, kEpsilon); // -660.0 degrees + EXPECT_NEAR(normalizeAngle(11 * kPi / 3), -kPi / 3, kEpsilon); // 660.0 degrees +} + +TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesBetweenPiAndMinusPiRange) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + // 0.0 and 90.0 degrees + EXPECT_NEAR((smallestAngleDiff(0, kPi / 2)), kPi / 2, kEpsilon); + // 90.0 and 0.0 degrees + EXPECT_NEAR((smallestAngleDiff(kPi / 2, 0)), -kPi / 2, kEpsilon); + // -45.0 and 45.0 degrees + EXPECT_NEAR((smallestAngleDiff(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon); + // 45.0 and -45.0 degrees + EXPECT_NEAR((smallestAngleDiff(kPi / 4, -kPi / 4)), -kPi / 2, kEpsilon); + // -45.0 and 135.0 degrees + EXPECT_NEAR((smallestAngleDiff(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon); + // 135.0 and -45.0 degrees + EXPECT_NEAR((smallestAngleDiff(kPi / 2, -kPi / 4)), -3 * kPi / 4, kEpsilon); +} + +TYPED_TEST(FloatingPointTest, SmallestAngleDiffGivenAnglesOutsidePiAndMinusPiRange) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + // -450.0 to 360.0 degrees + EXPECT_NEAR((smallestAngleDiff(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon); + // 360.0 to -450.0 degrees + EXPECT_NEAR((smallestAngleDiff(2 * kPi, -5 * kPi / 2)), -kPi / 2, kEpsilon); + // -450.0 to 270.0 degrees + EXPECT_NEAR((smallestAngleDiff(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon); + // -450.0 to 270.0 degrees + EXPECT_NEAR((smallestAngleDiff(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon); +} + +TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesBetweenPiAndMinusPiRange) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + // 0.0 and 90.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(0, kPi / 2)), kPi / 2, kEpsilon); + // 90.0 and 0.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(kPi / 2, 0)), kPi / 2, kEpsilon); + // -45.0 and 45.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(-kPi / 4, kPi / 4)), kPi / 2, kEpsilon); + // 45.0 and -45.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(kPi / 4, -kPi / 4)), kPi / 2, kEpsilon); + // -45.0 and 135.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(-kPi / 4, kPi / 2)), 3 * kPi / 4, kEpsilon); + // 135.0 and -45.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(kPi / 2, -kPi / 4)), 3 * kPi / 4, kEpsilon); +} + +TYPED_TEST(FloatingPointTest, AbsSmallestAngleDiffGivenAnglesOutsidePiAndMinusPiRange) { + using T = TypeParam; + + static constexpr T kEpsilon = epsilon_v; + static constexpr T kPi = std::numbers::pi_v; + + // -450.0 to 360.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(-5 * kPi / 2, 2 * kPi)), kPi / 2, kEpsilon); + // 360.0 to -450.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(2 * kPi, -5 * kPi / 2)), kPi / 2, kEpsilon); + // -450.0 to 270.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(-5 * kPi / 2, 3 * kPi / 2)), 0.0, kEpsilon); + // -450.0 to 270.0 degrees + EXPECT_NEAR((absSmallestAngleDiff(3 * kPi / 2, -5 * kPi / 2)), 0.0, kEpsilon); +} + +} // namespace robocin diff --git a/common/cpp/robocin/utility/iproto_convertible.h b/common/cpp/robocin/utility/iproto_convertible.h index 7e6cba76..43f5cd5c 100644 --- a/common/cpp/robocin/utility/iproto_convertible.h +++ b/common/cpp/robocin/utility/iproto_convertible.h @@ -14,11 +14,11 @@ class IProtoConvertible { IProtoConvertible& operator=(IProtoConvertible&&) noexcept = default; virtual ~IProtoConvertible() = default; - + virtual T toProto() const = 0; virtual void fromProto(T proto) = 0; }; } // namespace robocin -#endif // ROBOCIN_UTILITY_IPROTO_CONVERTIBLE \ No newline at end of file +#endif // ROBOCIN_UTILITY_IPROTO_CONVERTIBLE diff --git a/communication-ms/communication-jaiminho/communication/communication_main.cpp b/communication-ms/communication-jaiminho/communication/communication_main.cpp index 25f27574..db6de797 100644 --- a/communication-ms/communication-jaiminho/communication/communication_main.cpp +++ b/communication-ms/communication-jaiminho/communication/communication_main.cpp @@ -14,40 +14,36 @@ namespace parameters = ::robocin::parameters; namespace service_discovery = robocin::service_discovery; +using communication::CommunicationProcessor; using communication::ConsumerController; -using communication::RobotCommandMapper; +using communication::ICommunicationProcessor; using communication::IController; using communication::IMessageReceiver; using communication::IMessageSender; using communication::IPayloadMapper; -using communication::ICommunicationProcessor; using communication::MessageReceiver; using communication::MessageSender; using communication::Payload; using communication::PayloadMapper; using communication::ProducerController; -using communication::CommunicationProcessor; +using communication::RobotCommandMapper; using ::robocin::ConditionVariableConcurrentQueue; using ::robocin::IConcurrentQueue; +using ::robocin::IUdpMulticastSocketSender; using ::robocin::IZmqPoller; using ::robocin::IZmqPublisherSocket; using ::robocin::IZmqSubscriberSocket; -using ::robocin::IUdpMulticastSocketSender; using ::robocin::object_ptr; +using ::robocin::UdpMulticastSocketSender; using ::robocin::ZmqPoller; using ::robocin::ZmqPublisherSocket; using ::robocin::ZmqSubscriberSocket; -using ::robocin::UdpMulticastSocketSender; ////////////////////////////////////////////////////////////////////////////////////////////////// std::unique_ptr makeMessageReceiver() { - static constexpr std::array kNavigationTopics = { - service_discovery::kNavigationOutputTopic - }; - static constexpr std::array kGatewayTopics = { - service_discovery::kGameControllerRefereeTopic - }; + static constexpr std::array kNavigationTopics = {service_discovery::kNavigationOutputTopic}; + static constexpr std::array kGatewayTopics = {service_discovery::kGameControllerRefereeTopic}; std::unique_ptr gateway_socket = std::make_unique(); gateway_socket->connect(service_discovery::kGatewayAddress, kGatewayTopics); @@ -74,16 +70,17 @@ std::unique_ptr makeProducer(object_ptr> ////////////////////////////////////////////////////////////////////////////////////////////////// std::unique_ptr makeCommunicationProcessor() { - return std::make_unique( - std::make_unique(), - std::make_unique()); + return std::make_unique(std::make_unique(), + std::make_unique()); } std::unique_ptr makeMessageSender() { - std::unique_ptr communication_socket = std::make_unique(); + std::unique_ptr communication_socket + = std::make_unique(); communication_socket->bind(service_discovery::kCommunicationAddress); - std::unique_ptr robot_socket = std::make_unique(); + std::unique_ptr robot_socket + = std::make_unique(); robot_socket->connect(service_discovery::kRobotAddress, service_discovery::kRobotPort); return std::make_unique(std::move(communication_socket), std::move(robot_socket)); diff --git a/communication-ms/communication-jaiminho/communication/controller/consumer_controller.cpp b/communication-ms/communication-jaiminho/communication/controller/consumer_controller.cpp index 28c8e1b8..61e7a0c4 100644 --- a/communication-ms/communication-jaiminho/communication/controller/consumer_controller.cpp +++ b/communication-ms/communication-jaiminho/communication/controller/consumer_controller.cpp @@ -22,9 +22,10 @@ using ::protocols::communication::RobotInfo; } // namespace -ConsumerController::ConsumerController(object_ptr> messages, - std::unique_ptr communication_processor, - std::unique_ptr message_sender) : +ConsumerController::ConsumerController( + object_ptr> messages, + std::unique_ptr communication_processor, + std::unique_ptr message_sender) : messages_{messages}, communication_processor_{std::move(communication_processor)}, message_sender_{std::move(message_sender)} {} @@ -48,8 +49,8 @@ void ConsumerController::exec(std::span payloads) { if (std::optional robot_command = communication_processor_->process(payloads); robot_command != std::nullopt) { - if(robot_command->has_command()) { - // ilog("command: {} sent.", robot_command->command().DebugString()); + if (robot_command->has_command()) { + // ilog("command: {} sent.", robot_command->command().DebugString()); message_sender_->send(*robot_command); } } diff --git a/communication-ms/communication-jaiminho/communication/messaging/receiver/message_receiver.h b/communication-ms/communication-jaiminho/communication/messaging/receiver/message_receiver.h index 6cacc496..42eb3909 100644 --- a/communication-ms/communication-jaiminho/communication/messaging/receiver/message_receiver.h +++ b/communication-ms/communication-jaiminho/communication/messaging/receiver/message_receiver.h @@ -42,4 +42,4 @@ class MessageReceiver : public IMessageReceiver { } // namespace communication -#endif // COMMUNICATION_MESSAGING_RECEIVER_MESSAGE_RECEIVER_H \ No newline at end of file +#endif // COMMUNICATION_MESSAGING_RECEIVER_MESSAGE_RECEIVER_H diff --git a/communication-ms/communication-jaiminho/communication/messaging/receiver/payload.h b/communication-ms/communication-jaiminho/communication/messaging/receiver/payload.h index 95067403..e78be5bb 100644 --- a/communication-ms/communication-jaiminho/communication/messaging/receiver/payload.h +++ b/communication-ms/communication-jaiminho/communication/messaging/receiver/payload.h @@ -15,7 +15,8 @@ class Payload { std::vector<::protocols::navigation::Navigation> navigation); [[nodiscard]] std::span getNavigation() const; - [[nodiscard]] std::span getReferee() const; + [[nodiscard]] std::span + getReferee() const; [[nodiscard]] bool empty() const; diff --git a/communication-ms/communication-jaiminho/communication/messaging/receiver/payload_mapper.cpp b/communication-ms/communication-jaiminho/communication/messaging/receiver/payload_mapper.cpp index 785767ed..1dccc201 100644 --- a/communication-ms/communication-jaiminho/communication/messaging/receiver/payload_mapper.cpp +++ b/communication-ms/communication-jaiminho/communication/messaging/receiver/payload_mapper.cpp @@ -11,9 +11,9 @@ namespace { namespace service_discovery = robocin::service_discovery; +using ::robocin::ilog; using ::robocin::wlog; using ::robocin::ZmqDatagram; -using ::robocin::ilog; namespace rc { diff --git a/communication-ms/communication-jaiminho/communication/messaging/sender/message_sender.h b/communication-ms/communication-jaiminho/communication/messaging/sender/message_sender.h index b70b547f..20a21a0c 100644 --- a/communication-ms/communication-jaiminho/communication/messaging/sender/message_sender.h +++ b/communication-ms/communication-jaiminho/communication/messaging/sender/message_sender.h @@ -1,8 +1,8 @@ #ifndef COMMUNICATION_MESSAGING_SENDER_MESSAGE_SENDER_H #define COMMUNICATION_MESSAGING_SENDER_MESSAGE_SENDER_H -#include "robocin/network/zmq_publisher_socket.h" #include "robocin/network/udp_multicast_socket_sender.h" +#include "robocin/network/zmq_publisher_socket.h" #include diff --git a/communication-ms/communication-jaiminho/communication/processing/communication_processor.cpp b/communication-ms/communication-jaiminho/communication/processing/communication_processor.cpp index da7e859f..951264a9 100644 --- a/communication-ms/communication-jaiminho/communication/processing/communication_processor.cpp +++ b/communication-ms/communication-jaiminho/communication/processing/communication_processor.cpp @@ -5,7 +5,6 @@ #include #include #include - #include namespace communication { @@ -28,42 +27,41 @@ using ::protocols::third_party::game_controller::Referee; } // namespace tp std::vector navigationFromPayloads(std::span payloads) { - return payloads | std::views::transform(&Payload::getNavigation) | std::views::join - | std::ranges::to(); + return payloads | std::views::transform(&Payload::getNavigation) | std::views::join + | std::ranges::to(); } std::vector refereeFromPayloads(std::span payloads) { - return payloads | std::views::transform(&Payload::getReferee) | std::views::join - | std::ranges::to(); - + return payloads | std::views::transform(&Payload::getReferee) | std::views::join + | std::ranges::to(); } } // namespace CommunicationProcessor::CommunicationProcessor( std::unique_ptr parameters_handler_engine, - std::unique_ptr robot_command_mapper): + std::unique_ptr robot_command_mapper) : parameters_handler_engine_{std::move(parameters_handler_engine)}, robot_command_mapper_{std::move(robot_command_mapper)} {} std::optional CommunicationProcessor::process(std::span payloads) { - rc::RobotInfo communication_output; + rc::RobotInfo communication_output; - if(std::vector referees = refereeFromPayloads(payloads); !referees.empty()) { - last_game_controller_referee_ = std::move(referees.back()); - } + if (std::vector referees = refereeFromPayloads(payloads); !referees.empty()) { + last_game_controller_referee_ = std::move(referees.back()); + } - if(!last_game_controller_referee_) { - return std::nullopt; - } + if (!last_game_controller_referee_) { + return std::nullopt; + } - std::vector navigation = navigationFromPayloads(payloads); - if(navigation.empty()) { - return std::nullopt; - } - rc::Navigation last_navigation_ = navigation.back(); + std::vector navigation = navigationFromPayloads(payloads); + if (navigation.empty()) { + return std::nullopt; + } + rc::Navigation last_navigation_ = navigation.back(); - return communication_output; + return communication_output; } } // namespace communication diff --git a/communication-ms/communication-jaiminho/communication/processing/communication_processor.h b/communication-ms/communication-jaiminho/communication/processing/communication_processor.h index 60e886d0..bab389d2 100644 --- a/communication-ms/communication-jaiminho/communication/processing/communication_processor.h +++ b/communication-ms/communication-jaiminho/communication/processing/communication_processor.h @@ -20,8 +20,8 @@ class ICommunicationProcessor { virtual ~ICommunicationProcessor() = default; - virtual std::optional<::protocols::communication::RobotInfo> process(std::span payloads) - = 0; + virtual std::optional<::protocols::communication::RobotInfo> + process(std::span payloads) = 0; }; class CommunicationProcessor : public ICommunicationProcessor { diff --git a/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.cpp b/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.cpp index cdbd3971..d457be52 100644 --- a/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.cpp +++ b/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.cpp @@ -1,13 +1,13 @@ #include "communication/processing/robot_command/robot_command_mapper.h" #include -#include -#include -#include #include -#include -#include #include +#include +#include +#include +#include +#include #include #include #include @@ -19,16 +19,16 @@ using ::google::protobuf::util::TimeUtil; using ::robocin::object_ptr; namespace rc { - -using ::protocols::navigation::Navigation; -using ::protocols::communication::RobotInfo; -using ::protocols::communication::Command; -using ::protocols::communication::Flags; -using ::protocols::common::RobotId; + using ::protocols::common::MessageType; -using ::protocols::common::RobotVelocity; -using ::protocols::common::RobotKick; using ::protocols::common::RobotDribbler; +using ::protocols::common::RobotId; +using ::protocols::common::RobotKick; +using ::protocols::common::RobotVelocity; +using ::protocols::communication::Command; +using ::protocols::communication::Flags; +using ::protocols::communication::RobotInfo; +using ::protocols::navigation::Navigation; } // namespace rc @@ -40,45 +40,49 @@ using ::protocols::third_party::game_controller::Referee; } // namespace -RobotCommandMapper::RobotCommandMapper(){} - -rc::RobotInfo RobotCommandMapper::fromNavigationAndReferee(const tp::Referee& referee, const rc::Navigation& navigation) { - rc::RobotInfo result; - rc::Command* command = result.mutable_command(); - rc::RobotId* robot_id = command->mutable_robot_id(); - rc::RobotVelocity robot_velocity; - rc::RobotKick::KickCommand kick_command; - rc::RobotDribbler::DribblerCommand dribbler_command; - rc::Flags robot_flag; - - command->set_msg_type(rc::MessageType::SSL_SPEED); - - robot_id->CopyFrom(navigation.output(navigation.output_size() - 1).robot_id()); - - // *command->mutable_ref_command()->CopyFrom(referee.ref_command()); - - robot_velocity.mutable_velocity()->set_x(navigation.output(navigation.output_size() - 1).forward_velocity()); - robot_velocity.mutable_velocity()->set_y(navigation.output(navigation.output_size() - 1).left_velocity()); - robot_velocity.set_angular_velocity(navigation.output(navigation.output_size() - 1).angular_velocity()); - *command->mutable_robot_velocity() = robot_velocity; - - kick_command.set_kick_strength(5.0); - kick_command.set_is_front(false); - kick_command.set_is_chip(false); - kick_command.set_charge_capacitor(false); - kick_command.set_is_bypass_ir(false); - *command->mutable_kick_command() = kick_command; - - dribbler_command.set_dribbler_speed(0); - dribbler_command.set_is_active(false); - *command->mutable_dribbler_command() = dribbler_command; - - robot_flag.set_robot_locked(false); - robot_flag.set_critical_move(false); - robot_flag.set_global_speed(false); - *command->mutable_robot_flags() = robot_flag; - - return result; +RobotCommandMapper::RobotCommandMapper() {} + +rc::RobotInfo RobotCommandMapper::fromNavigationAndReferee(const tp::Referee& referee, + const rc::Navigation& navigation) { + rc::RobotInfo result; + rc::Command* command = result.mutable_command(); + rc::RobotId* robot_id = command->mutable_robot_id(); + rc::RobotVelocity robot_velocity; + rc::RobotKick::KickCommand kick_command; + rc::RobotDribbler::DribblerCommand dribbler_command; + rc::Flags robot_flag; + + command->set_msg_type(rc::MessageType::SSL_SPEED); + + robot_id->CopyFrom(navigation.output(navigation.output_size() - 1).robot_id()); + + // *command->mutable_ref_command()->CopyFrom(referee.ref_command()); + + robot_velocity.mutable_velocity()->set_x( + navigation.output(navigation.output_size() - 1).forward_velocity()); + robot_velocity.mutable_velocity()->set_y( + navigation.output(navigation.output_size() - 1).left_velocity()); + robot_velocity.set_angular_velocity( + navigation.output(navigation.output_size() - 1).angular_velocity()); + *command->mutable_robot_velocity() = robot_velocity; + + kick_command.set_kick_strength(5.0); + kick_command.set_is_front(false); + kick_command.set_is_chip(false); + kick_command.set_charge_capacitor(false); + kick_command.set_is_bypass_ir(false); + *command->mutable_kick_command() = kick_command; + + dribbler_command.set_dribbler_speed(0); + dribbler_command.set_is_active(false); + *command->mutable_dribbler_command() = dribbler_command; + + robot_flag.set_robot_locked(false); + robot_flag.set_critical_move(false); + robot_flag.set_global_speed(false); + *command->mutable_robot_flags() = robot_flag; + + return result; } } // namespace communication diff --git a/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.h b/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.h index 52785aa3..66f9d14a 100644 --- a/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.h +++ b/communication-ms/communication-jaiminho/communication/processing/robot_command/robot_command_mapper.h @@ -1,9 +1,9 @@ #ifndef COMMUNICATION_PROCESSING_ROBOT_COMMAND_ROBOT_COMMAND_MAPPER_H #define COMMUNICATION_PROCESSING_ROBOT_COMMAND_ROBOT_COMMAND_MAPPER_H -#include -#include #include +#include +#include #include namespace communication { @@ -19,17 +19,19 @@ class IRobotCommandMapper { virtual ~IRobotCommandMapper() = default; - virtual ::protocols::communication::RobotInfo fromNavigationAndReferee( - const ::protocols::third_party::game_controller::Referee& referee, - const ::protocols::navigation::Navigation& navigation ) = 0; + virtual ::protocols::communication::RobotInfo + fromNavigationAndReferee(const ::protocols::third_party::game_controller::Referee& referee, + const ::protocols::navigation::Navigation& navigation) + = 0; }; class RobotCommandMapper : public IRobotCommandMapper { public: RobotCommandMapper(); - ::protocols::communication::RobotInfo fromNavigationAndReferee( - const ::protocols::third_party::game_controller::Referee& referee, const ::protocols::navigation::Navigation& navigation) override; + ::protocols::communication::RobotInfo + fromNavigationAndReferee(const ::protocols::third_party::game_controller::Referee& referee, + const ::protocols::navigation::Navigation& navigation) override; }; } // namespace communication diff --git a/decision-ms/decision-guardiola/decision/decision_main.cpp b/decision-ms/decision-guardiola/decision/decision_main.cpp index b96d9e73..4950c1ea 100644 --- a/decision-ms/decision-guardiola/decision/decision_main.cpp +++ b/decision-ms/decision-guardiola/decision/decision_main.cpp @@ -1,5 +1,5 @@ -#include "decision/controller/producer_controller.h" #include "decision/controller/consumer_controller.h" +#include "decision/controller/producer_controller.h" #include #include @@ -16,19 +16,20 @@ namespace detection_util = ::robocin::detection_util; namespace service_discovery = robocin::service_discovery; using decision::ConsumerController; +using decision::DecisionProcessor; using decision::IController; +using decision::IDecisionProcessor; using decision::IMessageReceiver; using decision::IMessageSender; using decision::IPayloadMapper; -using decision::IDecisionProcessor; using decision::MessageReceiver; using decision::MessageSender; using decision::Payload; using decision::PayloadMapper; using decision::ProducerController; -using decision::DecisionProcessor; using ::robocin::ConditionVariableConcurrentQueue; using ::robocin::IConcurrentQueue; +using ::robocin::ilog; using ::robocin::IZmqPoller; using ::robocin::IZmqPublisherSocket; using ::robocin::IZmqSubscriberSocket; @@ -36,7 +37,6 @@ using ::robocin::object_ptr; using ::robocin::ZmqPoller; using ::robocin::ZmqPublisherSocket; using ::robocin::ZmqSubscriberSocket; -using ::robocin::ilog; std::unique_ptr makeMessageReceiver() { static constexpr std::array kPerceptionTopics = { @@ -71,8 +71,7 @@ std::unique_ptr makeProducer(object_ptr> ////////////////////////////////////////////////////////////////////////////////////////////////// std::unique_ptr makeDecisionProcessor() { - return std::make_unique( - std::make_unique()); + return std::make_unique(std::make_unique()); } std::unique_ptr makeMessageSender() { diff --git a/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp b/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp index 0fd7197b..f23f2c72 100644 --- a/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp +++ b/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp @@ -2,8 +2,8 @@ #include "decision/messaging/receiver/payload.h" -#include #include +#include #include #include #include @@ -18,14 +18,14 @@ using ::robocin::ilog; namespace rc { -using ::protocols::decision::Decision; -using ::protocols::perception::Detection; -using ::protocols::referee::GameStatus; -using ::protocols::decision::Behavior; using ::protocols::common::RobotId; -using ::protocols::decision::TacticalPlan; +using ::protocols::decision::Behavior; +using ::protocols::decision::Decision; using ::protocols::decision::DefensivePlan; using ::protocols::decision::OffensivePlan; +using ::protocols::decision::TacticalPlan; +using ::protocols::perception::Detection; +using ::protocols::referee::GameStatus; } // namespace rc @@ -53,7 +53,7 @@ std::optional DecisionProcessor::process(std::span last_game_status_ = gameStatus.back(); } - if(!last_game_status_) { + if (!last_game_status_) { return std::nullopt; } @@ -67,16 +67,17 @@ std::optional DecisionProcessor::process(std::span /////////////////////////////////////////////////////////////////////////////////// - // TODO: Implement the logic to generate the decision based on the last detection and the last game status. - for(const auto& robot : last_detection.robots()) { + // TODO: Implement the logic to generate the decision based on the last detection and the last + // game status. + for (const auto& robot : last_detection.robots()) { rc::Behavior* behavior = decision_output.add_behavior(); - behavior->set_id(0); + behavior->set_id(0); behavior->mutable_robot_id()->CopyFrom(robot.robot_id()); } rc::TacticalPlan* tplan = decision_output.mutable_plan(); - + rc::OffensivePlan* ofPlan = tplan->mutable_offensive(); rc::DefensivePlan* dfPlan = tplan->mutable_defensive(); diff --git a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp index 6d990227..dd23808e 100644 --- a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp +++ b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp @@ -26,8 +26,8 @@ using ::protocols::navigation::Navigation; } // namespace ConsumerController::ConsumerController(object_ptr> messages, - std::unique_ptr navigation_processor, - std::unique_ptr message_sender) : + std::unique_ptr navigation_processor, + std::unique_ptr message_sender) : messages_{messages}, navigation_processor_{std::move(navigation_processor)}, message_sender_{std::move(message_sender)} {} @@ -49,7 +49,7 @@ void ConsumerController::exec(std::span payloads) { } if (std::optional navigation_msg = navigation_processor_->process(payloads); - navigation_msg != std::nullopt) { + navigation_msg != std::nullopt) { message_sender_->send(*navigation_msg); } } diff --git a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.h b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.h index c15c9248..5159692a 100644 --- a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.h +++ b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.h @@ -15,10 +15,9 @@ namespace navigation { class ConsumerController : public IController { public: - ConsumerController( - ::robocin::object_ptr<::robocin::IConcurrentQueue> messages, - std::unique_ptr navigation_processor, - std::unique_ptr message_sender); + ConsumerController(::robocin::object_ptr<::robocin::IConcurrentQueue> messages, + std::unique_ptr navigation_processor, + std::unique_ptr message_sender); void run() override; diff --git a/navigation-ms/navigation-luffy/navigation/controller/producer_controller.h b/navigation-ms/navigation-luffy/navigation/controller/producer_controller.h index 253644ca..949a1356 100644 --- a/navigation-ms/navigation-luffy/navigation/controller/producer_controller.h +++ b/navigation-ms/navigation-luffy/navigation/controller/producer_controller.h @@ -24,4 +24,3 @@ class ProducerController : public IController { } // namespace navigation #endif // NAVIGATION_CONTROLLER_PRODUCER_CONTROLLER_H - diff --git a/navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.cpp b/navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.cpp index ccf2b9d7..c1277faa 100644 --- a/navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.cpp +++ b/navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.cpp @@ -43,16 +43,15 @@ Payload MessageReceiver::receive() { datagrams.emplace_back(std::move(behavior_zmq_datagram)); } - while(true) { + while (true) { ZmqDatagram perception_zmq_datagram = zmq_poller_->receive(*perception_socket_); if (perception_zmq_datagram.empty()) { break; } - + datagrams.emplace_back(std::move(perception_zmq_datagram)); } - if (datagrams.empty()) { // wlog("no datagram received after {} ms.", pRefereePollerTimeoutMs()); } diff --git a/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.cpp b/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.cpp index 2076e533..3d77fae0 100644 --- a/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.cpp +++ b/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.cpp @@ -12,15 +12,13 @@ using ::protocols::perception::Detection; } // namespace -Payload::Payload(std::vector behaviors, - std::vector detections) : +Payload::Payload(std::vector behaviors, std::vector detections) : behaviors_{std::move(behaviors)}, detections_{std::move(detections)} {} std::span Payload::getBehaviors() const { return behaviors_; } std::span Payload::getDetections() const { return detections_; } - bool Payload::empty() const { return behaviors_.empty() and detections_.empty(); } } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload_mapper.cpp b/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload_mapper.cpp index 9032f9c5..06c7c325 100644 --- a/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload_mapper.cpp +++ b/navigation-ms/navigation-luffy/navigation/messaging/receiver/payload_mapper.cpp @@ -33,15 +33,13 @@ Payload PayloadMapper::fromZmqDatagrams(std::span messages) c behavior.ParseFromString(std::string{zmq_datagram.message()}); // ilog("Received from behavior: {}", behavior.DebugString()); behaviors.emplace_back(std::move(behavior)); - - } + } if (zmq_datagram.topic() == service_discovery::kPerceptionDetectionTopic) { rc::Detection detection; detection.ParseFromString(std::string{zmq_datagram.message()}); // ilog("Received from perception: {}", detection.DebugString()); detections.emplace_back(std::move(detection)); - } - else { + } else { // wlog("zmq_datagram with topic '{}' not processed.", zmq_datagram.topic()); } } diff --git a/navigation-ms/navigation-luffy/navigation/messaging/sender/message_sender.h b/navigation-ms/navigation-luffy/navigation/messaging/sender/message_sender.h index 7bcc2aee..c727e409 100644 --- a/navigation-ms/navigation-luffy/navigation/messaging/sender/message_sender.h +++ b/navigation-ms/navigation-luffy/navigation/messaging/sender/message_sender.h @@ -19,7 +19,6 @@ class IMessageSender { virtual ~IMessageSender() = default; virtual void send(const ::protocols::navigation::Navigation& navigation) = 0; - }; class MessageSender : public IMessageSender { @@ -34,4 +33,4 @@ class MessageSender : public IMessageSender { } // namespace navigation -#endif // NAVIGATION_MESSAGING_SENDER_MESSAGE_SENDER_H \ No newline at end of file +#endif // NAVIGATION_MESSAGING_SENDER_MESSAGE_SENDER_H diff --git a/navigation-ms/navigation-luffy/navigation/navigation_main.cpp b/navigation-ms/navigation-luffy/navigation/navigation_main.cpp index 0ba88bc4..667b1476 100644 --- a/navigation-ms/navigation-luffy/navigation/navigation_main.cpp +++ b/navigation-ms/navigation-luffy/navigation/navigation_main.cpp @@ -1,5 +1,5 @@ -#include "navigation/controller/producer_controller.h" #include "navigation/controller/consumer_controller.h" +#include "navigation/controller/producer_controller.h" #include #include @@ -21,13 +21,14 @@ using navigation::INavigationProcessor; using navigation::IPayloadMapper; using navigation::MessageReceiver; using navigation::MessageSender; +using navigation::MotionParser; using navigation::NavigationProcessor; using navigation::Payload; using navigation::PayloadMapper; using navigation::ProducerController; -using navigation::MotionParser; using ::robocin::ConditionVariableConcurrentQueue; using ::robocin::IConcurrentQueue; +using ::robocin::ilog; using ::robocin::IZmqPoller; using ::robocin::IZmqPublisherSocket; using ::robocin::IZmqSubscriberSocket; @@ -35,7 +36,6 @@ using ::robocin::object_ptr; using ::robocin::ZmqPoller; using ::robocin::ZmqPublisherSocket; using ::robocin::ZmqSubscriberSocket; -using ::robocin::ilog; ////////////////////////////////////////////////////////////////////////////////////////////////// @@ -73,7 +73,8 @@ std::unique_ptr makeProducer(object_ptr> ////////////////////////////////////////////////////////////////////////////////////////////////// std::unique_ptr makeNavigationProcessor() { - return std::make_unique(std::make_unique()); // TODO: passar os parâmetros aqui + return std::make_unique( + std::make_unique()); // TODO: passar os parâmetros aqui } std::unique_ptr makeMessageSender() { diff --git a/navigation-ms/navigation-luffy/navigation/parameters/parameters.cpp b/navigation-ms/navigation-luffy/navigation/parameters/parameters.cpp index 2766c7c0..632c949a 100644 --- a/navigation-ms/navigation-luffy/navigation/parameters/parameters.cpp +++ b/navigation-ms/navigation-luffy/navigation/parameters/parameters.cpp @@ -1 +1 @@ -#include "navigation/parameters/parameters.h" \ No newline at end of file +#include "navigation/parameters/parameters.h" diff --git a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp index 534f146f..4c94f039 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.cpp @@ -6,62 +6,59 @@ namespace navigation { MotionParser::MotionParser() {} -RobotMove MotionParser::fromGoToPoint( - const ::protocols::behavior::GoToPoint& go_to_point, - const ::protocols::perception::Robot& robot) { - - robocin::Point2D robot_position = robocin::Point2D(robot.position().x(), robot.position().y()); - robocin::Point2D target_position = robocin::Point2D(go_to_point.target().x(), go_to_point.target().y()); - - double distanceToGoal = robot_position.distanceTo(target_position); - double max_velocity = 2.2; - - auto smallestAngleDiff = [](double lhs, double rhs) { - double angle = fmod(rhs - lhs, 2 * PI); - if (angle >= PI) { - return angle - 2 * PI; - } else if (angle < -PI) { - return angle + 2 * PI; - } - return angle; - }; - - auto fromPolar = [](auto length, auto angle) { - return std::make_pair(std::cos(angle) * length, std::sin(angle) * length); - }; - - if (distanceToGoal > 50) { - auto theta = (target_position - robot_position).angle(); - - auto dTheta = smallestAngleDiff(robot.angle(), go_to_point.target_angle()); - auto v_prop = std::abs(smallestAngleDiff(PI - 0.1, dTheta)) * max_velocity / (PI - 0.1); - - return RobotMove(fromPolar(v_prop, theta), 5 * dTheta); - - } else { - // anglePID() - auto dTheta = smallestAngleDiff(robot.angle(), go_to_point.target_angle()); - - return RobotMove(5 * dTheta); +RobotMove MotionParser::fromGoToPoint(const ::protocols::behavior::GoToPoint& go_to_point, + const ::protocols::perception::Robot& robot) { + + robocin::Point2D robot_position = robocin::Point2D(robot.position().x(), robot.position().y()); + robocin::Point2D target_position + = robocin::Point2D(go_to_point.target().x(), go_to_point.target().y()); + + double distanceToGoal = robot_position.distanceTo(target_position); + double max_velocity = 2.2; + + auto smallestAngleDiff = [](double lhs, double rhs) { + double angle = fmod(rhs - lhs, 2 * PI); + if (angle >= PI) { + return angle - 2 * PI; + } else if (angle < -PI) { + return angle + 2 * PI; } + return angle; + }; + + auto fromPolar = [](auto length, auto angle) { + return std::make_pair(std::cos(angle) * length, std::sin(angle) * length); + }; + + if (distanceToGoal > 50) { + auto theta = (target_position - robot_position).angle(); + + auto dTheta = smallestAngleDiff(robot.angle(), go_to_point.target_angle()); + auto v_prop = std::abs(smallestAngleDiff(PI - 0.1, dTheta)) * max_velocity / (PI - 0.1); + + return RobotMove(fromPolar(v_prop, theta), 5 * dTheta); + + } else { + // anglePID() + auto dTheta = smallestAngleDiff(robot.angle(), go_to_point.target_angle()); + + return RobotMove(5 * dTheta); + } } -RobotMove MotionParser::fromRotateInPoint( - const ::protocols::behavior::RotateInPoint& rotate_in_point, - const ::protocols::perception::Robot& robot) { - - // PROCESSAMENTO DO ROTATEINPOINT - return RobotMove{}; +RobotMove +MotionParser::fromRotateInPoint(const ::protocols::behavior::RotateInPoint& rotate_in_point, + const ::protocols::perception::Robot& robot) { + // PROCESSAMENTO DO ROTATEINPOINT + return RobotMove{}; } -RobotMove MotionParser::fromRotateOnSelf( - const ::protocols::behavior::RotateOnSelf& rotate_on_self, - const ::protocols::perception::Robot& robot) { - - // PROCESSAMENTO DO ROTATEINPOINT - return RobotMove{}; +RobotMove MotionParser::fromRotateOnSelf(const ::protocols::behavior::RotateOnSelf& rotate_on_self, + const ::protocols::perception::Robot& robot) { + // PROCESSAMENTO DO ROTATEINPOINT + return RobotMove{}; } } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h index 169122c0..73f08778 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h +++ b/navigation-ms/navigation-luffy/navigation/processing/motion_parser/motion_parser.h @@ -2,10 +2,8 @@ #define NAVIGATION_PROCESSING_MOTION_PARSER_H #include - #include #include - #include #define PI 3.14159265358979323846 @@ -23,34 +21,31 @@ class IMotionParser { virtual ~IMotionParser() = default; - virtual RobotMove fromGoToPoint( - const ::protocols::behavior::GoToPoint& go_to_point, - const ::protocols::perception::Robot& robot) = 0; + virtual RobotMove fromGoToPoint(const ::protocols::behavior::GoToPoint& go_to_point, + const ::protocols::perception::Robot& robot) + = 0; - virtual RobotMove fromRotateInPoint( - const ::protocols::behavior::RotateInPoint& rotate_in_point, - const ::protocols::perception::Robot& robot) = 0; + virtual RobotMove fromRotateInPoint(const ::protocols::behavior::RotateInPoint& rotate_in_point, + const ::protocols::perception::Robot& robot) + = 0; - virtual RobotMove fromRotateOnSelf( - const ::protocols::behavior::RotateOnSelf& rotate_on_self, - const ::protocols::perception::Robot& robot) = 0; + virtual RobotMove fromRotateOnSelf(const ::protocols::behavior::RotateOnSelf& rotate_on_self, + const ::protocols::perception::Robot& robot) + = 0; }; class MotionParser : public IMotionParser { public: MotionParser(); - RobotMove fromGoToPoint( - const ::protocols::behavior::GoToPoint& go_to_point, - const ::protocols::perception::Robot& robot) override; + RobotMove fromGoToPoint(const ::protocols::behavior::GoToPoint& go_to_point, + const ::protocols::perception::Robot& robot) override; - RobotMove fromRotateInPoint( - const ::protocols::behavior::RotateInPoint& rotate_in_point, - const ::protocols::perception::Robot& robot) override; + RobotMove fromRotateInPoint(const ::protocols::behavior::RotateInPoint& rotate_in_point, + const ::protocols::perception::Robot& robot) override; - RobotMove fromRotateOnSelf( - const ::protocols::behavior::RotateOnSelf& rotate_on_self, - const ::protocols::perception::Robot& robot) override; + RobotMove fromRotateOnSelf(const ::protocols::behavior::RotateOnSelf& rotate_on_self, + const ::protocols::perception::Robot& robot) override; }; } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp index 55082a4c..df1f4bdb 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp @@ -2,14 +2,11 @@ #include "navigation/messaging/receiver/payload.h" +#include #include #include -#include - #include - #include - #include namespace navigation { @@ -18,19 +15,19 @@ namespace parameters = ::robocin::parameters; namespace rc { - using ::protocols::behavior::Planning; - using ::protocols::behavior::GoToPoint; - using ::protocols::behavior::unification::Motion; - using ::protocols::behavior::unification::Behavior; - - using ::protocols::navigation::Navigation; - using ::protocols::navigation::Output; +using ::protocols::behavior::GoToPoint; +using ::protocols::behavior::Planning; +using ::protocols::behavior::unification::Behavior; +using ::protocols::behavior::unification::Motion; - using ::protocols::perception::Detection; - using ::protocols::perception::Robot; +using ::protocols::navigation::Navigation; +using ::protocols::navigation::Output; - using ::protocols::common::RobotId; - using ::protocols::common::PeripheralActuation; +using ::protocols::perception::Detection; +using ::protocols::perception::Robot; + +using ::protocols::common::PeripheralActuation; +using ::protocols::common::RobotId; } // namespace rc @@ -44,24 +41,21 @@ std::vector behaviorFromPayloads(std::span payloads std::vector detectionFromPayloads(std::span payloads) { return payloads | std::views::transform(&Payload::getDetections) | std::views::join | std::ranges::to(); - } } // namespace -NavigationProcessor::NavigationProcessor( - std::unique_ptr motion_parser) : +NavigationProcessor::NavigationProcessor(std::unique_ptr motion_parser) : motion_parser_(std::move(motion_parser)) {} std::optional NavigationProcessor::process(std::span payloads) { rc::Navigation navigation_output; - if(std::vector behaviors = behaviorFromPayloads(payloads); - !behaviors.empty()) { + if (std::vector behaviors = behaviorFromPayloads(payloads); !behaviors.empty()) { last_behavior_ = behaviors.back(); } - - if(!last_behavior_){ + + if (!last_behavior_) { return std::nullopt; } @@ -71,9 +65,9 @@ std::optional NavigationProcessor::process(std::spanoutput()){ + for (const auto& behavior_ : last_behavior_->output()) { rc::Output output; rc::Robot ally; @@ -83,12 +77,12 @@ std::optional NavigationProcessor::process(std::spanCopyFrom(ally.robot_id()); - - if(behavior_.has_motion()) { + + if (behavior_.has_motion()) { RobotMove move; - if(behavior_.motion().has_go_to_point()) { + if (behavior_.motion().has_go_to_point()) { move = motion_parser_->fromGoToPoint(behavior_.motion().go_to_point(), ally); } else if (behavior_.motion().has_rotate_in_point()) { move = motion_parser_->fromRotateInPoint(behavior_.motion().rotate_in_point(), ally); @@ -102,11 +96,12 @@ std::optional NavigationProcessor::process(std::spanCopyFrom(behavior_.motion().peripheral_actuation()); + if (behavior_.motion().has_peripheral_actuation()) { + output.mutable_peripheral_actuation()->CopyFrom( + behavior_.motion().peripheral_actuation()); } - // TODO: Add other fields to output + // TODO: Add other fields to output } else if (behavior_.has_planning()) { // PROCESSAMENTO DO PLANNING diff --git a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h index 73e8f0d7..7d431de4 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h +++ b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h @@ -38,7 +38,6 @@ class NavigationProcessor : public INavigationProcessor { std::unique_ptr motion_parser_; std::optional<::protocols::behavior::unification::Behavior> last_behavior_; - }; } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp b/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp index ef3808f1..fc7f1c61 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.cpp @@ -1 +1 @@ -#include \ No newline at end of file +#include diff --git a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h b/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h index 7e3fb6c0..d993ae65 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h +++ b/navigation-ms/navigation-luffy/navigation/processing/utils/robot_move.h @@ -9,15 +9,16 @@ class RobotMove { ::robocin::Point2Df velocity_; double angularVelocity_; -public: - RobotMove(::robocin::Point2Df velocity, double angularVelocity) - : velocity_(velocity), angularVelocity_(angularVelocity) {} + public: + RobotMove(::robocin::Point2Df velocity, double angularVelocity) : + velocity_(velocity), + angularVelocity_(angularVelocity) {} - RobotMove(double angularVelocity) - : velocity_(::robocin::Point2Df(0.0, 0.0)), angularVelocity_(angularVelocity) {} + RobotMove(double angularVelocity) : + velocity_(::robocin::Point2Df(0.0, 0.0)), + angularVelocity_(angularVelocity) {} - RobotMove() - : velocity_(::robocin::Point2Df(0.0, 0.0)), angularVelocity_(0.0) {} + RobotMove() : velocity_(::robocin::Point2Df(0.0, 0.0)), angularVelocity_(0.0) {} ::robocin::Point2Df velocity() const { return velocity_; } double angularVelocity() const { return angularVelocity_; } @@ -25,4 +26,4 @@ class RobotMove { } // namespace navigation -#endif // NAVIGATION_PROCESSING_ROBOT_MOVE_H \ No newline at end of file +#endif // NAVIGATION_PROCESSING_ROBOT_MOVE_H From 4a90299f5eacbba1f7f8262d7a558539f7b8f315 Mon Sep 17 00:00:00 2001 From: Marcela <96249442+maa134340@users.noreply.github.com> Date: Mon, 28 Oct 2024 18:25:27 -0300 Subject: [PATCH 2/7] Add Geometry.Math library (#41) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Adapts the Math library from Unification to Core. This library is used in Geometry.h, which provides several mathematical functions used throughout the code. --------- Co-authored-by: Matheus Paixão Gumercindo dos Santos <61971668+MatheusPaixaoG@users.noreply.github.com> --- common/cpp/robocin/geometry/CMakeLists.txt | 2 + common/cpp/robocin/geometry/mathematics.cpp | 1 + common/cpp/robocin/geometry/mathematics.h | 46 +++++++++++++++++++++ 3 files changed, 49 insertions(+) create mode 100644 common/cpp/robocin/geometry/mathematics.cpp create mode 100644 common/cpp/robocin/geometry/mathematics.h diff --git a/common/cpp/robocin/geometry/CMakeLists.txt b/common/cpp/robocin/geometry/CMakeLists.txt index 2a9a93e7..a9b0bfd9 100644 --- a/common/cpp/robocin/geometry/CMakeLists.txt +++ b/common/cpp/robocin/geometry/CMakeLists.txt @@ -2,6 +2,8 @@ robocin_cpp_library( NAME geometry HDRS point2d.h point3d.h + mathematics.h SRCS point2d.cpp point3d.cpp + mathematics.cpp ) diff --git a/common/cpp/robocin/geometry/mathematics.cpp b/common/cpp/robocin/geometry/mathematics.cpp new file mode 100644 index 00000000..69402430 --- /dev/null +++ b/common/cpp/robocin/geometry/mathematics.cpp @@ -0,0 +1 @@ +#include "robocin/geometry/mathematics.h" \ No newline at end of file diff --git a/common/cpp/robocin/geometry/mathematics.h b/common/cpp/robocin/geometry/mathematics.h new file mode 100644 index 00000000..7f21cd74 --- /dev/null +++ b/common/cpp/robocin/geometry/mathematics.h @@ -0,0 +1,46 @@ +#ifndef ROBOCIN_MATHEMATICS_H +#define ROBOCIN_MATHEMATICS_H + +#include +#include +#include +#include + +namespace mathematics { +/*! + * @tparam T arithmetic type. + * @return Returns true if T is float and the absolute value of f is within 0.00001f of 0.0. + * @return Returns true if T is double and the absolute value of d is within 0.000000000001 of + * 0.0. + * @return Returns true if T is an integer type and equals to 0. + */ +template +[[maybe_unused]] constexpr bool isNull(const T& value) noexcept { + if constexpr (std::is_floating_point_v) { + return (fabsf(value - 1.0F) < 0.00001F); + } else { + return value == 0; + } +} + +/*! + * @tparam T arithmetic type. + * @param value value to map. + * @param l_lower lower value from l-range. + * @param l_higher higher value from l-range. + * @param r_lower lower value from r-range. + * @param r_higher higher value from r-range. + * @return Returns value mapped from l-range to r-range. + */ +template +[[maybe_unused]] constexpr T +map(const T& value, const T& l_lower, const T& l_higher, const T& r_lower, const T& r_higher) { + if (isNull(l_higher - l_lower)) { + throw std::runtime_error("'l_lower' equals to 'l_higher'."); + } + return (value - l_lower) * (r_higher - r_lower) / (l_higher - l_lower) + r_lower; +} + +} // namespace mathematics + +#endif // ROBOCIN_MATHEMATICS_H From d8d2c1a508eae36d1356b16007e77bf7a19f1a58 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Elisson=20Ara=C3=BAjo?= <44985807+ersaraujo@users.noreply.github.com> Date: Tue, 29 Oct 2024 17:07:17 -0300 Subject: [PATCH 3/7] Fix common formatting (#77) --- common/cpp/robocin/geometry/mathematics.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/cpp/robocin/geometry/mathematics.cpp b/common/cpp/robocin/geometry/mathematics.cpp index 69402430..43d3bcbf 100644 --- a/common/cpp/robocin/geometry/mathematics.cpp +++ b/common/cpp/robocin/geometry/mathematics.cpp @@ -1 +1 @@ -#include "robocin/geometry/mathematics.h" \ No newline at end of file +#include "robocin/geometry/mathematics.h" From b98f814c8d336d6c67aac8ad34423e31c4f78dad Mon Sep 17 00:00:00 2001 From: Felipe Nunes Date: Tue, 29 Oct 2024 19:06:41 -0300 Subject: [PATCH 4/7] [common] Add const reference to proto interface methods (#75) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Definições dos `.proto` utilizados no projeto são geralmente grandes e com muitos atributos, evitar a cópia destes objetos ao fazer passagem por valor no `IProtoConvertible::fromProto()` é uma boa prática que podemos implementar aqui. Este PR deve ser fechado antes dos PR das `messages/` de cada serviço, pois é uma dependência direta. Co-authored-by: ersaraujo --- common/cpp/robocin/utility/iproto_convertible.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/cpp/robocin/utility/iproto_convertible.h b/common/cpp/robocin/utility/iproto_convertible.h index 43f5cd5c..0f698f57 100644 --- a/common/cpp/robocin/utility/iproto_convertible.h +++ b/common/cpp/robocin/utility/iproto_convertible.h @@ -16,7 +16,7 @@ class IProtoConvertible { virtual ~IProtoConvertible() = default; virtual T toProto() const = 0; - virtual void fromProto(T proto) = 0; + virtual void fromProto(const T& proto) = 0; }; } // namespace robocin From bf53a1bc712eed95c8b5f7c7796c5245b81ab0f2 Mon Sep 17 00:00:00 2001 From: Felipe Nunes Date: Wed, 30 Oct 2024 18:05:31 -0300 Subject: [PATCH 5/7] Fix optional uint32 number (#51) Previously, when sending `robot_id.number = 0`, no information would be serialized because this is the default value of uint32 type sent in the message. Using the field as `optional` adds explicit presence, thus sending the value despite being the default value. For reference see [protobuf documentation](https://protobuf.dev/programming-guides/field_presence/#how-to-enable-explicit-presence-in-proto3). --- protocols/protocols/common/robot_id.proto | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/protocols/protocols/common/robot_id.proto b/protocols/protocols/common/robot_id.proto index f4d44b89..625158df 100644 --- a/protocols/protocols/common/robot_id.proto +++ b/protocols/protocols/common/robot_id.proto @@ -14,5 +14,5 @@ message RobotId { // The color of the robot. optional Color color = 1; // The number of the robot. - uint32 number = 2; + optional uint32 number = 2; } From f8946b488e63a6117546b637e8720cc6714c66fe Mon Sep 17 00:00:00 2001 From: Felipe Nunes Date: Thu, 31 Oct 2024 12:23:10 -0300 Subject: [PATCH 6/7] [decision-ms] Add message mappers (#60) Adds message classes that encapsulates proto generated classes handling. All messages used in `decision-ms` are created, although this pull request does not add behavior to all of them, only the ones used in the hot path, such as: - RobotMessage - RobotId - Ball - Detection - GameStatus --- .../cpp/robocin/utility/iproto_convertible.h | 1 - .../decision/processing/CMakeLists.txt | 1 + .../processing/decision_processor.cpp | 29 +- .../processing/messages/CMakeLists.txt | 34 ++ .../messages/behavior/behavior_message.cpp | 27 ++ .../messages/behavior/behavior_message.h | 68 ++++ .../game_command/game_command_message.cpp | 6 + .../game_command/game_command_message.h | 212 ++++++++++++ .../common/game_event/game_event_message.cpp | 138 ++++++++ .../common/game_event/game_event_message.h | 314 ++++++++++++++++++ .../peripheral_actuation.cpp | 1 + .../peripheral_actuation.h | 24 ++ .../common/robot_id/robot_id_message.cpp | 35 ++ .../common/robot_id/robot_id_message.h | 34 ++ .../common/robot_kick/kick_command.cpp | 1 + .../messages/common/robot_kick/kick_command.h | 31 ++ .../messages/decision/decision_message.cpp | 14 + .../messages/decision/decision_message.h | 60 ++++ .../messages/motion/motion_message.cpp | 29 ++ .../messages/motion/motion_message.h | 126 +++++++ .../messages/perception/ball/ball_message.cpp | 89 +++++ .../messages/perception/ball/ball_message.h | 71 ++++ .../detection/detection_message.cpp | 55 +++ .../perception/detection/detection_message.h | 39 +++ .../perception/field/field_message.cpp | 40 +++ .../messages/perception/field/field_message.h | 42 +++ .../perception/robot/robot_message.cpp | 91 +++++ .../messages/perception/robot/robot_message.h | 85 +++++ .../messages/planning/planning_message.cpp | 5 + .../messages/planning/planning_message.h | 23 ++ .../messages/referee/game_status_message.cpp | 264 +++++++++++++++ .../messages/referee/game_status_message.h | 170 ++++++++++ docker-compose.dev.yaml | 4 + 33 files changed, 2139 insertions(+), 24 deletions(-) create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/CMakeLists.txt create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.h create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.h diff --git a/common/cpp/robocin/utility/iproto_convertible.h b/common/cpp/robocin/utility/iproto_convertible.h index 0f698f57..a6dbdbad 100644 --- a/common/cpp/robocin/utility/iproto_convertible.h +++ b/common/cpp/robocin/utility/iproto_convertible.h @@ -14,7 +14,6 @@ class IProtoConvertible { IProtoConvertible& operator=(IProtoConvertible&&) noexcept = default; virtual ~IProtoConvertible() = default; - virtual T toProto() const = 0; virtual void fromProto(const T& proto) = 0; }; diff --git a/decision-ms/decision-guardiola/decision/processing/CMakeLists.txt b/decision-ms/decision-guardiola/decision/processing/CMakeLists.txt index cbf1a6ac..985034af 100644 --- a/decision-ms/decision-guardiola/decision/processing/CMakeLists.txt +++ b/decision-ms/decision-guardiola/decision/processing/CMakeLists.txt @@ -3,4 +3,5 @@ robocin_cpp_library( HDRS decision_processor.h SRCS decision_processor.cpp DEPS common::output + messages ) diff --git a/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp b/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp index f23f2c72..730abe1a 100644 --- a/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp +++ b/decision-ms/decision-guardiola/decision/processing/decision_processor.cpp @@ -1,12 +1,17 @@ #include "decision/processing/decision_processor.h" #include "decision/messaging/receiver/payload.h" +#include "decision/processing/entities/world.h" +#include "decision/processing/messages/behavior/behavior_message.h" +#include "decision/processing/messages/decision/decision_message.h" +#include "decision/processing/messages/perception/detection/detection_message.h" #include #include #include #include #include +#include namespace decision { @@ -14,8 +19,6 @@ namespace parameters = ::robocin::parameters; namespace { -using ::robocin::ilog; - namespace rc { using ::protocols::common::RobotId; @@ -63,27 +66,7 @@ std::optional DecisionProcessor::process(std::span return std::nullopt; } - const rc::Detection last_detection = detections.back(); - - /////////////////////////////////////////////////////////////////////////////////// - - // TODO: Implement the logic to generate the decision based on the last detection and the last - // game status. - for (const auto& robot : last_detection.robots()) { - rc::Behavior* behavior = decision_output.add_behavior(); - - behavior->set_id(0); - behavior->mutable_robot_id()->CopyFrom(robot.robot_id()); - } - - rc::TacticalPlan* tplan = decision_output.mutable_plan(); - - rc::OffensivePlan* ofPlan = tplan->mutable_offensive(); - rc::DefensivePlan* dfPlan = tplan->mutable_defensive(); - - /////////////////////////////////////////////////////////////////////////////////// - - return decision_output; + return rc::Decision{}; } } // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/CMakeLists.txt b/decision-ms/decision-guardiola/decision/processing/messages/CMakeLists.txt new file mode 100644 index 00000000..f329b42a --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/CMakeLists.txt @@ -0,0 +1,34 @@ +robocin_cpp_library( + NAME messages + HDRS behavior/behavior_message.h + common/peripheral_actuation/peripheral_actuation.h + common/robot_id/robot_id_message.h + common/robot_kick/kick_command.h + common/game_command/game_command_message.h + common/game_event/game_event_message.h + decision/decision_message.h + motion/motion_message.h + planning/planning_message.h + perception/robot/robot_message.h + perception/ball/ball_message.h + perception/field/field_message.h + perception/detection/detection_message.h + referee/game_status_message.h + SRCS behavior/behavior_message.cpp + common/peripheral_actuation/peripheral_actuation.cpp + common/robot_id/robot_id_message.cpp + common/robot_kick/kick_command.cpp + common/game_command/game_command_message.cpp + common/game_event/game_event_message.cpp + decision/decision_message.cpp + motion/motion_message.cpp + planning/planning_message.cpp + perception/robot/robot_message.cpp + perception/ball/ball_message.cpp + perception/field/field_message.cpp + perception/detection/detection_message.cpp + referee/game_status_message.cpp + DEPS common::utility + common::output + protocols::protobufs +) diff --git a/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.cpp new file mode 100644 index 00000000..e600204e --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.cpp @@ -0,0 +1,27 @@ +#include "decision/processing/messages/behavior/behavior_message.h" + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" +#include "decision/processing/messages/planning/planning_message.h" + +#include +#include + +namespace decision { +BehaviorUnificationMessage::BehaviorUnificationMessage(OutputMessage output) : + output(std::move(output)) {} + +OutputMessage::OutputMessage(RobotIdMessage robot_id, + MotionMessage motion, + PlanningMessage planning) : + robot_id(std::move(robot_id)), + motion(std::move(motion)), + planning(std::move(planning)) {} + +BehaviorMessage::BehaviorMessage(std::optional id, + std::optional robot_id, + std::optional> target) : + id(id), + robot_id(std::move(robot_id)), + target(target) {} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.h b/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.h new file mode 100644 index 00000000..c56cd5c5 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/behavior/behavior_message.h @@ -0,0 +1,68 @@ +#ifndef DECISION_PROCESSING_MESSAGES_BEHAVIOR_BEHAVIOR_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_BEHAVIOR_BEHAVIOR_MESSAGE_H + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" +#include "decision/processing/messages/motion/motion_message.h" +#include "decision/processing/messages/planning/planning_message.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace decision { + +class OutputMessage : public robocin::IProtoConvertible { + public: + OutputMessage(RobotIdMessage robot_id = RobotIdMessage{}, + MotionMessage motion = MotionMessage{}, + PlanningMessage planning = PlanningMessage{}); + + RobotIdMessage robot_id; + MotionMessage motion; + PlanningMessage planning; + + [[nodiscard]] protocols::behavior::unification::Output toProto() const override { + return protocols::behavior::unification::Output{}; + }; + + void fromProto(const protocols::behavior::unification::Output& output_proto) override; +}; + +class BehaviorUnificationMessage + : public robocin::IProtoConvertible { + public: + BehaviorUnificationMessage(OutputMessage output = OutputMessage{}); + OutputMessage output; + + [[nodiscard]] protocols::behavior::unification::Behavior toProto() const override { + return protocols::behavior::unification::Behavior{}; + } + + void + fromProto(const protocols::behavior::unification::Behavior& unification_behavior_proto) override; +}; + +class BehaviorMessage : public robocin::IProtoConvertible { + public: + BehaviorMessage(std::optional id = std::nullopt, + std::optional robot_id = std::nullopt, + std::optional> target = std::nullopt); + std::optional id; + std::optional robot_id; + std::optional> target; + + [[nodiscard]] protocols::decision::Behavior toProto() const override { + return protocols::decision::Behavior{}; + } + + void fromProto(const protocols::decision::Behavior& behavior_proto) override; +}; + +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_BEHAVIOR_BEHAVIOR_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.cpp new file mode 100644 index 00000000..972cfd36 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.cpp @@ -0,0 +1,6 @@ +#include "decision/processing/messages/common/game_command/game_command_message.h" + +namespace decision { +// todo(fnap): implement +GameCommandMessage::GameCommandMessage() = default; +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.h b/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.h new file mode 100644 index 00000000..aedc32c9 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/game_command/game_command_message.h @@ -0,0 +1,212 @@ +#ifndef DECISION_PROCESSING_MESSAGES_COMMON_GAME_COMMAND_GAME_COMMAND_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_COMMON_GAME_COMMAND_GAME_COMMAND_MESSAGE_H + +#include +#include +#include +#include +#include +#include + +namespace decision { + +class GameCommandMessage : public robocin::IProtoConvertible { + public: + // Inner command classes + class HaltMessage : public robocin::IProtoConvertible { + public: + explicit HaltMessage(); + + [[nodiscard]] protocols::common::GameCommand::Halt toProto() const override { + return protocols::common::GameCommand::Halt{}; + }; + + void fromProto(const protocols::common::GameCommand::Halt& halt_proto) override; + }; + + class InGameMessage : public robocin::IProtoConvertible { + public: + explicit InGameMessage(); + + [[nodiscard]] protocols::common::GameCommand::InGame toProto() const override { + return protocols::common::GameCommand::InGame{}; + }; + + void fromProto(const protocols::common::GameCommand::InGame& in_game_proto) override; + }; + + class StopMessage : public robocin::IProtoConvertible { + public: + explicit StopMessage(); + + [[nodiscard]] protocols::common::GameCommand::Stop toProto() const override { + return protocols::common::GameCommand::Stop{}; + }; + + void fromProto(const protocols::common::GameCommand::Stop& stop_proto) override; + }; + + class BallPlacementMessage + : public robocin::IProtoConvertible { + public: + std::optional position; + std::optional remaining_time; // todo(fnap): discuss what to use with duration + + explicit BallPlacementMessage(std::optional position = std::nullopt, + std::optional remaining_time = std::nullopt); + + [[nodiscard]] protocols::common::GameCommand::BallPlacement toProto() const override { + return protocols::common::GameCommand::BallPlacement{}; + }; + + void + fromProto(const protocols::common::GameCommand::BallPlacement& ball_placement_proto) override; + }; + + class PrepareKickoffMessage + : public robocin::IProtoConvertible { + public: + explicit PrepareKickoffMessage(); + + [[nodiscard]] protocols::common::GameCommand::PrepareKickoff toProto() const override { + return protocols::common::GameCommand::PrepareKickoff{}; + }; + + void fromProto( + const protocols::common::GameCommand::PrepareKickoff& prepare_kick_off_proto) override; + }; + + class KickoffMessage + : public robocin::IProtoConvertible { + public: + std::optional remaining_time; + + explicit KickoffMessage(const google::protobuf::Duration& time); + + [[nodiscard]] protocols::common::GameCommand::Kickoff toProto() const override { + return protocols::common::GameCommand::Kickoff{}; + }; + + void fromProto(const protocols::common::GameCommand::Kickoff& kick_off_proto) override; + }; + + class PreparePenaltyMessage + : public robocin::IProtoConvertible { + public: + explicit PreparePenaltyMessage(); + + [[nodiscard]] protocols::common::GameCommand::PreparePenalty toProto() const override { + return protocols::common::GameCommand::PreparePenalty{}; + }; + + void + fromProto(const protocols::common::GameCommand::PreparePenalty& prepare_penalty_proto) override; + }; + + class PenaltyMessage + : public robocin::IProtoConvertible { + public: + std::optional remaining_time; + + explicit PenaltyMessage(const google::protobuf::Duration& time); + + [[nodiscard]] protocols::common::GameCommand::Penalty toProto() const override { + return protocols::common::GameCommand::Penalty{}; + }; + + void fromProto(const protocols::common::GameCommand::Penalty& penalty_proto) override; + }; + + class PrepareDirectFreeKickMessage + : public robocin::IProtoConvertible { + public: + explicit PrepareDirectFreeKickMessage(); + + [[nodiscard]] protocols::common::GameCommand::PrepareDirectFreeKick toProto() const override { + return protocols::common::GameCommand::PrepareDirectFreeKick{}; + }; + + void + fromProto(const protocols::common::GameCommand::PrepareDirectFreeKick& penalty_proto) override; + }; + + class DirectFreeKickMessage + : public robocin::IProtoConvertible { + public: + std::optional remaining_time; + explicit DirectFreeKickMessage(const google::protobuf::Duration& time); + + [[nodiscard]] protocols::common::GameCommand::DirectFreeKick toProto() const override { + return protocols::common::GameCommand::DirectFreeKick{}; + }; + + void fromProto( + const protocols::common::GameCommand::DirectFreeKick& direct_free_kick_proto) override; + }; + + class TimeoutMessage + : public robocin::IProtoConvertible { + public: + explicit TimeoutMessage(); + + [[nodiscard]] protocols::common::GameCommand::Timeout toProto() const override { + return protocols::common::GameCommand::Timeout{}; + }; + + void fromProto(const protocols::common::GameCommand::Timeout& timeout_proto) override; + }; + + class IntervalMessage + : public robocin::IProtoConvertible { + public: + explicit IntervalMessage(); + + [[nodiscard]] protocols::common::GameCommand::Interval toProto() const override { + return protocols::common::GameCommand::Interval{}; + }; + + void fromProto(const protocols::common::GameCommand::Interval& timeout_proto) override; + }; + + // Optional fields for each command type + std::optional halt; + std::optional in_game; + std::optional stop; + + std::optional home_ball_placement; + std::optional away_ball_placement; + + std::optional home_prepare_kickoff; + std::optional away_prepare_kickoff; + + std::optional home_kickoff; + std::optional away_kickoff; + + std::optional home_prepare_penalty; + std::optional away_prepare_penalty; + std::optional home_penalty; + std::optional away_penalty; + + std::optional home_prepare_direct_free_kick; + std::optional away_prepare_direct_free_kick; + + std::optional home_direct_free_kick; + std::optional away_direct_free_kick; + + std::optional home_timeout; + std::optional away_timeout; + + std::optional interval; + + // todo(fnap): implement + explicit GameCommandMessage(); + + [[nodiscard]] protocols::common::GameCommand toProto() const override { + return protocols::common::GameCommand{}; + }; + + void fromProto(const protocols::common::GameCommand& game_command_proto) override; +}; + +} // namespace decision +#endif // DECISION_PROCESSING_MESSAGES_COMMON_GAME_COMMAND_GAME_COMMAND_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.cpp new file mode 100644 index 00000000..b6e1a54e --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.cpp @@ -0,0 +1,138 @@ +#include "decision/processing/messages/common/game_event/game_event_message.h" + +#include + +namespace decision { +GameEventMessage::GameEventMessage( + std::vector sources, + std::optional timestamp, + std::optional ball_left_field_touch_line, + std::optional ball_left_field_goal_line, + std::optional ball_left_field_boundary, + std::optional aimless_kick, + std::optional goalkeeper_held_ball, + std::optional robot_too_close_to_defense_area, + std::optional robot_in_defense_area, + std::optional robot_pushed_robot, + std::optional robot_held_ball_deliberately, + std::optional robot_dribbled_ball_too_far, + std::optional robot_tipped_over, + std::optional goal) : + sources(std::move(sources)), + timestamp(timestamp), + ball_left_field_touch_line(std::move(ball_left_field_touch_line)), + ball_left_field_goal_line(std::move(ball_left_field_goal_line)), + ball_left_field_boundary(std::move(ball_left_field_boundary)), + aimless_kick(std::move(aimless_kick)), + goalkeeper_held_ball(std::move(goalkeeper_held_ball)), + robot_too_close_to_defense_area(std::move(robot_too_close_to_defense_area)), + robot_in_defense_area(std::move(robot_in_defense_area)), + robot_pushed_robot(std::move(robot_pushed_robot)), + robot_held_ball_deliberately(std::move(robot_held_ball_deliberately)), + robot_dribbled_ball_too_far(std::move(robot_dribbled_ball_too_far)), + robot_tipped_over(std::move(robot_tipped_over)), + goal(std::move(goal)) {}; + +void GameEventMessage::fromProto(const protocols::common::GameEvent& goal_proto) {} + +GameEventMessage::GameEventMessage(const protocols::common::GameEvent& game_event_proto) { + timestamp = game_event_proto.timestamp().seconds(); + + for (const auto& source : game_event_proto.sources()) { + sources.emplace_back(source); + } + + if (game_event_proto.has_ball_left_field_touch_line()) { + if (!ball_left_field_touch_line.has_value()) { + ball_left_field_touch_line.emplace(); + } + + ball_left_field_touch_line->fromProto(game_event_proto.ball_left_field_touch_line()); + } + + if (game_event_proto.has_ball_left_field_touch_line()) { + if (!ball_left_field_touch_line.has_value()) { + ball_left_field_touch_line.emplace(); + } + ball_left_field_touch_line->fromProto(game_event_proto.ball_left_field_touch_line()); + } + + if (game_event_proto.has_ball_left_field_goal_line()) { + if (!ball_left_field_goal_line.has_value()) { + ball_left_field_goal_line.emplace(); + } + ball_left_field_goal_line->fromProto(game_event_proto.ball_left_field_goal_line()); + } + + if (game_event_proto.has_ball_left_field_boundary()) { + if (!ball_left_field_boundary.has_value()) { + ball_left_field_boundary.emplace(); + } + ball_left_field_boundary->fromProto(game_event_proto.ball_left_field_boundary()); + } + + if (game_event_proto.has_aimless_kick()) { + if (!aimless_kick.has_value()) { + aimless_kick.emplace(); + } + aimless_kick->fromProto(game_event_proto.aimless_kick()); + } + + if (game_event_proto.has_goalkeeper_held_ball()) { + if (!goalkeeper_held_ball.has_value()) { + goalkeeper_held_ball.emplace(); + } + goalkeeper_held_ball->fromProto(game_event_proto.goalkeeper_held_ball()); + } + + if (game_event_proto.has_robot_too_close_to_defense_area()) { + if (!robot_too_close_to_defense_area.has_value()) { + robot_too_close_to_defense_area.emplace(); + } + robot_too_close_to_defense_area->fromProto(game_event_proto.robot_too_close_to_defense_area()); + } + + if (game_event_proto.has_robot_in_defense_area()) { + if (!robot_in_defense_area.has_value()) { + robot_in_defense_area.emplace(); + } + robot_in_defense_area->fromProto(game_event_proto.robot_in_defense_area()); + } + + if (game_event_proto.has_robot_pushed_robot()) { + if (!robot_pushed_robot.has_value()) { + robot_pushed_robot.emplace(); + } + robot_pushed_robot->fromProto(game_event_proto.robot_pushed_robot()); + } + + if (game_event_proto.has_robot_held_ball_deliberately()) { + if (!robot_held_ball_deliberately.has_value()) { + robot_held_ball_deliberately.emplace(); + } + robot_held_ball_deliberately->fromProto(game_event_proto.robot_held_ball_deliberately()); + } + + if (game_event_proto.has_robot_dribbled_ball_too_far()) { + if (!robot_dribbled_ball_too_far.has_value()) { + robot_dribbled_ball_too_far.emplace(); + } + robot_dribbled_ball_too_far->fromProto(game_event_proto.robot_dribbled_ball_too_far()); + } + + if (game_event_proto.has_robot_tipped_over()) { + if (!robot_tipped_over.has_value()) { + robot_tipped_over.emplace(); + } + robot_tipped_over->fromProto(game_event_proto.robot_tipped_over()); + } + + if (game_event_proto.has_goal()) { + if (!goal.has_value()) { + goal.emplace(); + } + goal->fromProto(game_event_proto.goal()); + } +}; + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.h b/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.h new file mode 100644 index 00000000..f8fb4aa1 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/game_event/game_event_message.h @@ -0,0 +1,314 @@ +#ifndef DECISION_PROCESSING_MESSAGES_COMMON_GAME_EVENT_GAME_EVENT_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_COMMON_GAME_EVENT_GAME_EVENT_MESSAGE_H + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace decision { + +enum Team { + TEAM_UNSPECIFIED = 0, + TEAM_HOME = 1, + TEAM_AWAY = 2, +}; + +class BallLeftFieldMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional left_field_position; + + explicit BallLeftFieldMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional left_field_position + = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::BallLeftField toProto() const override { + return protocols::common::GameEvent::BallLeftField{}; + }; + + void fromProto(const protocols::common::GameEvent::BallLeftField& ball_left_field_proto) override; +}; + +class BallLeftFieldBoundaryMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional left_field_boundary_position; + + explicit BallLeftFieldBoundaryMessage( + std::optional by_team = std::nullopt, + std::optional left_field_boundary_position = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::BallLeftFieldBoundary toProto() const override { + return protocols::common::GameEvent::BallLeftFieldBoundary{}; + }; + + void fromProto(const protocols::common::GameEvent::BallLeftFieldBoundary& + ball_left_field_boundary_proto) override; +}; + +class AimlessKickMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional left_field_position; + std::optional kick_position; + + explicit AimlessKickMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional left_field_position = std::nullopt, + std::optional kick_position = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::AimlessKick toProto() const override { + return protocols::common::GameEvent::AimlessKick{}; + }; + + void fromProto(const protocols::common::GameEvent::AimlessKick& aimless_kick_proto) override; +}; + +class GoalkeeperHeldBallMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional ball_position; + std::optional duration; // todo(fnap): discuss what to use with duration + + explicit GoalkeeperHeldBallMessage(std::optional by_team = std::nullopt, + std::optional ball_position = std::nullopt, + std::optional duration = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::GoalkeeperHeldBall toProto() const override { + return protocols::common::GameEvent::GoalkeeperHeldBall{}; + }; + + void fromProto( + const protocols::common::GameEvent::GoalkeeperHeldBall& goalkeeper_held_ball_proto) override; +}; + +class RobotTooCloseToDefenseAreaMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional robot_position; + std::optional distance_to_defense_area; + std::optional ball_position; + + explicit RobotTooCloseToDefenseAreaMessage( + std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional robot_position = std::nullopt, + std::optional distance_to_defense_area = std::nullopt, + std::optional ball_position = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotTooCloseToDefenseArea toProto() const override { + return protocols::common::GameEvent::RobotTooCloseToDefenseArea{}; + }; + + void fromProto(const protocols::common::GameEvent::RobotTooCloseToDefenseArea& + robot_too_close_to_defense_area_proto) override; +}; + +class RobotInDefenseAreaMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional robot_position; + std::optional distance_to_nearest_point_outside_area; + + explicit RobotInDefenseAreaMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional robot_position = std::nullopt, + std::optional distance_to_nearest_point_outside_area + = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotInDefenseArea toProto() const override { + return protocols::common::GameEvent::RobotInDefenseArea{}; + }; + + void fromProto( + const protocols::common::GameEvent::RobotInDefenseArea& robot_in_defense_area_proto) override; +}; + +class RobotPushedRobotMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional violator_robot; + std::optional victim_robot; + std::optional position; + std::optional pushed_distance; + + explicit RobotPushedRobotMessage(std::optional by_team = std::nullopt, + std::optional violator_robot = std::nullopt, + std::optional victim_robot = std::nullopt, + std::optional position = std::nullopt, + std::optional pushed_distance = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotPushedRobot toProto() const override { + return protocols::common::GameEvent::RobotPushedRobot{}; + }; + + void fromProto( + const protocols::common::GameEvent::RobotPushedRobot& robot_pushed_robot_proto) override; +}; + +class RobotHeldBallDeliberatelyMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional ball_position; + std::optional duration; // todo(fnap): discuss what to use with duration + + explicit RobotHeldBallDeliberatelyMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional ball_position + = std::nullopt, + std::optional duration = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotHeldBallDeliberately toProto() const override { + return protocols::common::GameEvent::RobotHeldBallDeliberately{}; + }; + + void fromProto(const protocols::common::GameEvent::RobotHeldBallDeliberately& + robot_held_ball_deliberately_proto) override; +}; + +class RobotDribbledBallTooFarMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional start_position; + std::optional end_position; + + explicit RobotDribbledBallTooFarMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional start_position + = std::nullopt, + std::optional end_position + = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotDribbledBallTooFar toProto() const override { + return protocols::common::GameEvent::RobotDribbledBallTooFar{}; + }; + + void fromProto(const protocols::common::GameEvent::RobotDribbledBallTooFar& + robot_dribbled_ball_too_far_proto) override; +}; + +class RobotTippedOverMessage + : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional by_robot; + std::optional robot_position; + std::optional ball_position; + + explicit RobotTippedOverMessage(std::optional by_team = std::nullopt, + std::optional by_robot = std::nullopt, + std::optional robot_position = std::nullopt, + std::optional ball_position = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::RobotTippedOver toProto() const override { + return protocols::common::GameEvent::RobotTippedOver{}; + }; + + void + fromProto(const protocols::common::GameEvent::RobotTippedOver& robot_tipped_over_proto) override; +}; + +class GoalMessage : public robocin::IProtoConvertible { + public: + std::optional by_team; + std::optional kicking_team; + std::optional kicking_robot; + std::optional ball_position_when_scored; + std::optional kick_position; + std::optional max_ball_height; + std::optional num_of_robots_of_scoring_team; + std::optional + last_touch_by_scoring_team_timestamp; // todo(fnap): discuss how to use timestamp + std::optional message; + + explicit GoalMessage(std::optional by_team = std::nullopt, + std::optional kicking_team = std::nullopt, + std::optional kicking_robot = std::nullopt, + std::optional ball_position_when_scored = std::nullopt, + std::optional kick_position = std::nullopt, + std::optional max_ball_height = std::nullopt, + std::optional num_of_robots_of_scoring_team = std::nullopt, + std::optional last_touch_by_scoring_team_timestamp = std::nullopt, + std::optional message = std::nullopt); + + [[nodiscard]] protocols::common::GameEvent::Goal toProto() const override { + return protocols::common::GameEvent::Goal{}; + }; + + void fromProto(const protocols::common::GameEvent::Goal& goal_proto) override; +}; + +class GameEventMessage : public robocin::IProtoConvertible { + public: + std::vector sources; + std::optional timestamp; // todo(fnap): discuss how to use timestamp + + // Oneof event fields as separate optionals + std::optional ball_left_field_touch_line; + std::optional ball_left_field_goal_line; + std::optional ball_left_field_boundary; + std::optional aimless_kick; + std::optional goalkeeper_held_ball; + std::optional robot_too_close_to_defense_area; + std::optional robot_in_defense_area; + std::optional robot_pushed_robot; + std::optional robot_held_ball_deliberately; + std::optional robot_dribbled_ball_too_far; + std::optional robot_tipped_over; + std::optional goal; + + explicit GameEventMessage( + std::vector sources = {}, + std::optional timestamp = std::nullopt, + std::optional ball_left_field_touch_line = std::nullopt, + std::optional ball_left_field_goal_line = std::nullopt, + std::optional ball_left_field_boundary = std::nullopt, + std::optional aimless_kick = std::nullopt, + std::optional goalkeeper_held_ball = std::nullopt, + std::optional robot_too_close_to_defense_area + = std::nullopt, + std::optional robot_in_defense_area = std::nullopt, + std::optional robot_pushed_robot = std::nullopt, + std::optional robot_held_ball_deliberately = std::nullopt, + std::optional robot_dribbled_ball_too_far = std::nullopt, + std::optional robot_tipped_over = std::nullopt, + std::optional goal = std::nullopt); + + explicit GameEventMessage(const protocols::common::GameEvent& game_event_proto); + + [[nodiscard]] protocols::common::GameEvent toProto() const override { + return protocols::common::GameEvent{}; + }; + + void fromProto(const protocols::common::GameEvent& goal_proto) override; +}; + +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_COMMON_GAME_EVENT_GAME_EVENT_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.cpp b/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.cpp new file mode 100644 index 00000000..6eff1410 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.cpp @@ -0,0 +1 @@ +#include "decision/processing/messages/common/peripheral_actuation/peripheral_actuation.h" diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.h b/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.h new file mode 100644 index 00000000..71f643ca --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/peripheral_actuation/peripheral_actuation.h @@ -0,0 +1,24 @@ +#ifndef DECISION_PROCESSING_MESSAGES_COMMON_PERIPHERAL_ACTUATION_PERIPHERAL_ACTUATION_H +#define DECISION_PROCESSING_MESSAGES_COMMON_PERIPHERAL_ACTUATION_PERIPHERAL_ACTUATION_H + +#include "decision/processing/messages/common/robot_kick/kick_command.h" + +#include +#include + +namespace decision { + +class PeripheralActuationMessage + : public robocin::IProtoConvertible { + public: + explicit PeripheralActuationMessage(KickCommandMessage kick_command + /*RobotDribblerMessage robot_dribbler*/); + KickCommandMessage kick_command; + + [[nodiscard]] protocols::common::PeripheralActuation toProto() const override { + return protocols::common::PeripheralActuation{}; + }; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_COMMON_PERIPHERAL_ACTUATION_PERIPHERAL_ACTUATION_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.cpp new file mode 100644 index 00000000..546db199 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.cpp @@ -0,0 +1,35 @@ +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include +#include + +namespace decision { +namespace { +namespace rc { +using protocols::common::RobotId; +using protocols::common::RobotId_Color; +} // namespace rc +} // namespace + +RobotIdMessage::Color getColor(rc::RobotId_Color color_proto) { + switch (color_proto) { + case protocols::common::RobotId_Color_COLOR_YELLOW: return RobotIdMessage::Color::COLOR_YELLOW; + case protocols::common::RobotId_Color_COLOR_BLUE: return RobotIdMessage::Color::COLOR_BLUE; + default: return RobotIdMessage::Color::COLOR_UNSPECIFIED; + } +} + +RobotIdMessage::RobotIdMessage(const rc::RobotId& robot_id_proto) { fromProto(robot_id_proto); }; + +void RobotIdMessage::fromProto(const rc::RobotId& robot_id_proto) { + if (robot_id_proto.has_color()) { + color = getColor(robot_id_proto.color()); + } + + number = robot_id_proto.number(); +}; + +RobotIdMessage::RobotIdMessage(uint32_t number, std::optional color) : + number(number), + color(color) {} +}; // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.h b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.h new file mode 100644 index 00000000..24dbc663 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_id/robot_id_message.h @@ -0,0 +1,34 @@ +#ifndef DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_ID_H +#define DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_ID_H + +#include +#include +#include + +namespace decision { +constexpr uint32_t kRobotIdMessageNumber = 0; + +class RobotIdMessage : public robocin::IProtoConvertible { + public: + enum Color { + COLOR_UNSPECIFIED = 0, + COLOR_YELLOW = 1, + COLOR_BLUE = 2, + }; + + explicit RobotIdMessage(uint32_t number = kRobotIdMessageNumber, + std::optional color = std::nullopt); + std::optional color; + uint32_t number; // number should ALWAYS be sent from proto + + explicit RobotIdMessage(const protocols::common::RobotId& robot_id_proto); + + [[nodiscard]] protocols::common::RobotId toProto() const override { + return protocols::common::RobotId{}; + }; + + void fromProto(const protocols::common::RobotId& robot_id_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_ID_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.cpp b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.cpp new file mode 100644 index 00000000..4cfb1b9a --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.cpp @@ -0,0 +1 @@ +#include "decision/processing/messages/common/robot_kick/kick_command.h" diff --git a/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.h b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.h new file mode 100644 index 00000000..40d38c03 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/common/robot_kick/kick_command.h @@ -0,0 +1,31 @@ +#ifndef DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_KICK_KICK_COMMAND_H +#define DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_KICK_KICK_COMMAND_H + +#include +#include + +namespace decision { + +class KickCommandMessage + : public robocin::IProtoConvertible { + public: + explicit KickCommandMessage(double strength, + bool is_front, + bool is_chip, + bool charge_capacitor, + bool is_bypass_ir); + double strength; + bool is_front; + bool is_chip; + bool charge_capacitor; + bool is_bypass_ir; + + [[nodiscard]] protocols::common::RobotKick::KickCommand toProto() const override { + return protocols::common::RobotKick::KickCommand{}; + }; + + void fromProto(const protocols::common::RobotKick::KickCommand& kick_command_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_COMMON_ROBOT_KICK_KICK_COMMAND_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.cpp new file mode 100644 index 00000000..12448eb0 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.cpp @@ -0,0 +1,14 @@ +#include "decision/processing/messages/decision/decision_message.h" + +namespace decision { + +TacticalPlanMessage::TacticalPlanMessage(OffensivePlanMessage offensive, + DefensivePlanMessage defensive) : + offensive(std::move(offensive)), + defensive(std::move(defensive)) {} + +DecisionMessage::DecisionMessage(std::vector behavior, TacticalPlanMessage plan) : + behavior(std::move(behavior)), + plan(std::move(plan)) {} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.h b/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.h new file mode 100644 index 00000000..69c8b960 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/decision/decision_message.h @@ -0,0 +1,60 @@ +#include "decision/processing/messages/behavior/behavior_message.h" +#include "protocols/decision/decision.pb.h" + +#include +#include + +namespace decision { +class DefensivePlanMessage : public robocin::IProtoConvertible { + public: + explicit DefensivePlanMessage() = default; + + [[nodiscard]] protocols::decision::DefensivePlan toProto() const override { + return protocols::decision::DefensivePlan{}; + }; + + void fromProto(const protocols::decision::DefensivePlan& defensive_plan_proto) override; +}; + +class OffensivePlanMessage : public robocin::IProtoConvertible { + public: + explicit OffensivePlanMessage() = default; + + [[nodiscard]] protocols::decision::OffensivePlan toProto() const override { + return protocols::decision::OffensivePlan{}; + }; + + void fromProto(const protocols::decision::OffensivePlan& offensive_plan_proto) override; +}; + +class TacticalPlanMessage : public robocin::IProtoConvertible { + public: + explicit TacticalPlanMessage(OffensivePlanMessage offensive = OffensivePlanMessage{}, + DefensivePlanMessage defensive = DefensivePlanMessage{}); + + OffensivePlanMessage offensive; + DefensivePlanMessage defensive; + + [[nodiscard]] protocols::decision::TacticalPlan toProto() const override { + return protocols::decision::TacticalPlan{}; + }; + + void fromProto(const protocols::decision::TacticalPlan& tactical_plan_proto) override; +}; + +class DecisionMessage : public robocin::IProtoConvertible { + public: + explicit DecisionMessage(std::vector behavior = std::vector{}, + TacticalPlanMessage plan = TacticalPlanMessage{}); + + std::vector behavior; + TacticalPlanMessage plan; + + [[nodiscard]] protocols::decision::Decision toProto() const override { + return protocols::decision::Decision{}; + }; + + void fromProto(const protocols::decision::Decision& decision_proto) override; +}; + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.cpp new file mode 100644 index 00000000..98917940 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.cpp @@ -0,0 +1,29 @@ +#include "decision/processing/messages/motion/motion_message.h" + +namespace decision { +MotionMessage::MotionMessage( + std::optional go_to_point, + std::optional go_to_point_with_trajectory, + std::optional rotate_in_point, + std::optional rotate_on_self) : + go_to_point(std::move(go_to_point)), + go_to_point_with_trajectory(std::move(go_to_point_with_trajectory)), + rotate_in_point(std::move(rotate_in_point)), + rotate_on_self(std::move(rotate_on_self)) {} + +GoToPointMessage::GoToPointMessage(std::optional> target, + std::optional target_angle, + std::optional moving_profile, + std::optional precision_to_target, + std::optional sync_rotate_with_linear_movement) : + target(target), + target_angle(target_angle), + moving_profile(moving_profile), + precision_to_target(precision_to_target), + sync_rotate_with_linear_movement(sync_rotate_with_linear_movement) {} + +RotateOnSelfMessage::RotateOnSelfMessage() = default; +RotateInPointMessage::RotateInPointMessage() = default; +GoToPointWithTrajectoryMessage::GoToPointWithTrajectoryMessage() = default; + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.h b/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.h new file mode 100644 index 00000000..7a2f8dd5 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/motion/motion_message.h @@ -0,0 +1,126 @@ +#ifndef DECISION_PROCESSING_MESSAGES_MOTION_MOTION_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_MOTION_MOTION_MESSAGE_H + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" +#include "protocols/behavior/behavior_unification.pb.h" +#include "robocin/geometry/point2d.h" + +#include +#include +#include +#include +#include +#include + +namespace decision { + +class GoToPointMessage : public robocin::IProtoConvertible { + public: + enum MovingProfile { + // Move safely profile + SafeInStopSpeed = 0, + + // Specific movement profile + BalancedInApproachBallSpeed = 1, + BalancedInCarryBallSpeed = 2, + BalancedInCarryBallLowSpeed = 3, + + // Goto point direct profile + DirectApproachBallSpeed = 4, + DirectSafeKickBallSpeed = 5, + DirectBalancedKickBallSpeed = 6, + DirectKickBallwithEnemyClose = 7, + gotoBallWithEnemy = 8, + PushBallInPenalty = 9, + TOTOZINHO = 10, + + // Default movement profile + BalancedInSlowSpeed = 11, + BalancedInMedianSpeed = 12, + BalancedInDefaultSpeed = 13, + BalancedInHighSpeed = 14, + + // Goalkepper profile + GoalkeeperInTopSpeed = 15, + + // Penalty push ball + PenaltyPushBall = 16, + }; + + enum PrecisionToTarget { + HIGH = 0, + NORMAL = 1, + }; + + GoToPointMessage(std::optional> target = std::nullopt, + std::optional target_angle = std::nullopt, + std::optional moving_profile = std::nullopt, + std::optional precision_to_target = std::nullopt, + std::optional sync_rotate_with_linear_movement = std::nullopt); + + std::optional> target; + std::optional target_angle; + std::optional moving_profile; + std::optional precision_to_target; + std::optional sync_rotate_with_linear_movement; + + [[nodiscard]] protocols::behavior::GoToPoint toProto() const override { + return protocols::behavior::GoToPoint{}; + }; + + void fromProto(const protocols::behavior::GoToPoint& go_to_point_proto) override; +}; + +class GoToPointWithTrajectoryMessage + : public robocin::IProtoConvertible { + public: + GoToPointWithTrajectoryMessage(); + [[nodiscard]] protocols::behavior::GoToPointWithTrajectory toProto() const override { + return protocols::behavior::GoToPointWithTrajectory{}; + }; + void fromProto(const protocols::behavior::GoToPointWithTrajectory& + go_to_point_with_trajectory_proto) override; +}; + +class RotateInPointMessage : public robocin::IProtoConvertible { + public: + RotateInPointMessage(); + [[nodiscard]] protocols::behavior::RotateInPoint toProto() const override { + return protocols::behavior::RotateInPoint{}; + }; + + void fromProto(const protocols::behavior::RotateInPoint& rotate_in_point_proto) override; +}; + +class RotateOnSelfMessage : public robocin::IProtoConvertible { + public: + RotateOnSelfMessage(); + [[nodiscard]] protocols::behavior::RotateOnSelf toProto() const override { + return protocols::behavior::RotateOnSelf{}; + }; + + void fromProto(const protocols::behavior::RotateOnSelf& rotate_on_self_proto) override; +}; + +class MotionMessage : public robocin::IProtoConvertible { + public: + MotionMessage(std::optional go_to_point = std::nullopt, + std::optional go_to_point_with_trajectory + = std::nullopt, + std::optional rotate_in_point = std::nullopt, + std::optional rotate_on_self = std::nullopt); + + std::optional go_to_point; + std::optional go_to_point_with_trajectory; + std::optional rotate_in_point; + std::optional rotate_on_self; + + [[nodiscard]] protocols::behavior::unification::Motion toProto() const override { + return protocols::behavior::unification::Motion{}; + }; + + void fromProto(const protocols::behavior::unification::Motion& motion_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_MOTION_MOTION_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.cpp new file mode 100644 index 00000000..ff05436c --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.cpp @@ -0,0 +1,89 @@ +#include "decision/processing/messages/perception/ball/ball_message.h" + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include + +namespace decision { + +KickInformationMessage::KickInformationMessage( + std::optional robot_id, + std::optional start_position, + std::optional start_velocity, + std::optional start_timestamp, + std::optional predicted_stop_position, + std::optional predicted_stop_timestamp) : + robot_id(std::move(robot_id)), + start_position(start_position), + start_velocity(start_velocity), + start_timestamp(start_timestamp), + predicted_stop_position(predicted_stop_position), + predicted_stop_timestamp(predicted_stop_timestamp) {} + +KickInformationMessage::KickInformationMessage( + const protocols::perception::Ball::KickInformation& kick_information_proto) { + KickInformationMessage::fromProto(kick_information_proto); +} + +void KickInformationMessage::fromProto( + const protocols::perception::Ball::KickInformation& kick_information_proto) { + if (kick_information_proto.has_robot_id()) { + robot_id = RobotIdMessage(kick_information_proto.robot_id()); + } + + if (kick_information_proto.has_start_position()) { + start_position = robocin::Point2Df(kick_information_proto.start_position().x(), + kick_information_proto.start_position().y()); + } + + if (kick_information_proto.has_start_velocity()) { + start_velocity = robocin::Point3Df(kick_information_proto.start_velocity().x(), + kick_information_proto.start_velocity().y(), + kick_information_proto.start_velocity().z()); + } + + if (kick_information_proto.has_predicted_stop_position()) { + predicted_stop_position + = robocin::Point2Df(kick_information_proto.predicted_stop_position().x(), + kick_information_proto.predicted_stop_position().y()); + } + + start_timestamp = kick_information_proto.start_timestamp() + .seconds(); // todo(fnap): discuss how to use this field + + predicted_stop_timestamp = kick_information_proto.predicted_stop_timestamp() + .seconds(); // todo(fnap): discuss how to use this field +} + +BallMessage::BallMessage(std::optional confidence, + std::optional position, + std::optional velocity, + std::optional kick_information) : + confidence(confidence), + position(position), + velocity(velocity), + kick_information(std::move(kick_information)) {} + +BallMessage::BallMessage(const protocols::perception::Ball& ball_proto) { + BallMessage::fromProto(ball_proto); +} + +void BallMessage::fromProto(const protocols::perception::Ball& ball_proto) { + confidence = ball_proto.confidence(); + position = robocin::Point3Df(ball_proto.position().x(), + ball_proto.position().y(), + ball_proto.position().z()); + + velocity = robocin::Point3Df(ball_proto.velocity().x(), + ball_proto.velocity().y(), + ball_proto.velocity().z()); + + if (ball_proto.has_kick_information()) { + if (!kick_information.has_value()) { + kick_information.emplace(); + } + + kick_information->fromProto(ball_proto.kick_information()); + } +}; +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.h b/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.h new file mode 100644 index 00000000..d41939ab --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/ball/ball_message.h @@ -0,0 +1,71 @@ +#ifndef DECISION_PROCESSING_MESSAGES_PERCEPTION_BALL_BALL_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_PERCEPTION_BALL_BALL_MESSAGE_H + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace decision { + +class KickInformationMessage + : public robocin::IProtoConvertible { + public: + std::optional robot_id; + std::optional start_position; + std::optional start_velocity; + std::optional + start_timestamp; // todo(fnap): discuss what to do with timestamp translation + std::optional predicted_stop_position; + std::optional + predicted_stop_timestamp; // todo(fnap): discuss what to do with timestamp translation + + explicit KickInformationMessage(std::optional robot_id = std::nullopt, + std::optional start_position = std::nullopt, + std::optional start_velocity = std::nullopt, + std::optional start_timestamp = std::nullopt, + std::optional predicted_stop_position + = std::nullopt, + std::optional predicted_stop_timestamp = std::nullopt); + + explicit KickInformationMessage( + const protocols::perception::Ball::KickInformation& kick_information_proto); + + [[nodiscard]] protocols::perception::Ball::KickInformation toProto() const override { + return protocols::perception::Ball::KickInformation{}; + } + + void + fromProto(const protocols::perception::Ball::KickInformation& kick_information_proto) override; +}; + +class BallMessage : public robocin::IProtoConvertible { + public: + std::optional confidence; + std::optional position; + std::optional velocity; + std::optional kick_information; + + explicit BallMessage(std::optional confidence = std::nullopt, + std::optional position = std::nullopt, + std::optional velocity = std::nullopt, + std::optional kick_information = std::nullopt); + + explicit BallMessage(const protocols::perception::Ball& ball_proto); + + [[nodiscard]] protocols::perception::Ball toProto() const override { + return protocols::perception::Ball{}; + } + + void fromProto(const protocols::perception::Ball& ball_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_PERCEPTION_BALL_BALL_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.cpp new file mode 100644 index 00000000..25371f78 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.cpp @@ -0,0 +1,55 @@ +#include "decision/processing/messages/perception/detection/detection_message.h" + +#include "decision/processing/messages/perception/field/field_message.h" + +namespace decision { +namespace { +namespace rc { +using protocols::perception::Detection; +} // namespace rc +} // namespace + +DetectionMessage::DetectionMessage(std::optional serial_id, + std::optional created_at, + std::optional framerate, + std::vector balls, + std::vector robots, + std::optional field) : + serial_id(serial_id), + created_at(created_at), + framerate(framerate), + balls(std::move(balls)), + robots(std::move(robots)), + field(std::move(field)) {}; + +rc::Detection DetectionMessage::toProto() const { return rc::Detection{}; }; + +void DetectionMessage::fromProto(const rc::Detection& detection_proto) { + + serial_id = detection_proto.serial_id(); + created_at + = detection_proto.created_at().seconds(); // todo(fnap): discuss the usage of this field + framerate = detection_proto.framerate(); + + if (detection_proto.has_field()) { + // Checking for std::nullopt FieldMessage + if (!field.has_value()) { + field.emplace(); + } + field->fromProto(detection_proto.field()); + } + + for (const auto& ball_proto : detection_proto.balls()) { + balls.emplace_back(ball_proto); + } + + for (const auto& robot_proto : detection_proto.robots()) { + robots.emplace_back(robot_proto); + } +}; + +DetectionMessage::DetectionMessage(const rc::Detection& detection_proto) { + fromProto(detection_proto); +} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.h b/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.h new file mode 100644 index 00000000..0a0bb475 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/detection/detection_message.h @@ -0,0 +1,39 @@ +#ifndef DECISION_PROCESSING_MESSAGES_PERCEPTION_DETECTION_DETECTION_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_PERCEPTION_DETECTION_DETECTION_MESSAGE_H + +#include "decision/processing/messages/perception/ball/ball_message.h" +#include "decision/processing/messages/perception/field/field_message.h" +#include "decision/processing/messages/perception/robot/robot_message.h" + +#include +#include +#include +#include + +namespace decision { + +class DetectionMessage : public robocin::IProtoConvertible { + public: + std::optional serial_id; + std::optional created_at; // todo(fnap): discuss what to do with timestamp + std::optional framerate; + std::vector balls; + std::vector robots; + std::optional field; + + explicit DetectionMessage(std::optional serial_id = std::nullopt, + std::optional created_at = std::nullopt, + std::optional framerate = std::nullopt, + std::vector balls = {}, + std::vector robots = {}, + std::optional field = std::nullopt); + + explicit DetectionMessage(const protocols::perception::Detection& detection_proto); + + [[nodiscard]] protocols::perception::Detection toProto() const override; + void fromProto(const protocols::perception::Detection& detection_proto) override; +}; + +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_PERCEPTION_DETECTION_DETECTION_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.cpp new file mode 100644 index 00000000..bf8a6057 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.cpp @@ -0,0 +1,40 @@ +#include "decision/processing/messages/perception/field/field_message.h" + +namespace decision { +FieldMessage::FieldMessage(std::optional serial_id, + std::optional length, + std::optional width, + std::optional goal_depth, + std::optional goal_width, + std::optional penalty_area_depth, + std::optional penalty_area_width, + std::optional boundary_width, + std::optional goal_center_to_penalty_mark) : + serial_id(serial_id), + length(length), + width(width), + goal_depth(goal_depth), + goal_width(goal_width), + penalty_area_depth(penalty_area_depth), + penalty_area_width(penalty_area_width), + boundary_width(boundary_width), + goal_center_to_penalty_mark(goal_center_to_penalty_mark) {} + +void FieldMessage::fromProto(const protocols::perception::Field& field_proto) { + // std::cout << "SERIAL ID: " << field_proto.serial_id() << std::endl; + serial_id = field_proto.serial_id(); + length = field_proto.length(); + width = field_proto.width(); + goal_depth = field_proto.goal_depth(); + goal_width = field_proto.goal_width(); + penalty_area_depth = field_proto.penalty_area_depth(); + penalty_area_width = field_proto.penalty_area_width(); + boundary_width = field_proto.boundary_width(); + goal_center_to_penalty_mark = field_proto.goal_center_to_penalty_mark(); +} + +FieldMessage::FieldMessage(const protocols::perception::Field& field_proto) { + FieldMessage::fromProto(field_proto); +} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.h b/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.h new file mode 100644 index 00000000..cf027601 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/field/field_message.h @@ -0,0 +1,42 @@ +#ifndef DECISION_PROCESSING_MESSAGES_PERCEPTION_FIELD_FIELD_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_PERCEPTION_FIELD_FIELD_MESSAGE_H + +#include +#include +#include +#include + +namespace decision { + +class FieldMessage : public robocin::IProtoConvertible { + public: + std::optional serial_id; + std::optional length; + std::optional width; + std::optional goal_depth; + std::optional goal_width; + std::optional penalty_area_depth; + std::optional penalty_area_width; + std::optional boundary_width; + std::optional goal_center_to_penalty_mark; + + explicit FieldMessage(std::optional serial_id = std::nullopt, + std::optional length = std::nullopt, + std::optional width = std::nullopt, + std::optional goal_depth = std::nullopt, + std::optional goal_width = std::nullopt, + std::optional penalty_area_depth = std::nullopt, + std::optional penalty_area_width = std::nullopt, + std::optional boundary_width = std::nullopt, + std::optional goal_center_to_penalty_mark = std::nullopt); + + explicit FieldMessage(const protocols::perception::Field& field_proto); + + [[nodiscard]] protocols::perception::Field toProto() const override { + return protocols::perception::Field{}; + } + + void fromProto(const protocols::perception::Field& field_proto) override; +}; +} // namespace decision +#endif // DECISION_PROCESSING_MESSAGES_PERCEPTION_FIELD_FIELD_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.cpp new file mode 100644 index 00000000..55763140 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.cpp @@ -0,0 +1,91 @@ +#include "decision/processing/messages/perception/robot/robot_message.h" + +#include +#include + +namespace decision { +namespace { +namespace rc { +using protocols::perception::Robot; +} +} // namespace + +///////////////////////////////////////////// Wheel ///////////////////////////////////////////// +Wheel::Wheel(std::optional wheel_id, std::optional motor_speed) : + wheel_id(wheel_id), + motor_speed(motor_speed) {}; + +//////////////////////////////////////////// Feedback /////////////////////////////////////////// + +FeedbackMessage::FeedbackMessage(std::optional dribbler_ball_contact, + std::optional kick_charge_percentage, + std::optional battery_percentage, + std::vector wheels) : + dribbler_ball_contact(dribbler_ball_contact), + kick_charge_percentage(kick_charge_percentage), + battery_percentage(battery_percentage), + wheels(std::move(wheels)) {}; + +FeedbackMessage::FeedbackMessage(const rc::Robot::Feedback& feedback_proto) { + fromProto(feedback_proto); +}; + +void FeedbackMessage::fromProto(const rc::Robot::Feedback& feedback_proto) { + dribbler_ball_contact = feedback_proto.dribbler_ball_contact(); + kick_charge_percentage = feedback_proto.kick_charge_percentage(); + battery_percentage = feedback_proto.battery_percentage(); +}; + +///////////////////////////////////////////// Robot ///////////////////////////////////////////// + +RobotMessage::RobotMessage(std::optional confidence, + std::optional robot_id, + std::optional position, + std::optional angle, + std::optional velocity, + std::optional angular_velocity, + std::optional radius, + std::optional height, + std::optional dribbler_width, + std::optional feedback) : + confidence(confidence), + robot_id(std::move(robot_id)), + position(position), + angle(angle), + velocity(velocity), + angular_velocity(angular_velocity), + radius(radius), + height(height), + dribbler_width(dribbler_width), + feedback(std::move(feedback)) {}; + +void RobotMessage::fromProto(const rc::Robot& robot_proto) { + confidence = robot_proto.confidence(); + position = robocin::Point2Df(robot_proto.position().x(), robot_proto.position().y()); + angle = robot_proto.angle(); + velocity = robocin::Point2Df(robot_proto.velocity().x(), robot_proto.velocity().y()); + angular_velocity = robot_proto.angular_velocity(); + radius = robot_proto.radius(); + height = robot_proto.height(); + dribbler_width = robot_proto.dribbler_width(); + + if (robot_proto.has_robot_id()) { + if (!robot_id.has_value()) { + robot_id.emplace(); + } + + robot_id->fromProto(robot_proto.robot_id()); + } + + if (robot_proto.has_feedback()) { + if (!feedback.has_value()) { + feedback.emplace(); + } + + feedback->fromProto(robot_proto.feedback()); + } +}; + +RobotMessage::RobotMessage(const rc::Robot& robot_proto) { RobotMessage::fromProto(robot_proto); } + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.h b/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.h new file mode 100644 index 00000000..7903ad9a --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/perception/robot/robot_message.h @@ -0,0 +1,85 @@ +#ifndef DECISION_PROCESSING_MESSAGES_PERCEPTION_ROBOT_ROBOT_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_PERCEPTION_ROBOT_ROBOT_MESSAGE_H + +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include +#include +#include +#include +#include + +namespace decision { + +class Wheel : public robocin::IProtoConvertible { + public: + std::optional wheel_id; + std::optional motor_speed; + + explicit Wheel(std::optional wheel_id = std::nullopt, + std::optional motor_speed = std::nullopt); + + [[nodiscard]] protocols::perception::Robot::Feedback::Wheel toProto() const override { + return protocols::perception::Robot::Feedback::Wheel{}; + } + + void fromProto(const protocols::perception::Robot::Feedback::Wheel& wheel_proto) override {}; +}; + +class FeedbackMessage : public robocin::IProtoConvertible { + public: + std::optional dribbler_ball_contact; + std::optional kick_charge_percentage; + std::optional battery_percentage; + + std::vector wheels; + + explicit FeedbackMessage(std::optional dribbler_ball_contact = std::nullopt, + std::optional kick_charge_percentage = std::nullopt, + std::optional battery_percentage = std::nullopt, + std::vector wheels = {}); + + explicit FeedbackMessage(const protocols::perception::Robot::Feedback& feedback_proto); + + [[nodiscard]] protocols::perception::Robot::Feedback toProto() const override { + return protocols::perception::Robot::Feedback{}; + } + + void fromProto(const protocols::perception::Robot::Feedback& feedback_proto) override; +}; + +class RobotMessage : public robocin::IProtoConvertible { + public: + std::optional confidence; + std::optional robot_id; + std::optional position; + std::optional angle; + std::optional velocity; + std::optional angular_velocity; + std::optional radius; + std::optional height; + std::optional dribbler_width; + std::optional feedback; + + explicit RobotMessage(std::optional confidence = std::nullopt, + std::optional robot_id = std::nullopt, + std::optional position = std::nullopt, + std::optional angle = std::nullopt, + std::optional velocity = std::nullopt, + std::optional angular_velocity = std::nullopt, + std::optional radius = std::nullopt, + std::optional height = std::nullopt, + std::optional dribbler_width = std::nullopt, + std::optional feedback = std::nullopt); + + explicit RobotMessage(const protocols::perception::Robot& robot_proto); + + [[nodiscard]] protocols::perception::Robot toProto() const override { + return protocols::perception::Robot{}; + } + + void fromProto(const protocols::perception::Robot& robot_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_PERCEPTION_ROBOT_ROBOT_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.cpp new file mode 100644 index 00000000..fca38b2b --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.cpp @@ -0,0 +1,5 @@ +#include "decision/processing/messages/planning/planning_message.h" + +namespace decision { +PlanningMessage::PlanningMessage() = default; +} diff --git a/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.h b/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.h new file mode 100644 index 00000000..818ff704 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/planning/planning_message.h @@ -0,0 +1,23 @@ +#ifndef DECISION_PROCESSING_MESSAGES_PLANNING_PLANNING_MESSAGE_H +#define DECISION_PROCESSING_MESSAGES_PLANNING_PLANNING_MESSAGE_H + +#include +#include +#include +#include + +namespace decision { +class PlanningMessage : public robocin::IProtoConvertible { + public: + PlanningMessage(); + + [[nodiscard]] protocols::behavior::Planning toProto() const override { + return protocols::behavior::Planning{}; + }; + + void fromProto(const protocols::behavior::Planning& planning_proto) override; +}; + +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_PLANNING_PLANNING_MESSAGE_H diff --git a/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.cpp b/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.cpp new file mode 100644 index 00000000..7fda9ff4 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.cpp @@ -0,0 +1,264 @@ +#include "decision/processing/messages/referee/game_status_message.h" + +#include +#include +#include +#include +#include + +namespace decision { +namespace { +namespace rc { +using ::protocols::common::GameStage; +using ::protocols::common::MatchType; +} // namespace rc +} // namespace + +/////////////////////////////////// GameEventsProposalMessage /////////////////////////////////// +GameEventsProposalMessage::GameEventsProposalMessage(std::optional proposal_id, + std::vector game_events, + std::optional was_accepted) : + proposal_id(std::move(proposal_id)), + was_accepted(was_accepted) {}; + +void GameEventsProposalMessage::fromProto( + const protocols::referee::GameStatus::GameEventsProposal& game_events_proposal_proto) { + proposal_id = game_events_proposal_proto.proposal_id(); + was_accepted = game_events_proposal_proto.was_accepted(); + + for (const auto& game_event : game_events_proposal_proto.game_events()) { + game_events.emplace_back(game_event); + } +} + +GameEventsProposalMessage::GameEventsProposalMessage( + const protocols::referee::GameStatus::GameEventsProposal& game_events_proposal_proto) { + GameEventsProposalMessage::fromProto(game_events_proposal_proto); +} + +/////////////////////////////////// TeamMessage /////////////////////////////////// + +TeamMessage::TeamMessage(std::optional name, + std::optional score, + std::vector robot_ids, + std::optional goalkeeper_id, + std::optional yellow_cards, + std::vector time_to_expire_active_yellow_cards, + std::optional red_cards, + std::optional timeouts_left, + std::optional total_timeout_time_left, + std::optional total_fouls_committed, + std::optional consecutive_ball_placement_failures, + std::optional is_ball_placement_enabled, + std::optional has_ball_placement_failures_reached_maximum, + std::optional maximum_allowed_robots, + std::optional is_robot_substitution_requested, + std::optional is_robot_substitution_allowed, + std::optional robot_substitutions_left, + std::optional robot_substitution_time_left) : + name(std::move(name)), + score(score), + robot_ids(std::move(robot_ids)), + goalkeeper_id(std::move(goalkeeper_id)), + yellow_cards(yellow_cards), + time_to_expire_active_yellow_cards(std::move(time_to_expire_active_yellow_cards)), + red_cards(red_cards), + timeouts_left(timeouts_left), + total_timeout_time_left(std::move(total_timeout_time_left)), + total_fouls_committed(total_fouls_committed), + consecutive_ball_placement_failures(consecutive_ball_placement_failures), + is_ball_placement_enabled(is_ball_placement_enabled), + has_ball_placement_failures_reached_maximum(has_ball_placement_failures_reached_maximum), + maximum_allowed_robots(maximum_allowed_robots), + is_robot_substitution_requested(is_robot_substitution_requested), + is_robot_substitution_allowed(is_robot_substitution_allowed), + robot_substitutions_left(robot_substitutions_left), + robot_substitution_time_left(robot_substitution_time_left) {} + +TeamMessage::TeamMessage(const protocols::referee::GameStatus::Team& team_proto) { + TeamMessage::fromProto(team_proto); +} + +void TeamMessage::fromProto(const protocols::referee::GameStatus::Team& team_proto) { + name = team_proto.name(); + score = team_proto.score(); + + for (const auto& robot_id : team_proto.robot_ids()) { + robot_ids.emplace_back(robot_id); + } + + if (team_proto.has_goalkeeper_id()) { + if (!goalkeeper_id.has_value()) { + goalkeeper_id.emplace(); + } + + goalkeeper_id->fromProto(team_proto.goalkeeper_id()); + } + + yellow_cards = team_proto.yellow_cards(); + + // time_to_expire_active_yellow_cards + for (const auto& time_to_expire_active_yellow_card : + team_proto.time_to_expire_active_yellow_cards()) { + // todo(fnap): discuss how to use duration + time_to_expire_active_yellow_cards.emplace_back(time_to_expire_active_yellow_card.seconds()); + } + + red_cards = team_proto.red_cards(); + timeouts_left = team_proto.timeouts_left(); + total_timeout_time_left = team_proto.total_timeout_time_left(); + total_fouls_committed = team_proto.total_fouls_committed(); + consecutive_ball_placement_failures = team_proto.consecutive_ball_placement_failures(); + is_ball_placement_enabled = team_proto.is_ball_placement_enabled(); + has_ball_placement_failures_reached_maximum + = team_proto.has_ball_placement_failures_reached_maximum(); + maximum_allowed_robots = team_proto.maximum_allowed_robots(); + is_robot_substitution_requested = team_proto.is_robot_substitution_requested(); + is_robot_substitution_allowed = team_proto.is_robot_substitution_allowed(); + robot_substitutions_left = team_proto.robot_substitutions_left(); + robot_substitution_time_left = team_proto.robot_substitution_time_left() + .seconds(); // todo(fnap): discuss how to use duration +} + +/////////////////////////////////// GameStatusMessage /////////////////////////////////// + +GameStatusMessage::GameStatusMessage(std::optional source_id, + std::optional description, + std::optional timestamp, + std::optional match_type, + std::optional home_team, + std::optional away_team, + std::optional game_stage, + std::optional game_stage_time_left, + std::optional total_commands_issued, + std::optional command_issued_timestamp, + std::optional command, + std::optional next_command, + std::vector game_events, + std::vector game_events_proposals) : + source_id(std::move(source_id)), + description(std::move(description)), + timestamp(timestamp), + match_type(match_type), + home_team(std::move(home_team)), + away_team(std::move(away_team)), + game_stage(game_stage), + game_stage_time_left(game_stage_time_left), + total_commands_issued(total_commands_issued), + command_issued_timestamp(command_issued_timestamp), + command(std::move(command)), + next_command(std::move(next_command)), + game_events(std::move(game_events)), + game_events_proposals(std::move(game_events_proposals)) {} + +GameStatusMessage::GameStatusMessage(const protocols::referee::GameStatus& game_status_proto) { + GameStatusMessage::fromProto(game_status_proto); +} + +GameStatusMessage::MatchType getMatchType(rc::MatchType match_type_proto) { + switch (match_type_proto) { + case protocols::common::MATCH_TYPE_GROUP_PHASE: + return GameStatusMessage::MatchType::MATCH_TYPE_GROUP_PHASE; + case protocols::common::MATCH_TYPE_ELIMINATION_PHASE: + return GameStatusMessage::MatchType::MATCH_TYPE_ELIMINATION_PHASE; + case protocols::common::MATCH_TYPE_FRIENDLY: + return GameStatusMessage::MatchType::MATCH_TYPE_FRIENDLY; + default: return GameStatusMessage::MatchType::MATCH_TYPE_UNSPECIFIED; + } +} + +GameStatusMessage::GameStage getGameStage(rc::GameStage game_stage_proto) { + switch (game_stage_proto) { + case protocols::common::GameStage::GAME_STAGE_NORMAL_FIRST_HALF_PRE: + return GameStatusMessage::GAME_STAGE_NORMAL_FIRST_HALF_PRE; + case protocols::common::GameStage::GAME_STAGE_NORMAL_FIRST_HALF: + return GameStatusMessage::GAME_STAGE_NORMAL_FIRST_HALF; + case protocols::common::GameStage::GAME_STAGE_NORMAL_HALF_TIME: + return GameStatusMessage::GAME_STAGE_NORMAL_HALF_TIME; + case protocols::common::GameStage::GAME_STAGE_NORMAL_SECOND_HALF_PRE: + return GameStatusMessage::GAME_STAGE_NORMAL_SECOND_HALF_PRE; + case protocols::common::GameStage::GAME_STAGE_NORMAL_SECOND_HALF: + return GameStatusMessage::GAME_STAGE_NORMAL_SECOND_HALF; + case protocols::common::GameStage::GAME_STAGE_EXTRA_TIME_BREAK: + return GameStatusMessage::GAME_STAGE_EXTRA_TIME_BREAK; + case protocols::common::GameStage::GAME_STAGE_EXTRA_FIRST_HALF_PRE: + return GameStatusMessage::GAME_STAGE_EXTRA_FIRST_HALF_PRE; + case protocols::common::GameStage::GAME_STAGE_EXTRA_FIRST_HALF: + return GameStatusMessage::GAME_STAGE_EXTRA_FIRST_HALF; + case protocols::common::GameStage::GAME_STAGE_EXTRA_HALF_TIME: + return GameStatusMessage::GAME_STAGE_EXTRA_HALF_TIME; + case protocols::common::GameStage::GAME_STAGE_EXTRA_SECOND_HALF_PRE: + return GameStatusMessage::GAME_STAGE_EXTRA_SECOND_HALF_PRE; + case protocols::common::GameStage::GAME_STAGE_EXTRA_SECOND_HALF: + return GameStatusMessage::GAME_STAGE_EXTRA_SECOND_HALF; + case protocols::common::GameStage::GAME_STAGE_PENALTY_SHOOTOUT_BREAK: + return GameStatusMessage::GAME_STAGE_PENALTY_SHOOTOUT_BREAK; + case protocols::common::GameStage::GAME_STAGE_PENALTY_SHOOTOUT: + return GameStatusMessage::GAME_STAGE_PENALTY_SHOOTOUT; + case protocols::common::GameStage::GAME_STAGE_POST_GAME: + return GameStatusMessage::GAME_STAGE_POST_GAME; + default: return GameStatusMessage::GameStage::GAME_STAGE_UNSPECIFIED; + } +} + +void GameStatusMessage::fromProto(const protocols::referee::GameStatus& game_status_proto) { + source_id = game_status_proto.source_id(); + description = game_status_proto.description(); + timestamp = game_status_proto.timestamp().seconds(); // todo(fnap): discuss how to use this field + match_type = getMatchType(game_status_proto.match_type()); + + if (game_status_proto.has_home_team()) { + if (!home_team.has_value()) { + home_team.emplace(); + } + + home_team->fromProto(game_status_proto.home_team()); + } + + if (game_status_proto.has_away_team()) { + if (!away_team.has_value()) { + away_team.emplace(); + } + + away_team->fromProto(game_status_proto.away_team()); + } + + game_stage = getGameStage(game_status_proto.game_stage()); + + if (game_status_proto.has_game_stage_time_left()) { + game_stage_time_left = game_status_proto.game_stage_time_left() + .seconds(); // todo(fnap): discuss how to use duration + } + + total_commands_issued = game_status_proto.total_commands_issued(); + command_issued_timestamp = game_status_proto.command_issued_timestamp() + .seconds(); // todo(fnap): discuss how to use timestamp + + if (game_status_proto.has_command()) { + if (!command.has_value()) { + command.emplace(); + } + + // implement, always returns empty command + command->fromProto(game_status_proto.command()); + } + + if (game_status_proto.has_next_command()) { + if (!next_command.has_value()) { + next_command.emplace(); + } + + // implement + next_command->fromProto(game_status_proto.next_command()); + } + + for (const auto& game_event : game_status_proto.game_events()) { + game_events.emplace_back(game_event); + } + + for (const auto& game_events_proposal : game_status_proto.game_events_proposals()) { + game_events_proposals.emplace_back(game_events_proposal); + } +} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.h b/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.h new file mode 100644 index 00000000..4bb7e5b6 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/messages/referee/game_status_message.h @@ -0,0 +1,170 @@ +#ifndef DECISION_PROCESSING_MESSAGES_REFEREE_GAME_STATUS_H +#define DECISION_PROCESSING_MESSAGES_REFEREE_GAME_STATUS_H + +#include "decision/processing/messages/common/game_command/game_command_message.h" +#include "decision/processing/messages/common/game_event/game_event_message.h" +#include "decision/processing/messages/common/robot_id/robot_id_message.h" + +#include +#include +#include +#include +#include + +namespace decision { + +class GameEventsProposalMessage + : public robocin::IProtoConvertible { + public: + std::optional proposal_id; + std::vector game_events; + std::optional was_accepted; + + explicit GameEventsProposalMessage(std::optional proposal_id, + std::vector game_events, + std::optional was_accepted); + + explicit GameEventsProposalMessage( + const protocols::referee::GameStatus::GameEventsProposal& game_events_proposal_proto); + + [[nodiscard]] protocols::referee::GameStatus::GameEventsProposal toProto() const override { + return protocols::referee::GameStatus::GameEventsProposal{}; + }; + + void fromProto(const protocols::referee::GameStatus::GameEventsProposal& + game_events_proposal_proto) override; +}; + +class TeamMessage : public robocin::IProtoConvertible { + public: + std::optional name; + std::optional score; + + std::vector robot_ids; + std::optional goalkeeper_id; + + std::optional yellow_cards; + std::vector time_to_expire_active_yellow_cards; + + std::optional red_cards; + + std::optional timeouts_left; + std::optional total_timeout_time_left; + + std::optional total_fouls_committed; + + std::optional consecutive_ball_placement_failures; + std::optional is_ball_placement_enabled; + std::optional has_ball_placement_failures_reached_maximum; + + std::optional maximum_allowed_robots; + + std::optional is_robot_substitution_requested; + std::optional is_robot_substitution_allowed; + + std::optional robot_substitutions_left; + std::optional + robot_substitution_time_left; // todo(fnap): discuss what to do with proto duration + + explicit TeamMessage(std::optional name = std::nullopt, + std::optional score = std::nullopt, + std::vector robot_ids = {}, + std::optional goalkeeper_id = std::nullopt, + std::optional yellow_cards = std::nullopt, + std::vector time_to_expire_active_yellow_cards = {}, + std::optional red_cards = std::nullopt, + std::optional timeouts_left = std::nullopt, + std::optional total_timeout_time_left + = std::nullopt, + std::optional total_fouls_committed = std::nullopt, + std::optional consecutive_ball_placement_failures = std::nullopt, + std::optional is_ball_placement_enabled = std::nullopt, + std::optional has_ball_placement_failures_reached_maximum + = std::nullopt, + std::optional maximum_allowed_robots = std::nullopt, + std::optional is_robot_substitution_requested = std::nullopt, + std::optional is_robot_substitution_allowed = std::nullopt, + std::optional robot_substitutions_left = std::nullopt, + std::optional robot_substitution_time_left = std::nullopt); + + explicit TeamMessage(const protocols::referee::GameStatus::Team& team_proto); + + [[nodiscard]] protocols::referee::GameStatus::Team toProto() const override { + return protocols::referee::GameStatus::Team{}; + }; + + void fromProto(const protocols::referee::GameStatus::Team& team_proto) override; +}; + +class GameStatusMessage : public robocin::IProtoConvertible { + public: + enum GameStage { + GAME_STAGE_UNSPECIFIED = 0, + GAME_STAGE_NORMAL_FIRST_HALF_PRE = 1, + GAME_STAGE_NORMAL_FIRST_HALF = 2, + GAME_STAGE_NORMAL_HALF_TIME = 3, + GAME_STAGE_NORMAL_SECOND_HALF_PRE = 4, + GAME_STAGE_NORMAL_SECOND_HALF = 5, + GAME_STAGE_EXTRA_TIME_BREAK = 6, + GAME_STAGE_EXTRA_FIRST_HALF_PRE = 7, + GAME_STAGE_EXTRA_FIRST_HALF = 8, + GAME_STAGE_EXTRA_HALF_TIME = 9, + GAME_STAGE_EXTRA_SECOND_HALF_PRE = 10, + GAME_STAGE_EXTRA_SECOND_HALF = 11, + GAME_STAGE_PENALTY_SHOOTOUT_BREAK = 12, + GAME_STAGE_PENALTY_SHOOTOUT = 13, + GAME_STAGE_POST_GAME = 14 + }; + + enum MatchType { + MATCH_TYPE_UNSPECIFIED = 0, + MATCH_TYPE_GROUP_PHASE = 1, + MATCH_TYPE_ELIMINATION_PHASE = 2, + MATCH_TYPE_FRIENDLY = 3, + }; + + std::optional source_id; + std::optional description; + std::optional timestamp; // todo(fnap): discuss how to use timestamp + std::optional match_type; + + std::optional home_team; + std::optional away_team; + + std::optional game_stage; + std::optional game_stage_time_left; // todo(fnap): discuss how to use duration + + std::optional total_commands_issued; + std::optional command_issued_timestamp; // todo(fnap): discuss how to use timestamp + std::optional command; + std::optional next_command; + + std::vector game_events; + std::vector game_events_proposals; + + explicit GameStatusMessage(std::optional source_id = std::nullopt, + std::optional description = std::nullopt, + std::optional timestamp = std::nullopt, + std::optional match_type = std::nullopt, + std::optional home_team = std::nullopt, + std::optional away_team = std::nullopt, + std::optional game_stage = std::nullopt, + std::optional game_stage_time_left = std::nullopt, + std::optional total_commands_issued = std::nullopt, + std::optional command_issued_timestamp = std::nullopt, + std::optional command = std::nullopt, + std::optional next_command = std::nullopt, + std::vector game_events = {}, + std::vector game_events_proposals = {}); + + explicit GameStatusMessage(const protocols::referee::GameStatus& game_status_proto); + + [[nodiscard]] protocols::referee::GameStatus toProto() const override { + return protocols::referee::GameStatus{}; + }; + + void fromProto(const protocols::referee::GameStatus& game_status_proto) override; +}; +} // namespace decision + +#endif // DECISION_PROCESSING_MESSAGES_REFEREE_GAME_STATUS_H diff --git a/docker-compose.dev.yaml b/docker-compose.dev.yaml index 6187e4c3..ede086f0 100644 --- a/docker-compose.dev.yaml +++ b/docker-compose.dev.yaml @@ -209,6 +209,7 @@ services: dockerfile: navigation-ms/navigation-luffy/navigation-luffy.Dockerfile volumes: - .ssl-core:/tmp/.ssl-core + attach: false decision-ms: container_name: decision-ms @@ -220,6 +221,7 @@ services: depends_on: - perception-ms - referee-ms + attach: true behavior-ms: container_name: behavior-ms @@ -228,6 +230,7 @@ services: dockerfile: behavior-ms/behavior-bruxo/behavior-bruxo.Dockerfile volumes: - .ssl-core:/tmp/.ssl-core + attach: false communication-ms: container_name: communication-ms @@ -239,6 +242,7 @@ services: depends_on: - gateway network_mode: "host" + attach: false volumes: From c06ede0f600ce185da348a8540a67576fff54002 Mon Sep 17 00:00:00 2001 From: Felipe Nunes Date: Thu, 31 Oct 2024 12:27:28 -0300 Subject: [PATCH 7/7] [decision-ms] Add decision world (#76) Adds decision world with allies, enemies, ball and game status (referee) information. --------- Co-authored-by: ersaraujo --- .../processing/entities/CMakeLists.txt | 5 +++ .../decision/processing/entities/world.cpp | 33 +++++++++++++++++ .../decision/processing/entities/world.h | 37 +++++++++++++++++++ 3 files changed, 75 insertions(+) create mode 100644 decision-ms/decision-guardiola/decision/processing/entities/CMakeLists.txt create mode 100644 decision-ms/decision-guardiola/decision/processing/entities/world.cpp create mode 100644 decision-ms/decision-guardiola/decision/processing/entities/world.h diff --git a/decision-ms/decision-guardiola/decision/processing/entities/CMakeLists.txt b/decision-ms/decision-guardiola/decision/processing/entities/CMakeLists.txt new file mode 100644 index 00000000..f6622e4e --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/entities/CMakeLists.txt @@ -0,0 +1,5 @@ +robocin_cpp_library( + NAME entities + HDRS world.h + SRCS world.cpp +) diff --git a/decision-ms/decision-guardiola/decision/processing/entities/world.cpp b/decision-ms/decision-guardiola/decision/processing/entities/world.cpp new file mode 100644 index 00000000..30206cce --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/entities/world.cpp @@ -0,0 +1,33 @@ +#include "decision/processing/entities/world.h" + +#include "decision/processing/messages/referee/game_status_message.h" + +namespace decision { + +void World::update(std::optional>& allies, + std::optional>& enemies, + std::optional& ball, + std::optional& field, + std::optional& game_status) { + if (allies.has_value()) { + this->allies = allies.value(); + } + + if (enemies.has_value()) { + this->enemies = enemies.value(); + } + + if (ball.has_value()) { + this->ball = std::move(ball.value()); + } + + if (field.has_value()) { + this->field = std::move(field.value()); + } + + if (game_status.has_value()) { + this->game_status = std::move(game_status.value()); + } +} + +} // namespace decision diff --git a/decision-ms/decision-guardiola/decision/processing/entities/world.h b/decision-ms/decision-guardiola/decision/processing/entities/world.h new file mode 100644 index 00000000..c8c421b2 --- /dev/null +++ b/decision-ms/decision-guardiola/decision/processing/entities/world.h @@ -0,0 +1,37 @@ +#ifndef DECISION_PROCESSING_ENTITIES_WORLD_H +#define DECISION_PROCESSING_ENTITIES_WORLD_H + +#include "decision/processing/messages/perception/ball/ball_message.h" +#include "decision/processing/messages/perception/field/field_message.h" +#include "decision/processing/messages/perception/robot/robot_message.h" +#include "decision/processing/messages/referee/game_status_message.h" + +namespace decision { + +class World { + public: + World() = default; + + World(const World&) = delete; + World& operator=(const World&) = delete; + World(World&&) noexcept = default; + World& operator=(World&&) noexcept = default; + + virtual ~World() = default; + + std::span allies; + std::span enemies; + BallMessage ball; + FieldMessage field; + GameStatusMessage game_status; // referee + + void update(std::optional>& allies, + std::optional>& enemies, + std::optional& ball, + std::optional& field, + std::optional& game_status); +}; + +} // namespace decision + +#endif // DECISION_PROCESSING_ENTITIES_WORLD_H