Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add game_status to navigation's subscriptions #90

Merged
merged 1 commit into from
Nov 9, 2024
Merged

Add game_status to navigation's subscriptions #90

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,10 +19,12 @@ using ::robocin::ZmqDatagram;

MessageReceiver::MessageReceiver(std::unique_ptr<IZmqSubscriberSocket> behavior_socket,
std::unique_ptr<IZmqSubscriberSocket> perception_socket,
std::unique_ptr<IZmqSubscriberSocket> game_status_socket,
std::unique_ptr<IZmqPoller> zmq_poller,
std::unique_ptr<IPayloadMapper> payload_mapper) :
behavior_socket_{std::move(behavior_socket)},
perception_socket_{std::move(perception_socket)},
game_status_socket_{std::move(game_status_socket)},
zmq_poller_{std::move(zmq_poller)},
payload_mapper_{std::move(payload_mapper)} {}

Expand Down Expand Up @@ -52,6 +54,15 @@ Payload MessageReceiver::receive() {
datagrams.emplace_back(std::move(perception_zmq_datagram));
}

while (true) {
ZmqDatagram game_status_zmq_datagram = zmq_poller_->receive(*game_status_socket_);
if (game_status_zmq_datagram.empty()) {
break;
}

datagrams.emplace_back(std::move(game_status_zmq_datagram));
}

if (datagrams.empty()) {
// wlog("no datagram received after {} ms.", pRefereePollerTimeoutMs());
}
Expand Down
2 changes: 2 additions & 0 deletions navigation-ms/navigation-luffy/navigation/messaging/receiver/message_receiver.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@ class MessageReceiver : public IMessageReceiver {
public:
MessageReceiver(std::unique_ptr<::robocin::IZmqSubscriberSocket> behavior_socket,
std::unique_ptr<::robocin::IZmqSubscriberSocket> perception_socket,
std::unique_ptr<::robocin::IZmqSubscriberSocket> game_status_socket,
std::unique_ptr<::robocin::IZmqPoller> zmq_poller,
std::unique_ptr<IPayloadMapper> payload_mapper);

Expand All @@ -36,6 +37,7 @@ class MessageReceiver : public IMessageReceiver {
private:
std::unique_ptr<::robocin::IZmqSubscriberSocket> behavior_socket_;
std::unique_ptr<::robocin::IZmqSubscriberSocket> perception_socket_;
std::unique_ptr<::robocin::IZmqSubscriberSocket> game_status_socket_;
std::unique_ptr<::robocin::IZmqPoller> zmq_poller_;
std::unique_ptr<IPayloadMapper> payload_mapper_;
};
Expand Down
13 changes: 10 additions & 3 deletions navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,18 +7,25 @@ namespace rc {

using ::protocols::behavior::unification::Behavior;
using ::protocols::perception::Detection;
using ::protocols::referee::GameStatus;

} // namespace rc

} // namespace

Payload::Payload(std::vector<rc::Behavior> behaviors, std::vector<rc::Detection> detections) :
Payload::Payload(std::vector<rc::Behavior> behaviors,
std::vector<rc::Detection> detections,
std::vector<rc::GameStatus> game_statuses) :
behaviors_{std::move(behaviors)},
detections_{std::move(detections)} {}
detections_{std::move(detections)},
game_statuses_{std::move(game_statuses)} {}

std::span<const rc::Behavior> Payload::getBehaviors() const { return behaviors_; }
std::span<const rc::Detection> Payload::getDetections() const { return detections_; }
std::span<const rc::GameStatus> Payload::getGameStatuses() const { return game_statuses_; }

bool Payload::empty() const { return behaviors_.empty() and detections_.empty(); }
bool Payload::empty() const {
return behaviors_.empty() and detections_.empty() and game_statuses_.empty();
}

} // namespace navigation
6 changes: 5 additions & 1 deletion navigation-ms/navigation-luffy/navigation/messaging/receiver/payload.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

