diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 03a18e12..0038f82c 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -87,6 +87,7 @@ add_message_files( Buff.msg RadarToSentry.msg SentryCmd.msg + VisualizeStateData.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg PowerManagementProcessStackOverflowData.msg diff --git a/rm_msgs/msg/referee/VisualizeStateData.msg b/rm_msgs/msg/referee/VisualizeStateData.msg new file mode 100644 index 00000000..1b532992 --- /dev/null +++ b/rm_msgs/msg/referee/VisualizeStateData.msg @@ -0,0 +1 @@ +bool[] state diff --git a/rm_msgs/srv/SetLimitVel.srv b/rm_msgs/srv/SetLimitVel.srv index 1e910e7f..61a7691c 100644 --- a/rm_msgs/srv/SetLimitVel.srv +++ b/rm_msgs/srv/SetLimitVel.srv @@ -1,3 +1,5 @@ float32 limit_vel +float32 slide_window --- float32 current_limit_vel +float32 current_slide_window diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index 3fce648e..220fc605 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -47,6 +47,8 @@ #include #include #include +#include "std_msgs/UInt32.h" +#include "rm_msgs/VisualizeStateData.h" #include "rm_referee/common/protocol.h" diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 0bd6f1c3..6dc3cb03 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -64,6 +64,8 @@ class RefereeBase virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); virtual void radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data); + virtual void customizeDisplayCmdCallBack(const std_msgs::UInt32ConstPtr& data); + virtual void visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataConstPtr& data); // send ui void sendSerialDataCallback(); @@ -94,6 +96,8 @@ class RefereeBase ros::Subscriber sentry_state_sub_; ros::Subscriber drone_pose_sub_; ros::Subscriber shoot_cmd_sub_; + ros::Subscriber customize_display_cmd_sub_; + ros::Subscriber visualize_state_data_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; @@ -120,6 +124,7 @@ class RefereeBase DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{}; StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, *joint_temperature_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{}; + VisualizeStateTriggerChangeUi* visualize_state_trigger_change_ui_{}; FixedUi* fixed_ui_{}; @@ -127,7 +132,7 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; - EngineerActionFlashUi* engineer_action_flash_ui_{}; + CustomizeDisplayFlashUi* customize_display_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; CustomInfoSender* custom_info_sender{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 34f5a7dc..7ae978ee 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -40,12 +40,12 @@ class FlashGroupUi : public GroupUiBase std::string graph_name_; }; -class EngineerActionFlashUi : public FlashGroupUi +class CustomizeDisplayFlashUi : public FlashGroupUi { public: - explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue) + explicit CustomizeDisplayFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashGroupUi(rpc_value, base, "customize_display", graph_queue, character_queue) { if (rpc_value.hasMember("data")) { @@ -57,7 +57,7 @@ class EngineerActionFlashUi : public FlashGroupUi } } } - void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); + void updateCmdData(const uint32_t& data); private: void display(const ros::Time& time) override; diff --git a/rm_referee/include/rm_referee/ui/trigger_change_ui.h b/rm_referee/include/rm_referee/ui/trigger_change_ui.h index 7fd04bfc..74d8539c 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -252,4 +252,27 @@ class FrictionSpeedTriggerChangeUi : public TriggerChangeUi double wheel_speed_; }; +class VisualizeStateTriggerChangeUi : public TriggerChangeGroupUi +{ +public: + explicit VisualizeStateTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& name, + std::deque* graph_queue, std::deque* character_queue) + : TriggerChangeGroupUi(rpc_value, base, name, graph_queue, character_queue) + { + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + for (int i = 0; i < static_cast(rpc_value["data"].size()); i++) + { + graph_vector_.insert( + std::pair(std::to_string(i), new Graph(data[i]["config"], base_, id_++))); + } + } + }; + void updateUiColor(const std::vector& data); + +private: + void update() override; +}; + } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index b8969e93..412ae7b8 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -50,6 +50,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) "/sentry_target_to_referee", 1, &RefereeBase::sentryAttackingTargetCallback, this); RefereeBase::radar_to_referee_sub_ = nh.subscribe("/radar_to_referee", 1, &RefereeBase::radarToRefereeCallBack, this); + RefereeBase::customize_display_cmd_sub_ = + nh.subscribe("/customize_display_ui", 1, &RefereeBase::customizeDisplayCmdCallBack, this); + RefereeBase::visualize_state_data_sub_ = + nh.subscribe("/visualize_state", 1, &RefereeBase::visualizeStateDataCallBack, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -88,6 +92,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "stone") stone_num_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "stone_num", &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "visualize_state") + visualize_state_trigger_change_ui_ = + new VisualizeStateTriggerChangeUi(rpc_value[i], base_, "visualize_state", &graph_queue_, &character_queue_); } ui_nh.getParam("time_change", rpc_value); @@ -148,8 +155,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "exceed_bullet_speed") exceed_bullet_speed_flash_ui_ = new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "engineer_action") - engineer_action_flash_ui_ = new EngineerActionFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "customize_display") + customize_display_flash_ui_ = + new CustomizeDisplayFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -157,8 +165,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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 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") @@ -247,6 +253,8 @@ void RefereeBase::addUi() target_distance_time_change_ui_->addForQueue(); if (friend_bullets_time_change_group_ui_) friend_bullets_time_change_group_ui_->addForQueue(); + if (visualize_state_trigger_change_ui_) + visualize_state_trigger_change_ui_->addForQueue(); add_ui_times_++; } @@ -453,8 +461,8 @@ void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& da stone_num_trigger_change_ui_->updateStringUiData(std::to_string(data->stone_num)); if (servo_mode_trigger_change_ui_ && !is_adding_) servo_mode_trigger_change_ui_->updateStringUiData(data->control_mode); - if (engineer_action_flash_ui_ && !is_adding_) - engineer_action_flash_ui_->updateEngineerUiCmdData(data, ros::Time::now()); + if (customize_display_flash_ui_ && !is_adding_) + customize_display_flash_ui_->updateCmdData(data->symbol); } void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data) { @@ -585,4 +593,21 @@ void RefereeBase::radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& d radar_to_sentry_->updateRadarToSentryData(data); } +void RefereeBase::customizeDisplayCmdCallBack(const std_msgs::UInt32ConstPtr& data) +{ + if (customize_display_flash_ui_ && !is_adding_) + customize_display_flash_ui_->updateCmdData(data->data); +} + +void RefereeBase::visualizeStateDataCallBack(const rm_msgs::VisualizeStateDataConstPtr& data) +{ + if (visualize_state_trigger_change_ui_ && !is_adding_) + { + std::vector state; + for (auto state_data : data->state) + state.push_back(state_data); + visualize_state_trigger_change_ui_->updateUiColor(state); + } +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index b61f4753..34c752b9 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -122,14 +122,13 @@ void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool graph_queue_->push_back(*graph); } -void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, - const ros::Time& last_get_data_time) +void CustomizeDisplayFlashUi::updateCmdData(const uint32_t& data) { - symbol_ = data->symbol; - display(last_get_data_time); + symbol_ = data; + display(ros::Time::now()); } -void EngineerActionFlashUi::display(const ros::Time& time) +void CustomizeDisplayFlashUi::display(const ros::Time& time) { for (auto graph : graph_vector_) { diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index b91865de..77d76bcf 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -450,4 +450,21 @@ void FrictionSpeedTriggerChangeUi::update() updateForQueue(true); } +void VisualizeStateTriggerChangeUi::updateUiColor(const std::vector& data) +{ + for (int i = 0; i < static_cast(data.size()); i++) + { + graph_vector_.find(std::to_string(i)) + ->second->setColor(data[i] ? rm_referee::GraphColor::GREEN : rm_referee::GraphColor::PINK); + } + update(); +} + +void VisualizeStateTriggerChangeUi::update() +{ + for (auto graph : graph_vector_) + graph.second->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + } // namespace rm_referee