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..670783a572 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="marking_node", + output="screen", + parameters=[param_config_filepath], + on_exit=Shutdown(), ) ] ) diff --git a/src/rj_constants/include/rj_constants/constants.hpp b/src/rj_constants/include/rj_constants/constants.hpp index e388b13df7..b35eb99d3d 100644 --- a/src/rj_constants/include/rj_constants/constants.hpp +++ b/src/rj_constants/include/rj_constants/constants.hpp @@ -7,6 +7,7 @@ /// all weights in kilograms // Number of identifiable robots on one team +// IF U CHANGE THIS NUMBER CHECK Marking.msg inline constexpr size_t kNumShells = 16; // Number of playing robots on one team diff --git a/src/rj_msgs/CMakeLists.txt b/src/rj_msgs/CMakeLists.txt index 8d7117d82f..2abb75b3ae 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/Marking.msg # Agent Request Messages request/JoinWallRequest.msg @@ -84,6 +85,7 @@ rosidl_generate_interfaces( # Services srv/AgentCommunication.srv srv/KickerPicker.srv + srv/Marking.srv srv/ListJoysticks.srv srv/PlanHypotheticalPath.srv srv/QuickCommands.srv diff --git a/src/rj_msgs/msg/Marking.msg b/src/rj_msgs/msg/Marking.msg new file mode 100644 index 0000000000..01098ec518 --- /dev/null +++ b/src/rj_msgs/msg/Marking.msg @@ -0,0 +1,2 @@ +# This is a message to pick a robot to mark +uint8[16] mark_robot_ids # ID of the robot to mark on the opposing team diff --git a/src/rj_msgs/srv/Marking.srv b/src/rj_msgs/srv/Marking.srv new file mode 100644 index 0000000000..93f10863da --- /dev/null +++ b/src/rj_msgs/srv/Marking.srv @@ -0,0 +1,5 @@ +# Request a robot to be added to or removed from the marking group +uint8 robot_id # ID of the robot (0 to kNumShells-1) +bool join +--- +bool success # Always true diff --git a/src/rj_strategy/CMakeLists.txt b/src/rj_strategy/CMakeLists.txt index b00d47d05a..979ec93532 100644 --- a/src/rj_strategy/CMakeLists.txt +++ b/src/rj_strategy/CMakeLists.txt @@ -47,7 +47,6 @@ set(RJ_STRATEGY_SRC src/agent/position/goalie.cpp src/agent/position/idle.cpp src/agent/position/line.cpp - src/agent/position/marker.cpp src/agent/position/offense.cpp src/agent/position/penalty_non_kicker.cpp src/agent/position/penalty_player.cpp @@ -64,6 +63,7 @@ set(RJ_STRATEGY_SRC # Coordinators src/coordinator/kicker_picker_client.cpp + src/coordinator/marking_client.cpp ) # rj_strategy library @@ -115,6 +115,23 @@ ament_target_dependencies(kicker_picker_node ${RJ_STRATEGY_DEPS} ) +# Marking Node +add_executable(marking_node + ${RJ_STRATEGY_SRC} + src/coordinator/marking_client.cpp + src/coordinator/marking.cpp +) + +target_include_directories(marking_node + PUBLIC + $ + $ +) + +ament_target_dependencies(marking_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 marking_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..fd784e18b3 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position.hpp @@ -42,15 +42,16 @@ // Coordinators #include "rj_strategy/coordinator/kicker_picker_client.hpp" +#include "rj_strategy/coordinator/marking_client.hpp" // tell compiler this class exists, but no need to import the whole header class AgentActionClient; namespace strategy { -// Client Handles for coordinators struct ClientHandles { std::unique_ptr kicker_picker; + std::unique_ptr marking; }; /* @@ -318,12 +319,12 @@ class Position { // protected to allow WorldState to be accessed directly by deriveed WorldState* last_world_state_; - // Current goalie - int goalie_id_; - // Client Handles std::shared_ptr client_handles_; + // Current goalie + int goalie_id_; + private: /** * @brief allow derived classes to change behavior of get_task(). See 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..7d8cec1e6d 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/defense.hpp @@ -16,9 +16,7 @@ #include #include "rj_strategy/agent/position.hpp" -#include "rj_strategy/agent/position/marker.hpp" #include "rj_strategy/agent/position/waller.hpp" - namespace strategy { /* @@ -44,9 +42,9 @@ 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 + static constexpr int kMaxWallers{2}; + static constexpr RJ::Seconds kMarkingGroupJoinTimeout{2.0}; + static constexpr float kMarkingDistanceFactor{0.55f}; /** * @brief The derived_get_task method returns the task for the defensive robot @@ -120,8 +118,10 @@ class Defense : public Position { int get_waller_id(); State current_state_ = JOINING_WALL; - int get_marker_target_id(); - Marker marker_; + bool sent_join_marking_group_request_ = false; + RJ::Time request_time_; + + bool pending_marking_state_ = false; }; } // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp index 278630d6da..c1051c302d 100644 --- a/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp +++ b/src/rj_strategy/include/rj_strategy/agent/position/goal_kicker.hpp @@ -41,4 +41,4 @@ class GoalKicker : public Position { std::optional derived_get_task(RobotIntent intent) override; }; -} // namespace strategy \ No newline at end of file +} // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp b/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp deleted file mode 100644 index 1c2eb7ebb3..0000000000 --- a/src/rj_strategy/include/rj_strategy/agent/position/marker.hpp +++ /dev/null @@ -1,44 +0,0 @@ -#pragma once - -#include - -#include -#include -#include -#include - -#include "rj_strategy/agent/position.hpp" -#include "rj_strategy/agent/position/role_interface.hpp" - -namespace strategy { - -/** - * Represents a Marker, a defensive role that targets an enemy robot - * and follows it around the field while it is on our side, blocking passes. - */ -class Marker : public RoleInterface { -private: - int target_{-1}; - - // Calculated from field dimensions - Prevent the marker from - // marking any enemies that are out of range; e. g. on the other - // side of the field or the sidelines. - float y_bound{FieldDimensions::kDefaultDimensions.length() / 2}; - float marker_follow_cutoff{FieldDimensions::kDefaultDimensions.width() / 2}; - -public: - Marker(FieldDimensions field_dimensions); - ~Marker() = default; - Marker(const Marker& other) = default; - Marker(Marker&& other) = default; - Marker& operator=(const Marker& other) = default; - Marker& operator=(Marker&& other) = default; - - std::optional get_task(RobotIntent intent, const WorldState* world_state, - FieldDimensions field_dimensions) override; - - void choose_target(const WorldState* ws); - int get_target(); - bool target_out_of_bounds(const WorldState* ws); -}; -} // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp new file mode 100644 index 0000000000..0350ef0f3b --- /dev/null +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking.hpp @@ -0,0 +1,61 @@ +#pragma once + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "rj_strategy/coordinator.hpp" + +namespace strategy { + +class Marking : public Coordinator { +public: + static constexpr uint8_t kInvalidRobotId = kNumShells; + + Marking(); + ~Marking() override = default; + Marking(const Marking&) = delete; + Marking& operator=(const Marking&) = delete; + Marking(Marking&&) = delete; + Marking& operator=(Marking&&) = delete; + + void service_callback(RequestPtr request, ResponsePtr response); + +private: + void publish_marking_list(); + void update_danger_scores(); + uint8_t find_their_robot_in_possession(); + uint8_t most_dangerous_robot(uint8_t robotInPossession); + + static constexpr int kMaxMarkers = 2; + // this is the threshold value for switching + // you can play with these constants if the current results aren't good enough + static constexpr double kSuperDangerSub = 3.2; + static constexpr double kDangerDistToBall = 3.0; + static constexpr double kDangerDistToGoal = 5.0; + static constexpr double kDangerDistToOurRobots = 3.0; + static constexpr double kDangerAngle = 2.0; + static constexpr double kPossessionThreshold = 0.3; + int num_markers_; + + std::array marking_list_{}; + std::array danger_score_{}; + std::array enemy_to_friends_{}; + std::unordered_set unassigned_markers_queue_; + WorldState last_world_state_; + FieldDimensions field_dimensions_ = FieldDimensions::kDefaultDimensions; + rclcpp::Subscription::SharedPtr world_state_sub_; +}; + +} // namespace strategy diff --git a/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp new file mode 100644 index 0000000000..5a42eb8a7c --- /dev/null +++ b/src/rj_strategy/include/rj_strategy/coordinator/marking_client.hpp @@ -0,0 +1,72 @@ +#pragma once + +#include + +#include +#include + +#include +#include + +#include "rj_constants/constants.hpp" + +namespace strategy { + +/** + * @brief Client for interacting with the marking coordinator. + * + * Manages membership in the kicker group and tracks the currently selected kicker. + */ +class MarkingClient { +public: + static constexpr uint8_t kInvalidRobotId = kNumShells; + + using StatusCallback = std::function; + + explicit MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id); + ~MarkingClient() = default; + MarkingClient(const MarkingClient&) = delete; + MarkingClient& operator=(const MarkingClient&) = delete; + MarkingClient(MarkingClient&&) = delete; + MarkingClient& operator=(MarkingClient&&) = delete; + + /** + * @brief Join the marking group. + * @param callback Called with current membership status after attempt to join. + */ + void join_group(StatusCallback callback = nullptr); + + /** + * @brief Leave the marking 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 marker group. + */ + [[nodiscard]] bool am_i_member() const; + + /** + * @brief Get the currently selected enemey robot id marking. + * @return robot ID of selected robot, or kInvalidRobotId if none selected. + */ + [[nodiscard]] uint8_t who_am_i_marking() const; + + /** + * @brief Check if this robot is currently marking. + */ + [[nodiscard]] bool am_i_marking() 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 am_i_marking_{false}; + uint8_t selected_robot_marking_id_{kInvalidRobotId}; +}; + +} // namespace strategy diff --git a/src/rj_strategy/src/agent/position/defense.cpp b/src/rj_strategy/src/agent/position/defense.cpp index eb80336a45..2df06e0e71 100644 --- a/src/rj_strategy/src/agent/position/defense.cpp +++ b/src/rj_strategy/src/agent/position/defense.cpp @@ -2,9 +2,9 @@ namespace strategy { -Defense::Defense(int r_id) : Position(r_id, "Defense"), marker_{field_dimensions_} {} +Defense::Defense(int r_id) : Position(r_id, "Defense") {} -Defense::Defense(Position&& other) : Position{std::move(other)}, marker_{field_dimensions_} { +Defense::Defense(Position&& other) : Position{std::move(other)} { position_name_ = "Defense"; walling_robots_ = {}; } @@ -21,14 +21,30 @@ std::string Defense::get_current_state() { } Defense::State Defense::update_state() { - State next_state = current_state_; - // handle transitions between states WorldState* world_state = last_world_state_; rj_geometry::Point robot_position = world_state->get_robot(true, robot_id_).pose.position(); rj_geometry::Point ball_position = world_state->ball.position; double distance_to_ball = robot_position.dist_to(ball_position); + // Update state based on coordinator async calls resolving + if (pending_marking_state_) { + // Defensive check: Only transition if we are still in the process of entering. + // We might have timed out and moved to another state in the meantime. + pending_marking_state_ = false; + if (current_state_ != ENTERING_MARKING) { + return IDLING; + } + + if (client_handles_->marking->am_i_member() && client_handles_->marking->am_i_marking()) { + return MARKING; + } else { + return IDLING; + } + } + + State next_state = current_state_; + if (current_state_ != WALLING && current_state_ != JOINING_WALL && waller_id_ != -1) { send_leave_wall_request(); walling_robots_ = {(u_int8_t)robot_id_}; @@ -37,6 +53,7 @@ Defense::State Defense::update_state() { switch (current_state_) { case IDLING: + next_state = JOINING_WALL; break; case JOINING_WALL: send_join_wall_request(); @@ -78,19 +95,37 @@ 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; + if (!client_handles_->marking->am_i_member() || + !client_handles_->marking->am_i_marking()) { + next_state = IDLING; } break; case ENTERING_MARKING: - marker_.choose_target(world_state); - int target_id = marker_.get_target(); - if (target_id == -1) { - next_state = ENTERING_MARKING; - } else { - next_state = MARKING; + // SPDLOG_INFO("Robot {}: entering marking", robot_id_); + + if (!sent_join_marking_group_request_) { + sent_join_marking_group_request_ = true; + request_time_ = RJ::now(); + + client_handles_->marking->join_group([this](const bool is_member) { + if (is_member) { + pending_marking_state_ = true; + } + }); + } + auto elapsed = RJ::now() - request_time_; + if (elapsed > kMarkingGroupJoinTimeout) { + // reset flag + sent_join_marking_group_request_ = false; + // ensure not in coordinator group + client_handles_->marking->leave_group(); + SPDLOG_INFO("Robot {}: Timeout on join group, IDLING now", robot_id_); + next_state = IDLING; } + + break; } return next_state; @@ -162,8 +197,19 @@ std::optional Defense::state_to_task(RobotIntent intent) { intent.motion_command = empty_motion_cmd; return intent; } else if (current_state_ == MARKING) { - // Marker marker = Marker((u_int8_t) robot_id_); - return marker_.get_task(intent, last_world_state_, this->field_dimensions_); + rj_geometry::Point targetPoint = + last_world_state_->get_robot(false, client_handles_->marking->who_am_i_marking()) + .pose.position(); + + rj_geometry::Point ballPoint = last_world_state_->ball.position; + rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(kMarkingDistanceFactor); + planning::LinearMotionInstant goal{targetPoint + targetToBall}; + // SPDLOG_INFO("Location to mark: {}, {}", (targetPoint + targetToBall).x(), (targetPoint + + // targetToBall).y()); + intent.motion_command = + planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; + + return intent; } return std::nullopt; diff --git a/src/rj_strategy/src/agent/position/goal_kicker.cpp b/src/rj_strategy/src/agent/position/goal_kicker.cpp index c98f3cb108..00f03ce936 100644 --- a/src/rj_strategy/src/agent/position/goal_kicker.cpp +++ b/src/rj_strategy/src/agent/position/goal_kicker.cpp @@ -36,4 +36,4 @@ void GoalKicker::derived_pass_ball() {} void GoalKicker::derived_acknowledge_ball_in_transit() {} -} // namespace strategy \ No newline at end of file +} // namespace strategy diff --git a/src/rj_strategy/src/agent/position/marker.cpp b/src/rj_strategy/src/agent/position/marker.cpp deleted file mode 100644 index f66f1625f2..0000000000 --- a/src/rj_strategy/src/agent/position/marker.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "rj_strategy/agent/position/marker.hpp" - -namespace strategy { -Marker::Marker(FieldDimensions field_dimensions) { - this->y_bound = field_dimensions.length() / 2; - this->marker_follow_cutoff = field_dimensions.width() / 2; -} - -std::optional Marker::get_task(RobotIntent intent, const WorldState* world_state, - [[maybe_unused]] FieldDimensions field_dimensions) { - this->y_bound = field_dimensions.length() / 2; - this->marker_follow_cutoff = field_dimensions.width() / 2; - - rj_geometry::Point targetPoint = world_state->get_robot(false, target_).pose.position(); - rj_geometry::Point ballPoint = world_state->ball.position; - rj_geometry::Point targetToBall = (ballPoint - targetPoint).normalized(0.55f); - planning::LinearMotionInstant goal{targetPoint + targetToBall, rj_geometry::Point{0.0, 0.0}}; - intent.motion_command = - planning::MotionCommand{"path_target", goal, planning::FaceBall{}, true}; - return intent; -} - -void Marker::choose_target(const WorldState* ws) { - // TODO: (James Vogt, github: jvogt23) - // If we ever use multiple Markers, they should choose different - // robots to track from each other. Logic for this operation must be - // added because multiple markers currently mark the same robot. - for (int i = 0; i < kNumShells; i++) { - if (std::fabs(ws->get_robot(false, i).pose.position().x()) < marker_follow_cutoff && - ws->get_robot(false, i).pose.position().y() < y_bound && - (ws->ball.position - ws->get_robot(false, i).pose.position()).mag() > .25) { - target_ = i; - return; - } - } - target_ = -1; -} - -bool Marker::target_out_of_bounds(const WorldState* ws) { - if (target_ == -1) return true; - if (ws->get_robot(false, target_).pose.position().y() > y_bound) { - target_ = -1; - return true; - } - return false; -} - -int Marker::get_target() { return target_; } -} // namespace strategy \ No newline at end of file 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..5bb16c5532 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,7 @@ 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_->marking = 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/coordinator/marking.cpp b/src/rj_strategy/src/coordinator/marking.cpp new file mode 100644 index 0000000000..6a209e6fc6 --- /dev/null +++ b/src/rj_strategy/src/coordinator/marking.cpp @@ -0,0 +1,253 @@ +#include "rj_strategy/coordinator/marking.hpp" + +namespace strategy { + +Marking::Marking() : Coordinator("marking_srv", "marking_data", "marking_node"), num_markers_{0} { + // Subscribe to world state + marking_list_.fill(kInvalidRobotId); // initializes to no valid markers + enemy_to_friends_.fill(kInvalidRobotId); // matches the enemy robot to who is marking them + danger_score_.fill( + std::numeric_limits::infinity()); // everyone starts with an infinite danger score + world_state_sub_ = this->create_subscription( + 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); + publish_marking_list(); + }); +} + +void Marking::service_callback(RequestPtr request, ResponsePtr response) { + if (request->join == false) { + bool marking = (marking_list_[request->robot_id] != kInvalidRobotId); + if (marking) { + uint8_t enemy_id = marking_list_[request->robot_id]; + marking_list_[request->robot_id] = kInvalidRobotId; + enemy_to_friends_[enemy_id] = kInvalidRobotId; + num_markers_--; + // The robot is leaving so replace the robot its marking if possible + const auto& enemy_robot = last_world_state_.get_robot(false, enemy_id); + double min = std::numeric_limits::infinity(); + uint8_t waiting_robot_id = kInvalidRobotId; + // Find closest robot in queue for replacement + // Queue is queue of robots that want to join marking but aren't good enough + // Not close enough to mark or we exceeed the max num of markers + for (uint8_t id : unassigned_markers_queue_) { + const auto& i_robot = last_world_state_.get_robot(true, id); + double distance = i_robot.pose.position().dist_to(enemy_robot.pose.position()); + if (distance < min) { + min = distance; + waiting_robot_id = id; + } + } + if (kInvalidRobotId != waiting_robot_id) { + unassigned_markers_queue_.erase(waiting_robot_id); + marking_list_[waiting_robot_id] = enemy_id; + enemy_to_friends_[enemy_id] = waiting_robot_id; + num_markers_++; + } + } else { + unassigned_markers_queue_.erase(request->robot_id); + } + response->success = true; + return; + } + if (num_markers_ < kMaxMarkers) { + // this means we can add this prospective marker as a marker + uint8_t robotInPossession = find_their_robot_in_possession(); + + uint8_t most_dangerous = most_dangerous_robot(robotInPossession); + if (most_dangerous != kInvalidRobotId) { + // Assign most dangerous unmarked robot without ball + enemy_to_friends_[most_dangerous] = request->robot_id; + marking_list_[request->robot_id] = most_dangerous; + num_markers_++; + } else { + unassigned_markers_queue_.insert(request->robot_id); + } + } else { + // should we kick someone out (is this new robot a better marker) + double better_distance = 0; + uint8_t kick_out_this_robot_id = kInvalidRobotId; + const auto& robot_requesting = last_world_state_.get_robot(true, request->robot_id); + rj_geometry::Point robot_requesting_pos = robot_requesting.pose.position(); + // Kicked out robot is one that is furthest from its marker and new robot is closer than it + for (size_t i = 0; i < kNumShells; ++i) { + if (marking_list_[i] != kInvalidRobotId) { + uint8_t enemy_id = marking_list_[i]; + const auto& i_robot = last_world_state_.get_robot(true, i); + const auto& enemy_robot = last_world_state_.get_robot(false, enemy_id); + double dist = + (i_robot.pose.position().dist_to(enemy_robot.pose.position())) - + (robot_requesting_pos.dist_to(enemy_robot.pose.position())); + if (dist > better_distance) { + better_distance = dist; + kick_out_this_robot_id = i; + } + } + } + if (kick_out_this_robot_id != kInvalidRobotId) { + uint8_t enemy_id = marking_list_[kick_out_this_robot_id]; + marking_list_[kick_out_this_robot_id] = kInvalidRobotId; + enemy_to_friends_[enemy_id] = request->robot_id; + marking_list_[request->robot_id] = enemy_id; + } else { + unassigned_markers_queue_.insert(request->robot_id); + } + } + + response->success = true; +} + +void Marking::publish_marking_list() { + // make this run on a timer + update_danger_scores(); + + // find the guy with possession of the ball if exists so we don't mark them + uint8_t robotInPossession = find_their_robot_in_possession(); + + // finding most dangerous of non-marked robots + uint8_t most_dangerous = most_dangerous_robot(robotInPossession); + + bool assigned = false; + // check if anyone being marked has the ball + // if so, remove them from being marked and assign them the most dangerous robot + for (size_t i = 0; i < kNumShells; ++i) { + if (marking_list_[i] == robotInPossession && robotInPossession != kInvalidRobotId) { + enemy_to_friends_[robotInPossession] = kInvalidRobotId; + marking_list_[i] = most_dangerous; + enemy_to_friends_[most_dangerous] = i; + assigned = true; + } + } + + // If the most dangerous is not assigned then find the robot that is assigned to least dangerous + if (most_dangerous != kInvalidRobotId && !assigned) { + uint8_t not_dangerous_robot_id = kInvalidRobotId; + double max_danger_sub = 0.0; + for (size_t i = 0; i < kNumShells; ++i) { + if (marking_list_[i] != kInvalidRobotId) { + uint8_t enemy_id = marking_list_[i]; + double danger_sub = danger_score_[enemy_id] - danger_score_[most_dangerous]; + if (danger_sub > max_danger_sub) { + max_danger_sub = danger_sub; + not_dangerous_robot_id = enemy_id; + } + } + } + // seeing if most dangerous of non-marked robots is significantly more dangerous than any + // marked robot + // This only gets rid of the most dangerous non-marked robot + // , subsequent runs will pick up next most dangerous + if (max_danger_sub > kSuperDangerSub && not_dangerous_robot_id != kInvalidRobotId) { + uint8_t friend_id = enemy_to_friends_[not_dangerous_robot_id]; + enemy_to_friends_[not_dangerous_robot_id] = kInvalidRobotId; + marking_list_[friend_id] = most_dangerous; + enemy_to_friends_[most_dangerous] = friend_id; + } + } + + publisher_->publish(rj_msgs::msg::Marking().set__mark_robot_ids(marking_list_)); +} + +void Marking::update_danger_scores() { + // danger score calculation is distance_to_ball * constant + distance_to_goal * constant - + // distance_from_our_closest_robot * constant - danger_angle * constant lower danger score is + // more dangerous + + const auto& ball_pos = last_world_state_.ball.position; + const auto& goal_loc = field_dimensions_.our_goal_loc(); + const auto& field_center = field_dimensions_.center_field_loc(); + + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + double dist_to_goal = robot.pose.position().dist_to(goal_loc); + + double min = std::numeric_limits::infinity(); + for (uint8_t j = 0; j < kNumShells; j++) { + const auto& i_friend = last_world_state_.get_robot(true, j); + if (!i_friend.visible) { + continue; + } + double dist = robot.pose.position().dist_to(i_friend.pose.position()); + if (dist < min) { + min = dist; + } + } + + double angle_between = 0.0; // Default to 0 (not dangerous) + // Check if beyond midfield + + if (field_dimensions_.our_half().hit(robot.pose.position())) { + const auto& vec_goal_to_center = field_center - goal_loc; + const auto& vec_goal_to_robot = robot.pose.position() - goal_loc; + + double cosTheta = vec_goal_to_center.dot(vec_goal_to_robot) / + (vec_goal_to_center.mag() * vec_goal_to_robot.mag()); + + if (cosTheta > 1.0) cosTheta = 1.0; + if (cosTheta < -1.0) cosTheta = -1.0; + double central_angle = std::abs(std::acos(cosTheta)); // [0, PI/2] + // Normalize + double normalized_danger = (M_PI_2 - central_angle) / M_PI_2; + if (normalized_danger < 0.0) normalized_danger = 0.0; // Clamp + + // Scales angles so that more central angles close together and more sideline are futher + // apart + const double kDangerAngleExponent = 0.25; + angle_between = std::pow(normalized_danger, kDangerAngleExponent); + } + + double danger_score = dist_to_ball * kDangerDistToBall + dist_to_goal * kDangerDistToGoal - + min * kDangerDistToOurRobots - angle_between * kDangerAngle; + + danger_score_[i] = danger_score; + } +} + +uint8_t Marking::find_their_robot_in_possession() { + uint8_t robotInPossession = kInvalidRobotId; + double min_dist_to_ball = std::numeric_limits::infinity(); + const auto& ball_pos = last_world_state_.ball.position; + for (uint8_t i = 0; i < kNumShells; i++) { + const auto& robot = last_world_state_.get_robot(false, i); + if (!robot.visible) { + continue; + } + double dist_to_ball = ball_pos.dist_to(robot.pose.position()); + if (dist_to_ball < min_dist_to_ball && dist_to_ball < kPossessionThreshold) { + robotInPossession = i; + min_dist_to_ball = dist_to_ball; + } + } + + return robotInPossession; +} + +uint8_t Marking::most_dangerous_robot(uint8_t robotInPossession) { + uint8_t most_dangerous = kInvalidRobotId; + double min = std::numeric_limits::infinity(); + for (size_t i = 0; i < kNumShells; ++i) { + // don't include marked robots or the guy with the ball in most dangerous calculation + if (enemy_to_friends_[i] != kInvalidRobotId || i == robotInPossession) { + continue; + } + if (danger_score_[i] < min) { + most_dangerous = i; + min = danger_score_[i]; + } + } + return most_dangerous; +} +} // namespace strategy + +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/src/rj_strategy/src/coordinator/marking_client.cpp b/src/rj_strategy/src/coordinator/marking_client.cpp new file mode 100644 index 0000000000..87421e15f8 --- /dev/null +++ b/src/rj_strategy/src/coordinator/marking_client.cpp @@ -0,0 +1,108 @@ +#include "rj_strategy/coordinator/marking_client.hpp" + +namespace strategy { + +/** + * @brief Client for interacting with the Marking coordinator. + * + * Manages membership in the kicker group and tracks the currently selected kicker. + */ + +MarkingClient::MarkingClient(rclcpp::Node::SharedPtr node, uint8_t robot_id) + : node_{std::move(node)}, + robot_id_{robot_id}, + selected_robot_marking_id_{MarkingClient::kInvalidRobotId} { + client_ = node_->create_client("marking_srv"); +} + +void MarkingClient::join_group(StatusCallback callback) { + if (am_i_member_) { + return; + } + + if (!client_->wait_for_service(std::chrono::seconds(1))) { + SPDLOG_ERROR("Marking service not available."); + if (callback) { + callback(false); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->join = 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. + if (!future.valid() || !future.get()->success) { + if (callback) { + callback(false); + } + return; + } + + am_i_member_ = true; + + // Create subscription to track selected kicker. + subscription_ = node_->create_subscription( + "marking_data", rclcpp::QoS(1).transient_local(), + [this](const rj_msgs::msg::Marking::SharedPtr msg) { + selected_robot_marking_id_ = msg->mark_robot_ids[robot_id_]; + am_i_marking_ = (selected_robot_marking_id_ != kInvalidRobotId); + }); + + callback(true); + + }); +} + +void MarkingClient::leave_group(StatusCallback callback) { + if (!am_i_member_) { + if (callback) { + callback(false); + } + return; + } + + auto request = std::make_shared(); + request->robot_id = robot_id_; + request->join = 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(false); + } + return; + } + + am_i_member_ = false; + + // Resetting the shared ptr releases our pointer to the + // subscription. ROS only keeps a weak_ptr, so this will + // deallocate the subscription. The callback will no longer be + // called. + subscription_.reset(); + am_i_marking_ = false; + selected_robot_marking_id_ = kInvalidRobotId; + + if (callback) { + callback(true); + } + }); +} + +bool MarkingClient::am_i_member() const { return am_i_member_; } + +uint8_t MarkingClient::who_am_i_marking() const { return selected_robot_marking_id_; } + +bool MarkingClient::am_i_marking() const { return am_i_marking_; } + +} // namespace strategy