diff --git a/rj_common/include/rj_common/network.hpp b/rj_common/include/rj_common/network.hpp index 886e1221a57..bf4e8d9dfdf 100644 --- a/rj_common/include/rj_common/network.hpp +++ b/rj_common/include/rj_common/network.hpp @@ -59,16 +59,21 @@ static const std::string kSharedVisionSourceAddress = "224.5.23.2"; * one (or try them all in worst-case). */ -static const std::string kRefereeInterface = "127.0.0.1"; +static const std::string kRefereeInterface = "192.168.20.119"; /* static const std::string kRefereeInterface = "172.0.0.1"; */ static const std::string kVisionInterface = kRefereeInterface; // In all but rare cirucmstances, this should match kRefereeInterface. -static const std::string kBaseStationAddress = "10.42.0.136"; +// The network address of the base station +static const std::string kBaseStationAddress = "10.42.0.252"; +// The Port (on the local machine to bind the base station communication socket to) +static const int kBaseStationBindPort = 8000; +// The Port (on the base station) to send control messages to static const int kBaseStationPort = 8000; - -static const int kIncomingBaseStationDataPort = 8001; -static const int kIncomingBaseStationAliveRobotsPort = 8002; +// The Port (on the local machine) to receive robot status messages at +static const int kIncomingRobotStatusPort = 8001; +// The Port (on the local machine) to receive alive robots messages at +static const int kIncomingAliveRobotsPort = 8002; static const int kSimVisionPort = 10020; // was 10020 before 1-30-2022 static const int kSimBlueStatusPort = 30011; diff --git a/rj_msgs/msg/AliveRobots.msg b/rj_msgs/msg/AliveRobots.msg index 66e80c95d5e..af98bd8488e 100644 --- a/rj_msgs/msg/AliveRobots.msg +++ b/rj_msgs/msg/AliveRobots.msg @@ -1 +1,3 @@ -uint8[] alive_robots \ No newline at end of file +# kNumShells is currently 16. (Please keep this as a constant size, but find +# some way to tie this to the constant) +bool[16] alive_robots \ No newline at end of file diff --git a/soccer/src/soccer/radio/network_radio.cpp b/soccer/src/soccer/radio/network_radio.cpp index 8be0796a961..e77d17a4fcc 100644 --- a/soccer/src/soccer/radio/network_radio.cpp +++ b/soccer/src/soccer/radio/network_radio.cpp @@ -17,32 +17,33 @@ using ip::udp; namespace radio { NetworkRadio::NetworkRadio() - : socket(io_service), + : base_station_socket_(io_service_), robot_status_buffer_{}, alive_robots_buffer_{}, send_buffers_(kNumShells) { - socket.open(udp::v4()); - socket.bind(udp::endpoint(udp::v4(), kIncomingBaseStationDataPort)); + base_station_socket_.open(udp::v4()); + base_station_socket_.bind(udp::endpoint(udp::v4(), kBaseStationBindPort)); start_receive(); - - alive_robots_pub_ = - this->create_publisher("strategy/alive_robots", rclcpp::QoS(1)); } void NetworkRadio::start_receive() { - socket.async_receive_from(boost::asio::buffer(robot_status_buffer_), robot_status_endpoint, - [this](const boost::system::error_code& error, - std::size_t num_bytes) { receive_packet(error, num_bytes); }); - - socket.async_receive_from( - boost::asio::buffer(alive_robots_buffer_), alive_robots_endpoint, - [this](const boost::system::error_code& error, std::size_t num_bytes) { - publish_alive_robots(error, num_bytes); - }); + base_station_socket_.async_receive_from( + boost::asio::buffer(robot_status_buffer_), robot_status_endpoint_, + [this](const boost::system::error_code& error, size_t num_bytes) { + receive_robot_status(error, num_bytes); + } + ); + + base_station_socket_.async_receive_from( + boost::asio::buffer(alive_robots_buffer_), alive_robots_endpoint_, + [this](const boost::system::error_code& error, size_t num_bytes) { + receive_alive_robots(error, num_bytes); + } + ); } -void NetworkRadio::send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, +void NetworkRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion, const rj_msgs::msg::ManipulatorSetpoint& manipulator, strategy::Positions role) { // Build the control packet for this robot. @@ -52,25 +53,30 @@ void NetworkRadio::send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion // NOLINTNEXTLINE(cppcoreguidelines-pro-type-reinterpret-cast) auto* body = reinterpret_cast(&forward_packet_buffer[0]); - ConvertTx::ros_to_rtp(manipulator, motion, robot_id, body, role, _blue_team); + ConvertTx::ros_to_rtp(manipulator, motion, robot_id, body, role, blue_team()); - socket.async_send_to( - boost::asio::buffer(forward_packet_buffer), base_station_endpoint, - [](const boost::system::error_code& error, [[maybe_unused]] std::size_t num_bytes) { + base_station_socket_.async_send_to( + boost::asio::buffer(forward_packet_buffer), control_message_endpoint_, + [](const boost::system::error_code& error, [[maybe_unused]] std::size_t num_Bytes) { if (static_cast(error)) { SPDLOG_ERROR("Error Sending: {}", error.message()); } - }); + } + ); } -void NetworkRadio::receive() { +void NetworkRadio::poll_receive() { // Let boost::asio handle callbacks - io_service.poll(); + io_service_.poll(); +} + +void NetworkRadio::switch_team(bool blue_team) { + // TODO (Nate): Send some command to the base station to switch teams. } -void NetworkRadio::receive_packet(const boost::system::error_code& error, std::size_t num_bytes) { +void NetworkRadio::receive_robot_status(const boost::system::error_code& error, size_t num_bytes) { if (static_cast(error)) { - SPDLOG_ERROR("Error sending: {}.", error); + SPDLOG_ERROR("Error Receiving Robot Status: {}.", error.message()); return; } if (num_bytes != sizeof(rtp::RobotStatusMessage)) { @@ -89,28 +95,35 @@ void NetworkRadio::receive_packet(const boost::system::error_code& error, std::s ConvertRx::rtp_to_status(*msg, &status); ConvertRx::status_to_ros(status, &status_ros); - publish(robot_id, status_ros); + publish_robot_status(robot_id, status_ros); // Restart receiving start_receive(); } -void NetworkRadio::switch_team(bool blue_team) { _blue_team = blue_team; } +void NetworkRadio::receive_alive_robots(const boost::system::error_code& error, size_t num_bytes) { + if (static_cast(error)) { + SPDLOG_ERROR("Error Receiving Alive Robots: {}", error.message()); + return; + } + + if (num_bytes != 2) { + SPDLOG_ERROR("Invalid Packet Length: expected {}, got {}", 2, num_bytes); + return; + } -void NetworkRadio::publish_alive_robots(const boost::system::error_code& error, - std::size_t num_bytes) { - uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[1]); - std::vector alive_robots = {}; + uint16_t alive = (alive_robots_buffer_[0] << 8) | (alive_robots_buffer_[0]); for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) { if (alive & (1 << robot_id) != 0) { - alive_robots.push_back(robot_id); + alive_robots_[robot_id] = true; + } else { + alive_robots_[robot_id] = false; } } - // publish a message containing the alive robots rj_msgs::msg::AliveRobots alive_message{}; - alive_message.alive_robots = alive_robots; - alive_robots_pub_->publish(alive_message); + alive_message.alive_robots = alive_robots_; + publish_alive_robots(alive_message); } } // namespace radio diff --git a/soccer/src/soccer/radio/network_radio.hpp b/soccer/src/soccer/radio/network_radio.hpp index 99242196613..d79ce9a88e7 100644 --- a/soccer/src/soccer/radio/network_radio.hpp +++ b/soccer/src/soccer/radio/network_radio.hpp @@ -27,48 +27,66 @@ class NetworkRadio : public Radio { NetworkRadio(); protected: - void send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, + // Send Control Message through the Base Station to the Robots + void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion, const rj_msgs::msg::ManipulatorSetpoint& manipulator, strategy::Positions role) override; - void receive() override; - void switch_team(bool blue) override; + + // Poll the asynchronous boost::asio receiver + void poll_receive() override; - boost::asio::ip::udp::endpoint base_station_endpoint = boost::asio::ip::udp::endpoint( - boost::asio::ip::address::from_string(kBaseStationAddress), kBaseStationPort); - - boost::asio::ip::udp::endpoint robot_status_endpoint = boost::asio::ip::udp::endpoint( - boost::asio::ip::address::from_string("127.0.0.1"), kIncomingBaseStationDataPort); - boost::asio::ip::udp::endpoint alive_robots_endpoint = boost::asio::ip::udp::endpoint( - boost::asio::ip::address::from_string("127.0.0.1"), kIncomingBaseStationAliveRobotsPort); - - // Fuck Around and Find Out - // https://stackoverflow.com/questions/26243008/error-initializing-a-boost-udp-socket-with-a-boost-io-service - boost::asio::io_service io_service; - boost::asio::ip::udp::socket socket; - - void receive_packet(const boost::system::error_code& error, std::size_t num_bytes); + // Switch teams and let the base station know + void switch_team(bool blue_team) override; +private: + /** + * @brief Setup the base_station_socket_ to receive robot status and alive robot messages + * from the base station + * + */ void start_receive(); - // Written by `async_receive_from`. - std::array robot_status_buffer_; - - // Written by 'async_receive_from`. - std::array alive_robots_buffer_; - - // Read from by `async_send_to` - std::vector> send_buffers_{}; - - rclcpp::Publisher::SharedPtr alive_robots_pub_; + /** + * @brief Parse the alive robots from a packet received via the base station. + * + * @param error + * @param num_bytes + */ + void receive_robot_status(const boost::system::error_code& error, size_t num_bytes); /** - * @brief Publish a vector of alive robots to the "strategy/alive_robots" endpoint - * + * @brief Parse the alive robots from a packet received via the base station. + * + * @param error + * @param num_bytes */ - void publish_alive_robots(const boost::system::error_code& error, std::size_t num_bytes); + void receive_alive_robots(const boost::system::error_code& error, size_t num_bytes); -private: - bool _blue_team = false; + // Where to send control messages to + boost::asio::ip::udp::endpoint control_message_endpoint_ = boost::asio::ip::udp::endpoint( + boost::asio::ip::address::from_string(kBaseStationAddress), kBaseStationPort); + // Buffer to send a set of control messages + std::vector> send_buffers_{}; + + // What local endpoint to expect robot statuses to be received at + boost::asio::ip::udp::endpoint robot_status_endpoint_ = boost::asio::ip::udp::endpoint( + boost::asio::ip::address::from_string("127.0.0.1"), kIncomingRobotStatusPort); + // Buffer for an incoming robot status from the base station + std::array robot_status_buffer_{}; + + // Where local endpoint to expect alive robots to be received at + boost::asio::ip::udp::endpoint alive_robots_endpoint_ = boost::asio::ip::udp::endpoint( + boost::asio::ip::address::from_string("127.0.0.1"), kIncomingAliveRobotsPort); + // Buffer for an alive robots message from the base station + std::array alive_robots_buffer_{}; + // if alive_robots_[robot_id] = true => robot[robot_id] is alive + std::array alive_robots_{}; + + // Keep io_service above the socket + // https://stackoverflow.com/questions/26243008/error-initializing-a-boost-udp-socket-with-a-boost-io-service + boost::asio::io_service io_service_; + // The socket used to communicate with the base station + boost::asio::ip::udp::socket base_station_socket_; }; } // namespace radio diff --git a/soccer/src/soccer/radio/radio.cpp b/soccer/src/soccer/radio/radio.cpp index c00f0db9194..99c6c6644dc 100644 --- a/soccer/src/soccer/radio/radio.cpp +++ b/soccer/src/soccer/radio/radio.cpp @@ -15,7 +15,10 @@ Radio::Radio() team_color_sub_ = create_subscription( referee::topics::kTeamColorTopic, rclcpp::QoS(1).transient_local(), [this](rj_msgs::msg::TeamColor::SharedPtr color) { // NOLINT - switch_team(color->is_blue); + if (color->is_blue != blue_team_) { + blue_team_ = color->is_blue; + switch_team(color->is_blue); + } }); for (size_t i = 0; i < kNumShells; i++) { @@ -34,24 +37,34 @@ Radio::Radio() if (i == 0) { SPDLOG_INFO("\033[92mRobot 0 Received Normal Communication\033[0m"); } - send(i, *motion, manipulators_cached_.at(i), positions_.at(i)); + send_control_message(i, *motion, manipulators_cached_.at(i), positions_.at(i)); }); } + alive_robots_pub_ = create_publisher( + "/alive_robots", rclcpp::QoS(1)); + tick_timer_ = create_wall_timer(std::chrono::milliseconds(100), [this]() { tick(); }); } -void Radio::publish(int robot_id, const rj_msgs::msg::RobotStatus& robot_status) { +void Radio::publish_robot_status(int robot_id, const rj_msgs::msg::RobotStatus& robot_status) { robot_status_pubs_.at(robot_id)->publish(robot_status); } +void Radio::publish_alive_robots(const rj_msgs::msg::AliveRobots& alive_robots) { + alive_robots_pub_->publish(alive_robots); +} + +const bool Radio::blue_team() { return blue_team_; } + void Radio::tick() { - receive(); + poll_receive(); RJ::Time update_time = RJ::now(); for (size_t i = 0; i < kNumShells; i++) { if (last_updates_.at(i) + RJ::Seconds(PARAM_timeout) < update_time) { + // Send Alive Robots an Empty Motion Command (i.e. `STOP`) using rj_msgs::msg::ManipulatorSetpoint; using rj_msgs::msg::MotionSetpoint; @@ -66,10 +79,7 @@ void Radio::tick() { .kick_speed(0) .dribbler_speed(0); last_updates_.at(i) = RJ::now(); - if (i == 0) { - SPDLOG_INFO("\033[92mRobot 0 Timeout Reached\033[0m"); - } - send(i, motion, manipulator, positions_.at(i)); + send_control_message(i, motion, manipulator, positions_.at(i)); } } } diff --git a/soccer/src/soccer/radio/radio.hpp b/soccer/src/soccer/radio/radio.hpp index fe867a1c670..4789a76bcf4 100644 --- a/soccer/src/soccer/radio/radio.hpp +++ b/soccer/src/soccer/radio/radio.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "robot_intent.hpp" #include "robot_status.hpp" @@ -28,38 +29,106 @@ DECLARE_FLOAT64(kRadioParamModule, timeout); * @details This is the abstract superclass for NetworkRadio and SimRadio, which do * the actual work - this just declares the interface and handles sending stop commands when no new * commands come in for a while. + * + * The radio should handle: + * 1. Sending Control Messages to the Robots + * * Alive Robots should be receiving real commands + * * Non-responsive (dead) robots should receive stop commands + * 2. Receiving Robot Status Messages and Publish to the robot status topic + * 3. Calculate Alive Robots and Publish to the alive robots topic */ class Radio : public rclcpp::Node { public: Radio(); protected: - void publish(int robot_id, const rj_msgs::msg::RobotStatus& robot_status); + /** + * @brief Send a control message to the corresponding robot. + * + * @param robot_id The robot to send to + * @param motion The (x (m/s), y (m/s), z (rad/s)) velocities for the robot to move at + * @param manipulator The Shoot Mode, Trigger Mode, Kick Speed, and Dribbler Speed for the Robot + * @param role The position for the robot + */ + virtual void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion, + const rj_msgs::msg::ManipulatorSetpoint& manipulator, + strategy::Positions role) = 0; + + /** + * @brief Poll the receiver service for Messages. + * + */ + virtual void poll_receive() = 0; + + /** + * @brief Switch the team this radio is sending data to. + * + * @param blue_team + */ + virtual void switch_team(bool blue_team) = 0; + + /** + * @brief Wrapper over the local publisher to publish a robot status for a given robot + * + * @param robot_id The robot id of the robot, whose status to publish + * @param robot_status The status of the robot + */ + void publish_robot_status(int robot_id, const rj_msgs::msg::RobotStatus& robot_status); + + /** + * @brief Wrapper over the private publisher to publish a message containing alive robots + * + * @param alive_robots A message containing the alive robots + */ + void publish_alive_robots(const rj_msgs::msg::AliveRobots& alive_robots); + + const bool blue_team(); - virtual void send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, - const rj_msgs::msg::ManipulatorSetpoint& manipulator, - strategy::Positions role) = 0; - virtual void receive() = 0; - virtual void switch_team(bool blue) = 0; private: + /** + * @brief Poll the receiver and send empty motion commands to robots that software has + * not updated for a long time. + * + */ void tick(); + // Time between consecutive calls to tick(). + std::chrono::milliseconds tick_period = std::chrono::milliseconds(100); + // Ros timer to trigger tick every tick_period + rclcpp::TimerBase::SharedPtr tick_timer_; + // The position of each robot std::array positions_; + // Ros publishers to send robot statuses std::array::SharedPtr, kNumShells> robot_status_pubs_; + + // Ros publisher to update alive robots + rclcpp::Publisher::SharedPtr alive_robots_pub_; + + // Ros subscribers to receive velocity commands, which are sent to the robot std::array::SharedPtr, kNumShells> motion_subs_; + // Last Update Timestamps (per robot) + std::array last_updates_ = {}; + // Cached last velocity command + std::array motions_; + + // Ros subscribers to receive auxillary control (i.e. shoot_mode, trigger_mode, kick_speed, and dribbler_speed) + // which are stored and sent to the robot std::array::SharedPtr, kNumShells> manipulator_subs_; - rclcpp::Subscription::SharedPtr team_color_sub_; - rclcpp::TimerBase::SharedPtr tick_timer_; + // Cached auxillary control information std::array manipulators_cached_; - std::array last_updates_ = {}; + // Ros subscriber for the team's color. + rclcpp::Subscription::SharedPtr team_color_sub_; + // Whether or not the current team color is blue + bool blue_team_; + + // Ros param provider for initializing the radio node ::params::LocalROS2ParamProvider param_provider_; - std::array motions_; }; } // namespace radio diff --git a/soccer/src/soccer/radio/sim_radio.cpp b/soccer/src/soccer/radio/sim_radio.cpp index f386e8cf299..5c0797f63ea 100644 --- a/soccer/src/soccer/radio/sim_radio.cpp +++ b/soccer/src/soccer/radio/sim_radio.cpp @@ -94,8 +94,15 @@ SimRadio::SimRadio(bool blue_team) ip::udp::endpoint(address_, blue_team_ ? kSimBlueCommandPort : kSimYellowCommandPort); sim_control_endpoint_ = ip::udp::endpoint(address_, kSimCommandPort); - alive_robots_timer_ = create_wall_timer(std::chrono::milliseconds(500), - std::bind(&SimRadio::publish_alive_robots, this)); + alive_robots_timer_ = create_wall_timer(std::chrono::milliseconds(500), [this]() { + rj_msgs::msg::AliveRobots alive_message{}; + alive_message.alive_robots = alive_robots_; + publish_alive_robots(alive_message); + }); + + for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) { + alive_robots_[robot_id] = true; + } alive_robots_pub_ = this->create_publisher("strategy/alive_robots", rclcpp::QoS(1)); @@ -112,7 +119,7 @@ SimRadio::SimRadio(bool blue_team) sim::topics::kSimPlacementSrv, placement_callback); } -void SimRadio::send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, +void SimRadio::send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion, const rj_msgs::msg::ManipulatorSetpoint& manipulator, strategy::Positions role) { RobotControl sim_packet; @@ -140,7 +147,7 @@ void SimRadio::send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, socket_.send_to(buffer(out), robot_control_endpoint_); } -void SimRadio::receive() { io_service_.poll(); } +void SimRadio::poll_receive() { io_service_.poll(); } void SimRadio::start_receive() { // Set a receive callback @@ -168,7 +175,7 @@ void SimRadio::handle_receive(const std::string& data) { ConvertRx::sim_to_status(sim_status, &status); ConvertRx::status_to_ros(status, &status_ros); - publish(status_ros.robot_id, status_ros); + publish_robot_status(status_ros.robot_id, status_ros); } } @@ -230,16 +237,4 @@ void SimRadio::send_sim_command(const SimulatorCommand& cmd) { } } -void SimRadio::publish_alive_robots() { - std::vector alive_robots = {}; - for (uint8_t robot_id = 0; robot_id < kNumShells; robot_id++) { - alive_robots.push_back(robot_id); - } - - // publish a message containing the alive robots - rj_msgs::msg::AliveRobots alive_message{}; - alive_message.alive_robots = alive_robots; - alive_robots_pub_->publish(alive_message); -} - } // namespace radio diff --git a/soccer/src/soccer/radio/sim_radio.hpp b/soccer/src/soccer/radio/sim_radio.hpp index a1958b7c3a8..d2deb00b7d8 100644 --- a/soccer/src/soccer/radio/sim_radio.hpp +++ b/soccer/src/soccer/radio/sim_radio.hpp @@ -22,11 +22,16 @@ class SimRadio : public Radio { SimRadio(bool blue_team = false); protected: - void send(int robot_id, const rj_msgs::msg::MotionSetpoint& motion, + // + void send_control_message(uint8_t robot_id, const rj_msgs::msg::MotionSetpoint& motion, const rj_msgs::msg::ManipulatorSetpoint& manipulator, strategy::Positions role) override; - void receive() override; - void switch_team(bool blue) override; + + // Poll the asynchronous receiver + void poll_receive() override; + + // Switch teams and let the UI know + void switch_team(bool blue_team) override; private: void stop_robots(); @@ -38,10 +43,10 @@ class SimRadio : public Radio { // For ball and robot placement void send_sim_command(const SimulatorCommand& cmd); - // Publishing Alive Robots + // Timer to publish alive robots (i.e. all robots are alive) rclcpp::TimerBase::SharedPtr alive_robots_timer_; + std::array alive_robots_{}; rclcpp::Publisher::SharedPtr alive_robots_pub_; - void publish_alive_robots(); boost::asio::io_service io_service_; boost::asio::ip::udp::socket socket_; diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.cpp b/soccer/src/soccer/strategy/agent/agent_action_client.cpp index 5592ff8f0b5..b0fa134b6cf 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.cpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.cpp @@ -113,8 +113,7 @@ void AgentActionClient::game_settings_callback(const rj_msgs::msg::GameSettings: bool AgentActionClient::check_robot_alive(u_int8_t robot_id) { if (!is_simulated_) { - return std::find(alive_robots_.begin(), alive_robots_.end(), robot_id) != - alive_robots_.end(); + return alive_robots_.at(robot_id); } else { if (this->world_state()->get_robot(true, robot_id).visible) { rj_geometry::Point robot_position = diff --git a/soccer/src/soccer/strategy/agent/agent_action_client.hpp b/soccer/src/soccer/strategy/agent/agent_action_client.hpp index ca2a5094fe2..c06acf6c61b 100644 --- a/soccer/src/soccer/strategy/agent/agent_action_client.hpp +++ b/soccer/src/soccer/strategy/agent/agent_action_client.hpp @@ -126,7 +126,7 @@ class AgentActionClient : public rclcpp::Node { FieldDimensions field_dimensions_; PlayState play_state_ = PlayState::halt(); - std::vector alive_robots_ = {}; + std::array alive_robots_{}; bool is_simulated_ = false; static constexpr double field_padding_ = 0.3;