fix alive robots

RoboJackets · Apr 29, 2024 · 92aaa9e · 92aaa9e
1 parent 6ec0a0b
commit 92aaa9e
Show file tree

Hide file tree

Showing 4 changed files with 15 additions and 3 deletions.
diff --git a/soccer/src/soccer/radio/network_radio.cpp b/soccer/src/soccer/radio/network_radio.cpp
@@ -20,7 +20,8 @@ NetworkRadio::NetworkRadio()
     : control_message_socket_(io_service_),
       robot_status_socket_(io_service_),
       alive_robots_socket_(io_service_),
-      send_buffers_(kNumShells) {
+      send_buffers_(kNumShells),
+      Radio() {
     control_message_socket_.open(udp::v4());
     control_message_socket_.bind(udp::endpoint(udp::v4(), kControlMessageSocketPort));
 
@@ -111,6 +112,7 @@ void NetworkRadio::receive_robot_status(const boost::system::error_code& error,
 }
 
 void NetworkRadio::receive_alive_robots(const boost::system::error_code& error, size_t num_bytes) {
+    SPDLOG_INFO("Receiving Alive Robots");
     if (static_cast<bool>(error)) {
         SPDLOG_ERROR("Error Receiving Alive Robots: {}", error.message());
         start_alive_robots_receive();
@@ -123,14 +125,20 @@ void NetworkRadio::receive_alive_robots(const boost::system::error_code& error,
         return;
     }
 
-    uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[0]);
+    uint16_t alive = (((uint16_t) alive_robots_buffer_[1]) << 8) | (alive_robots_buffer_[0]);
+    SPDLOG_INFO("Alive: {}", alive);
+
+    // uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[0]);
     for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) {
         if ((alive & (1 << robot_id)) != 0) {
             alive_robots_[robot_id] = true;
         } else {
             alive_robots_[robot_id] = false;
         }
     }
+    for (int robot_id = 0; robot_id < 16; robot_id++) {
+        SPDLOG_INFO("Robot {} is Alive {}", robot_id, alive_robots_[robot_id]);
+    }
 
     rj_msgs::msg::AliveRobots alive_message{};
     alive_message.alive_robots = alive_robots_;

diff --git a/soccer/src/soccer/radio/radio.cpp b/soccer/src/soccer/radio/radio.cpp
@@ -49,6 +49,8 @@ void Radio::publish_robot_status(int robot_id, const rj_msgs::msg::RobotStatus&
 }
 
 void Radio::publish_alive_robots(const rj_msgs::msg::AliveRobots& alive_robots) {
+    // SPDLOG_INFO("publishing alive robots {}", alive_robots);
+    SPDLOG_INFO("publishing alive robots");
     alive_robots_pub_->publish(alive_robots);
 }
 

diff --git a/soccer/src/soccer/strategy/agent/position/offense.cpp b/soccer/src/soccer/strategy/agent/position/offense.cpp
@@ -15,7 +15,7 @@ std::optional<RobotIntent> Offense::derived_get_task(RobotIntent intent) {
     if (current_state_ != new_state) {
         reset_timeout();
 
-        SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_));
+        // SPDLOG_INFO("Robot {}: now {}", robot_id_, state_to_name(current_state_));
         if (current_state_ == SEEKING) {
             broadcast_seeker_request(rj_geometry::Point{}, false);
         }

diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp
@@ -25,6 +25,8 @@ std::optional<RobotIntent> RobotFactoryPosition::derived_get_task([
                                            current_play_state_);
     }
 
+
+
     // Update our state
     process_play_state();