#include <protocols/behavior/behavior_unification.pb.h>
#include <protocols/perception/detection.pb.h>
#include <protocols/referee/game_status.pb.h>
#include <robocin/network/zmq_datagram.h>
#include <span>
#include <vector>
Expand All @@ -12,14 +13,17 @@ namespace navigation {
class Payload {
public:
Payload(std::vector<::protocols::behavior::unification::Behavior> behaviors,
std::vector<::protocols::perception::Detection> detections);
std::vector<::protocols::perception::Detection> detections,
std::vector<::protocols::referee::GameStatus> game_statuses);

[[nodiscard]] std::span<const ::protocols::behavior::unification::Behavior> getBehaviors() const;
[[nodiscard]] std::span<const ::protocols::perception::Detection> getDetections() const;
[[nodiscard]] std::span<const ::protocols::referee::GameStatus> getGameStatuses() const;

[[nodiscard]] bool empty() const;

private:
std::vector<::protocols::referee::GameStatus> game_statuses_;
std::vector<::protocols::behavior::unification::Behavior> behaviors_;
std::vector<::protocols::perception::Detection> detections_;
};
Expand Down
10 changes: 9 additions & 1 deletion navigation-ms/navigation-luffy/navigation/messaging/receiver/payload_mapper.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@ namespace rc {

using ::protocols::behavior::unification::Behavior;
using ::protocols::perception::Detection;
using ::protocols::referee::GameStatus;

} // namespace rc

Expand All @@ -26,6 +27,7 @@ using ::protocols::perception::Detection;
Payload PayloadMapper::fromZmqDatagrams(std::span<const ZmqDatagram> messages) const {
std::vector<rc::Behavior> behaviors;
std::vector<rc::Detection> detections;
std::vector<rc::GameStatus> game_statuses;

for (const ZmqDatagram& zmq_datagram : messages) {
if (zmq_datagram.topic() == service_discovery::kBehaviorUnificationTopic) {
Expand All @@ -39,12 +41,18 @@ Payload PayloadMapper::fromZmqDatagrams(std::span<const ZmqDatagram> messages) c
detection.ParseFromString(std::string{zmq_datagram.message()});
// ilog("Received from perception: {}", detection.DebugString());
detections.emplace_back(std::move(detection));
}
if (zmq_datagram.topic() == service_discovery::kRefereeGameStatusTopic) {
rc::GameStatus game_status;
game_status.ParseFromString(std::string{zmq_datagram.message()});
// ilog("Received from referee: {}", game_status.DebugString());
game_statuses.emplace_back(std::move(game_status));
} else {
// wlog("zmq_datagram with topic '{}' not processed.", zmq_datagram.topic());
}
}

return Payload{std::move(behaviors), std::move(detections)};
return Payload{std::move(behaviors), std::move(detections), std::move(game_statuses)};
}

} // namespace navigation
9 changes: 9 additions & 0 deletions navigation-ms/navigation-luffy/navigation/navigation_main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,20 +48,29 @@ std::unique_ptr<IMessageReceiver> makeMessageReceiver() {
service_discovery::kPerceptionDetectionTopic,
};

static constexpr std::array kRefereeTopics = {
service_discovery::kRefereeGameStatusTopic,
};

std::unique_ptr<IZmqSubscriberSocket> behavior_socket = std::make_unique<ZmqSubscriberSocket>();
behavior_socket->connect(service_discovery::kBehaviorAddress, kBehaviorTopics);

std::unique_ptr<IZmqSubscriberSocket> perception_socket = std::make_unique<ZmqSubscriberSocket>();
perception_socket->connect(service_discovery::kPerceptionAddress, kPerceptionTopics);

std::unique_ptr<IZmqSubscriberSocket> game_status_socket = std::make_unique<ZmqSubscriberSocket>();
game_status_socket->connect(service_discovery::kRefereeAddress, kRefereeTopics);

std::unique_ptr<IZmqPoller> zmq_poller = std::make_unique<ZmqPoller>();
zmq_poller->push(*behavior_socket);
zmq_poller->push(*perception_socket);
zmq_poller->push(*game_status_socket);

std::unique_ptr<IPayloadMapper> payload_mapper = std::make_unique<PayloadMapper>();

return std::make_unique<MessageReceiver>(std::move(behavior_socket),
std::move(perception_socket),
std::move(game_status_socket),
std::move(zmq_poller),
std::move(payload_mapper));
}
Expand Down
42 changes: 26 additions & 16 deletions navigation-ms/navigation-luffy/navigation/processing/navigation_processor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include <protocols/behavior/planning.pb.h>
#include <protocols/navigation/navigation.pb.h>
#include <protocols/perception/detection.pb.h>
#include <protocols/referee/game_status.pb.h>
#include <ranges>

