diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index f314d28f..b88d15bb 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -36,6 +36,7 @@ + diff --git a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp index e69de29b..81e94188 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/command/interaction/sentry_decision.cpp @@ -0,0 +1,223 @@ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include "referee/command/field.hpp" +#include "referee/command/interaction/header.hpp" + +namespace rmcs_core::referee::command::interaction { + +class SentryDecision + : public rmcs_executor::Component + , public rclcpp::Node { +public: + SentryDecision() + : Node{ + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} { + + register_input("/referee/id", robot_id_); + register_input("/referee/sentry/mode", sentry_mode_); + + register_input("/referee/sentry/decision/enabled", decision_enabled_, false); + register_input("/referee/sentry/decision/confirm_revive", confirm_revive_, false); + register_input( + "/referee/sentry/decision/exchange_instant_revive", exchange_instant_revive_, false); + register_input( + "/referee/sentry/decision/bullet_exchange_value", bullet_exchange_value_, false); + register_input( + "/referee/sentry/decision/remote_bullet_exchange_count", + requested_remote_bullet_exchange_count_, false); + register_input( + "/referee/sentry/decision/remote_hp_exchange_count", + requested_remote_hp_exchange_count_, false); + register_input("/referee/sentry/decision/mode", requested_mode_, false); + register_input( + "/referee/sentry/decision/activate_energy_mechanism", activate_energy_mechanism_, + false); + + register_output("/referee/command/interaction/sentry_decision", sentry_decision_field_); + + double resend_rate = 5.0; + if (get_parameter("resend_rate", resend_rate) && resend_rate > 0.0) { + resend_interval_ = std::chrono::duration_cast( + std::chrono::duration{1.0 / resend_rate}); + } + + int64_t repeat_count = repeat_count_; + if (get_parameter("repeat_count", repeat_count)) + repeat_count_ = std::max(repeat_count, 0); + } + + void before_updating() override { + decision_enabled_ready_ = decision_enabled_.ready(); + confirm_revive_ready_ = confirm_revive_.ready(); + exchange_instant_revive_ready_ = exchange_instant_revive_.ready(); + bullet_exchange_value_ready_ = bullet_exchange_value_.ready(); + requested_remote_bullet_exchange_count_ready_ = + requested_remote_bullet_exchange_count_.ready(); + requested_remote_hp_exchange_count_ready_ = requested_remote_hp_exchange_count_.ready(); + requested_mode_ready_ = requested_mode_.ready(); + activate_energy_mechanism_ready_ = activate_energy_mechanism_.ready(); + + if (!decision_enabled_.ready()) + decision_enabled_.bind_directly(default_false_); + if (!confirm_revive_.ready()) + confirm_revive_.bind_directly(default_false_); + if (!exchange_instant_revive_.ready()) + exchange_instant_revive_.bind_directly(default_false_); + if (!bullet_exchange_value_.ready()) + bullet_exchange_value_.bind_directly(default_u16_); + if (!requested_remote_bullet_exchange_count_.ready()) + requested_remote_bullet_exchange_count_.bind_directly(default_u8_); + if (!requested_remote_hp_exchange_count_.ready()) + requested_remote_hp_exchange_count_.bind_directly(default_u8_); + if (!requested_mode_.ready()) + requested_mode_.bind_directly(default_mode_); + if (!activate_energy_mechanism_.ready()) + activate_energy_mechanism_.bind_directly(default_false_); + } + + void update() override { + *sentry_decision_field_ = Field{}; + + if (!*decision_enabled_ || !has_decision_request() || !is_sentry_robot()) { + reset_pending_command(); + return; + } + + const uint32_t command = make_command(); + if (!last_command_ || command != *last_command_) { + last_command_ = command; + remaining_repeats_ = repeat_count_; + next_sent_ = std::chrono::steady_clock::time_point::min(); + } + + const auto now = std::chrono::steady_clock::now(); + if (remaining_repeats_ == 0 || now < next_sent_) + return; + + auto full_robot_id = rmcs_msgs::FullRobotId{*robot_id_}; + outgoing_header_.command_id = 0x0120; + outgoing_header_.sender_id = full_robot_id; + outgoing_header_.receiver_id = rmcs_msgs::FullRobotId::REFEREE_SERVER; + outgoing_command_ = command; + + *sentry_decision_field_ = + Field{[this](std::byte* buffer) { return write_sentry_decision(buffer); }}; + + --remaining_repeats_; + next_sent_ = now + resend_interval_; + } + +private: + bool is_sentry_robot() const { + return *robot_id_ == rmcs_msgs::RobotId::RED_SENTRY + || *robot_id_ == rmcs_msgs::RobotId::BLUE_SENTRY; + } + + bool has_decision_request() const { + return confirm_revive_ready_ || exchange_instant_revive_ready_ + || bullet_exchange_value_ready_ || requested_remote_bullet_exchange_count_ready_ + || requested_remote_hp_exchange_count_ready_ || requested_mode_ready_ + || activate_energy_mechanism_ready_; + } + + static uint8_t normalized_mode(uint8_t requested, uint8_t current) { + current = current >= 1 && current <= 3 ? current : default_mode_value; + return requested >= 1 && requested <= 3 ? requested : current; + } + + uint32_t make_command() const { + const uint16_t requested_bullet = + bullet_exchange_value_ready_ ? *bullet_exchange_value_ : 0; + const uint16_t bullet_exchange_value = + std::min(requested_bullet, max_bullet_exchange_value); + + const uint8_t remote_bullet_count = std::min( + requested_remote_bullet_exchange_count_ready_ ? *requested_remote_bullet_exchange_count_ + : 0, + max_count); + const uint8_t remote_hp_count = std::min( + requested_remote_hp_exchange_count_ready_ ? *requested_remote_hp_exchange_count_ + : 0, + max_count); + const uint8_t mode = + normalized_mode(requested_mode_ready_ ? *requested_mode_ : *sentry_mode_, *sentry_mode_); + + uint32_t command = 0; + command |= *confirm_revive_ ? 1u << 0 : 0; + command |= *exchange_instant_revive_ ? 1u << 1 : 0; + command |= static_cast(bullet_exchange_value & 0x07ff) << 2; + command |= static_cast(remote_bullet_count & 0x0f) << 13; + command |= static_cast(remote_hp_count & 0x0f) << 17; + command |= static_cast(mode & 0x03) << 21; + command |= *activate_energy_mechanism_ ? 1u << 23 : 0; + return command; + } + + size_t write_sentry_decision(std::byte* buffer) const { + return write_field(buffer, outgoing_header_, outgoing_command_); + } + + void reset_pending_command() { + last_command_.reset(); + remaining_repeats_ = 0; + next_sent_ = std::chrono::steady_clock::time_point::min(); + } + + static constexpr uint16_t max_bullet_exchange_value = 0x07ff; + static constexpr uint8_t max_count = 0x0f; + static constexpr uint8_t default_mode_value = 3; + + bool default_false_ = false; + uint16_t default_u16_ = 0; + uint8_t default_u8_ = 0; + uint8_t default_mode_ = default_mode_value; + + InputInterface robot_id_; + InputInterface sentry_mode_; + + InputInterface decision_enabled_; + InputInterface confirm_revive_; + InputInterface exchange_instant_revive_; + InputInterface bullet_exchange_value_; + InputInterface requested_remote_bullet_exchange_count_; + InputInterface requested_remote_hp_exchange_count_; + InputInterface requested_mode_; + InputInterface activate_energy_mechanism_; + + bool decision_enabled_ready_ = false; + bool confirm_revive_ready_ = false; + bool exchange_instant_revive_ready_ = false; + bool bullet_exchange_value_ready_ = false; + bool requested_remote_bullet_exchange_count_ready_ = false; + bool requested_remote_hp_exchange_count_ready_ = false; + bool requested_mode_ready_ = false; + bool activate_energy_mechanism_ready_ = false; + + OutputInterface sentry_decision_field_; + + Header outgoing_header_{}; + uint32_t outgoing_command_ = 0; + + std::optional last_command_; + int64_t repeat_count_ = 3; + int64_t remaining_repeats_ = 0; + std::chrono::steady_clock::duration resend_interval_ = std::chrono::milliseconds{200}; + std::chrono::steady_clock::time_point next_sent_ = + std::chrono::steady_clock::time_point::min(); +}; + +} // namespace rmcs_core::referee::command::interaction + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::referee::command::interaction::SentryDecision, rmcs_executor::Component) diff --git a/rmcs_ws/src/rmcs_core/src/referee/status.cpp b/rmcs_ws/src/rmcs_core/src/referee/status.cpp index c080a355..0c582690 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status.cpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status.cpp @@ -56,10 +56,35 @@ class Status register_output("/referee/ally/infantry_2_hp", ally_infantry_2_hp_, 0); register_output("/referee/ally/outpost/hp", ally_outpost_hp_, 0); register_output("/referee/ally/base/hp", ally_base_hp_, 0); + register_output("/referee/ally/hero_position_x", ally_hero_position_x_, 0.0); + register_output("/referee/ally/hero_position_y", ally_hero_position_y_, 0.0); + register_output("/referee/ally/engineer_position_x", ally_engineer_position_x_, 0.0); + register_output("/referee/ally/engineer_position_y", ally_engineer_position_y_, 0.0); + register_output("/referee/ally/infantry_1_position_x", ally_infantry_1_position_x_, 0.0); + register_output("/referee/ally/infantry_1_position_y", ally_infantry_1_position_y_, 0.0); + register_output("/referee/ally/infantry_2_position_x", ally_infantry_2_position_x_, 0.0); + register_output("/referee/ally/infantry_2_position_y", ally_infantry_2_position_y_, 0.0); + register_output("/referee/opponent/hero_position_x", opponent_hero_position_x_, 0.0); + register_output("/referee/opponent/hero_position_y", opponent_hero_position_y_, 0.0); + register_output( + "/referee/opponent/engineer_position_x", opponent_engineer_position_x_, 0.0); + register_output( + "/referee/opponent/engineer_position_y", opponent_engineer_position_y_, 0.0); + register_output( + "/referee/opponent/infantry_3_position_x", opponent_infantry_3_position_x_, 0.0); + register_output( + "/referee/opponent/infantry_3_position_y", opponent_infantry_3_position_y_, 0.0); + register_output( + "/referee/opponent/infantry_4_position_x", opponent_infantry_4_position_x_, 0.0); + register_output( + "/referee/opponent/infantry_4_position_y", opponent_infantry_4_position_y_, 0.0); + register_output( + "/referee/opponent/uav_position_x", opponent_uav_position_x_, 0.0); + register_output( + "/referee/opponent/uav_position_y", opponent_uav_position_y_, 0.0); + register_output("/referee/opponent/sentry_position_x", opponent_sentry_position_x_, 0.0); + register_output("/referee/opponent/sentry_position_y", opponent_sentry_position_y_, 0.0); register_output("/referee/current_hp", robot_current_hp_); - register_output("/referee/position/x", robot_position_x_, 0.0); - register_output("/referee/position/y", robot_position_y_, 0.0); - register_output("/referee/position/angle", robot_position_angle_, 0.0); register_output("/referee/shooter/bullet_allowance", robot_bullet_allowance_, false); register_output( "/referee/shooter/42mm_bullet_allowance", robot_42mm_bullet_allowance_, false); @@ -81,18 +106,36 @@ class Status register_output( "/referee/map_command/received_timestamp", map_command_received_timestamp_, 0.0); register_output( - "/referee/map_command/event/target_position_x", map_command_event_target_position_x_, - 0.0); + "/referee/map_command/event/target_position_x", + map_command_event_target_position_x_, 0.0); register_output( - "/referee/map_command/event/target_position_y", map_command_event_target_position_y_, - 0.0); + "/referee/map_command/event/target_position_y", + map_command_event_target_position_y_, 0.0); register_output("/referee/map_command/event/keyboard", map_command_event_keyboard_, 0); register_output( "/referee/map_command/event/target_robot_id", map_command_event_target_robot_id_, 0); register_output("/referee/map_command/event/source", map_command_event_source_, 0); - register_output("/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); + register_output( + "/referee/map_command/event/timestamp", map_command_event_timestamp_, 0.0); register_output("/referee/map_command/event/sequence", map_command_event_sequence_, 0); + register_output( + "/referee/sentry/can_confirm_free_revive", sentry_can_confirm_free_revive_, false); + register_output( + "/referee/sentry/can_exchange_instant_revive", sentry_can_exchange_instant_revive_, + false); + register_output("/referee/sentry/instant_revive_cost", sentry_instant_revive_cost_, 0); + register_output("/referee/sentry/exchanged_bullet_allowance", sentry_exchanged_bullet_, 0); + register_output( + "/referee/sentry/remote_bullet_exchange_count", sentry_remote_bullet_exchange_count_, + 0); + register_output( + "/referee/sentry/exchangeable_bullet_allowance", sentry_exchangeable_bullet_, 0); + register_output("/referee/sentry/mode", sentry_mode_, 3); + register_output( + "/referee/sentry/energy_mechanism_activatable", sentry_energy_mechanism_activatable_, + false); + robot_status_watchdog_.reset(5'000); } @@ -167,8 +210,14 @@ class Status update_shoot_data(); else if (command_id == 0x0208) update_bullet_allowance(); + else if (command_id == 0x020B) + update_game_robot_position(); + else if (command_id == 0x020D) + update_sentry_info(); else if (command_id == 0x0303) update_map_command(); + else if (command_id == 0x0305) + update_map_robot_data(); } void update_game_status() { @@ -238,12 +287,7 @@ class Status *robot_buffer_energy_ = static_cast(data.buffer_energy); } - void update_robot_position() { - auto& data = reinterpret_cast(frame_.body.data); - *robot_position_x_ = data.x; - *robot_position_y_ = data.y; - *robot_position_angle_ = data.angle; - } + void update_robot_position() {} void update_hurt_data() {} @@ -263,6 +307,26 @@ class Status *robot_fortress_17mm_bullet_allowance_ = data.projectile_allowance_fortress; } + void update_game_robot_position() { + if (frame_.header.data_length < sizeof(GameRobotPosition)) { + RCLCPP_WARN( + logger_, "Game robot position length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + auto& data = reinterpret_cast(frame_.body.data); + + *ally_hero_position_x_ = data.hero_x; + *ally_hero_position_y_ = data.hero_y; + *ally_engineer_position_x_ = data.engineer_x; + *ally_engineer_position_y_ = data.engineer_y; + *ally_infantry_1_position_x_ = data.standard_3_x; + *ally_infantry_1_position_y_ = data.standard_3_y; + *ally_infantry_2_position_x_ = data.standard_4_x; + *ally_infantry_2_position_y_ = data.standard_4_y; + } + void update_map_command() { if (frame_.header.data_length < sizeof(MapCommand)) { RCLCPP_WARN( @@ -284,11 +348,12 @@ class Status *map_command_received_timestamp_ = std::chrono::duration(now.time_since_epoch()).count(); - if (has_last_map_command_ && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { + if (has_last_map_command_ + && std::memcmp(&last_map_command_, &data, sizeof(data)) == 0) { return; } - last_map_command_ = data; + last_map_command_ = data; has_last_map_command_ = true; *map_command_event_target_position_x_ = data.target_position_x; @@ -299,6 +364,62 @@ class Status *map_command_event_timestamp_ = *map_command_received_timestamp_; *map_command_event_sequence_ += 1; } + + void update_sentry_info() { + if (frame_.header.data_length < sizeof(SentryInfo)) { + RCLCPP_WARN( + logger_, "Sentry info length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + auto& data = reinterpret_cast(frame_.body.data); + + const uint32_t sentry_info = data.sentry_info; + *sentry_exchanged_bullet_ = sentry_info & 0x07ff; + *sentry_remote_bullet_exchange_count_ = (sentry_info >> 11) & 0x0f; + *sentry_can_confirm_free_revive_ = (sentry_info >> 19) & 0x01; + *sentry_can_exchange_instant_revive_ = (sentry_info >> 20) & 0x01; + *sentry_instant_revive_cost_ = (sentry_info >> 21) & 0x03ff; + + const uint16_t sentry_info_2 = data.sentry_info_2; + *sentry_exchangeable_bullet_ = (sentry_info_2 >> 1) & 0x07ff; + *sentry_mode_ = (sentry_info_2 >> 12) & 0x03; + *sentry_energy_mechanism_activatable_ = (sentry_info_2 >> 14) & 0x01; + } + + void update_map_robot_data() { + if (frame_.header.data_length < sizeof(OpponentMapRobotData)) { + RCLCPP_WARN( + logger_, "Map robot data length invalid: %u", + static_cast(frame_.header.data_length)); + return; + } + + auto& data = reinterpret_cast(frame_.body.data); + + constexpr double centimeter_to_meter = 0.01; + + *opponent_hero_position_x_ = data.opponent_hero_position_x * centimeter_to_meter; + *opponent_hero_position_y_ = data.opponent_hero_position_y * centimeter_to_meter; + *opponent_engineer_position_x_ = + data.opponent_engineer_position_x * centimeter_to_meter; + *opponent_engineer_position_y_ = + data.opponent_engineer_position_y * centimeter_to_meter; + *opponent_infantry_3_position_x_ = + data.opponent_infantry_3_position_x * centimeter_to_meter; + *opponent_infantry_3_position_y_ = + data.opponent_infantry_3_position_y * centimeter_to_meter; + *opponent_infantry_4_position_x_ = + data.opponent_infantry_4_position_x * centimeter_to_meter; + *opponent_infantry_4_position_y_ = + data.opponent_infantry_4_position_y * centimeter_to_meter; + *opponent_uav_position_x_ = data.opponent_uav_position_x * centimeter_to_meter; + *opponent_uav_position_y_ = data.opponent_uav_position_y * centimeter_to_meter; + *opponent_sentry_position_x_ = data.opponent_sentry_position_x * centimeter_to_meter; + *opponent_sentry_position_y_ = data.opponent_sentry_position_y * centimeter_to_meter; + } + // When referee system loses connection unexpectedly, // use these indicators make sure the robot safe. // Muzzle: Cooling priority with level 1 @@ -339,10 +460,27 @@ class Status OutputInterface ally_infantry_2_hp_; OutputInterface ally_outpost_hp_; OutputInterface ally_base_hp_; + OutputInterface ally_hero_position_x_; + OutputInterface ally_hero_position_y_; + OutputInterface ally_engineer_position_x_; + OutputInterface ally_engineer_position_y_; + OutputInterface ally_infantry_1_position_x_; + OutputInterface ally_infantry_1_position_y_; + OutputInterface ally_infantry_2_position_x_; + OutputInterface ally_infantry_2_position_y_; + OutputInterface opponent_hero_position_x_; + OutputInterface opponent_hero_position_y_; + OutputInterface opponent_engineer_position_x_; + OutputInterface opponent_engineer_position_y_; + OutputInterface opponent_infantry_3_position_x_; + OutputInterface opponent_infantry_3_position_y_; + OutputInterface opponent_infantry_4_position_x_; + OutputInterface opponent_infantry_4_position_y_; + OutputInterface opponent_uav_position_x_; + OutputInterface opponent_uav_position_y_; + OutputInterface opponent_sentry_position_x_; + OutputInterface opponent_sentry_position_y_; OutputInterface robot_current_hp_; - OutputInterface robot_position_x_; - OutputInterface robot_position_y_; - OutputInterface robot_position_angle_; OutputInterface robot_bullet_allowance_; OutputInterface robot_42mm_bullet_allowance_; OutputInterface robot_fortress_17mm_bullet_allowance_; @@ -366,6 +504,15 @@ class Status OutputInterface map_command_event_sequence_; MapCommand last_map_command_{}; bool has_last_map_command_ = false; + + OutputInterface sentry_can_confirm_free_revive_; + OutputInterface sentry_can_exchange_instant_revive_; + OutputInterface sentry_instant_revive_cost_; + OutputInterface sentry_exchanged_bullet_; + OutputInterface sentry_remote_bullet_exchange_count_; + OutputInterface sentry_exchangeable_bullet_; + OutputInterface sentry_mode_; + OutputInterface sentry_energy_mechanism_activatable_; }; } // namespace rmcs_core::referee diff --git a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp index de3911af..9474338f 100644 --- a/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp +++ b/rmcs_ws/src/rmcs_core/src/referee/status/field.hpp @@ -76,6 +76,19 @@ struct __attribute__((packed)) BulletAllowance { uint16_t projectile_allowance_fortress; }; +struct __attribute__((packed)) GameRobotPosition { + float hero_x; + float hero_y; + float engineer_x; + float engineer_y; + float standard_3_x; + float standard_3_y; + float standard_4_x; + float standard_4_y; + float reserved_1; + float reserved_2; +}; + struct __attribute__((packed)) MapCommand { float target_position_x; float target_position_y; @@ -83,6 +96,25 @@ struct __attribute__((packed)) MapCommand { uint8_t target_robot_id; uint16_t cmd_source; }; -static_assert(sizeof(MapCommand) == 12); + +struct __attribute__((packed)) OpponentMapRobotData { + uint16_t opponent_hero_position_x; + uint16_t opponent_hero_position_y; + uint16_t opponent_engineer_position_x; + uint16_t opponent_engineer_position_y; + uint16_t opponent_infantry_3_position_x; + uint16_t opponent_infantry_3_position_y; + uint16_t opponent_infantry_4_position_x; + uint16_t opponent_infantry_4_position_y; + uint16_t opponent_uav_position_x; + uint16_t opponent_uav_position_y; + uint16_t opponent_sentry_position_x; + uint16_t opponent_sentry_position_y; +}; + +struct __attribute__((packed)) SentryInfo { + uint32_t sentry_info; + uint16_t sentry_info_2; +}; } // namespace rmcs_core::referee::status