working waller coord

Author: sanatd33

src/rj_strategy/include/rj_strategy/coordinator/waller.hpp

@@ -31,8 +31,6 @@ class Waller
 private:
     std::array<u_int8_t, kNumShells> walling_robots_;
     u_int8_t num_wallers_ = 0;
-    WorldState last_world_state_;
-    rclcpp::Subscription<rj_msgs::msg::WorldState>::SharedPtr world_state_sub_;
     static constexpr int kMaxWallers = 4;
 
     void update_wallers();

src/rj_strategy/src/agent/position/defense.cpp

@@ -26,10 +26,6 @@ Defense::State Defense::update_state() {
     rj_geometry::Point ball_position = world_state->ball.position;
     double distance_to_ball = robot_position.dist_to(ball_position);
 
-    if (current_state_ != WALLING && current_state_ != JOINING_WALL) {
-        client_handles_->waller->leave_group();
-    }
-
     switch (current_state_) {
         case IDLING:
             next_state = JOINING_WALL;
@@ -65,6 +61,7 @@ Defense::State Defense::update_state() {
             if (check_is_done()) {
                 next_state = IDLING;
             }
+            break;
         case MARKING:
             if (marker_.get_target() == -1 || marker_.target_out_of_bounds(world_state)) {
                 next_state = ENTERING_MARKING;
@@ -182,9 +179,7 @@ void Defense::derived_acknowledge_ball_in_transit() {
 
 
 void Defense::die() {
-    if (current_state_ == WALLING) {
-        client_handles_->waller->leave_group();
-    }
+    client_handles_->waller->leave_group();
 }
 
 void Defense::revive() { current_state_ = JOINING_WALL; }

src/rj_strategy/src/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);
         }

src/rj_strategy/src/coordinator/waller.cpp

@@ -5,12 +5,6 @@ namespace strategy {
 Waller::Waller()
     : Coordinator("waller_srv", "waller_data", "waller_node") {
     walling_robots_.fill(-1);
-    world_state_sub_ = this->create_subscription<rj_msgs::msg::WorldState>(
-        vision_filter::topics::kWorldStateTopic, rclcpp::QoS(1),
-        [this](rj_msgs::msg::WorldState::SharedPtr world_state) {  // NOLINT
-            last_world_state_ = rj_convert::convert_from_ros(*world_state);
-            update_wallers();
-        });
 }
 
 void Waller::service_callback(RequestPtr request, ResponsePtr response) {
@@ -30,25 +24,16 @@ void Waller::service_callback(RequestPtr request, ResponsePtr response) {
         if (it != walling_robots_.end()) {
             num_wallers_--;
             *it = -1;
-            response->success = true;
             update_wallers();
-        } else {
-            response->success = false;
         }
+        response->success = true;
     }
 }
 
 void Waller::update_wallers() {
     auto prev_wallers = walling_robots_;
     std::sort(walling_robots_.begin(), walling_robots_.end());
-    auto must_republish = false;
-    for (int i = 0; i < num_wallers_ && !must_republish; i++) {
-        if (prev_wallers[i] != walling_robots_[i])  must_republish = true;
-    }
-
-    if (must_republish) {
-        publisher_->publish(rj_msgs::msg::Waller().set__wall_list(walling_robots_).set__wall_size(num_wallers_));
-    }
+    publisher_->publish(rj_msgs::msg::Waller().set__wall_list(walling_robots_).set__wall_size(num_wallers_));
 }
 
 }  // namespace strategy

src/rj_strategy/src/coordinator/waller_client.cpp

@@ -44,7 +44,7 @@ void WallerClient::join_group(StatusCallback callback) {
                         walling_robots_ = msg->wall_list;
                         num_wallers_ = msg->wall_size;
                         am_i_member_ = std::find(walling_robots_.begin(), walling_robots_.end(), robot_id_) != walling_robots_.end();
-
+                        
                         if (callback) {
                             callback(Result{am_i_member_});
                         }
@@ -56,7 +56,7 @@ void WallerClient::join_group(StatusCallback callback) {
 }
 
 void WallerClient::leave_group(StatusCallback callback) {
-    if (!am_i_member_) {
+    if (!am_i_member_ && !request_pending_) {
         if (callback) {
             callback(Result{false});
         }
@@ -95,7 +95,7 @@ bool WallerClient::am_i_member() const { return am_i_member_; }
 
 std::optional<rj_geometry::Point> WallerClient::get_walling_point(const WorldState* world_state,
                                         FieldDimensions field_dimensions) const {
-    if (!am_i_member_) return nullopt;
+    if (!am_i_member_) return std::nullopt;
 
     // Creates Minimum wall radius is slightly greater than  box bounds
     // Dimension accessors should be edited when we figure out how we are doing dimensions realtime
@@ -122,8 +122,7 @@ std::optional<rj_geometry::Point> WallerClient::get_walling_point(const WorldSta
     auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius;
 
     auto it = std::find(walling_robots_.begin(), walling_robots_.end(), robot_id_);
-    if (it == walling_robots_.end()) return std::nullopt;
-    auto waller_pos = std::distance(walling_robots_.begin(), it);
+    auto waller_pos = std::distance(walling_robots_.begin(), it)+1;
 
     rj_geometry::Point target_point{};
     auto angle = (mid_point - goal_pos).angle();