diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index b2431d41..ece8bafd 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -32,11 +32,10 @@ class RefereeBase virtual void robotHurtDataCallBack(const rm_msgs::RobotHurt& robot_hurt_data, const ros::Time& last_get_data_time); virtual void bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bullet_allowance, const ros::Time& last_get_data_time); - virtual void updateHeroStateDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data, - const ros::Time& last_get_data_time); virtual void interactiveDataCallBack(const rm_referee::InteractiveData& interactive_data, const ros::Time& last_get_data_time); virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time); + virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data); // sub call back virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state); @@ -59,6 +58,7 @@ class RefereeBase virtual void sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data); virtual void sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data); virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); + virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data); // send ui void sendSerialDataCallback(); @@ -82,10 +82,10 @@ class RefereeBase ros::Subscriber balance_state_sub_; ros::Subscriber radar_receive_sub_; ros::Subscriber map_sentry_sub_; - ros::Subscriber sentry_deviate_sub_; ros::Subscriber radar_to_sentry_sub_; ros::Subscriber sentry_cmd_sub_; ros::Subscriber radar_cmd_sub_; + ros::Subscriber sentry_state_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; @@ -114,7 +114,11 @@ class RefereeBase CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; - HeroStateFlashUi* hero_state_flash_ui_{}; + HeroHitFlashUi* hero_hit_flash_ui_{}; + + InteractiveSender* interactive_data_sender_{}; + InteractiveSender* enemy_hero_state_sender_{}; + InteractiveSender* sentry_state_sender_{}; GroupUiBase* graph_queue_sender_{}; std::deque graph_queue_; @@ -123,7 +127,6 @@ class RefereeBase ros::Time radar_interactive_data_last_send_; ros::Time sentry_interactive_data_last_send_; ros::Time sentry_cmd_data_last_send_, radar_cmd_data_last_send_; - UiBase* interactive_data_sender_{}; Base& base_; ros::Timer add_ui_timer_, send_serial_data_timer_; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index adfee5e0..b0a39ebd 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -49,21 +49,19 @@ class SpinFlashUi : public FlashUi uint8_t chassis_mode_; }; -class HeroStateFlashUi : public FlashUi +class HeroHitFlashUi : public FlashUi { public: - explicit HeroStateFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashUi(rpc_value, base, "hero_state", graph_queue, character_queue) + explicit HeroHitFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashUi(rpc_value, base, " hero_hit", graph_queue, character_queue) { - ros::NodeHandle nh; - timer_ = nh.createTimer(ros::Duration(delay_), std::bind(&HeroStateFlashUi::delayDisplay, this), false, false); - }; - void updateHeroStateData(const rm_msgs::GameRobotHp& data, const ros::Time& last_get_data_time); + } + void updateHittingConfig(const rm_msgs::GameRobotHp& msg); private: - void delayDisplay(); - ros::Timer timer_; - bool enemy_hero_die_{ false }; + void display(const ros::Time& time) override; + bool hitted_; + rm_msgs::GameRobotHp last_hp_msg_; }; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 69cb8c0a..2f9fe030 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -41,14 +41,7 @@ class UiBase void sendCharacter(const ros::Time& time, Graph* graph); void sendSingleGraph(const ros::Time& time, Graph* graph); - void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); - void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); - void sendMapSentryData(const rm_referee::MapSentryData& data); - void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); - void sendCustomInfoData(std::wstring data); void transferInt(const int data); - void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data); - void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); @@ -76,6 +69,25 @@ class UiBase const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; }; +class InteractiveSender : public UiBase +{ +public: + explicit InteractiveSender(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) + : UiBase(rpc_value, base, graph_queue, character_queue){}; + + void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); + void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); + void sendMapSentryData(const rm_referee::MapSentryData& data); + void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); + void sendCustomInfoData(std::wstring data); + void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data); + void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); + +protected: + std::wstring last_custom_info_; +}; + class GroupUiBase : public UiBase { public: diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 191f461e..e5ea5222 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -141,7 +141,7 @@ int Referee::unpack(uint8_t* rx_data) game_robot_hp_data.stamp = last_get_data_time_; referee_ui_.updateEnemyHeroState(game_robot_hp_data, last_get_data_time_); - referee_ui_.updateHeroStateDataCallBack(game_robot_hp_data, last_get_data_time_); + referee_ui_.updateHeroHitDataCallBack(game_robot_hp_data); game_robot_hp_pub_.publish(game_robot_hp_data); break; } @@ -590,32 +590,30 @@ int Referee::unpack(uint8_t* rx_data) void Referee::getRobotInfo() { base_.robot_color_ = base_.robot_id_ >= 100 ? "blue" : "red"; - if (base_.robot_id_ != rm_referee::RobotId::BLUE_SENTRY && base_.robot_id_ != rm_referee::RobotId::RED_SENTRY) + switch (base_.robot_id_) { - switch (base_.robot_id_) - { - case rm_referee::RobotId::BLUE_HERO: - base_.client_id_ = rm_referee::ClientId::BLUE_HERO_CLIENT; - break; - case rm_referee::RobotId::BLUE_ENGINEER: - base_.client_id_ = rm_referee::ClientId::BLUE_ENGINEER_CLIENT; - break; - case rm_referee::RobotId::BLUE_STANDARD_3: - base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_3_CLIENT; - break; - case rm_referee::RobotId::BLUE_STANDARD_4: - base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_4_CLIENT; - break; - case rm_referee::RobotId::BLUE_STANDARD_5: - base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_5_CLIENT; - break; - case rm_referee::RobotId::BLUE_AERIAL: - base_.client_id_ = rm_referee::ClientId::BLUE_AERIAL_CLIENT; - break; - case rm_referee::RobotId::RED_HERO: - base_.client_id_ = rm_referee::ClientId::RED_HERO_CLIENT; - break; - case rm_referee::RobotId::RED_ENGINEER: + case rm_referee::RobotId::BLUE_HERO: + base_.client_id_ = rm_referee::ClientId::BLUE_HERO_CLIENT; + break; + case rm_referee::RobotId::BLUE_ENGINEER: + base_.client_id_ = rm_referee::ClientId::BLUE_ENGINEER_CLIENT; + break; + case rm_referee::RobotId::BLUE_STANDARD_3: + base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_3_CLIENT; + break; + case rm_referee::RobotId::BLUE_STANDARD_4: + base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_4_CLIENT; + break; + case rm_referee::RobotId::BLUE_STANDARD_5: + base_.client_id_ = rm_referee::ClientId::BLUE_STANDARD_5_CLIENT; + break; + case rm_referee::RobotId::BLUE_AERIAL: + base_.client_id_ = rm_referee::ClientId::BLUE_AERIAL_CLIENT; + break; + case rm_referee::RobotId::RED_HERO: + base_.client_id_ = rm_referee::ClientId::RED_HERO_CLIENT; + break; + case rm_referee::RobotId::RED_ENGINEER: base_.client_id_ = rm_referee::ClientId::RED_ENGINEER_CLIENT; break; case rm_referee::RobotId::RED_STANDARD_3: @@ -630,7 +628,12 @@ void Referee::getRobotInfo() case rm_referee::RobotId::RED_AERIAL: base_.client_id_ = rm_referee::ClientId::RED_AERIAL_CLIENT; break; - } + case rm_referee::RobotId::BLUE_SENTRY: + base_.client_id_ = rm_referee::ClientId::BLUE_AERIAL_CLIENT; + break; + case rm_referee::RobotId::RED_SENTRY: + base_.client_id_ = rm_referee::ClientId::RED_AERIAL_CLIENT; + break; } } } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 30a65b31..9bed0804 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -3,6 +3,7 @@ // #include "rm_referee/referee_base.h" +#include namespace rm_referee { @@ -35,20 +36,20 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/map_sentry_data", 10, &RefereeBase::mapSentryCallback, this); RefereeBase::radar_receive_sub_ = nh.subscribe("/rm_radar", 10, &RefereeBase::radarReceiveCallback, this); - RefereeBase::sentry_deviate_sub_ = - nh.subscribe("/deviate", 10, &RefereeBase::sentryDeviateCallback, this); RefereeBase::radar_to_sentry_sub_ = nh.subscribe( "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); RefereeBase::sentry_cmd_sub_ = nh.subscribe("/sentry_cmd", 1, &RefereeBase::sendSentryCmdCallback, this); RefereeBase::radar_cmd_sub_ = nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); + RefereeBase::sentry_state_sub_ = + nh.subscribe("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); add_ui_frequency_ = getParam(nh, "add_ui_frequency", 5); add_ui_max_times_ = getParam(nh, "add_ui_max_times", 10); - interactive_data_sender_ = new UiBase(rpc_value, base_); + interactive_data_sender_ = new InteractiveSender(rpc_value, base_); if (nh.hasParam("ui")) { ros::NodeHandle ui_nh(nh, "ui"); @@ -134,8 +135,19 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "spin") spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "hero_state") - hero_state_flash_ui_ = new HeroStateFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "hero_hit") + hero_hit_flash_ui_ = new HeroHitFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + } + } + if (nh.hasParam("interactive_data")) + { + nh.getParam("interactive_data", rpc_value); + for (int i = 0; i < rpc_value.size(); i++) + { + if (rpc_value[i]["name"] == "enemy_hero_state") + enemy_hero_state_sender_ = new InteractiveSender(rpc_value[i], base_); + if (rpc_value[i]["name"] == "sentry_state") + sentry_state_sender_ = new InteractiveSender(rpc_value[i], base_); } } @@ -289,31 +301,31 @@ void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, void RefereeBase::updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, const ros::Time& last_get_data_time) { - std::wstring data; - if (base_.robot_id_ < 100 && base_.robot_id_ != RED_SENTRY) - { - if (game_robot_hp_data.blue_1_robot_hp > 0) - data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.blue_1_robot_hp); - else - data = L"敌方英雄死亡"; - } - else if (base_.robot_id_ >= 100 && base_.robot_id_ != BLUE_SENTRY) + if (enemy_hero_state_sender_) { - if (game_robot_hp_data.red_1_robot_hp > 0) - data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.red_1_robot_hp); - else - data = L"敌方英雄死亡"; + std::wstring data; + if (base_.robot_id_ < 100) + { + if (game_robot_hp_data.blue_1_robot_hp > 0) + data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.blue_1_robot_hp); + else + data = L"敌方英雄死亡"; + } + else if (base_.robot_id_ >= 100) + { + if (game_robot_hp_data.red_1_robot_hp > 0) + data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.red_1_robot_hp); + else + data = L"敌方英雄死亡"; + } + enemy_hero_state_sender_->sendCustomInfoData(data); } - else - return; - interactive_data_sender_->sendCustomInfoData(data); } -void RefereeBase::updateHeroStateDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data, - const ros::Time& last_get_data_time) +void RefereeBase::updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data) { - if (hero_state_flash_ui_) - hero_state_flash_ui_->updateHeroStateData(game_robot_hp_data, last_get_data_time); + if (hero_hit_flash_ui_) + hero_hit_flash_ui_->updateHittingConfig(game_robot_hp_data); } void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { @@ -514,4 +526,11 @@ void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data) } } +void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data) +{ + std::wstring_convert> converter; + if (sentry_state_sender_) + sentry_state_sender_->sendCustomInfoData(converter.from_bytes(data->data)); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 07e22fc0..f4574694 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -15,16 +15,21 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once else graph_->setOperation(rm_referee::GraphOperation::DELETE); } - else if (state && time - last_send_ > delay_) + else if (time - last_send_ > delay_) { ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); - graph_->setOperation(graph_->getOperation() == rm_referee::GraphOperation::ADD ? - rm_referee::GraphOperation::DELETE : - rm_referee::GraphOperation::ADD); + if (state) + graph_->setOperation(rm_referee::GraphOperation::ADD); + else + graph_->setOperation(rm_referee::GraphOperation::DELETE); + // graph_->setOperation(graph_->getOperation() == rm_referee::GraphOperation::ADD ? + // rm_referee::GraphOperation::DELETE : + // rm_referee::GraphOperation::ADD); } if (graph_->isRepeated()) return; graph_->updateLastConfig(); + last_send_ = time; UiBase::updateForQueue(); } @@ -55,38 +60,31 @@ void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, display(last_get_data_time); } -void HeroStateFlashUi::updateHeroStateData(const rm_msgs::GameRobotHp& data, const ros::Time& last_get_data_time) +void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) { - if (base_.robot_id_ < 100) + if (base_.robot_id_ > 100) { - if (data.blue_1_robot_hp > 0 && enemy_hero_die_) - { - FlashUi::updateFlashUiForQueue(last_get_data_time, true, true); - timer_.start(); - } - if (data.blue_1_robot_hp == 0) - enemy_hero_die_ = false; - else - enemy_hero_die_ = true; + hitted_ = + (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 175 || last_hp_msg_.red_base_hp - msg.red_base_hp > 175); } - else if (base_.robot_id_ >= 100) + else { - if (data.red_1_robot_hp > 0 && enemy_hero_die_) - { - FlashUi::updateFlashUiForQueue(last_get_data_time, true, true); - timer_.start(); - } - if (data.red_1_robot_hp == 0) - enemy_hero_die_ = false; - else - enemy_hero_die_ = true; + hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 175 || + last_hp_msg_.blue_base_hp - msg.blue_base_hp > 175); } + last_hp_msg_ = msg; } -void HeroStateFlashUi::delayDisplay() + +void HeroHitFlashUi::display(const ros::Time& time) { - graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateFlashUiForQueue(ros::Time::now(), false, true); - timer_.stop(); + if (hitted_) + { + graph_->setOperation(rm_referee::GraphOperation::ADD); + FlashUi::updateFlashUiForQueue(time, true, true); + } + else + FlashUi::updateFlashUiForQueue(time, hitted_, false); } + } // namespace rm_referee // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index a1fa91c9..073d6ab9 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -148,7 +148,7 @@ void UiBase::display(const ros::Time& time, bool state, bool once) displayTwice(); } -void UiBase::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data) +void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data) { uint8_t tx_data[sizeof(InteractiveData)] = { 0 }; auto student_interactive_data = (InteractiveData*)tx_data; @@ -165,7 +165,7 @@ void UiBase::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char sendSerial(ros::Time::now(), sizeof(InteractiveData)); } -void UiBase::sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data) +void InteractiveSender::sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data) { int data_len; uint8_t tx_data[sizeof(CurrentSentryPosData)] = { 0 }; @@ -198,7 +198,7 @@ void UiBase::sendUi(const ros::Time& time) sendSingleGraph(time, graph_); } -void UiBase::sendMapSentryData(const rm_referee::MapSentryData& data) +void InteractiveSender::sendMapSentryData(const rm_referee::MapSentryData& data) { uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; @@ -228,8 +228,13 @@ void UiBase::sendMapSentryData(const rm_referee::MapSentryData& data) clearTxBuffer(); } -void UiBase::sendCustomInfoData(std::wstring data) +void InteractiveSender::sendCustomInfoData(std::wstring data) { + if (data == last_custom_info_ || ros::Time::now() - last_send_ < ros::Duration(0.35)) + return; + else + last_custom_info_ = data; + int data_len; rm_referee::CustomInfo tx_data; data_len = static_cast(sizeof(rm_referee::CustomInfo)); @@ -251,10 +256,11 @@ void UiBase::sendCustomInfoData(std::wstring data) tx_data.user_data[2 * i + 1] = (characters[i] >> 8) & 0xFF; } pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::CUSTOM_INFO_CMD, data_len); + last_send_ = ros::Time::now(); sendSerial(ros::Time::now(), data_len); } -void UiBase::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) +void InteractiveSender::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) { uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; auto radar_interactive_data = (rm_referee::ClientMapReceiveData*)tx_data; @@ -315,7 +321,7 @@ void UiBase::sendSingleGraph(const ros::Time& time, Graph* graph) sendSerial(time, data_len); } -void UiBase::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data) +void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data) { int data_len; rm_referee::SentryInfo tx_data; @@ -330,7 +336,7 @@ void UiBase::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data) sendSerial(ros::Time::now(), data_len); } -void UiBase::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) +void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) { int data_len; rm_referee::RadarInfo tx_data;