namespace navigation {
Expand All @@ -15,11 +16,12 @@ namespace parameters = ::robocin::parameters;

namespace rc {

using ::protocols::behavior::GoToPoint;
using ::protocols::behavior::Planning;
using ::protocols::behavior::unification::Behavior;
using ::protocols::behavior::unification::Motion;

using ::protocols::referee::GameStatus;

using ::protocols::navigation::Navigation;
using ::protocols::navigation::Output;

Expand All @@ -43,6 +45,11 @@ std::vector<rc::Detection> detectionFromPayloads(std::span<const Payload> payloa
| std::ranges::to<std::vector>();
}

std::vector<rc::GameStatus> gameStatusFromPayloads(std::span<const Payload> payloads) {
return payloads | std::views::transform(&Payload::getGameStatuses) | std::views::join
| std::ranges::to<std::vector>();
}

} // namespace

NavigationProcessor::NavigationProcessor(std::unique_ptr<IMotionParser> motion_parser) :
Expand All @@ -55,7 +62,12 @@ std::optional<rc::Navigation> NavigationProcessor::process(std::span<const Paylo
last_behavior_ = behaviors.back();
}

if (!last_behavior_) {
if (std::vector<rc::GameStatus> game_statuses = gameStatusFromPayloads(payloads);
!game_statuses.empty()) {
last_game_status_ = game_statuses.back();
}

if (!last_behavior_ or !last_game_status_) {
return std::nullopt;
}

Expand All @@ -67,28 +79,27 @@ std::optional<rc::Navigation> NavigationProcessor::process(std::span<const Paylo
rc::Detection last_detection = detections.back();

///////////////////////////////////////////////////////////////////////////
for (const auto& behavior_ : last_behavior_->output()) {
for (const auto& behavior : last_behavior_->output()) {
rc::Output output;
rc::Robot ally;

for (const auto& robot : last_detection.robots()) {
if (robot.robot_id().number() == behavior_.robot_id().number()) {
if (robot.robot_id().number() == behavior.robot_id().number()) {
ally = robot;
break;
}
}
if (ally.has_robot_id()) {
maa134340 marked this conversation as resolved.
Show resolved Hide resolved
output.mutable_robot_id()->CopyFrom(ally.robot_id());

if (behavior_.has_motion()) {
if (behavior.has_motion()) {
RobotMove move;
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);
} else if (behavior_.motion().has_rotate_on_self()) {
move = motion_parser_->fromRotateOnSelf(behavior_.motion().rotate_on_self(), ally);
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);
} else if (behavior.motion().has_rotate_on_self()) {
move = motion_parser_->fromRotateOnSelf(behavior.motion().rotate_on_self(), ally);
} else {
// PROCESSAMENTO DO GO_TO_POINT_WITH_TRAJECTORY
}
Expand All @@ -97,14 +108,13 @@ std::optional<rc::Navigation> NavigationProcessor::process(std::span<const Paylo
output.set_forward_velocity(move.velocity().x);
output.set_angular_velocity(move.angularVelocity());

if (behavior_.motion().has_peripheral_actuation()) {
output.mutable_peripheral_actuation()->CopyFrom(
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

} else if (behavior_.has_planning()) {
} else if (behavior.has_planning()) {
// PROCESSAMENTO DO PLANNING
} else {
// PROCESSAMENTO DO NAVIGATION
Expand Down
3 changes: 2 additions & 1 deletion navigation-ms/navigation-luffy/navigation/processing/navigation_processor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
#include <optional>
#include <protocols/behavior/behavior_unification.pb.h>
#include <protocols/navigation/navigation.pb.h>
#include <protocols/referee/game_status.pb.h>
#include <robocin/parameters/parameters.h>

namespace navigation {
Expand Down Expand Up @@ -36,8 +37,8 @@ class NavigationProcessor : public INavigationProcessor {

private:
std::unique_ptr<IMotionParser> motion_parser_;

std::optional<::protocols::behavior::unification::Behavior> last_behavior_;
std::optional<::protocols::referee::GameStatus> last_game_status_;
};

} // namespace navigation
Expand Down