From 5486577ce2d6f0df798f17fc784c457653f0aee0 Mon Sep 17 00:00:00 2001 From: Jonathan Gil Date: Sun, 29 Sep 2024 19:55:05 -0400 Subject: [PATCH 1/3] doubled value of wall spacing in waller.cpp --- soccer/src/soccer/strategy/agent/position/waller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/soccer/src/soccer/strategy/agent/position/waller.cpp b/soccer/src/soccer/strategy/agent/position/waller.cpp index 6919cdb7eb6..7416c0472cc 100644 --- a/soccer/src/soccer/strategy/agent/position/waller.cpp +++ b/soccer/src/soccer/strategy/agent/position/waller.cpp @@ -33,7 +33,7 @@ std::optional Waller::get_task(RobotIntent intent, const WorldState rj_geometry::Point mid_point{(goal_center_point) + (ball_dir_vector * min_wall_rad)}; // Calculate the wall spacing - auto wall_spacing = robot_diameter_multiplier_ * kRobotDiameter + kBallRadius; + auto wall_spacing = 2 * robot_diameter_multiplier_ * kRobotDiameter + kBallRadius; // Calculate the target point rj_geometry::Point target_point{}; From 3cb6ac545d4696cf84a6ea5abe886ada3e798d5d Mon Sep 17 00:00:00 2001 From: Jonathan Gil Date: Sun, 3 Nov 2024 20:49:22 -0500 Subject: [PATCH 2/3] Created runner.cpp/.hpp node to make robot run in rectangle --- soccer/#CMakeLists.txt# | 310 ++++++++++++++ soccer/src/soccer/CMakeLists.txt | 2 +- .../strategy/agent/#agent_action_client.cpp# | 388 ++++++++++++++++++ .../strategy/agent/.#agent_action_client.hpp | 1 + .../agent/position/robot_factory_position.cpp | 8 +- .../agent/position/robot_factory_position.hpp | 1 + .../soccer/strategy/agent/position/runner.cpp | 79 ++++ .../soccer/strategy/agent/position/runner.hpp | 48 +++ 8 files changed, 835 insertions(+), 2 deletions(-) create mode 100644 soccer/#CMakeLists.txt# create mode 100644 soccer/src/soccer/strategy/agent/#agent_action_client.cpp# create mode 120000 soccer/src/soccer/strategy/agent/.#agent_action_client.hpp create mode 100644 soccer/src/soccer/strategy/agent/position/runner.cpp create mode 100644 soccer/src/soccer/strategy/agent/position/runner.hpp diff --git a/soccer/#CMakeLists.txt# b/soccer/#CMakeLists.txt# new file mode 100644 index 00000000000..00ad3d4f8ba --- /dev/null +++ b/soccer/#CMakeLists.txt# @@ -0,0 +1,310 @@ +# ====================================================================== +# Preamble +# ====================================================================== +cmake_minimum_required(VERSION 3.16) +project(soccer LANGUAGES CXX) + +# ====================================================================== +# Find package +# ====================================================================== +find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(rj_msgs REQUIRED) +find_package(rj_drawing_msgs REQUIRED) +find_package(rj_geometry_msgs REQUIRED) + +# Qt5 +find_package( + Qt5 + COMPONENTS Widgets + Xml + Core + OpenGL + Network + Svg + REQUIRED) +message(STATUS "Found Qt5: ${Qt5Widgets_DIR}") +add_definitions(-DQT_NO_KEYWORDS) # Remove QT slots/signals/emit keywords + +# Google Protobuf +find_package(Protobuf REQUIRED) +include_directories(SYSTEM ${PROTOBUF_INCLUDE_DIR}) + +# Python +find_package(PythonInterp 3.2 REQUIRED) +find_package(PythonLibs 3.2 REQUIRED) + +# Eigen - used for linear algebra +find_package(Eigen3 REQUIRED) + +# SDL2 +include(FindPkgConfig) +pkg_search_module(SDL2 REQUIRED sdl2) + +# Pthread +find_package(Threads REQUIRED) + +# Boost +set(Boost_USE_STATIC_LIBS OFF) +set(Boost_USE_MULTITHREADED ON) +set(Boost_USE_STATIC_RUNTIME OFF) +find_package( + Boost + COMPONENTS system + REQUIRED) + +find_package(fmt REQUIRED) +find_package(spdlog REQUIRED) + +# ====================================================================== +# QT build tool things +# ====================================================================== +set(CMAKE_AUTOMOC ON) +set(CMAKE_AUTORCC ON) +set(CMAKE_AUTOUIC ON) + +# ====================================================================== +# Define Targets +# ====================================================================== +# Build stand-alone soccer dylib This is linked into soccer and our unit tests, as well as being a +# python module +add_library(robocup SHARED) + +set_target_properties(robocup PROPERTIES PREFIX "") +set_target_properties(robocup PROPERTIES SUFFIX ".so") + +add_executable(rj_vision_filter) + +add_executable(soccer) +add_executable(log_viewer) +add_executable(internal_referee_node) +add_executable(external_referee_node) +add_executable(planner_node) +add_executable(control_node) +add_executable(sim_radio_node) +add_executable(network_radio_node) +add_executable(manual_control_node) +add_executable(global_param_server_node) +add_executable(agent_action_client_node) + +if(BUILD_TESTING) + add_executable(test-soccer) +endif() + +add_subdirectory(src) + +# ====================================================================== +# Dependencies List +# ====================================================================== + +# ---- robocup ---- +set(ROBOCUP_DEPS_SYSTEM_INCLUDE_DIRS + ${EIGEN_INCLUDE_DIRS} + ${LIBUSB_1_INCLUDE_DIRS} + ${PYTHON_INCLUDE_DIRS} + ${RRT_INCLUDE_DIR} + ${SDL2_INCLUDE_DIRS}) + +set(ROBOCUP_DEPS_SYSTEM_LIBRARIES + ${Boost_LIBRARIES} + ${Boost_SYSTEM_LIBRARY} + fmt + GL + GLU + glut + ${PYTHON_LINK_DIRS} + ${PYTHON_LIBRARIES} + Qt5::Widgets + Qt5::Xml + Qt5::Core + Qt5::OpenGL + Qt5::Network + Qt5::Svg + spdlog + Threads::Threads + RRT + spnav + ${SDL2_LIBRARIES}) + +set(ROBOCUP_DEPS_INCLUDE_DIRS + ${CMAKE_CURRENT_SOURCE_DIR}/src/soccer ${CMAKE_CURRENT_SOURCE_DIR}/include + ${CMAKE_CURRENT_BINARY_DIR}/robocup_autogen/include) + +set(ROBOCUP_DEPS_LIBRARIES + rj_common + rj_param_utils + rj_protos + rc-fshare + rj_param_utils + rj_topic_utils + rj_utils + config_client) + +# ---- vision_filter ---- +set(RJ_VISION_FILTER_DEPS_SYSTEM_LIBRARIES spdlog) + +set(RJ_VISION_FILTER_DEPS_LIBRARIES rj_param_utils robocup) + +# ---- soccer ---- +set(SOCCER_DEPS_SYSTEM_LIBRARIES + Qt5::Widgets + Qt5::Xml + Qt5::Core + Qt5::OpenGL + Qt5::Network + Qt5::Svg) + +set(SOCCER_DEPS_LIBRARIES robocup) + +# ---- log_viewer ---- +set(LOG_VIEWER_DEPS_SYSTEM_LIBRARIES + Qt5::Core + Qt5::Widgets + Qt5::OpenGL + Qt5::Xml + Qt5::Svg) + +set(LOG_VIEWER_DEPS_LIBRARIES robocup) + +set(REFEREE_NODE_DEPS_SYSTEM_LIBRARIES) +set(REFEREE_NODE_DEPS_LIBRARIES robocup) + +set(RADIO_NODE_DEPS_SYSTEM_LIBRARIES) +set(RADIO_NODE_DEPS_LIBRARIES robocup) + +set(CONTROL_NODE_DEPS_SYSTEM_LIBRARIES) +set(CONTROL_NODE_DEPS_LIBRARIES robocup) + +set(PLANNER_NODE_DEPS_SYSTEM_LIBRARIES) +set(PLANNER_NODE_DEPS_LIBRARIES robocup) + +set(MANUAL_CONTROL_NODE_DEPS_SYSTEM_LIBRARIES) +set(MANUAL_CONTROL_NODE_DEPS_LIBRARIES robocup) + +set(GLOBAL_PARAM_SERVER_NODE_DEPS_SYSTEM_LIBRARIES) +set(GLOBAL_PARAM_SERVER_NODE_DEPS_LIBRARIES robocup) + +set(AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES) +set(AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES robocup) + +# ====================================================================== +# Include and Linking +# ====================================================================== + +# ---- Robocup ---- +ament_target_dependencies(robocup PUBLIC ament_index_cpp) + +target_include_directories(robocup SYSTEM PUBLIC ${ROBOCUP_DEPS_SYSTEM_INCLUDE_DIRS}) +target_include_directories(robocup PUBLIC ${ROBOCUP_DEPS_INCLUDE_DIRS}) +target_link_libraries(robocup PUBLIC ${ROBOCUP_DEPS_SYSTEM_LIBRARIES} ${ROBOCUP_DEPS_LIBRARIES}) + +ament_target_dependencies(robocup PUBLIC rj_geometry_msgs) +ament_target_dependencies(robocup PUBLIC rj_drawing_msgs) +ament_target_dependencies(robocup PUBLIC rj_msgs) + +# ---- rj_vision_filter ---- +target_link_libraries(rj_vision_filter PRIVATE robocup ${RJ_VISION_FILTER_DEPS_LIBRARIES} ${RJ_VISION_FILTER_DEPS_SYSTEM_LIBRARIES}) +ament_target_dependencies(rj_vision_filter PUBLIC ament_index_cpp) + +# ---- Soccer ---- +target_link_libraries(soccer PUBLIC ${SOCCER_DEPS_SYSTEM_LIBRARIES} ${SOCCER_DEPS_LIBRARIES}) + +# ---- log_viewer ---- +target_link_libraries(log_viewer PRIVATE ${LOG_VIEWER_DEPS_SYSTEM_LIBRARIES} + ${LOG_VIEWER_DEPS_LIBRARIES}) + +# -- external_referee_node -- +target_link_libraries(external_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_LIBRARIES} + ${REFEREE_NODE_DEPS_LIBRARIES}) + +# -- internal_referee_node -- +target_link_libraries(internal_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_LIBRARIES} + ${REFEREE_NODE_DEPS_LIBRARIES}) + +# -- sim_radio_node -- +target_link_libraries(sim_radio_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}) + +# -- control_node -- +target_link_libraries(control_node PRIVATE ${CONTROL_NODE_DEPS_SYSTEM_LIBRARIES} + ${CONTROL_NODE_DEPS_LIBRARIES}) + +# -- planner_node -- +target_link_libraries(planner_node PRIVATE ${PLANNER_NODE_DEPS_SYSTEM_LIBRARIES} + ${PLANNER_NODE_DEPS_LIBRARIES}) + +# -- manual_control_node -- +target_link_libraries(manual_control_node PRIVATE ${MANUAL_CONTROL_NODE_DEPS_SYSTEM_LIBRARIES} + ${MANUAL_CONTROL_NODE_DEPS_LIBRARIES}) + +# -- global_param_server_node -- +target_link_libraries(global_param_server_node PRIVATE ${GLOBAL_PARAM_SERVER_NODE_DEPS_SYSTEM_LIBRARIES} + ${GLOBAL_PARAM_SERVER_NODE_DEPS_LIBRARIES}) + +# -- agent_action_client_node -- +target_link_libraries(agent_action_client_node PRIVATE ${AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES} + ${AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES}) + +# ====================================================================== +# Testing +# ====================================================================== +# Add a test runner target "test-soccer" to run all tests in this directory +if(BUILD_TESTING) + enable_testing() + add_subdirectory(testing) + + target_link_libraries(test-soccer PRIVATE robocup) + target_link_libraries(test-soccer PRIVATE Qt5::Core Qt5::Widgets Qt5::Xml) + + set(test_libs geometry2d_testing rj_param_utils_testing) + foreach(test_lib ${test_libs}) + target_link_libraries( + test-soccer + PRIVATE + "-Wl,--whole-archive -L$ -l${test_lib} -Wl,--no-whole-archive") + target_link_libraries(test-soccer PRIVATE ${test_lib}) + endforeach() + + target_link_libraries(test-soccer PRIVATE gtest gtest_main) +endif() + +# ====================================================================== +# Packaging +# ====================================================================== +install( + TARGETS robocup + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +install( + TARGETS soccer + internal_referee_node + external_referee_node + rj_vision_filter + planner_node + control_node + sim_radio_node + network_radio_node + manual_control_node + global_param_server_node + agent_action_client_node + DESTINATION lib/${CMAKE_PROJECT_NAME}) + +install( + FILES config/realShopField.xml + DESTINATION share/${CMAKE_PROJECT_NAME}/config + RENAME soccer-real.cfg) + +install( + FILES config/sim.xml + DESTINATION share/${CMAKE_PROJECT_NAME}/config + RENAME soccer-sim.cfg) + +if(BUILD_TESTING) + install(TARGETS test-soccer DESTINATION lib/${CMAKE_PROJECT_NAME}) +endif() diff --git a/soccer/src/soccer/CMakeLists.txt b/soccer/src/soccer/CMakeLists.txt index ed6a2c41e65..dd187076bea 100644 --- a/soccer/src/soccer/CMakeLists.txt +++ b/soccer/src/soccer/CMakeLists.txt @@ -93,7 +93,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 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 a8802d828cd..1f91a181b0e 100644 --- a/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp +++ b/soccer/src/soccer/strategy/agent/position/robot_factory_position.hpp @@ -28,6 +28,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 From cc3d6d867c58b434875dc712bf93798e9738454d Mon Sep 17 00:00:00 2001 From: Jonathan Gil Date: Sun, 3 Nov 2024 22:21:15 -0500 Subject: [PATCH 3/3] created soccermom node --- launch/soccer.launch.py | 8 + soccer/#CMakeLists.txt# | 310 --------------------- soccer/CMakeLists.txt | 6 + soccer/src/soccer/CMakeLists.txt | 6 + soccer/src/soccer/soccer_mom.cpp | 17 ++ soccer/src/soccer/soccer_mom.hpp | 16 ++ soccer/src/soccer/soccer_mom_main.cpp | 7 + soccer/src/soccer/soccer_mom_node_main.cpp | 7 + 8 files changed, 67 insertions(+), 310 deletions(-) delete mode 100644 soccer/#CMakeLists.txt# create mode 100644 soccer/src/soccer/soccer_mom.cpp create mode 100644 soccer/src/soccer/soccer_mom.hpp create mode 100644 soccer/src/soccer/soccer_mom_main.cpp create mode 100644 soccer/src/soccer/soccer_mom_node_main.cpp 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# deleted file mode 100644 index 00ad3d4f8ba..00000000000 --- a/soccer/#CMakeLists.txt# +++ /dev/null @@ -1,310 +0,0 @@ -# ====================================================================== -# Preamble -# ====================================================================== -cmake_minimum_required(VERSION 3.16) -project(soccer LANGUAGES CXX) - -# ====================================================================== -# Find package -# ====================================================================== -find_package(ament_cmake REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(rj_msgs REQUIRED) -find_package(rj_drawing_msgs REQUIRED) -find_package(rj_geometry_msgs REQUIRED) - -# Qt5 -find_package( - Qt5 - COMPONENTS Widgets - Xml - Core - OpenGL - Network - Svg - REQUIRED) -message(STATUS "Found Qt5: ${Qt5Widgets_DIR}") -add_definitions(-DQT_NO_KEYWORDS) # Remove QT slots/signals/emit keywords - -# Google Protobuf -find_package(Protobuf REQUIRED) -include_directories(SYSTEM ${PROTOBUF_INCLUDE_DIR}) - -# Python -find_package(PythonInterp 3.2 REQUIRED) -find_package(PythonLibs 3.2 REQUIRED) - -# Eigen - used for linear algebra -find_package(Eigen3 REQUIRED) - -# SDL2 -include(FindPkgConfig) -pkg_search_module(SDL2 REQUIRED sdl2) - -# Pthread -find_package(Threads REQUIRED) - -# Boost -set(Boost_USE_STATIC_LIBS OFF) -set(Boost_USE_MULTITHREADED ON) -set(Boost_USE_STATIC_RUNTIME OFF) -find_package( - Boost - COMPONENTS system - REQUIRED) - -find_package(fmt REQUIRED) -find_package(spdlog REQUIRED) - -# ====================================================================== -# QT build tool things -# ====================================================================== -set(CMAKE_AUTOMOC ON) -set(CMAKE_AUTORCC ON) -set(CMAKE_AUTOUIC ON) - -# ====================================================================== -# Define Targets -# ====================================================================== -# Build stand-alone soccer dylib This is linked into soccer and our unit tests, as well as being a -# python module -add_library(robocup SHARED) - -set_target_properties(robocup PROPERTIES PREFIX "") -set_target_properties(robocup PROPERTIES SUFFIX ".so") - -add_executable(rj_vision_filter) - -add_executable(soccer) -add_executable(log_viewer) -add_executable(internal_referee_node) -add_executable(external_referee_node) -add_executable(planner_node) -add_executable(control_node) -add_executable(sim_radio_node) -add_executable(network_radio_node) -add_executable(manual_control_node) -add_executable(global_param_server_node) -add_executable(agent_action_client_node) - -if(BUILD_TESTING) - add_executable(test-soccer) -endif() - -add_subdirectory(src) - -# ====================================================================== -# Dependencies List -# ====================================================================== - -# ---- robocup ---- -set(ROBOCUP_DEPS_SYSTEM_INCLUDE_DIRS - ${EIGEN_INCLUDE_DIRS} - ${LIBUSB_1_INCLUDE_DIRS} - ${PYTHON_INCLUDE_DIRS} - ${RRT_INCLUDE_DIR} - ${SDL2_INCLUDE_DIRS}) - -set(ROBOCUP_DEPS_SYSTEM_LIBRARIES - ${Boost_LIBRARIES} - ${Boost_SYSTEM_LIBRARY} - fmt - GL - GLU - glut - ${PYTHON_LINK_DIRS} - ${PYTHON_LIBRARIES} - Qt5::Widgets - Qt5::Xml - Qt5::Core - Qt5::OpenGL - Qt5::Network - Qt5::Svg - spdlog - Threads::Threads - RRT - spnav - ${SDL2_LIBRARIES}) - -set(ROBOCUP_DEPS_INCLUDE_DIRS - ${CMAKE_CURRENT_SOURCE_DIR}/src/soccer ${CMAKE_CURRENT_SOURCE_DIR}/include - ${CMAKE_CURRENT_BINARY_DIR}/robocup_autogen/include) - -set(ROBOCUP_DEPS_LIBRARIES - rj_common - rj_param_utils - rj_protos - rc-fshare - rj_param_utils - rj_topic_utils - rj_utils - config_client) - -# ---- vision_filter ---- -set(RJ_VISION_FILTER_DEPS_SYSTEM_LIBRARIES spdlog) - -set(RJ_VISION_FILTER_DEPS_LIBRARIES rj_param_utils robocup) - -# ---- soccer ---- -set(SOCCER_DEPS_SYSTEM_LIBRARIES - Qt5::Widgets - Qt5::Xml - Qt5::Core - Qt5::OpenGL - Qt5::Network - Qt5::Svg) - -set(SOCCER_DEPS_LIBRARIES robocup) - -# ---- log_viewer ---- -set(LOG_VIEWER_DEPS_SYSTEM_LIBRARIES - Qt5::Core - Qt5::Widgets - Qt5::OpenGL - Qt5::Xml - Qt5::Svg) - -set(LOG_VIEWER_DEPS_LIBRARIES robocup) - -set(REFEREE_NODE_DEPS_SYSTEM_LIBRARIES) -set(REFEREE_NODE_DEPS_LIBRARIES robocup) - -set(RADIO_NODE_DEPS_SYSTEM_LIBRARIES) -set(RADIO_NODE_DEPS_LIBRARIES robocup) - -set(CONTROL_NODE_DEPS_SYSTEM_LIBRARIES) -set(CONTROL_NODE_DEPS_LIBRARIES robocup) - -set(PLANNER_NODE_DEPS_SYSTEM_LIBRARIES) -set(PLANNER_NODE_DEPS_LIBRARIES robocup) - -set(MANUAL_CONTROL_NODE_DEPS_SYSTEM_LIBRARIES) -set(MANUAL_CONTROL_NODE_DEPS_LIBRARIES robocup) - -set(GLOBAL_PARAM_SERVER_NODE_DEPS_SYSTEM_LIBRARIES) -set(GLOBAL_PARAM_SERVER_NODE_DEPS_LIBRARIES robocup) - -set(AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES) -set(AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES robocup) - -# ====================================================================== -# Include and Linking -# ====================================================================== - -# ---- Robocup ---- -ament_target_dependencies(robocup PUBLIC ament_index_cpp) - -target_include_directories(robocup SYSTEM PUBLIC ${ROBOCUP_DEPS_SYSTEM_INCLUDE_DIRS}) -target_include_directories(robocup PUBLIC ${ROBOCUP_DEPS_INCLUDE_DIRS}) -target_link_libraries(robocup PUBLIC ${ROBOCUP_DEPS_SYSTEM_LIBRARIES} ${ROBOCUP_DEPS_LIBRARIES}) - -ament_target_dependencies(robocup PUBLIC rj_geometry_msgs) -ament_target_dependencies(robocup PUBLIC rj_drawing_msgs) -ament_target_dependencies(robocup PUBLIC rj_msgs) - -# ---- rj_vision_filter ---- -target_link_libraries(rj_vision_filter PRIVATE robocup ${RJ_VISION_FILTER_DEPS_LIBRARIES} ${RJ_VISION_FILTER_DEPS_SYSTEM_LIBRARIES}) -ament_target_dependencies(rj_vision_filter PUBLIC ament_index_cpp) - -# ---- Soccer ---- -target_link_libraries(soccer PUBLIC ${SOCCER_DEPS_SYSTEM_LIBRARIES} ${SOCCER_DEPS_LIBRARIES}) - -# ---- log_viewer ---- -target_link_libraries(log_viewer PRIVATE ${LOG_VIEWER_DEPS_SYSTEM_LIBRARIES} - ${LOG_VIEWER_DEPS_LIBRARIES}) - -# -- external_referee_node -- -target_link_libraries(external_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_LIBRARIES} - ${REFEREE_NODE_DEPS_LIBRARIES}) - -# -- internal_referee_node -- -target_link_libraries(internal_referee_node PRIVATE ${REFEREE_NODE_DEPS_SYSTEM_LIBRARIES} - ${REFEREE_NODE_DEPS_LIBRARIES}) - -# -- sim_radio_node -- -target_link_libraries(sim_radio_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}) - -# -- control_node -- -target_link_libraries(control_node PRIVATE ${CONTROL_NODE_DEPS_SYSTEM_LIBRARIES} - ${CONTROL_NODE_DEPS_LIBRARIES}) - -# -- planner_node -- -target_link_libraries(planner_node PRIVATE ${PLANNER_NODE_DEPS_SYSTEM_LIBRARIES} - ${PLANNER_NODE_DEPS_LIBRARIES}) - -# -- manual_control_node -- -target_link_libraries(manual_control_node PRIVATE ${MANUAL_CONTROL_NODE_DEPS_SYSTEM_LIBRARIES} - ${MANUAL_CONTROL_NODE_DEPS_LIBRARIES}) - -# -- global_param_server_node -- -target_link_libraries(global_param_server_node PRIVATE ${GLOBAL_PARAM_SERVER_NODE_DEPS_SYSTEM_LIBRARIES} - ${GLOBAL_PARAM_SERVER_NODE_DEPS_LIBRARIES}) - -# -- agent_action_client_node -- -target_link_libraries(agent_action_client_node PRIVATE ${AGENT_ACTION_CLIENT_NODE_DEPS_SYSTEM_LIBRARIES} - ${AGENT_ACTION_CLIENT_NODE_DEPS_LIBRARIES}) - -# ====================================================================== -# Testing -# ====================================================================== -# Add a test runner target "test-soccer" to run all tests in this directory -if(BUILD_TESTING) - enable_testing() - add_subdirectory(testing) - - target_link_libraries(test-soccer PRIVATE robocup) - target_link_libraries(test-soccer PRIVATE Qt5::Core Qt5::Widgets Qt5::Xml) - - set(test_libs geometry2d_testing rj_param_utils_testing) - foreach(test_lib ${test_libs}) - target_link_libraries( - test-soccer - PRIVATE - "-Wl,--whole-archive -L$ -l${test_lib} -Wl,--no-whole-archive") - target_link_libraries(test-soccer PRIVATE ${test_lib}) - endforeach() - - target_link_libraries(test-soccer PRIVATE gtest gtest_main) -endif() - -# ====================================================================== -# Packaging -# ====================================================================== -install( - TARGETS robocup - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -install( - TARGETS soccer - internal_referee_node - external_referee_node - rj_vision_filter - planner_node - control_node - sim_radio_node - network_radio_node - manual_control_node - global_param_server_node - agent_action_client_node - DESTINATION lib/${CMAKE_PROJECT_NAME}) - -install( - FILES config/realShopField.xml - DESTINATION share/${CMAKE_PROJECT_NAME}/config - RENAME soccer-real.cfg) - -install( - FILES config/sim.xml - DESTINATION share/${CMAKE_PROJECT_NAME}/config - RENAME soccer-sim.cfg) - -if(BUILD_TESTING) - install(TARGETS test-soccer DESTINATION lib/${CMAKE_PROJECT_NAME}) -endif() 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 dd187076bea..21ea3b0d95b 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 @@ -132,6 +133,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) @@ -174,6 +177,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); +}