diff --git a/install/setup.bash b/install/setup.bash index da36e7c200..10ea0f7c07 100644 --- a/install/setup.bash +++ b/install/setup.bash @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd "`dirname "${BASH_SOURCE[0]}"`" > /dev/null _colcon_prefix_chain_bash_source_script "$COLCON_CURRENT_PREFIX/local_setup.bash" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_bash_source_script \ No newline at end of file +unset _colcon_prefix_chain_bash_source_script diff --git a/install/setup.zsh b/install/setup.zsh index a1ba46044b..54799fde6f 100644 --- a/install/setup.zsh +++ b/install/setup.zsh @@ -28,4 +28,4 @@ COLCON_CURRENT_PREFIX="$(builtin cd -q "`dirname "${(%):-%N}"`" > /dev/null && p _colcon_prefix_chain_zsh_source_script "$COLCON_CURRENT_PREFIX/local_setup.zsh" unset COLCON_CURRENT_PREFIX -unset _colcon_prefix_chain_zsh_source_script \ No newline at end of file +unset _colcon_prefix_chain_zsh_source_script diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index f8f992d0f8..646f0a80a6 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -237,6 +237,13 @@ def generate_launch_description(): output="screen", parameters=[param_config_filepath], on_exit=Shutdown(), + ), + Node( + package="rj_strategy", + executable="waller_node", + output="screen", + parameters=[param_config_filepath], + on_exit=Shutdown(), ) ] ) diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 8d7117d82f..32ba6aa23b 100644 --- a/src/rj_msgs/CMakeLists.txt +++ b/src/rj_msgs/CMakeLists.txt @@ -59,6 +59,7 @@ rosidl_generate_interfaces( # Coordinators msg/KickerPicker.msg + msg/Waller.msg # Agent Request Messages request/JoinWallRequest.msg @@ -84,6 +85,7 @@ rosidl_generate_interfaces( # Services srv/AgentCommunication.srv srv/KickerPicker.srv + srv/Waller.srv srv/ListJoysticks.srv srv/PlanHypotheticalPath.srv srv/QuickCommands.srv diff --git a/src/rj_msgs/msg/Waller.msg b/src/rj_msgs/msg/Waller.msg new file mode 100644 index 0000000000..a58613731e --- /dev/null +++ b/src/rj_msgs/msg/Waller.msg @@ -0,0 +1,3 @@ +# This is a message for the waller coordinator +uint8[16] wall_list # Sorted IDs of robots on the wall +uint8 wall_size # Number of robots on the wall \ No newline at end of file diff --git a/src/rj_msgs/srv/Waller.srv b/src/rj_msgs/srv/Waller.srv new file mode 100644 index 0000000000..d2c40501cd --- /dev/null +++ b/src/rj_msgs/srv/Waller.srv @@ -0,0 +1,5 @@ +# Request a robot to be added to or removed from the waller selection group +uint8 robot_id # ID of the robot (0 to kNumShells-1) +bool joining # True to add robot to waller group, false to remove it +--- +bool success # True if the ACTION succeded (True for joining means successfully joined, for leaving means successfully left) \ No newline at end of file diff --git a/src/rj_strategy/CMakeLists.txt b/src/rj_strategy/CMakeLists.txt index b00d47d05a..e8c1758dda 100644 --- a/src/rj_strategy/CMakeLists.txt +++ b/src/rj_strategy/CMakeLists.txt @@ -56,7 +56,6 @@ set(RJ_STRATEGY_SRC src/agent/position/seeker.cpp src/agent/position/smartidling.cpp src/agent/position/solo_offense.cpp - src/agent/position/waller.cpp src/agent/position/zoner.cpp # Communication @@ -64,6 +63,7 @@ set(RJ_STRATEGY_SRC # Coordinators src/coordinator/kicker_picker_client.cpp + src/coordinator/waller_client.cpp ) # rj_strategy library @@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node ${RJ_STRATEGY_DEPS} ) +# Waller Node +add_executable(waller_node + ${RJ_STRATEGY_SRC} + src/coordinator/waller_client.cpp + src/coordinator/waller.cpp +) + +target_include_directories(waller_node + PUBLIC + $ + $ +) + +ament_target_dependencies(waller_node + ${RJ_STRATEGY_DEPS} +) + # Agent Action Clients add_executable(agent_action_clients ${RJ_STRATEGY_SRC} @@ -146,7 +163,7 @@ install( ) install( - TARGETS straight_line_test_node kicker_picker_node agent_action_clients + TARGETS straight_line_test_node kicker_picker_node waller_node agent_action_clients DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/rj_strategy/include/rj_strategy/agent/position.hpp b/src/rj_strategy/include/rj_strategy/agent/position.hpp index e384a74ce6..900e4eea55 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position.hpp @@ -42,6 +42,7 @@ // Coordinators #include "rj_strategy/coordinator/kicker_picker_client.hpp" +#include "rj_strategy/coordinator/waller_client.hpp" // tell compiler this class exists, but no need to import the whole header class AgentActionClient; @@ -51,6 +52,7 @@ namespace strategy { // Client Handles for coordinators struct ClientHandles { std::unique_ptr kicker_picker; + std::unique_ptr waller; }; /* diff --git a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp index 280ba778a9..6676f06785 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -17,7 +17,6 @@ #include "rj_strategy/agent/position.hpp" #include "rj_strategy/agent/position/marker.hpp" -#include "rj_strategy/agent/position/waller.hpp" namespace strategy { @@ -44,10 +43,6 @@ class Defense : public Position { void revive() override; private: - // static constexpr int kMaxWallers{6}; - static constexpr int kMaxWallers{ - static_cast(kNumShells)}; // This effectively turns off marking - /** * @brief The derived_get_task method returns the task for the defensive robot * to do based on the game situation. The method will continuously look to assign @@ -72,54 +67,9 @@ class Defense : public Position { }; State update_state(); - + State current_state_ = IDLING; std::optional state_to_task(RobotIntent intent); - /** - * @brief Sends a JoinWallRequest in broadcast to the other robots - */ - void send_join_wall_request(); - - /** - * @brief Sends a LeaveWallRequest to each of the robots in walling_robots_. - */ - void send_leave_wall_request(); - - /** - * @brief Adds the new waller to this robot's list of wallers and updates this robot's position - * in the wall. - * - * @param join_request the request received from another robot about joining the wall - * @return communication::JoinWallResponse A confirmation for the other robot to join the wall - * with this robot's ID - */ - communication::JoinWallResponse handle_join_wall_request( - communication::JoinWallRequest join_request); - - /** - * @brief Removes a given robot from this robot's list of wallers. - * - * @param leave_request the request from the robot who is leaving the wall - * @return communication::Acknowledge acknowledgement of the other robot's communication - */ - communication::Acknowledge handle_leave_wall_request( - communication::LeaveWallRequest leave_request); - - /** - * @brief Handles the response from the currently walling robots to find this robot's place in - * the wall. - * - * @param join_response the response from another robot that this robot can join the wall - */ - void handle_join_wall_response(communication::JoinWallResponse join_response); - - std::vector walling_robots_ = {}; - int waller_id_ = -1; - - // current state of the defense agent (state machine) - int get_waller_id(); - State current_state_ = JOINING_WALL; - int get_marker_target_id(); Marker marker_; }; diff --git a/src/rj_strategy/include/rj_strategy/agent/position/waller.hpp b/src/rj_strategy/include/rj_strategy/agent/position/waller.hpp deleted file mode 100644 index 70400725f9..0000000000 --- a/src/rj_strategy/include/rj_strategy/agent/position/waller.hpp +++ /dev/null @@ -1,49 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include "rj_strategy/agent/position.hpp" -#include "rj_strategy/agent/position/role_interface.hpp" - -namespace strategy { - -/* - * The Waller role provides the implementation for a defensive robot that implements - * this class to have a waller-like behavior where it aims to serve as a wall between - * the ball and the goal. - */ -class Waller : public RoleInterface { -public: - Waller(int waller_num, std::vector walling_robots); - ~Waller() = default; - - /** - * @brief Returns a waller behavior which aims to intercept the path - * between the ball and the center of the goal - * - * @param [RobotIntent intent] [RobotIntent of the Defensive Robot] - * @return [RobotIntent with next target point for the robot] - */ - std::optional get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) override; - -private: - std::string defense_type_; - int waller_pos_; - static constexpr double kRobotDiameterMultiplier = 1.5; - std::vector walling_robots_; -}; - -} // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/coordinator/waller.hpp b/src/rj_strategy/include/rj_strategy/coordinator/waller.hpp new file mode 100644 index 0000000000..2a4d55d544 --- /dev/null +++ b/src/rj_strategy/include/rj_strategy/coordinator/waller.hpp @@ -0,0 +1,39 @@ +#pragma once + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "rj_strategy/coordinator.hpp" + +namespace strategy { + +class Waller : public Coordinator { +public: + Waller(); + ~Waller() override = default; + Waller(const Waller&) = delete; + Waller& operator=(const Waller&) = delete; + Waller(Waller&&) = delete; + Waller& operator=(Waller&&) = delete; + + void service_callback(RequestPtr request, ResponsePtr response); + +private: + std::array walling_robots_; + uint8_t num_wallers_ = 0; + static constexpr int kMaxWallers = 4; + + void update_wallers(); +}; + +} // namespace strategy \ No newline at end of file diff --git a/src/rj_strategy/include/rj_strategy/coordinator/waller_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/waller_client.hpp new file mode 100644 index 0000000000..c610ab7991 --- /dev/null +++ b/src/rj_strategy/include/rj_strategy/coordinator/waller_client.hpp @@ -0,0 +1,92 @@ +#pragma once + +#include + +#include +#include + +#include +#include +#include + +#include "rj_strategy/coordinator/waller.hpp" + +namespace strategy { + +/** + * @brief Client for interacting with the Waller coordinator. + * + * Manages membership in the waller group and tracks the currently waller list. + */ +class WallerClient { +public: + struct Result { + bool success{false}; + }; + + using StatusCallback = std::function; + + explicit WallerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id); + ~WallerClient() = default; + WallerClient(const WallerClient&) = delete; + WallerClient& operator=(const WallerClient&) = delete; + WallerClient(WallerClient&&) = delete; + WallerClient& operator=(WallerClient&&) = delete; + + /** + * @brief Join the waller group. + * @param callback Called with current membership status after attempt to join. + */ + void join_group(StatusCallback callback = nullptr); + + /** + * @brief Leave the waller group. + * @param callback Called with current membership status after attempt to leave. + */ + void leave_group(StatusCallback callback = nullptr); + + /** + * @brief Check if this robot is a member of the waller group. + */ + [[nodiscard]] bool am_i_member() const; + + /** + * @brief Get the target walling point + * @return target walling point of this robot. + */ + [[nodiscard]] std::optional get_walling_point( + const WorldState* world_state, FieldDimensions field_dimensions) const; + +private: + rclcpp::Node::SharedPtr node_; + const uint8_t robot_id_; // NOLINT(cppcoreguidelines-avoid-const-or-ref-data-members) -- class + // isn't move/copy-able anyway + rclcpp::Client::SharedPtr client_; + rclcpp::Subscription::SharedPtr subscription_; + + bool am_i_member_ = false; + bool request_pending_ = false; + std::array walling_robots_; + int num_wallers_ = 0; + + static constexpr double kRobotDiameterMultiplier = 1.5; + + struct WallerGeometry { + double min_wall_radius; + double wall_spacing; + rj_geometry::Point goal_pos; + rj_geometry::Point ball_pos; + rj_geometry::Point robot_pos; + }; + + WallerGeometry calculate_wall_geometry(const WorldState* world_state, + FieldDimensions dimensions) const; + rj_geometry::Point get_target_position(WallerGeometry& waller_geometry, long waller_pos) const; + std::optional get_parent_id(WallerGeometry& waller_geometry, + rj_geometry::Point target_point, long waller_pos) const; + rj_geometry::Point get_target_position_with_parent(WallerGeometry& waller_geometry, + rj_geometry::Point target_point, + rj_geometry::Point parent_point) const; +}; + +} // namespace strategy \ No newline at end of file diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index eb80336a45..591e2d4641 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -6,13 +6,10 @@ Defense::Defense(int r_id) : Position(r_id, "Defense"), marker_{field_dimensions Defense::Defense(Position&& other) : Position{std::move(other)}, marker_{field_dimensions_} { position_name_ = "Defense"; - walling_robots_ = {}; } std::optional Defense::derived_get_task(RobotIntent intent) { - // SPDLOG_INFO("waller length (sus) {}, {}", walling_robots_.size(), robot_id_); current_state_ = update_state(); - // waller_id_ = get_waller_id(); return state_to_task(intent); } @@ -29,31 +26,21 @@ 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 && waller_id_ != -1) { - send_leave_wall_request(); - walling_robots_ = {(u_int8_t)robot_id_}; - waller_id_ = -1; - } - switch (current_state_) { case IDLING: + // Defensive Priority goes Walling > Marking > Zoning > Idle + next_state = JOINING_WALL; break; case JOINING_WALL: - send_join_wall_request(); - // SPDLOG_INFO("join wall {}", robot_id_); - next_state = WALLING; - walling_robots_ = {(u_int8_t)robot_id_}; + client_handles_->waller->join_group([this](WallerClient::Result result) { + if (result.success) + current_state_ = WALLING; + else + current_state_ = ENTERING_MARKING; + }); break; case WALLING: - // If a wall is already full, - // Remove the robot with the highest ID from a wall - // and make them a marker instead. - if (walling_robots_.size() > kMaxWallers && - this->robot_id_ == *max_element(walling_robots_.begin(), walling_robots_.end())) { - send_leave_wall_request(); - // SPDLOG_INFO("leave wall {}", robot_id_); - next_state = ENTERING_MARKING; - } + if (!client_handles_->waller->am_i_member()) next_state = IDLING; break; case SEARCHING: break; @@ -78,6 +65,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; @@ -97,9 +85,6 @@ Defense::State Defense::update_state() { } std::optional Defense::state_to_task(RobotIntent intent) { - // if (robot_id_ == 2) { - // SPDLOG_INFO("{} current state of 2", current_state_); - // } if (current_state_ == IDLING) { auto empty_motion_cmd = planning::MotionCommand{}; intent.motion_command = empty_motion_cmd; @@ -142,10 +127,15 @@ std::optional Defense::state_to_task(RobotIntent intent) { intent.is_active = true; return intent; } else if (current_state_ == WALLING) { - if (!walling_robots_.empty() && waller_id_ != -1) { - Waller waller{waller_id_, walling_robots_}; - return waller.get_task(intent, last_world_state_, this->field_dimensions_); - } + auto walling_point = + client_handles_->waller->get_walling_point(last_world_state_, field_dimensions_); + if (walling_point) { + planning::LinearMotionInstant target{walling_point.value()}; + intent.motion_command = + planning::MotionCommand{"path_target", target, planning::FaceBall{}}; + } else + intent.motion_command = planning::MotionCommand{}; + return intent; } else if (current_state_ == FACING) { rj_geometry::Point robot_position = last_world_state_->get_robot(true, robot_id_).pose.position(); @@ -172,17 +162,6 @@ std::optional Defense::state_to_task(RobotIntent intent) { void Defense::receive_communication_response(communication::AgentPosResponseWrapper response) { // Call to super Position::receive_communication_response(response); - - // Handle join wall response - if (const communication::JoinWallRequest* join_request = - std::get_if(&response.associated_request)) { - for (communication::AgentResponseVariant response : response.responses) { - if (const communication::JoinWallResponse* join_response = - std::get_if(&response)) { - handle_join_wall_response(*join_response); - } - } - } } communication::PosAgentResponseWrapper Defense::receive_communication_request( @@ -191,106 +170,10 @@ communication::PosAgentResponseWrapper Defense::receive_communication_request( communication::PosAgentResponseWrapper response = Position::receive_communication_request(request); - // Handle join and leave wall request - if (const communication::JoinWallRequest* join_request = - std::get_if(&request.request)) { - response.response = handle_join_wall_request(*join_request); - } else if (const communication::LeaveWallRequest* leave_request = - std::get_if(&request.request)) { - response.response = handle_leave_wall_request(*leave_request); - } - // Return the response return response; } -void Defense::send_join_wall_request() { - communication::JoinWallRequest join_request{}; - join_request.robot_id = robot_id_; - communication::generate_uid(join_request); - - communication::PosAgentRequestWrapper communication_request{}; - communication_request.request = join_request; - communication_request.target_agents = {}; - communication_request.urgent = false; - communication_request.broadcast = true; - - communication_requests_.push_back(communication_request); - - current_state_ = WALLING; -} - -void Defense::send_leave_wall_request() { - communication::LeaveWallRequest leave_request{}; - leave_request.robot_id = robot_id_; - communication::generate_uid(leave_request); - - communication::PosAgentRequestWrapper communication_request{}; - communication_request.request = leave_request; - communication_request.target_agents = walling_robots_; - communication_request.urgent = true; - communication_request.broadcast = false; - - communication_requests_.push_back(communication_request); -} - -communication::JoinWallResponse Defense::handle_join_wall_request( - communication::JoinWallRequest join_request) { - for (size_t i = 0; i < walling_robots_.size(); i++) { - if (walling_robots_[i] == join_request.robot_id) { - break; - } else if (walling_robots_[i] > join_request.robot_id) { - walling_robots_.insert(walling_robots_.begin() + i, join_request.robot_id); - waller_id_ = get_waller_id(); - break; - } else if (i == walling_robots_.size() - 1) { - walling_robots_.push_back(join_request.robot_id); - waller_id_ = get_waller_id(); - } - } - - communication::JoinWallResponse join_response{}; - join_response.robot_id = robot_id_; - communication::generate_uid(join_response); - - return join_response; -} - -communication::Acknowledge Defense::handle_leave_wall_request( - communication::LeaveWallRequest leave_request) { - if (robot_id_ != leave_request.robot_id) { - for (int i = walling_robots_.size() - 1; i > 0; i--) { - if (walling_robots_[i] == leave_request.robot_id) { - walling_robots_.erase(walling_robots_.begin() + i); - waller_id_ = get_waller_id(); - break; - } else if (walling_robots_[i] < leave_request.robot_id) { - break; - } - } - } - - communication::Acknowledge acknowledge_response{}; - communication::generate_uid(acknowledge_response); - - return acknowledge_response; -} - -void Defense::handle_join_wall_response(communication::JoinWallResponse join_response) { - for (size_t i = 0; i < walling_robots_.size(); i++) { - if (walling_robots_[i] == join_response.robot_id) { - return; - } else if (walling_robots_[i] > join_response.robot_id) { - walling_robots_.insert(walling_robots_.begin() + i, join_response.robot_id); - waller_id_ = get_waller_id(); - return; - } else if (i == walling_robots_.size() - 1) { - walling_robots_.push_back(join_response.robot_id); - waller_id_ = get_waller_id(); - } - } -} - void Defense::derived_acknowledge_pass() { current_state_ = FACING; } void Defense::derived_pass_ball() { current_state_ = PASSING; } @@ -300,16 +183,7 @@ void Defense::derived_acknowledge_ball_in_transit() { chasing_ball = false; } -int Defense::get_waller_id() { - return find(walling_robots_.begin(), walling_robots_.end(), robot_id_) - - walling_robots_.begin() + 1; -} - -void Defense::die() { - if (current_state_ == WALLING) { - send_leave_wall_request(); - } -} +void Defense::die() { client_handles_->waller->leave_group(); } void Defense::revive() { current_state_ = JOINING_WALL; } diff --git a/src/rj_strategy/src/agent/position/offense.cpp b/src/rj_strategy/src/agent/position/offense.cpp index b63fd8c590..0c5e722e94 100644 --- a/src/rj_strategy/src/agent/position/offense.cpp +++ b/src/rj_strategy/src/agent/position/offense.cpp @@ -15,7 +15,7 @@ std::optional 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/src/rj_strategy/src/agent/position/robot_factory_position.cpp b/src/rj_strategy/src/agent/position/robot_factory_position.cpp index 350c8653ce..d2ac855d51 100644 --- a/src/rj_strategy/src/agent/position/robot_factory_position.cpp +++ b/src/rj_strategy/src/agent/position/robot_factory_position.cpp @@ -5,6 +5,8 @@ namespace strategy { RobotFactoryPosition::RobotFactoryPosition(int r_id, rclcpp::Node::SharedPtr node) : Position(r_id, "RobotFactoryPosition") { client_handles_->kicker_picker = std::make_unique(node, r_id); + client_handles_->waller = std::make_unique(node, r_id); + if (robot_id_ == 0) { current_position_ = std::make_unique(robot_id_); } else if (robot_id_ == 1 || robot_id_ == 2) { diff --git a/src/rj_strategy/src/agent/position/waller.cpp b/src/rj_strategy/src/agent/position/waller.cpp deleted file mode 100644 index 76f364a346..0000000000 --- a/src/rj_strategy/src/agent/position/waller.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "rj_strategy/agent/position/waller.hpp" - -namespace strategy { - -Waller::Waller(int waller_num, std::vector walling_robots) { - defense_type_ = "Waller"; - waller_pos_ = waller_num; - walling_robots_ = walling_robots; -} - -std::optional Waller::get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) { - // 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 - // from vision - float box_w{field_dimensions.penalty_long_dist()}; - float box_h{field_dimensions.penalty_short_dist()}; - float line_w{field_dimensions.line_width()}; - double min_wall_rad{(kRobotRadius * 4.0f) + line_w + - hypot(static_cast(box_w) / 2, static_cast((box_h)))}; - - auto ball_pos = world_state->ball.position; - - auto robot_id = intent.robot_id; - - auto robot_pos = world_state->get_robot(true, robot_id).pose.position(); - auto goal_pos = rj_geometry::Point{0, 0}; - auto num_wallers = walling_robots_.size(); - - // Find ball_direction unit vector - rj_geometry::Point ball_dir_vector{(ball_pos - goal_pos)}; - - ball_dir_vector = ball_dir_vector.normalized(); - - // Find target Point - rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)}; - - auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; - - rj_geometry::Point target_point{}; - auto angle = (mid_point - goal_pos).angle(); - auto delta_angle = (wall_spacing * (waller_pos_ - num_wallers / 2. - 0.5)) / min_wall_rad; - auto target_angle = angle - delta_angle; - - target_point = - (goal_pos + rj_geometry::Point{1, 0}).normalized(min_wall_rad).rotated(target_angle); - - if (abs(robot_pos.dist_to(goal_pos) - min_wall_rad) < kRobotRadius && - robot_pos.dist_to(target_point) > kRobotRadius) { - uint8_t parent_id = - [&]() { // Assigning a value to avoid any undefined behavior; will be changed - if (target_point.x() < robot_pos.x() && waller_pos_ > 1 && - waller_pos_ <= num_wallers) { - return static_cast(walling_robots_[waller_pos_ - 2]); - } else if (target_point.x() >= robot_pos.x() && waller_pos_ >= 1 && - waller_pos_ < num_wallers) { - return static_cast(walling_robots_[waller_pos_]); - } else { - return static_cast(robot_id); - } - }(); - - if ((target_point.x() < robot_pos.x() && waller_pos_ != 1) || - (target_point.x() > robot_pos.x() && waller_pos_ != num_wallers)) { - auto parent_point = world_state->get_robot(true, parent_id).pose.position(); - angle = (parent_point - goal_pos).angle(); - delta_angle = wall_spacing / min_wall_rad; - target_angle = - angle + delta_angle * (signbit(target_point.x() - robot_pos.x()) ? -1 : 1); - - target_point = (goal_pos + rj_geometry::Point{1, 0}) - .normalized(min_wall_rad) - .rotated(target_angle); - } - } - - auto location_instant = planning::LinearMotionInstant{target_point, rj_geometry::Point{}}; - - auto target_cmd = - planning::MotionCommand{"path_target", location_instant, planning::FaceBall{}}; - intent.motion_command = target_cmd; - - return intent; -} - -} // namespace strategy diff --git a/src/rj_strategy/src/coordinator/waller.cpp b/src/rj_strategy/src/coordinator/waller.cpp new file mode 100644 index 0000000000..6df1ed60d7 --- /dev/null +++ b/src/rj_strategy/src/coordinator/waller.cpp @@ -0,0 +1,47 @@ +#include "rj_strategy/coordinator/waller.hpp" + +namespace strategy { + +Waller::Waller() : Coordinator("waller_srv", "waller_data", "waller_node") { + walling_robots_.fill(std::numeric_limits::max()); +} + +void Waller::service_callback(RequestPtr request, ResponsePtr response) { + if (request->joining) { + if (num_wallers_ == kMaxWallers) { + response->success = false; + } else { + if (std::find(walling_robots_.begin(), walling_robots_.end(), request->robot_id) == + walling_robots_.end()) { + walling_robots_[num_wallers_] = request->robot_id; + num_wallers_++; + update_wallers(); + } + response->success = true; + } + } else { + auto it = std::find(walling_robots_.begin(), walling_robots_.end(), request->robot_id); + if (it != walling_robots_.end()) { + num_wallers_--; + *it = std::numeric_limits::max(); + update_wallers(); + } + response->success = true; + } +} + +void Waller::update_wallers() { + std::sort(walling_robots_.begin(), walling_robots_.end()); + publisher_->publish( + rj_msgs::msg::Waller().set__wall_list(walling_robots_).set__wall_size(num_wallers_)); +} + +} // namespace strategy + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/rj_strategy/src/coordinator/waller_client.cpp b/src/rj_strategy/src/coordinator/waller_client.cpp new file mode 100644 index 0000000000..59d7da9614 --- /dev/null +++ b/src/rj_strategy/src/coordinator/waller_client.cpp @@ -0,0 +1,203 @@ +#include "rj_strategy/coordinator/waller_client.hpp" + +namespace strategy { + +WallerClient::WallerClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) + : node_{std::move(node)}, robot_id_{robot_id} { + client_ = node_->create_client("waller_srv"); + walling_robots_.fill(std::numeric_limits::max()); +} + +void WallerClient::join_group(StatusCallback callback) { + if (am_i_member_ || request_pending_) { + return; + } + + if (!client_->wait_for_service(std::chrono::seconds(1))) { + SPDLOG_ERROR("Waller service not available."); + if (callback) { + callback(Result{false}); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->joining = true; + + request_pending_ = true; + client_->async_send_request( + request, [this, callback = std::move(callback)]( + rclcpp::Client::SharedFuture + future) { // 6 NOLINT(performance-unnecessary-value-param) -- + // ROS2 async callbacks require value capture. + request_pending_ = false; + if (!future.valid() || !future.get()->success) { + if (callback) { + callback(Result{false}); + } + } + + am_i_member_ = true; + + subscription_ = node_->create_subscription( + "waller_data", rclcpp::QoS(1).transient_local(), + [this, callback = std::move(callback)](const rj_msgs::msg::Waller::SharedPtr msg) { + 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_}); + } + }); + + }); +} + +void WallerClient::leave_group(StatusCallback callback) { + if (!am_i_member_ && !request_pending_) { + if (callback) { + callback(Result{false}); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->joining = false; + + client_->async_send_request( + request, [this, callback = std::move(callback)]( + rclcpp::Client::SharedFuture + future) { // 6 NOLINT(performance-unnecessary-value-param) -- + // ROS2 async callbacks require value capture. + if (!future.valid() || !future.get()->success) { + if (callback) { + callback(Result{false}); + } + return; + } + + am_i_member_ = false; + + subscription_.reset(); + walling_robots_.fill(std::numeric_limits::max()); + num_wallers_ = 0; + + if (callback) { + callback(Result{true}); + } + }); +} + +bool WallerClient::am_i_member() const { return am_i_member_; } + +std::optional WallerClient::get_walling_point( + const WorldState* world_state, FieldDimensions field_dimensions) const { + if (!am_i_member_) return std::nullopt; + + auto waller_geometry = calculate_wall_geometry(world_state, field_dimensions); + auto waller_pos = + std::distance(walling_robots_.begin(), + std::find(walling_robots_.begin(), walling_robots_.end(), robot_id_)); + + // Target point where the waller should end up + auto target_point = get_target_position(waller_geometry, waller_pos); + + // If we can, follow our parent instead of going directly to the target + if (auto parent_id = get_parent_id(waller_geometry, target_point, waller_pos)) { + auto parent_point = world_state->get_robot(true, *parent_id).pose.position(); + return get_target_position_with_parent(waller_geometry, target_point, parent_point); + } + + return target_point; +} + +WallerClient::WallerGeometry WallerClient::calculate_wall_geometry( + const WorldState* world_state, FieldDimensions field_dimensions) const { + WallerGeometry waller_geometry{}; + + // 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 + // from vision + float box_w{field_dimensions.penalty_long_dist()}; + float box_h{field_dimensions.penalty_short_dist()}; + float line_w{field_dimensions.line_width()}; + waller_geometry.min_wall_radius = + (kRobotRadius * 4.0f) + line_w + + hypot(static_cast(box_w) / 2, static_cast((box_h))); + + waller_geometry.robot_pos = world_state->get_robot(true, robot_id_).pose.position(); + waller_geometry.goal_pos = field_dimensions.our_goal_loc(); + waller_geometry.ball_pos = world_state->ball.position; + waller_geometry.wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; + + return waller_geometry; +} + +rj_geometry::Point WallerClient::get_target_position(WallerGeometry& waller_geometry, + long waller_pos) const { + // Find target point of this robot + rj_geometry::Point ball_dir_vector = + rj_geometry::Point(waller_geometry.ball_pos - waller_geometry.goal_pos).normalized(); + + // This serves as the center of the wall arc + rj_geometry::Point mid_point{(waller_geometry.goal_pos) + + (ball_dir_vector * waller_geometry.min_wall_radius)}; + auto angle = (mid_point - waller_geometry.goal_pos).angle(); + + // Wallers are distributed evenly across the wall arc based on their position in the wall list + auto delta_angle = (waller_geometry.wall_spacing * (waller_pos - num_wallers_ / 2. - 0.5)) / + waller_geometry.min_wall_radius; + auto target_angle = angle - delta_angle; + + // Calculate the target point using polar coordinates with wall radius and target angle + return rj_geometry::Point(1, 0) + .normalized(waller_geometry.min_wall_radius) + .rotated(target_angle); +} + +std::optional WallerClient::get_parent_id(WallerGeometry& waller_geometry, + rj_geometry::Point target_point, + long waller_pos) const { + // Finds the parent point along the wall + auto distance_from_arc = abs(waller_geometry.robot_pos.dist_to(waller_geometry.goal_pos) - + waller_geometry.min_wall_radius); + auto distance_from_target = waller_geometry.robot_pos.dist_to(target_point); + + // We are not along the wall arco or we are close to our target, do not follow the parent + if (distance_from_arc >= kRobotRadius || distance_from_target <= kRobotRadius) + return std::nullopt; + + // We need to move to the left so our parent is the robot to the left of us + if (target_point.x() < waller_geometry.robot_pos.x() && waller_pos > 0) + return static_cast(walling_robots_[waller_pos - 1]); + // We need to move to the right so our parent is the robot to the right of us + else if (target_point.x() >= waller_geometry.robot_pos.x() && waller_pos < num_wallers_ - 1) + return static_cast(walling_robots_[waller_pos + 1]); + + // We are the first robot in the arc, we should not follow anyone + return std::nullopt; +} + +rj_geometry::Point WallerClient::get_target_position_with_parent( + WallerGeometry& waller_geometry, rj_geometry::Point target_point, + rj_geometry::Point parent_point) const { + // Find target point of this robot by following some parent robot + auto angle = (parent_point - waller_geometry.goal_pos).angle(); + + // Go to {wall_spacing} behind the parent + // Uses signbit() to determine whether the parent is to the left or to the right + auto delta_angle = waller_geometry.wall_spacing / waller_geometry.min_wall_radius; + auto target_angle = + angle + delta_angle * (signbit(target_point.x() - waller_geometry.robot_pos.x()) ? -1 : 1); + + // Calculate the target position using polar coordinates + return rj_geometry::Point(1, 0) + .normalized(waller_geometry.min_wall_radius) + .rotated(target_angle); +} + +} // namespace strategy