diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 2030e5df..0bd6f1c3 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -25,7 +25,6 @@ class RefereeBase // unpack call back virtual void robotStatusDataCallBack(const rm_msgs::GameRobotStatus& game_robot_status_data, const ros::Time& last_get_data_time); - virtual void updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, const ros::Time& last_get_data_time); virtual void gameStatusDataCallBack(const rm_msgs::GameStatus& game_status_data, const ros::Time& last_get_data_time); virtual void capacityDataCallBack(const rm_msgs::PowerManagementSampleAndStatusData& data, ros::Time& last_get_data_time); @@ -61,7 +60,7 @@ class RefereeBase virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data); virtual void sendSentryCmdCallback(const rm_msgs::SentryCmdConstPtr& data); virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); - virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data); + virtual void sendCustomInfoCallback(const std_msgs::StringConstPtr& data); virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); virtual void radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data); @@ -131,8 +130,7 @@ class RefereeBase EngineerActionFlashUi* engineer_action_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; - // CustomInfoSender* enemy_hero_state_sender_{}; - CustomInfoSender* sentry_state_sender_{}; + CustomInfoSender* custom_info_sender{}; BulletNumShare* bullet_num_share_{}; SentryToRadar* sentry_to_radar_{}; RadarToSentry* radar_to_sentry_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index ce3937f1..34f5a7dc 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -33,6 +33,7 @@ class FlashGroupUi : public GroupUiBase } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; + void updateFlashUiForQueue(const ros::Time& time, bool state, bool once); void updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph); protected: @@ -90,13 +91,32 @@ class SpinFlashUi : public FlashUi uint8_t chassis_mode_; }; -class HeroHitFlashUi : public FlashUi +class HeroHitFlashUi : public FlashGroupUi { public: 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) + : FlashGroupUi(rpc_value, base, " hero_hit", graph_queue, character_queue) { + graph_vector_.insert(std::pair("1", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("2", new Graph(rpc_value["config"], base_, id_++))); + for (auto it : graph_vector_) + { + if (it.first == "1") + { + it.second->setStartX(960 + 50); + it.second->setStartY(540 + 50); + it.second->setEndX(960 - 50); + it.second->setEndY(540 - 50); + } + else if (it.first == "2") + { + it.second->setStartX(960 - 50); + it.second->setStartY(540 + 50); + it.second->setEndX(960 + 50); + it.second->setEndY(540 - 50); + } + } } void updateHittingConfig(const rm_msgs::GameRobotHp& msg); diff --git a/rm_referee/include/rm_referee/ui/time_change_ui.h b/rm_referee/include/rm_referee/ui/time_change_ui.h index 7211515a..e662af6d 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -347,14 +347,14 @@ class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); - int ui_start_y; + int ui_start_y = 0; for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) { if (it == graph_vector_.begin()) ui_start_y = it->second->getConfig().start_y; else { - ui_start_y += 40; + ui_start_y -= 40; it->second->setStartY(ui_start_y); } } @@ -363,7 +363,7 @@ class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi private: void updateConfig() override; - int hero_bullets_, standard3_bullets_, standard4_bullets_, standard5_bullets_; + int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; }; } // namespace rm_referee diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 034ce909..6fdd5984 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -140,7 +140,6 @@ int Referee::unpack(uint8_t* rx_data) game_robot_hp_data.red_base_hp = game_robot_hp_ref.red_base_hp; game_robot_hp_data.stamp = last_get_data_time_; - referee_ui_.updateEnemyHeroState(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; @@ -686,6 +685,12 @@ void Referee::getRobotInfo() case rm_referee::RobotId::RED_SENTRY: base_.client_id_ = rm_referee::ClientId::RED_AERIAL_CLIENT; break; + case rm_referee::RobotId::RED_RADAR: + base_.client_id_ = rm_referee::ClientId::RED_AERIAL_CLIENT; + break; + case rm_referee::RobotId::BLUE_RADAR: + base_.client_id_ = rm_referee::ClientId::BLUE_AERIAL_CLIENT; + break; } } } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 45e94aaf..b8969e93 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -41,7 +41,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::radar_cmd_sub_ = nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); RefereeBase::sentry_state_sub_ = - nh.subscribe("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this); + nh.subscribe("/custom_info", 1, &RefereeBase::sendCustomInfoCallback, this); RefereeBase::drone_pose_sub_ = nh.subscribe("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this); RefereeBase::shoot_cmd_sub_ = nh.subscribe("/controllers/shooter_controller/command", 1, @@ -159,8 +159,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) { // if (rpc_value[i]["name"] == "enemy_hero_state") // enemy_hero_state_sender_ = new CustomInfoSender(rpc_value[i], base_); - if (rpc_value[i]["name"] == "sentry_state") - sentry_state_sender_ = new CustomInfoSender(rpc_value[i], base_); + if (rpc_value[i]["name"] == "custom_info") + custom_info_sender = new CustomInfoSender(rpc_value[i], base_); if (rpc_value[i]["name"] == "bullet_num_share") bullet_num_share_ = new BulletNumShare(rpc_value[i], base_); if (rpc_value[i]["name"] == "sentry_to_radar") @@ -341,29 +341,6 @@ void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, if (fixed_ui_ && !is_adding_) fixed_ui_->updateForQueue(); } -void RefereeBase::updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, - const ros::Time& last_get_data_time) -{ - // if (enemy_hero_state_sender_) - // { - // 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); - // } -} void RefereeBase::updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data) { @@ -470,10 +447,6 @@ void RefereeBase::cardCmdDataCallback(const rm_msgs::StateCmd::ConstPtr& data) } void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& data) { - /*if (progress_time_change_ui_ && !is_adding_) - progress_time_change_ui_->updateEngineerUiData(data, ros::Time::now());*/ - /* if (drag_state_trigger_change_ui_ && !is_adding_) - drag_state_trigger_change_ui_->updateStringUiData(data->drag_state);*/ if (gripper_state_trigger_change_ui_ && !is_adding_) gripper_state_trigger_change_ui_->updateStringUiData(data->gripper_state); if (stone_num_trigger_change_ui_ && !is_adding_) @@ -571,11 +544,11 @@ void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data) } } -void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data) +void RefereeBase::sendCustomInfoCallback(const std_msgs::StringConstPtr& data) { std::wstring_convert> converter; - if (sentry_state_sender_) - sentry_state_sender_->sendCustomInfoData(converter.from_bytes(data->data)); + if (custom_info_sender) + custom_info_sender->sendCustomInfoData(converter.from_bytes(data->data)); } void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data) diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index d31f39a5..b61f4753 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -35,6 +35,66 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once UiBase::updateForQueue(); } +void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once) +{ + if (once) + { + if (state) + { + for (auto graph : graph_vector_) + graph.second->setOperation(rm_referee::GraphOperation::ADD); + for (auto character : character_vector_) + character.second->setOperation(rm_referee::GraphOperation::ADD); + } + else + { + for (auto graph : graph_vector_) + graph.second->setOperation(rm_referee::GraphOperation::DELETE); + for (auto character : character_vector_) + character.second->setOperation(rm_referee::GraphOperation::DELETE); + } + } + else if (time - last_send_ > delay_) + { + ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); + if (state) + { + for (auto graph : graph_vector_) + graph.second->setOperation(rm_referee::GraphOperation::ADD); + for (auto character : character_vector_) + character.second->setOperation(rm_referee::GraphOperation::ADD); + } + else + { + for (auto graph : graph_vector_) + graph.second->setOperation(rm_referee::GraphOperation::DELETE); + for (auto character : character_vector_) + character.second->setOperation(rm_referee::GraphOperation::DELETE); + } + } + + bool is_repeat = true; + for (auto it : graph_vector_) + if (!it.second->isRepeated()) + is_repeat = false; + for (auto it : character_vector_) + if (!it.second->isRepeated()) + is_repeat = false; + if (is_repeat) + return; + + for (auto it : graph_vector_) + it.second->updateLastConfig(); + for (auto it : character_vector_) + it.second->updateLastConfig(); + + last_send_ = time; + for (auto it : character_vector_) + character_queue_->push_back(*it.second); + for (auto it : graph_vector_) + graph_queue_->push_back(*it.second); +} + void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph) { if (once) @@ -126,9 +186,9 @@ void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) void HeroHitFlashUi::display(const ros::Time& time) { if (hitted_) - FlashUi::updateFlashUiForQueue(time, true, true); + FlashGroupUi::updateFlashUiForQueue(time, true, true); else - FlashUi::updateFlashUiForQueue(time, false, false); + FlashGroupUi::updateFlashUiForQueue(time, false, false); } void ExceedBulletSpeedFlashUi::display(const ros::Time& time) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 7b07d9aa..9df99502 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -482,4 +482,5 @@ void FriendBulletsTimeChangeGroupUi::updateConfig() it.second->setIntNum(standard5_bullets_); } } + } // namespace rm_referee