diff --git a/navigation-ms/navigation-luffy/navigation/controller/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/controller/CMakeLists.txt index f295b735..c56a318e 100644 --- a/navigation-ms/navigation-luffy/navigation/controller/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/controller/CMakeLists.txt @@ -13,4 +13,5 @@ robocin_cpp_library( messaging_receiver messaging_sender processing + messages ) diff --git a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp index 91c5487f..0469d82e 100644 --- a/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp +++ b/navigation-ms/navigation-luffy/navigation/controller/consumer_controller.cpp @@ -49,9 +49,10 @@ void ConsumerController::exec(std::span payloads) { return; } - if (std::optional navigation_msg = navigation_processor_->process(payloads); + if (std::optional<::protocols::navigation::Navigation> navigation_msg + = navigation_processor_->process(payloads); navigation_msg != std::nullopt) { - message_sender_->send(navigation_msg->toProto()); + message_sender_->send(*navigation_msg); } } diff --git a/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt index c6ab83d2..5c332166 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/processing/CMakeLists.txt @@ -5,4 +5,5 @@ robocin_cpp_library( DEPS navigation_entities navigation_utils motion_parser + messages ) diff --git a/navigation-ms/navigation-luffy/navigation/processing/messages/CMakeLists.txt b/navigation-ms/navigation-luffy/navigation/processing/messages/CMakeLists.txt index b0fdaffb..79c797b3 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/messages/CMakeLists.txt +++ b/navigation-ms/navigation-luffy/navigation/processing/messages/CMakeLists.txt @@ -3,7 +3,9 @@ robocin_cpp_library( HDRS behavior/behavior_message.h common/peripheral_actuation/peripheral_actuation.h common/robot_id/robot_id_message.h + common/robot_dribbler/robot_dribbler_message.h common/robot_kick/kick_command.h + common/robot_pose/robot_pose_message.h common/game_command/game_command_message.h common/game_event/game_event_message.h decision/decision_message.h @@ -14,10 +16,13 @@ robocin_cpp_library( perception/field/field_message.h perception/detection/detection_message.h referee/game_status_message.h + navigation/navigation_message.h SRCS behavior/behavior_message.cpp common/peripheral_actuation/peripheral_actuation.cpp common/robot_id/robot_id_message.cpp + common/robot_dribbler/robot_dribbler_message.cpp common/robot_kick/kick_command.cpp + common/robot_pose/robot_pose_message.cpp common/game_command/game_command_message.cpp common/game_event/game_event_message.cpp decision/decision_message.cpp @@ -28,6 +33,7 @@ robocin_cpp_library( perception/field/field_message.cpp perception/detection/detection_message.cpp referee/game_status_message.cpp + navigation/navigation_message.cpp DEPS common::utility common::output protocols::protobufs diff --git a/navigation-ms/navigation-luffy/navigation/processing/messages/common/robot_pose/robot_pose_message.h b/navigation-ms/navigation-luffy/navigation/processing/messages/common/robot_pose/robot_pose_message.h index ee3af0c4..5ec0a12d 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/messages/common/robot_pose/robot_pose_message.h +++ b/navigation-ms/navigation-luffy/navigation/processing/messages/common/robot_pose/robot_pose_message.h @@ -10,11 +10,13 @@ namespace navigation { class RobotPoseMessage : public robocin::IProtoConvertible { public: - explicit RobotPoseMessage(robocin::Point2Df position, float orientation); - explicit RobotPoseMessage(const protocols::common::RobotPose& robot_pose_proto); robocin::Point2Df position; float orientation; + RobotPoseMessage(robocin::Point2Df position = robocin::Point2Df{0, 0}, float orientation = 0.0F); + + explicit RobotPoseMessage(const protocols::common::RobotPose& robot_pose_proto); + [[nodiscard]] protocols::common::RobotPose toProto() const override; void fromProto(const protocols::common::RobotPose& robot_pose_proto) override; diff --git a/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.cpp b/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.cpp index 320418cc..76c1bb99 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.cpp @@ -1,21 +1,14 @@ #include "navigation/processing/messages/navigation/navigation_message.h" -#include "navigation_message.h" - -#include -#include - namespace navigation { -void NavigationMessage::fromProto( - const protocols::navigation::Navigation& unification_behavior_proto) { - for (const auto& output_proto : unification_behavior_proto.output()) { - output.emplace_back(output_proto); +void NavigationMessage::fromProto(const protocols::navigation::Navigation& navigation_proto) { + for (const auto& output_proto : navigation_proto.output()) { + output->emplace_back(output_proto); } } -NavigationMessage::NavigationMessage( - const protocols::navigation::Navigation& unification_behavior_proto) { - NavigationMessage::fromProto(unification_behavior_proto); +NavigationMessage::NavigationMessage(const protocols::navigation::Navigation& navigation_proto) { + NavigationMessage::fromProto(navigation_proto); } void NavigationOutputMessage::fromProto(const protocols::navigation::Output& output_proto) { @@ -64,7 +57,7 @@ protocols::navigation::Output NavigationOutputMessage::toProto() const { return protocols::navigation::Output{}; }; -NavigationMessage::NavigationMessage(std::vector output) : +NavigationMessage::NavigationMessage(std::optional> output) : output(std::move(output)) {} protocols::navigation::Navigation NavigationMessage::toProto() const { diff --git a/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.h b/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.h index d7d74173..36df89a5 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.h +++ b/navigation-ms/navigation-luffy/navigation/processing/messages/navigation/navigation_message.h @@ -7,8 +7,6 @@ #include #include -#include -#include #include #include #include @@ -51,15 +49,16 @@ class NavigationOutputMessage : public robocin::IProtoConvertible { public: - explicit NavigationMessage(std::vector output = {}); + explicit NavigationMessage(std::optional> output + = std::nullopt); - explicit NavigationMessage(const protocols::navigation::Navigation& unification_behavior_proto); + explicit NavigationMessage(const protocols::navigation::Navigation& navigation_proto); - std::vector output; + std::optional> output; [[nodiscard]] protocols::navigation::Navigation toProto() const override; - void fromProto(const protocols::navigation::Navigation& unification_behavior_proto) override; + void fromProto(const protocols::navigation::Navigation& navigation_proto) 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 955c932e..c1e540ed 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp +++ b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp @@ -11,6 +11,7 @@ #include #include +#include #include #include #include @@ -48,16 +49,15 @@ std::vector gameStatusFromPayloads(std::span payl NavigationProcessor::NavigationProcessor(std::unique_ptr motion_parser) : motion_parser_(std::move(motion_parser)) {} -std::optional NavigationProcessor::process(std::span payloads) { - NavigationMessage navigation_output; - +std::optional<::protocols::navigation::Navigation> +NavigationProcessor::process(std::span payloads) { if (std::vector behaviors = behaviorFromPayloads(payloads); !behaviors.empty()) { - last_behavior_ = BehaviorUnificationMessage(behaviors.back()); + last_behavior_ = behaviors.back(); } if (std::vector game_statuses = gameStatusFromPayloads(payloads); !game_statuses.empty()) { - last_game_status_ = GameStatusMessage(game_statuses.back()); + last_game_status_ = game_statuses.back(); } if (!last_behavior_ or !last_game_status_) { @@ -69,55 +69,13 @@ std::optional NavigationProcessor::process(std::spanbehavior_outputs) { - NavigationOutputMessage output; - RobotMessage ally; - - for (auto& robot : last_detection.robots) { - if (robot.robot_id->number == behavior.robot_id->number) { - ally = RobotMessage(robot); - break; - } - } - if (ally.robot_id) { - output.robot_id = ally.robot_id; - - if (behavior.motion) { - RobotMove move; - if (behavior.motion->go_to_point) { - move = motion_parser_->fromGoToPoint(behavior.motion->go_to_point.value(), ally); - } else if (behavior.motion->rotate_in_point) { - move = motion_parser_->fromRotateInPoint(behavior.motion->rotate_in_point.value(), ally); - } else if (behavior.motion->rotate_on_self) { - move = motion_parser_->fromRotateOnSelf(behavior.motion->rotate_on_self.value(), ally); - } else { - // PROCESSAMENTO DO GO_TO_POINT_WITH_TRAJECTORY - } - - output.left_velocity = move.velocity().y; - output.forward_velocity = move.velocity().x; - output.angular_velocity = move.angularVelocity(); - - if (behavior.motion->peripheral_actuation) { - output.peripheral_actuation = behavior.motion->peripheral_actuation; - } - - // TODO: Add other fields to output - - } else if (behavior.planning) { - // PROCESSAMENTO DO PLANNING - } else { - // PROCESSAMENTO DO NAVIGATION - } - } - - navigation_output.output.emplace_back(output); - } + ::protocols::navigation::Navigation navigation_proto; + //////////////////////////////////////////////////////////////////////////// - return navigation_output; + return navigation_proto; } } // namespace navigation diff --git a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h index 89b7a1f3..78674c36 100644 --- a/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h +++ b/navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h @@ -11,7 +11,9 @@ #include #include +#include #include +#include #include namespace navigation { @@ -27,19 +29,21 @@ class INavigationProcessor { virtual ~INavigationProcessor() = default; - virtual std::optional process(std::span payloads) = 0; + virtual std::optional<::protocols::navigation::Navigation> + process(std::span payloads) = 0; }; class NavigationProcessor : public INavigationProcessor { public: explicit NavigationProcessor(std::unique_ptr motion_parser); - std::optional process(std::span payloads) override; + std::optional<::protocols::navigation::Navigation> + process(std::span payloads) override; private: std::unique_ptr motion_parser_; - std::optional last_behavior_; - std::optional last_game_status_; + std::optional<::protocols::behavior::unification::Behavior> last_behavior_; + std::optional<::protocols::referee::GameStatus> last_game_status_; }; } // namespace navigation