diff --git a/launch/soccer.launch.py b/launch/soccer.launch.py index d364b823474..177373db773 100644 --- a/launch/soccer.launch.py +++ b/launch/soccer.launch.py @@ -166,6 +166,14 @@ def generate_launch_description(): parameters=[param_config_filepath], on_exit=Shutdown(), ), + Node( + condition=IfCondition(PythonExpression([run_sim])), + package="rj_robocup", + executable="soccer_mom_node", + output="screen", + parameters=[param_config_filepath], + on_exit=Shutdown(), + ), Node( package="rj_robocup", executable="planner_node", diff --git a/soccer/CMakeLists.txt b/soccer/CMakeLists.txt index df564030637..63a592f57df 100644 --- a/soccer/CMakeLists.txt +++ b/soccer/CMakeLists.txt @@ -75,6 +75,7 @@ set_target_properties(robocup PROPERTIES SUFFIX ".so") add_executable(rj_vision_filter) +add_executable(soccer_mom_node) add_executable(soccer) add_executable(log_viewer) add_executable(internal_referee_node) @@ -225,6 +226,10 @@ target_link_libraries(internal_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_L target_link_libraries(sim_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES} ${RADIO_NODE_DEPS_LIBRARIES}) +# -- soccer_mom_node -- +target_link_libraries(soccer_mom_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES} + ${RADIO_NODE_DEPS_LIBRARIES}) + # -- network_radio_node -- target_link_libraries(network_radio_node PRIVATE ${RADIO_NODE_DEPS_SYSTEM_LIBRARIES} ${RADIO_NODE_DEPS_LIBRARIES}) @@ -289,6 +294,7 @@ install( planner_node control_node sim_radio_node + soccer_mom_node network_radio_node manual_control_node global_param_server_node diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index 452c350b9e2..eaaa3e0334e 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -54,6 +54,7 @@ set(ROBOCUP_LIB_SRC radio/packet_convert.cpp radio/sim_radio.cpp radio/radio.cpp + soccer_mom.cpp referee/referee_base.cpp referee/external_referee.cpp referee/internal_referee.cpp @@ -94,7 +95,7 @@ set(ROBOCUP_LIB_SRC strategy/agent/position/zoner.cpp strategy/agent/position/pivot_test.cpp strategy/agent/position/solo_offense.cpp - + strategy/agent/position/runner.cpp ) set(SOCCER_TEST_SRC @@ -133,6 +134,8 @@ set(CONTROL_NODE_SRC control/control_node_main.cpp) set(SIM_RADIO_NODE_SRC radio/sim_radio_node_main.cpp) +set(SOCCER_MOM_NODE_SRC soccer_mom_node_main.cpp) + set(NETWORK_RADIO_NODE_SRC radio/network_radio_node_main.cpp) set(MANUAL_CONTROL_NODE_SRC joystick/manual_control_node_main.cpp) @@ -175,6 +178,9 @@ target_sources(sim_radio_node PRIVATE ${SIM_RADIO_NODE_SRC}) # ---- sim_radio_node ---- target_sources(network_radio_node PRIVATE ${NETWORK_RADIO_NODE_SRC}) +# ---- soccer_mom_node ---- +target_sources(soccer_mom_node PRIVATE ${NETWORK_RADIO_NODE_SRC}) + # ---- manual_control_node ---- target_sources(manual_control_node PRIVATE ${MANUAL_CONTROL_NODE_SRC}) diff --git a/soccer/src/soccer/soccer_mom.cpp b/soccer/src/soccer/soccer_mom.cpp new file mode 100644 index 00000000000..1c290be5986 --- /dev/null +++ b/soccer/src/soccer/soccer_mom.cpp @@ -0,0 +1,17 @@ +#include "soccer_mom.hpp" + +SoccerMom::SoccerMom() : rclcpp::Node("SoccerMom") { // extends the ROS node and we call it "SoccerMom" + soccer_mom_pub_ = create_publisher("team_fruit", rclcpp::QoS(1)); //instantiate SoccerMom publisher + team_color_sub_ = create_subscription ( + referee::topics::kTeamColorTopic, rclcpp::QoS(1), + [this](rj_msgs::msg::TeamColor::SharedPtr color) { + auto message = std_msgs::msg::String(); // define msg we're gonna send out "String" + if (color->is_blue) { + message.data = "blueberries"; + } else { + message.data = "bananas"; + } + soccer_mom_pub_->publish(message); + } + ); + } diff --git a/soccer/src/soccer/soccer_mom.hpp b/soccer/src/soccer/soccer_mom.hpp new file mode 100644 index 00000000000..525068970b0 --- /dev/null +++ b/soccer/src/soccer/soccer_mom.hpp @@ -0,0 +1,16 @@ +#pragma once //so that we don't have duplicate identifiers + +#include +#include + +#include "std_msgs/msg/string.hpp" +#include "rj_msgs/msg/team_color.hpp" + +class SoccerMom : public rclcpp::Node { //extends the ROS node +public: + SoccerMom(); +private: + rclcpp::Subscription::SharedPtr team_color_sub_; // subscriber + rclcpp::Publisher::SharedPtr soccer_mom_pub_; //publisher + +}; diff --git a/soccer/src/soccer/soccer_mom_main.cpp b/soccer/src/soccer/soccer_mom_main.cpp new file mode 100644 index 00000000000..db365700c77 --- /dev/null +++ b/soccer/src/soccer/soccer_mom_main.cpp @@ -0,0 +1,7 @@ +#include "soccer_mom.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto soccer_mom = std::make_shared(); + rclcpp::spin(soccer_mom); +} diff --git a/soccer/src/soccer/soccer_mom_node_main.cpp b/soccer/src/soccer/soccer_mom_node_main.cpp new file mode 100644 index 00000000000..db365700c77 --- /dev/null +++ b/soccer/src/soccer/soccer_mom_node_main.cpp @@ -0,0 +1,7 @@ +#include "soccer_mom.hpp" + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + auto soccer_mom = std::make_shared(); + rclcpp::spin(soccer_mom); +} diff --git a/soccer/src/soccer/strategy/agent/#agent_action_client.cpp# b/soccer/src/soccer/strategy/agent/#agent_action_client.cpp# new file mode 100644 index 00000000000..587ca5dc9cf --- /dev/null +++ b/soccer/src/soccer/strategy/agent/#agent_action_client.cpp# @@ -0,0 +1,388 @@ +#include "agent_action_client.hpp" + +#include + +#include + +#include "game_state.hpp" +#include "rj_constants/topic_names.hpp" + +namespace strategy { +using RobotMove = rj_msgs::action::RobotMove; +using GoalHandleRobotMove = rclcpp_action::ClientGoalHandle; + +AgentActionClient::AgentActionClient() : AgentActionClient(0) { + // unclear why I need to explicitly create a default constructor, but compiler throws error when + // not here https://stackoverflow.com/questions/47704900/error-use-of-deleted-function +} + +AgentActionClient::AgentActionClient(int r_id) + : rclcpp::Node(::fmt::format("agent_{}_action_client_node", r_id), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true) + .allow_undeclared_parameters(true)), + current_position_{std::make_unique(r_id)}, + robot_id_{r_id} { + // create a ptr to ActionClient + client_ptr_ = rclcpp_action::create_client(this, "robot_move"); + + current_state_publisher_ = create_publisher( + fmt::format("strategy/positon/robot_state/robot_{}", r_id), 1); + + world_state_sub_ = create_subscription( + ::vision_filter::topics::kWorldStateTopic, 1, + [this](rj_msgs::msg::WorldState::SharedPtr msg) { world_state_callback(msg); }); + + play_state_sub_ = create_subscription( + ::referee::topics::kPlayStateTopic, 1, + [this](const rj_msgs::msg::PlayState::SharedPtr msg) { play_state_callback(msg); }); + + field_dimensions_sub_ = create_subscription( + "config/field_dimensions", rclcpp::QoS(1).transient_local(), + [this](rj_msgs::msg::FieldDimensions::SharedPtr msg) { field_dimensions_callback(msg); }); + + alive_robots_sub_ = create_subscription( + ::radio::topics::kAliveRobotsTopic, 1, + [this](rj_msgs::msg::AliveRobots::SharedPtr msg) { alive_robots_callback(msg); }); + + game_settings_sub_ = create_subscription( + "config/game_settings", 1, + [this](rj_msgs::msg::GameSettings::SharedPtr msg) { game_settings_callback(msg); }); + + goalie_id_sub_ = create_subscription( + ::referee::topics::kGoalieTopic, rclcpp::QoS(1).transient_local(), + [this](rj_msgs::msg::Goalie::SharedPtr msg) { goalie_id_callback(msg->goalie_id); }); + + robot_communication_srv_ = create_service( + fmt::format("agent_{}_incoming", r_id), + [this](const std::shared_ptr request, + std::shared_ptr response) { + receive_communication_callback(request, response); + }); + + // Create clients + for (size_t i = 0; i < kNumShells; i++) { + robot_communication_cli_[i] = + create_client(fmt::format("agent_{}_incoming", i)); + } + + int hz = 10; + get_task_timer_ = create_wall_timer(std::chrono::milliseconds(1000 / hz), + std::bind(&AgentActionClient::get_task, this)); + + int agent_communication_hz = 60; + get_communication_timer_ = + create_wall_timer(std::chrono::milliseconds(1000 / agent_communication_hz), [this]() { + get_communication(); + check_communication_timeout(); + }); +} + +void AgentActionClient::world_state_callback(const rj_msgs::msg::WorldState::SharedPtr& msg) { + if (current_position_ == nullptr) { + return; + } + + WorldState world_state = rj_convert::convert_from_ros(*msg); + // avoid mutex issues w/ world state (probably not an issue in AC, but + // already here so why not) + auto lock = std::lock_guard(world_state_mutex_); + last_world_state_ = std::move(world_state); +} + +void AgentActionClient::play_state_callback(const rj_msgs::msg::PlayState::SharedPtr& msg) { + if (current_position_ == nullptr) { + return; + } + + PlayState play_state = rj_convert::convert_from_ros(*msg); + play_state_ = play_state; + current_position_->update_play_state(play_state); +} + +void AgentActionClient::field_dimensions_callback( + const rj_msgs::msg::FieldDimensions::SharedPtr& msg) { + if (current_position_ == nullptr) { + return; + } + + FieldDimensions field_dimensions = rj_convert::convert_from_ros(*msg); + field_dimensions_ = field_dimensions; + current_position_->update_field_dimensions(field_dimensions); +} + +void AgentActionClient::goalie_id_callback(int goalie_id) { + if (current_position_) { + current_position_->set_goalie_id(goalie_id); + } + + goalie_id_ = goalie_id; +} + +void AgentActionClient::alive_robots_callback(const rj_msgs::msg::AliveRobots::SharedPtr& msg) { + alive_robots_ = msg->alive_robots; + + current_position_->update_alive_robots(alive_robots_); +} + +void AgentActionClient::game_settings_callback(const rj_msgs::msg::GameSettings::SharedPtr& msg) { + is_simulated_ = msg->simulation; +} + +bool AgentActionClient::check_robot_alive(u_int8_t robot_id) { + if (!is_simulated_) { + return alive_robots_.at(robot_id); + } else { + if (this->world_state()->get_robot(true, robot_id).visible) { + rj_geometry::Point robot_position = + this->world_state()->get_robot(true, robot_id).pose.position(); + rj_geometry::Rect padded_field_rect = field_dimensions_.field_coordinates(); + padded_field_rect.pad(field_padding_); + return padded_field_rect.contains_point(robot_position); + } + return false; + } +} + +void AgentActionClient::get_task() { + auto lock = std::lock_guard(world_state_mutex_); + + auto optional_task = + current_position_->get_task(last_world_state_, field_dimensions_, play_state_); + + if (optional_task.has_value()) { + RobotIntent task = optional_task.value(); + + // note that because these are our RobotIntent structs, this comparison + // uses our custom struct overloads + if (task != last_task_) { + last_task_ = task; + send_new_goal(); + } + } + + current_state_publisher_->publish(rj_msgs::build().state( + rj_convert::convert_to_ros(current_position_->get_current_state()))); +} + +void AgentActionClient::send_new_goal() { + using namespace std::placeholders; + + if (!client_ptr_->wait_for_action_server()) { + SPDLOG_ERROR("Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = RobotMove::Goal(); + goal_msg.robot_intent = rj_convert::convert_to_ros(last_task_); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = [this](auto arg) { + goal_response_callback(arg); + }; + send_goal_options.feedback_callback = [this](auto arg1, auto arg2) { + feedback_callback(arg1, arg2); + }; + send_goal_options.result_callback = [this](auto arg) { + result_callback(arg); + }; + client_ptr_->async_send_goal(goal_msg, send_goal_options); +} + +[[nodiscard]] WorldState* AgentActionClient::world_state() { + // thread-safe getter for world_state + auto lock = std::lock_guard(world_state_mutex_); + return &last_world_state_; +} + +// With the get_task function deleted, we need to initialize the new class once +// in the initializer. This will call the class which will analyze the +// situation based on the current tick. + +void AgentActionClient::goal_response_callback( + GoalHandleRobotMove::SharedPtr goal_handle) { + + if (!goal_handle) { + current_position_->set_goal_canceled(); + } +} + +void AgentActionClient::feedback_callback( + GoalHandleRobotMove::SharedPtr, const std::shared_ptr feedback) { + double time_left = rj_convert::convert_from_ros(feedback->time_left).count(); + if (current_position_ == nullptr) { + current_position_->set_time_left(time_left); + } +} + +void AgentActionClient::result_callback(const GoalHandleRobotMove::WrappedResult& result) { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: + // TODO: handle other return codes + current_position_->set_is_done(); + break; + case rclcpp_action::ResultCode::ABORTED: + return; + case rclcpp_action::ResultCode::CANCELED: + return; + default: + return; + } +} + +void AgentActionClient::get_communication() { + // Don't even humor requests from robots that aren't alive + if (!check_robot_alive(robot_id_)) { + return; + } + + bool any_robots_alive = false; + for (u_int8_t i = 0; i < kNumShells; i++) { + if (check_robot_alive(i)) { + any_robots_alive = true; + break; + } + } + + if (!any_robots_alive) { + return; + } + + auto optional_communication_request = current_position_->send_communication_request(); + if (optional_communication_request.empty()) { + return; + } + + for (int i = 0; i < optional_communication_request.size(); i++) { + auto communication_request = optional_communication_request.front(); + optional_communication_request.pop_front(); + + // create a buffer to hold the responses and the outgoing request + communication::AgentPosResponseWrapper buffered_response; + + auto request = std::make_shared(); + request->agent_request = rj_convert::convert_to_ros(communication_request.request); + + // send communication requests + std::vector sent_robot_ids = {}; + if (communication_request.broadcast) { + for (u_int8_t i = 0; i < kNumShells; i++) { + if (i != robot_id_ && check_robot_alive(i)) { + robot_communication_cli_[i]->async_send_request( + request, [this, i](const std::shared_future< + rj_msgs::srv::AgentCommunication::Response::SharedPtr> + response) { + receive_response_callback(response, ((u_int8_t)i)); + }); + sent_robot_ids.push_back(i); + } + } + // set broadcast to true in buffer + buffered_response.broadcast = true; + } else { + for (u_int8_t i : communication_request.target_agents) { + if (i != robot_id_ && check_robot_alive(i)) { + robot_communication_cli_[i]->async_send_request( + request, + [this, i](const std::shared_future< + rj_msgs::srv::AgentCommunication::Response::SharedPtr> + response) { receive_response_callback(response, i); }); + sent_robot_ids.push_back(i); + } + } + } + + buffered_response.to_robot_ids = sent_robot_ids; + buffered_response.associated_request = communication_request.request; + buffered_response.urgent = communication_request.urgent; + buffered_response.created = RJ::now(); + buffered_responses_.push_back(buffered_response); + } +} + +void AgentActionClient::receive_communication_callback( + const std::shared_ptr& request, + const std::shared_ptr& response) { + if (current_position_ == nullptr) { + communication::AgentResponse agent_response; + communication::AgentRequest agent_request = + rj_convert::convert_from_ros(request->agent_request); + communication::Acknowledge acknowledge{}; + communication::generate_uid(acknowledge); + agent_response.associated_request = agent_request; + agent_response.response = acknowledge; + response->agent_response = rj_convert::convert_to_ros(agent_response); + } else { + // Convert agent request into AgentToPosCommRequest + communication::AgentPosRequestWrapper agent_request; + communication::AgentRequest received_request = + rj_convert::convert_from_ros(request->agent_request); + agent_request.request = received_request; + + // Give the current position the request and receive the response to send back + communication::PosAgentResponseWrapper pos_to_agent_response = + current_position_->receive_communication_request(agent_request); + + // Convert PosToAgentCommResponse into AgentResponse + communication::AgentResponse agent_response{received_request, + pos_to_agent_response.response}; + response->agent_response = rj_convert::convert_to_ros(agent_response); + } +} + +void AgentActionClient::receive_response_callback( + const std::shared_future& response, + u_int8_t robot_id) { + // Convert response from other agent to c++ + communication::AgentResponse agent_response = + rj_convert::convert_from_ros(response.get()->agent_response); + + for (u_int32_t i = 0; i < buffered_responses_.size(); i++) { + if (buffered_responses_[i].associated_request == agent_response.associated_request) { + // add the robot id in the corresponding (increasing) location in the received_robot_ids + if (buffered_responses_[i].received_robot_ids.empty()) { + buffered_responses_[i].received_robot_ids.push_back(robot_id); + buffered_responses_[i].responses.push_back(agent_response.response); + } else { + for (u_int32_t j = 0; j < buffered_responses_[i].received_robot_ids.size(); j++) { + if (j == buffered_responses_[i].received_robot_ids.size() - 1) { + // The response should be added at the end of the buffer + buffered_responses_[i].received_robot_ids.push_back(robot_id); + buffered_responses_[i].responses.push_back(agent_response.response); + break; + } else if (buffered_responses_[i].received_robot_ids[j] > robot_id) { + // The response should be added at i in the buffer + buffered_responses_[i].received_robot_ids.insert( + buffered_responses_[i].received_robot_ids.begin() + j, robot_id); + buffered_responses_[i].responses.insert( + buffered_responses_[i].responses.begin() + j, agent_response.response); + break; + } + } + } + + // if the message is urgent -> relay the response + // if we've received a response from all of the robots we want -> relay the response + if (buffered_responses_[i].urgent || + (buffered_responses_[i].broadcast && + buffered_responses_[i].received_robot_ids.size() >= 5) || + (buffered_responses_[i].received_robot_ids.size() == + buffered_responses_[i].to_robot_ids.size())) { + current_position_->receive_communication_response(buffered_responses_[i]); + buffered_responses_.erase(buffered_responses_.begin() + i); + return; + } + } + } +} + +void AgentActionClient::check_communication_timeout() { + for (u_int32_t i = 0; i < buffered_responses_.size(); i++) { + if (RJ::now() - buffered_responses_[i].created > timeout_duration_) { + current_position_->receive_communication_response(buffered_responses_[i]); + buffered_responses_.erase(buffered_responses_.begin() + i); + } + } +} + +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/.#agent_action_client.hpp b/soccer/src/soccer/strategy/agent/.#agent_action_client.hpp new file mode 120000 index 00000000000..f61121409dc --- /dev/null +++ b/soccer/src/soccer/strategy/agent/.#agent_action_client.hpp @@ -0,0 +1 @@ +jonathan@jonathan-QEMU-Virtual-Machine.72265:1729470116 \ No newline at end of file diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp index 7634ff96220..54f20341e3b 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.cpp @@ -216,7 +216,13 @@ bool RobotFactoryPosition::am_closest_kicker() { } void RobotFactoryPosition::set_default_position() { - // zoner defense testing + if (robot_id_ == 1) { + set_current_position(); + } else { + set_current_position(); + } + return; + // zoner defense testing // if (robot_id_ == goalie_id_) { // return; // } diff --git a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp index aae7765c166..925392c923d 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -29,6 +29,7 @@ #include "strategy/agent/position/smartidling.hpp" #include "strategy/agent/position/solo_offense.hpp" #include "strategy/agent/position/zoner.hpp" +#include "strategy/agent/position/runner.hpp" namespace strategy { diff --git a/soccer/src/soccer/strategy/agent/position/runner.cpp b/soccer/src/soccer/strategy/agent/position/runner.cpp new file mode 100644 index 00000000000..1aa4c96e206 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/runner.cpp @@ -0,0 +1,79 @@ +#include "runner.hpp" + +#include + +#include "position.hpp" + +namespace strategy { + +Runner::Runner(int r_id) : Position{r_id, "Runner"} {} + +Runner::Runner(const Position& other) : Position{other} {} + +std::string Runner::get_current_state() { return "Runner"; } + +Runner::State Runner::next_state() { + if (!check_is_done()) { + return current_state_; + } + switch(current_state_) { + case TOP_LEFT: + return TOP_RIGHT; + case TOP_RIGHT: + return BOTTOM_RIGHT; + case BOTTOM_RIGHT: + return BOTTOM_LEFT; + case BOTTOM_LEFT: + return TOP_LEFT; + default: + return current_state_; + } +} + +/** + * @brief Does nothing; this position is a special case + */ +void Runner::derived_acknowledge_pass() {} +/** + * @brief Does nothing; this position is a special case + */ +void Runner::derived_pass_ball(){ + +}; +/** + * @brief Does nothing; this position is a special case + */ +void Runner::derived_acknowledge_ball_in_transit() {} + +std::optional Runner::derived_get_task(RobotIntent intent) { + current_state_ = next_state(); + + // Calculate task based on state + return state_to_task(intent); +}; + + +std::optional Runner::state_to_task(RobotIntent intent) { + planning::LinearMotionInstant target; + switch (current_state_) { + case TOP_LEFT: + target = planning::LinearMotionInstant{rj_geometry::Point{2.0, 2.5}}; + break; + case TOP_RIGHT: + target = planning::LinearMotionInstant{rj_geometry::Point{-2.0, 2.5}}; + break; + case BOTTOM_RIGHT: + target = planning::LinearMotionInstant{rj_geometry::Point{-2.0, 6.5}}; + break; + case BOTTOM_LEFT: + target = planning::LinearMotionInstant{rj_geometry::Point{2.0, 6.5}}; + break; + } + + planning::MotionCommand prep_command{"path_target", target, planning::FaceTarget{}}; + intent.motion_command = prep_command; + return intent; + +} + +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/runner.hpp b/soccer/src/soccer/strategy/agent/position/runner.hpp new file mode 100644 index 00000000000..6d5638c3805 --- /dev/null +++ b/soccer/src/soccer/strategy/agent/position/runner.hpp @@ -0,0 +1,48 @@ +#include + +#include "position.hpp" + +namespace strategy { +class Runner : public Position { +public: + Runner(int r_id); + ~Runner() = default; + Runner(const Position& other); + + /** + * @brief Does nothing; this position is a special case + */ + void derived_acknowledge_pass() override; + /** + * @brief Does nothing; this position is a special case + */ + void derived_pass_ball() override; + /** + * @brief Does nothing; this position is a special case + */ + void derived_acknowledge_ball_in_transit() override; + + std::string get_current_state() override; + +private: + std::optional derived_get_task(RobotIntent intent) override; + + + enum State { + TOP_LEFT, + TOP_RIGHT, + BOTTOM_LEFT, + BOTTOM_RIGHT + }; + + State current_state_ = State::TOP_LEFT; + + /** + * @return what the state should be right now. called on each get_task tick + */ + State next_state(); + + std::optional state_to_task(RobotIntent intent); + +}; +} // namespace strategy diff --git a/soccer/src/soccer/strategy/agent/position/waller.cpp b/soccer/src/soccer/strategy/agent/position/waller.cpp index c841f7dc4d1..ac61386c2aa 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.cpp +++ b/soccer/src/soccer/strategy/agent/position/waller.cpp @@ -35,7 +35,9 @@ std::optional Waller::get_task(RobotIntent intent, const WorldState // Find target Point rj_geometry::Point mid_point{(goal_pos) + (ball_dir_vector * min_wall_rad)}; - auto wall_spacing = kRobotDiameterMultiplier * kRobotDiameter + kBallRadius; + + // Calculate the wall spacing + auto wall_spacing = 2 * robot_diameter_multiplier_ * kRobotDiameter + kBallRadius; rj_geometry::Point target_point{}; auto angle = (mid_point - goal_pos).angle();