From 504c950220a4e10de078c17924a1fd4b7fffe476 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 11 Sep 2023 23:03:32 +0800 Subject: [PATCH 01/29] modify ui queue --- rm_referee/include/rm_referee/referee_base.h | 11 +++- rm_referee/include/rm_referee/ui/flash_ui.h | 6 +- .../include/rm_referee/ui/time_change_ui.h | 22 +++---- .../include/rm_referee/ui/trigger_change_ui.h | 15 +++-- rm_referee/include/rm_referee/ui/ui_base.h | 11 ++-- rm_referee/src/referee_base.cpp | 66 +++++++++++-------- rm_referee/src/ui/time_change_ui.cpp | 4 +- rm_referee/src/ui/trigger_change_ui.cpp | 16 +++++ rm_referee/src/ui/ui_base.cpp | 4 +- 9 files changed, 99 insertions(+), 56 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 4be31103..055d7a4d 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -13,6 +13,7 @@ #include "rm_referee/ui/time_change_ui.h" #include "rm_referee/ui/flash_ui.h" + namespace rm_referee { class RefereeBase @@ -97,7 +98,15 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; - std::vector graph_queue_; + std::queue graph_queue_; + //std::queue character_queue_; + Graph returnGraph(std::queue& queue_) +{ +Graph first_element = queue_.front(); +queue_.pop(); +return first_element; +} +std::vector ui_buffer; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 016bb5c4..d3a49f82 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,7 +12,7 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -24,7 +24,7 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : FlashUi(rpc_value, base, "cover", graph_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; @@ -36,7 +36,7 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : FlashUi(rpc_value, base, "spin", graph_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); 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 2d8020f6..a9c1b375 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,7 +12,7 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -26,7 +26,7 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : GroupUiBase(rpc_value, base, graph_queue) { graph_name_ = graph_name; @@ -42,7 +42,7 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "capacitor", graph_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,7 +55,7 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "effort", graph_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -68,7 +68,7 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "progress", graph_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); @@ -81,7 +81,7 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "dart", graph_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); @@ -93,7 +93,7 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "rotation", graph_queue) { if (rpc_value.hasMember("data")) @@ -125,7 +125,7 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue) { if (rpc_value.hasMember("data")) @@ -171,7 +171,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue) { XmlRpc::XmlRpcValue config; @@ -232,7 +232,7 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,7 +245,7 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::string name) : TimeChangeUi(rpc_value, base, name, graph_queue) { 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 82abac28..055b432d 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,7 +13,7 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { if (graph_name == "chassis") @@ -23,11 +23,12 @@ class TriggerChangeUi : public UiBase }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + void updateForQueue(); }; class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "chassis", graph_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) @@ -58,7 +59,7 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "shooter", graph_queue) { graph_->setContent("0"); @@ -76,7 +77,7 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue) { graph_->setContent("0"); @@ -94,7 +95,7 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "target", graph_queue) { graph_->setContent("armor"); @@ -116,7 +117,7 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue) { } @@ -177,7 +178,7 @@ class PolygonTriggerChangeGroupUi : public GroupUiBase class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "camera", graph_queue) { if (rpc_value.hasMember("camera_name")) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 03725530..c59f1ea4 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -18,13 +19,14 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) if (rpc_value["config"].hasMember("delay")) delay_ = ros::Duration(static_cast(rpc_value["config"]["delay"])); graph_queue_ = graph_queue; + //character_queue_ = character_queue; }; ~UiBase() = default; virtual void add(); @@ -57,7 +59,8 @@ class UiBase Base& base_; Graph* graph_; static int id_; - std::vector* graph_queue_; + std::queue * graph_queue_; + std::queue * character_queue_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -69,7 +72,7 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : UiBase(rpc_value, base, graph_queue){}; ~GroupUiBase() = default; void add() override; @@ -93,7 +96,7 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : GroupUiBase(rpc_value, base, graph_queue) { for (int i = 0; i < static_cast(rpc_value.size()); i++) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index f39e89e1..f7737096 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -113,7 +113,7 @@ void RefereeBase::addUi() add_ui_timer_.stop(); if (!graph_queue_.empty()) while (graph_queue_.size() > 0) - graph_queue_.pop_back(); + graph_queue_.pop(); is_adding_ = false; send_graph_ui_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; @@ -159,6 +159,7 @@ void RefereeBase::addUi() add_ui_times_++; } + void RefereeBase::sendGraphQueueCallback() { if (graph_queue_.empty()) @@ -169,48 +170,61 @@ void RefereeBase::sendGraphQueueCallback() ROS_WARN_THROTTLE(2.0, "Sending UI too frequently, please modify the configuration file or code to " "reduce the frequency"); while (graph_queue_.size() > 50) - graph_queue_.pop_back(); + graph_queue_.pop(); } - int index = graph_queue_.size() - 1; if (!is_adding_) { + +/* + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + sendCharacter(time, graph_); +*/ if (graph_queue_.size() >= 7) { - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1), - &graph_queue_.at(index - 2), &graph_queue_.at(index - 3), - &graph_queue_.at(index - 4), &graph_queue_.at(index - 5), - &graph_queue_.at(index - 6)); - for (int i = 0; i < 7; i++) - graph_queue_.pop_back(); - } + for(int i=0;i<7;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), + &ui_buffer.at(2), &ui_buffer.at(3), + &ui_buffer.at(4), &ui_buffer.at(5), + &ui_buffer.at(6)); + ui_buffer.clear(); + } else if (graph_queue_.size() >= 5) { - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1), - &graph_queue_.at(index - 2), &graph_queue_.at(index - 3), - &graph_queue_.at(index - 4)); - for (int i = 0; i < 5; i++) - graph_queue_.pop_back(); + for(int i=0;i<5;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), + &ui_buffer.at(2), &ui_buffer.at(3), + &ui_buffer.at(4)); + ui_buffer.clear(); } else if (graph_queue_.size() >= 2) { - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1)); - for (int i = 0; i < 2; i++) - graph_queue_.pop_back(); + for(int i=0;i<2;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1)); + ui_buffer.clear(); } else if (graph_queue_.size() == 1) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); + ui_buffer.clear(); } + } - else - { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); - } + else{ + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); + ui_buffer.clear(); + } send_graph_ui_timer_.start(); } @@ -270,7 +284,7 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) is_adding_ = true; if (!graph_queue_.empty()) while (graph_queue_.size() > 0) - graph_queue_.pop_back(); + graph_queue_.pop(); send_graph_ui_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 98f80c55..9503b3c6 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -30,7 +30,7 @@ void TimeChangeUi::updateForQueue() if (graph_queue_ && !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) { - graph_queue_->push_back(*graph_); + graph_queue_->push(*graph_); last_send_ = ros::Time::now(); } } @@ -44,7 +44,7 @@ void TimeChangeGroupUi::updateForQueue() graph.second->setOperation(rm_referee::GraphOperation::UPDATE); if (graph_queue_ && !graph.second->isRepeated()) { - graph_queue_->push_back(*graph.second); + graph_queue_->push(*graph.second); last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index f0355b2a..bf04e83f 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -11,7 +11,23 @@ void TriggerChangeUi::setContent(const std::string& content) graph_->setContent(content); display(); } +/* +void TriggerChangeUi::updateForQueue() +{ + + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_.push(characters); + else + sendSingleGraph(time, graph_); +} +*/ void ChassisTriggerChangeUi::update() { if (s_l_ == rm_msgs::DbusData::MID && s_r_ == rm_msgs::DbusData::UP) diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index dd0f427f..6b82f985 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -18,7 +18,7 @@ void UiBase::addForQueue(int add_times) for (int i = 0; i < add_times; i++) { graph_->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push_back(*graph_); + graph_queue_->push(*graph_); last_send_ = ros::Time::now(); } } @@ -51,7 +51,7 @@ void GroupUiBase::addForQueue(int add_times) for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push_back(*graph.second); + graph_queue_->push(*graph.second); last_send_ = ros::Time::now(); } } From b1878cdee33a70b777222be704327470a6393e1d Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 12 Sep 2023 22:59:08 +0800 Subject: [PATCH 02/29] modify ui sender --- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/include/rm_referee/ui/flash_ui.h | 10 +- .../include/rm_referee/ui/time_change_ui.h | 44 +++--- .../include/rm_referee/ui/trigger_change_ui.h | 58 +++++--- rm_referee/include/rm_referee/ui/ui_base.h | 12 +- rm_referee/src/referee_base.cpp | 72 +++++----- rm_referee/src/ui/trigger_change_ui.cpp | 132 +++++++++++++++--- 7 files changed, 226 insertions(+), 104 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 055d7a4d..32d5f8cd 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -99,7 +99,7 @@ class RefereeBase GroupUiBase* graph_queue_sender_{}; std::queue graph_queue_; - //std::queue character_queue_; + std::queue character_queue_; Graph returnGraph(std::queue& queue_) { Graph first_element = queue_.front(); diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index d3a49f82..256ddfb5 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,7 +12,7 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) + std::queue * graph_queue, std::queue * character_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -24,8 +24,8 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : FlashUi(rpc_value, base, "cover", graph_queue){}; + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; private: @@ -36,8 +36,8 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : FlashUi(rpc_value, base, "spin", graph_queue){}; + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : FlashUi(rpc_value, base, "spin", graph_queue, character_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); private: 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 a9c1b375..a132edfd 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,8 +12,8 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : UiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); } @@ -26,8 +26,8 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : GroupUiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; } @@ -42,8 +42,8 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "capacitor", graph_queue){}; + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "capacitor", graph_queue, character_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,8 +55,8 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "effort", graph_queue){}; + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "effort", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); private: @@ -68,8 +68,8 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "progress", graph_queue){}; + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "progress", graph_queue, character_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); private: @@ -81,8 +81,8 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "dart", graph_queue){}; + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "dart", graph_queue, character_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); private: @@ -93,8 +93,8 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "rotation", graph_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "rotation", graph_queue, character_queue) { if (rpc_value.hasMember("data")) { @@ -125,8 +125,8 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue, character_queue) { if (rpc_value.hasMember("data")) { @@ -171,8 +171,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue, character_queue) { XmlRpc::XmlRpcValue config; @@ -232,8 +232,8 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "pitch", graph_queue){}; + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,9 +245,9 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue, std::string name) - : TimeChangeUi(rpc_value, base, name, graph_queue) + : TimeChangeUi(rpc_value, base, name, graph_queue,character_queue) { if (rpc_value.hasMember("data")) { 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 055b432d..c0282a48 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,8 +13,8 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : UiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { if (graph_name == "chassis") graph_ = new Graph(rpc_value["config"], base_, 1); @@ -23,13 +23,34 @@ class TriggerChangeUi : public UiBase }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; - void updateForQueue(); + void updateForQueue(bool check_repeat = true); + void updateTwiceForQueue(bool check_repeat = true); }; + + +class TriggerChangeGroupUi : public GroupUiBase +{ +public: + explicit TriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, + std::queue * graph_queue, std::queue * character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) + { + if (graph_name == "chassis") + graph_ = new Graph(rpc_value["config"], base_, 1); + else + graph_ = new Graph(rpc_value["config"], base_, id_++); + }; + virtual void setContent(const std::string& content); + virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + void updateForQueue(bool check_repeat = true); + void updateTwiceForQueue(bool check_repeat = true); +}; + class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "chassis", graph_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "chassis", graph_queue, character_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) graph_->setContent("raw"); @@ -59,8 +80,8 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "shooter", graph_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "shooter", graph_queue, character_queue) { graph_->setContent("0"); } @@ -77,8 +98,8 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue, character_queue) { graph_->setContent("0"); } @@ -95,8 +116,8 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "target", graph_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "target", graph_queue, character_queue) { graph_->setContent("armor"); if (base_.robot_color_ == "red") @@ -117,8 +138,8 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue, character_queue) { } void updateTrackID(int id); @@ -129,10 +150,11 @@ class TargetViewAngleTriggerChangeUi : public TriggerChangeUi int track_id_; }; -class PolygonTriggerChangeGroupUi : public GroupUiBase +class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi { public: - explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base) : GroupUiBase(rpc_value, base) + explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); XmlRpc::XmlRpcValue config; @@ -172,14 +194,14 @@ class PolygonTriggerChangeGroupUi : public GroupUiBase } } void update() override; - virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + }; class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "camera", graph_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "camera", graph_queue, character_queue) { if (rpc_value.hasMember("camera_name")) { diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index c59f1ea4..6403f7f3 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -19,14 +19,14 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr ) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) if (rpc_value["config"].hasMember("delay")) delay_ = ros::Duration(static_cast(rpc_value["config"]["delay"])); graph_queue_ = graph_queue; - //character_queue_ = character_queue; + character_queue_ = character_queue; }; ~UiBase() = default; virtual void add(); @@ -72,8 +72,8 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) - : UiBase(rpc_value, base, graph_queue){}; + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + : UiBase(rpc_value, base, graph_queue, character_queue){}; ~GroupUiBase() = default; void add() override; void update() override; @@ -96,8 +96,8 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) - : GroupUiBase(rpc_value, base, graph_queue) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { for (int i = 0; i < static_cast(rpc_value.size()); i++) graph_vector_.insert( diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index f7737096..bd2a213c 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -47,44 +47,44 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "chassis") - chassis_trigger_change_ui_ = new ChassisTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + chassis_trigger_change_ui_ = new ChassisTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "shooter") - shooter_trigger_change_ui_ = new ShooterTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + shooter_trigger_change_ui_ = new ShooterTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gimbal") - gimbal_trigger_change_ui_ = new GimbalTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + gimbal_trigger_change_ui_ = new GimbalTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target") - target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target_view_angle") - target_view_angle_trigger_change_ui_ = new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + target_view_angle_trigger_change_ui_ = new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "camera") - camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("time_change", rpc_value); for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "capacitor") - capacitor_time_change_ui_ = new CapacitorTimeChangeUi(rpc_value[i], base_, &graph_queue_); + capacitor_time_change_ui_ = new CapacitorTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "effort") - effort_time_change_ui_ = new EffortTimeChangeUi(rpc_value[i], base_, &graph_queue_); + effort_time_change_ui_ = new EffortTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "progress") - progress_time_change_ui_ = new ProgressTimeChangeUi(rpc_value[i], base_, &graph_queue_); + progress_time_change_ui_ = new ProgressTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "dart_status") - dart_status_time_change_ui_ = new DartStatusTimeChangeUi(rpc_value[i], base_, &graph_queue_); + dart_status_time_change_ui_ = new DartStatusTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "rotation") - rotation_time_change_ui_ = new RotationTimeChangeUi(rpc_value[i], base_, &graph_queue_); + rotation_time_change_ui_ = new RotationTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "lane_line") - lane_line_time_change_ui_ = new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_); + lane_line_time_change_ui_ = new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "pitch") - pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_); + pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "balance_pitch") - balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_); + balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "engineer_joint1") - engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint1"); + engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint1"); if (rpc_value[i]["name"] == "engineer_joint2") - engineer_joint2_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint2"); + engineer_joint2_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint2"); if (rpc_value[i]["name"] == "engineer_joint3") - engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint3"); + engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint3"); } ui_nh.getParam("fixed", rpc_value); @@ -94,9 +94,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "cover") - cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_); + 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_); + spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } @@ -162,27 +162,35 @@ void RefereeBase::addUi() void RefereeBase::sendGraphQueueCallback() { - if (graph_queue_.empty()) + if (graph_queue_.empty() || character_queue_.empty()) return; if (graph_queue_.size() > 50) { - ROS_WARN_THROTTLE(2.0, "Sending UI too frequently, please modify the configuration file or code to " + ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " "reduce the frequency"); while (graph_queue_.size() > 50) graph_queue_.pop(); } + if (character_queue_.size() > 7) + { + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); + while (character_queue_.size() > 7) + character_queue_.pop(); + } + if (!is_adding_) { + if(!character_queue_.empty()) + { + graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); + character_queue_.pop(); + } -/* - std::string characters = graph_->getCharacters(); - if (!characters.empty()) - sendCharacter(time, graph_); -*/ - if (graph_queue_.size() >= 7) + else if (graph_queue_.size() >= 7) { for(int i=0;i<7;i++) ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); @@ -213,16 +221,14 @@ void RefereeBase::sendGraphQueueCallback() } else if (graph_queue_.size() == 1) { - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); - ui_buffer.clear(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); + graph_queue_.pop(); } } else{ - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); - ui_buffer.clear(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); + graph_queue_.pop(); } send_graph_ui_timer_.start(); diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index bf04e83f..18f0611d 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -11,23 +11,116 @@ void TriggerChangeUi::setContent(const std::string& content) graph_->setContent(content); display(); } -/* -void TriggerChangeUi::updateForQueue() + +void TriggerChangeUi::updateForQueue(bool check_repeat) { + if (check_repeat) + if (graph_->isRepeated()) + return; + graph_->updateLastConfig(); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_->push(*graph_); + else + graph_queue_->push(*graph_); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) +} +void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) +{ + if (check_repeat) + if (graph_->isRepeated()) return; + graph_->updateLastConfig(); - std::string characters = graph_->getCharacters(); - if (!characters.empty()) - character_queue_.push(characters); - else - sendSingleGraph(time, graph_); + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + { + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + } + else{ + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + } + +} + +void TriggerChangeGroupUi::setContent(const std::string& content) +{ + graph_->setContent(content); + display(); +} + +void TriggerChangeGroupUi::updateForQueue(bool check_repeat) +{ + if (check_repeat) + { + 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(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : character_vector_) + character_queue_->push(*graph_); + for (auto it : graph_vector_) + graph_queue_->push(*graph_); } -*/ + +void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) +{ + if (check_repeat) + { + 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(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : character_vector_) + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + for (auto it : graph_vector_) + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + +} + + void ChassisTriggerChangeUi::update() { if (s_l_ == rm_msgs::DbusData::MID && s_r_ == rm_msgs::DbusData::UP) @@ -37,7 +130,7 @@ void ChassisTriggerChangeUi::update() power_limit_state_ == rm_common::PowerLimit::CHARGE); graph_->setOperation(rm_referee::GraphOperation::UPDATE); checkModeChange(); - displayTwice(); + updateTwiceForQueue(); } void ChassisTriggerChangeUi::displayInCapacity() @@ -47,7 +140,7 @@ void ChassisTriggerChangeUi::displayInCapacity() updateConfig(254, 0); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void ChassisTriggerChangeUi::checkModeChange() @@ -70,7 +163,7 @@ void ChassisTriggerChangeUi::checkModeChange() else if ((ros::Time::now() - trigger_time).toSec() > mode_change_threshold_) { is_different = false; - display(false); + updateForQueue(false); } } } @@ -174,7 +267,7 @@ void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - TriggerChangeUi::display(); + updateForQueue(); } void ShooterTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -218,7 +311,7 @@ void GimbalTriggerChangeUi::update() updateConfig(gimbal_mode_, gimbal_eject_); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void GimbalTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -261,7 +354,7 @@ void TargetTriggerChangeUi::update() else updateConfig(gimbal_eject_, shoot_frequency_, det_armor_target_, det_color_ == rm_msgs::StatusChangeRequest::RED); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - TriggerChangeUi::display(); + updateForQueue(); } void TargetTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -322,7 +415,7 @@ void TargetViewAngleTriggerChangeUi::update() { updateConfig(track_id_ == 0, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void TargetViewAngleTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -343,7 +436,8 @@ void PolygonTriggerChangeGroupUi::update() { for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(); + } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) @@ -367,6 +461,6 @@ void CameraTriggerChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(); } } // namespace rm_referee From a85d00805600337601da110ae3083ab4c9192b7f Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 17 Sep 2023 21:02:26 +0800 Subject: [PATCH 03/29] modify the ui and interactive data sender --- rm_referee/include/rm_referee/referee_base.h | 17 +++--- rm_referee/include/rm_referee/ui/flash_ui.h | 2 + rm_referee/include/rm_referee/ui/ui_base.h | 6 +- rm_referee/src/referee_base.cpp | 58 +++++++++++++------- rm_referee/src/ui/flash_ui.cpp | 40 +++++++++++++- rm_referee/src/ui/time_change_ui.cpp | 6 +- rm_referee/src/ui/ui_base.cpp | 26 +++++++-- 7 files changed, 116 insertions(+), 39 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 32d5f8cd..fee8f335 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -52,8 +52,8 @@ class RefereeBase virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data); virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); - // send graph_type ui - void sendGraphQueueCallback(); + // send ui + void sendSerialDataCallback(); ros::Subscriber joint_state_sub_; ros::Subscriber actuator_state_sub_; @@ -102,16 +102,19 @@ class RefereeBase std::queue character_queue_; Graph returnGraph(std::queue& queue_) { -Graph first_element = queue_.front(); -queue_.pop(); -return first_element; + Graph first_element = queue_.front(); + queue_.pop(); + return first_element; } -std::vector ui_buffer; + std::vector ui_buffer; + rm_referee::ClientMapReceiveData radar_receive_data; + rm_referee::MapSentryData map_sentry_data; + bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; Base& base_; - ros::Timer add_ui_timer_, send_graph_ui_timer_; + ros::Timer add_ui_timer_, send_serial_data_timer_; int add_ui_times_, add_ui_max_times_, add_ui_frequency_; double send_ui_queue_delay_; bool add_ui_flag_ = false, is_adding_ = false; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 256ddfb5..7a084919 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -19,6 +19,7 @@ class FlashUi : public UiBase } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; + void updateForQueue(const ros::Time& time, bool state, bool once); }; class CoverFlashUi : public FlashUi @@ -30,6 +31,7 @@ class CoverFlashUi : public FlashUi private: void display(const ros::Time& time) override; + uint8_t cover_state_; }; diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 6403f7f3..21e4ac42 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -41,7 +41,7 @@ class UiBase void sendSingleGraph(const ros::Time& time, Graph* graph); void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); void sendRadarInteractiveData(rm_referee::ClientMapReceiveData& data); - void sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data); + void sendMapSentryData(rm_referee::MapSentryData& data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); @@ -67,6 +67,7 @@ class UiBase ros::Time last_send_; ros::Duration delay_ = ros::Duration(0.); const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; + }; class GroupUiBase : public UiBase @@ -103,6 +104,9 @@ class FixedUi : public GroupUiBase graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; + ~FixedUi() = default; + void updateForQueue(); }; + } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index bd2a213c..4d491ce9 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -102,8 +102,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) add_ui_timer_ = nh.createTimer(ros::Duration(1. / add_ui_frequency_), std::bind(&RefereeBase::addUi, this), false, false); - send_graph_ui_timer_ = nh.createTimer(ros::Duration(send_ui_queue_delay_), - std::bind(&RefereeBase::sendGraphQueueCallback, this), false, true); + send_serial_data_timer_ = nh.createTimer(ros::Duration(send_ui_queue_delay_), + std::bind(&RefereeBase::sendSerialDataCallback, this), false, true); } void RefereeBase::addUi() { @@ -115,7 +115,7 @@ void RefereeBase::addUi() while (graph_queue_.size() > 0) graph_queue_.pop(); is_adding_ = false; - send_graph_ui_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); + send_serial_data_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; } @@ -160,7 +160,7 @@ void RefereeBase::addUi() } -void RefereeBase::sendGraphQueueCallback() +void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() || character_queue_.empty()) return; @@ -173,18 +173,28 @@ void RefereeBase::sendGraphQueueCallback() graph_queue_.pop(); } - if (character_queue_.size() > 7) + if (character_queue_.size() > 6) { - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently"); while (character_queue_.size() > 7) character_queue_.pop(); } - if (!is_adding_) { - if(!character_queue_.empty()) + if(send_radar_receive_data_) + { + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + send_radar_receive_data_ = false; + } + + else if (send_map_sentry_data_) + { + interactive_data_sender_->sendMapSentryData(map_sentry_data); + send_map_sentry_data_ = false; + } + + else if(!character_queue_.empty()) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); character_queue_.pop(); @@ -226,18 +236,19 @@ void RefereeBase::sendGraphQueueCallback() } } - else{ + else + { graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); graph_queue_.pop(); - } - send_graph_ui_timer_.start(); + } + send_serial_data_timer_.start(); } void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, const ros::Time& last_get_data_time) { if (fixed_ui_ && !is_adding_) - fixed_ui_->update(); + fixed_ui_->updateForQueue(); } void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { @@ -291,7 +302,7 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) if (!graph_queue_.empty()) while (graph_queue_.size() > 0) graph_queue_.pop(); - send_graph_ui_timer_.setPeriod(ros::Duration(0.05)); + send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; } @@ -366,15 +377,22 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data } void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { - rm_referee::ClientMapReceiveData send_data; - send_data.target_position_x = data->target_position_x; - send_data.target_position_y = data->target_position_y; - send_data.target_robot_ID = data->target_robot_ID; + radar_receive_data.target_position_x = data->target_position_x; + radar_receive_data.target_position_y = data->target_position_y; + radar_receive_data.target_robot_ID = data->target_robot_ID; - interactive_data_sender_->sendRadarInteractiveData(send_data); + send_radar_receive_data_ = true; } void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) { - interactive_data_sender_->sendMapSentryData(data); + map_sentry_data.intention = data->intention; + map_sentry_data.start_position_x = data->start_position_x; + map_sentry_data.start_position_y = data->start_position_y; + for (int i = 0; i < 49; i++) + { + map_sentry_data.delta_x[i] = data->delta_x[i]; + map_sentry_data.delta_y[i] = data->delta_y[i]; + } + send_map_sentry_data_ = true; } } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ad897bab..a40f6bfc 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -6,11 +6,47 @@ namespace rm_referee { + +void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) +{ + if (once) + { + if (state) + graph_->setOperation(rm_referee::GraphOperation::ADD); + else + graph_->setOperation(rm_referee::GraphOperation::DELETE); + } + else if (state && 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 (graph_->isRepeated()) + return; + graph_->updateLastConfig(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + { + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + } + else{ + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + } +} + void CoverFlashUi::display(const ros::Time& time) { if (!cover_state_) graph_->setOperation(rm_referee::GraphOperation::DELETE); - UiBase::display(time, cover_state_, true); + FlashUi::updateForQueue(time, cover_state_, true); } void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, @@ -24,7 +60,7 @@ void SpinFlashUi::display(const ros::Time& time) { if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) graph_->setOperation(rm_referee::GraphOperation::DELETE); - UiBase::display(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); + FlashUi::updateForQueue(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); } void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 9503b3c6..d27bf654 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -129,7 +129,7 @@ void ProgressTimeChangeUi::updateEngineerUiData(const rm_msgs::EngineerUi::Const { total_steps_ = data->total_steps; finished_data_ = data->finished_step; - TimeChangeUi::update(); + TimeChangeUi::updateForQueue(); } void DartStatusTimeChangeUi::updateConfig() @@ -157,7 +157,7 @@ void DartStatusTimeChangeUi::updateDartClientCmd(const rm_msgs::DartClientCmd::C const ros::Time& last_get_data_time) { dart_launch_opening_status_ = data->dart_launch_opening_status; - TimeChangeUi::update(); + TimeChangeUi::updateForQueue(); } void RotationTimeChangeUi::updateConfig() @@ -284,7 +284,7 @@ void PitchAngleTimeChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(ros::Time::now()); + updateForQueue(); } void PitchAngleTimeChangeUi::updateConfig() diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 6b82f985..91c16be6 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -150,20 +150,20 @@ void UiBase::sendUi(const ros::Time& time) sendSingleGraph(time, graph_); } -void UiBase::sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data) +void UiBase::sendMapSentryData(rm_referee::MapSentryData& data) { uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; for (int i = 0; i < 128; i++) tx_buffer_[i] = 0; - map_sentry_data->intention = data->intention; - map_sentry_data->start_position_x = data->start_position_x; - map_sentry_data->start_position_y = data->start_position_y; + map_sentry_data->intention = data.intention; + map_sentry_data->start_position_x = data.start_position_x; + map_sentry_data->start_position_y = data.start_position_y; for (int i = 0; i < 49; i++) { - map_sentry_data->delta_x[i] = data->delta_x[i]; - map_sentry_data->delta_y[i] = data->delta_y[i]; + map_sentry_data->delta_x[i] = data.delta_x[i]; + map_sentry_data->delta_y[i] = data.delta_y[i]; } pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::MAP_SENTRY_CMD, sizeof(rm_referee::MapSentryData)); tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::MapSentryData) + k_tail_length_); @@ -362,6 +362,20 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr sendSerial(time, data_len); } +void FixedUi::updateForQueue() +{ + if (ros::Time::now() - last_send_ > delay_) + for (auto graph : graph_vector_) + { + graph.second->setOperation(rm_referee::GraphOperation::UPDATE); + if (graph_queue_ && !graph.second->isRepeated()) + { + graph_queue_->push(*graph.second); + last_send_ = ros::Time::now(); + } + } +} + void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const { memset(tx_buffer, 0, k_frame_length_); From f137f3205325136f911b648736546c938a486b46 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 17 Sep 2023 21:39:49 +0800 Subject: [PATCH 04/29] Modify the ui and robot interactive data sender --- rm_referee/src/ui/time_change_ui.cpp | 2 -- rm_referee/src/ui/trigger_change_ui.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index d27bf654..18badf57 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -282,8 +282,6 @@ void PitchAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState: void PitchAngleTimeChangeUi::update() { - updateConfig(); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(); } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 18f0611d..90a6ebac 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -437,7 +437,6 @@ void PolygonTriggerChangeGroupUi::update() for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(); - } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) From e8e28a7a34f8d88d79293c5f39e1f84c6d509fc4 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 19 Sep 2023 20:31:50 +0800 Subject: [PATCH 05/29] Modify the ui and robot interactive data sender --- rm_referee/include/rm_referee/referee_base.h | 12 +- rm_referee/include/rm_referee/ui/flash_ui.h | 8 +- .../include/rm_referee/ui/time_change_ui.h | 22 ++-- .../include/rm_referee/ui/trigger_change_ui.h | 28 ++--- rm_referee/include/rm_referee/ui/ui_base.h | 18 ++- rm_referee/src/referee_base.cpp | 107 ++++++++++-------- rm_referee/src/ui/flash_ui.cpp | 14 +-- rm_referee/src/ui/time_change_ui.cpp | 19 +++- rm_referee/src/ui/trigger_change_ui.cpp | 16 +-- rm_referee/src/ui/ui_base.cpp | 40 ++++--- 10 files changed, 155 insertions(+), 129 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index fee8f335..81d1b97e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -98,18 +98,12 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; - std::queue graph_queue_; - std::queue character_queue_; - Graph returnGraph(std::queue& queue_) -{ - Graph first_element = queue_.front(); - queue_.pop(); - return first_element; -} - std::vector ui_buffer; + std::deque graph_queue_; + std::deque character_queue_; rm_referee::ClientMapReceiveData radar_receive_data; rm_referee::MapSentryData map_sentry_data; + ros::Time radar_receive_last_send, map_sentry_last_send; bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 7a084919..6c84fd32 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,8 +12,8 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) - : UiBase(rpc_value, base, graph_queue) + std::deque * graph_queue, std::deque * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); } @@ -25,7 +25,7 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; @@ -38,7 +38,7 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : FlashUi(rpc_value, base, "spin", graph_queue, character_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); 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 a132edfd..017eff7f 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,7 +12,7 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -26,7 +26,7 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; @@ -42,7 +42,7 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "capacitor", graph_queue, character_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,7 +55,7 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "effort", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -68,7 +68,7 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "progress", graph_queue, character_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); @@ -81,7 +81,7 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "dart", graph_queue, character_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); @@ -93,7 +93,7 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "rotation", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -125,7 +125,7 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -171,7 +171,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue, character_queue) { XmlRpc::XmlRpcValue config; @@ -232,7 +232,7 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,7 +245,7 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue, std::string name) : TimeChangeUi(rpc_value, base, name, graph_queue,character_queue) { 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 c0282a48..2a7fa677 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,7 +13,7 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque* graph_queue, std::deque* character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { if (graph_name == "chassis") @@ -32,24 +32,24 @@ class TriggerChangeGroupUi : public GroupUiBase { public: explicit TriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - if (graph_name == "chassis") - graph_ = new Graph(rpc_value["config"], base_, 1); - else - graph_ = new Graph(rpc_value["config"], base_, id_++); + graph_name_ = graph_name; }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; void updateForQueue(bool check_repeat = true); void updateTwiceForQueue(bool check_repeat = true); + +protected: + std::string graph_name_; }; class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "chassis", graph_queue, character_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) @@ -80,7 +80,7 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "shooter", graph_queue, character_queue) { graph_->setContent("0"); @@ -98,7 +98,7 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue, character_queue) { graph_->setContent("0"); @@ -116,7 +116,7 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "target", graph_queue, character_queue) { graph_->setContent("armor"); @@ -138,7 +138,7 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue, character_queue) { } @@ -153,7 +153,7 @@ class TargetViewAngleTriggerChangeUi : public TriggerChangeUi class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi { public: - explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); @@ -194,13 +194,13 @@ class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi } } void update() override; - + //virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){} void setContent(const std::string &content) override; }; class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "camera", graph_queue, character_queue) { if (rpc_value.hasMember("camera_name")) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 21e4ac42..70cf2d6c 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -19,7 +19,7 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr ) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr ) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) @@ -59,21 +59,20 @@ class UiBase Base& base_; Graph* graph_; static int id_; - std::queue * graph_queue_; - std::queue * character_queue_; + std::deque* graph_queue_; + std::deque* character_queue_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; ros::Time last_send_; ros::Duration delay_ = ros::Duration(0.); const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; - }; class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue = nullptr, std::deque * character_queue = nullptr) : UiBase(rpc_value, base, graph_queue, character_queue){}; ~GroupUiBase() = default; void add() override; @@ -97,16 +96,15 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - for (int i = 0; i < static_cast(rpc_value.size()); i++) + for(int i = 0; i < static_cast(rpc_value.size()); i++) graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; - ~FixedUi() = default; void updateForQueue(); + int update_fixed_ui_times = 0; }; - } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 4d491ce9..3892e676 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -88,7 +88,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) } ui_nh.getParam("fixed", rpc_value); - fixed_ui_ = new FixedUi(rpc_value, base_, &graph_queue_); + fixed_ui_ = new FixedUi(rpc_value, base_, &graph_queue_, &character_queue_); ui_nh.getParam("flash", rpc_value); for (int i = 0; i < rpc_value.size(); i++) @@ -109,11 +109,10 @@ void RefereeBase::addUi() { if (add_ui_times_ > add_ui_max_times_) { - ROS_INFO("End add"); + ROS_INFO_THROTTLE(2.0, "End adding"); add_ui_timer_.stop(); if (!graph_queue_.empty()) - while (graph_queue_.size() > 0) - graph_queue_.pop(); + graph_queue_.clear(); is_adding_ = false; send_serial_data_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; @@ -162,85 +161,101 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { - if (graph_queue_.empty() || character_queue_.empty()) + if (graph_queue_.empty() && character_queue_.empty()){ + ROS_INFO_THROTTLE(1.0, "No data to send"); return; + } if (graph_queue_.size() > 50) - { ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " "reduce the frequency"); - while (graph_queue_.size() > 50) - graph_queue_.pop(); - } - if (character_queue_.size() > 6) - { - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently"); - while (character_queue_.size() > 7) - character_queue_.pop(); - } + if (character_queue_.size() > 20) + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); if (!is_adding_) { + if (graph_queue_.size() > 50) + { + ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, now reduce the queue"); + while (graph_queue_.size() > 50) + graph_queue_.pop_front(); + } + + if (character_queue_.size() > 8) + { + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, now reduce the queue"); + while (character_queue_.size() > 7) + character_queue_.pop_front(); + } + if(send_radar_receive_data_) { + if(ros::Time::now() - radar_receive_last_send < ros::Duration(0.2)) + ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + radar_receive_last_send = ros::Time::now(); send_radar_receive_data_ = false; } else if (send_map_sentry_data_) { + if(ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) + ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); interactive_data_sender_->sendMapSentryData(map_sentry_data); + radar_receive_last_send = ros::Time::now(); send_map_sentry_data_ = false; } - else if(!character_queue_.empty()) + else if(!character_queue_.empty() && graph_queue_.size() <= 10) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); - character_queue_.pop(); + character_queue_.pop_front(); + ROS_INFO_THROTTLE(1.0, " send character"); } else if (graph_queue_.size() >= 7) { - for(int i=0;i<7;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), - &ui_buffer.at(2), &ui_buffer.at(3), - &ui_buffer.at(4), &ui_buffer.at(5), - &ui_buffer.at(6)); - ui_buffer.clear(); - } + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4), &graph_queue_.at(5), + &graph_queue_.at(6)); + ROS_INFO_THROTTLE(1.0, " send 7 graph"); + + for (int i = 0; i < 7; i++) + graph_queue_.pop_front(); + } else if (graph_queue_.size() >= 5) { - for(int i=0;i<5;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), - &ui_buffer.at(2), &ui_buffer.at(3), - &ui_buffer.at(4)); - ui_buffer.clear(); + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4)); + ROS_INFO_THROTTLE(1.0, " send 5 graph"); + for (int i = 0; i < 5; i++) + graph_queue_.pop_front(); } else if (graph_queue_.size() >= 2) { - for(int i=0;i<2;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1)); - ui_buffer.clear(); + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); + for (int i = 0; i < 2; i++) + graph_queue_.pop_front(); + + ROS_INFO_THROTTLE(1.0, " send 2 graph"); } else if (graph_queue_.size() == 1) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); - graph_queue_.pop(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); + graph_queue_.pop_front(); } - } else { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); - graph_queue_.pop(); - + if(!graph_queue_.empty()) + { + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); + graph_queue_.pop_front(); + } } send_serial_data_timer_.start(); } @@ -300,11 +315,11 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) add_ui_flag_ = false; is_adding_ = true; if (!graph_queue_.empty()) - while (graph_queue_.size() > 0) - graph_queue_.pop(); + graph_queue_.clear(); send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; + } if (data->s_r != rm_msgs::DbusData::UP) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index a40f6bfc..b4a178b8 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -25,6 +25,7 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) } if (graph_->isRepeated()) return; + graph_->updateLastConfig(); if (base_.robot_id_ == 0 || base_.client_id_ == 0) @@ -32,14 +33,11 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) std::string characters = graph_->getCharacters(); if (!characters.empty()) - { - for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); - } - else{ - for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); - } + character_queue_->push_back(*graph_); + else + graph_queue_->push_back(*graph_); + + ROS_INFO_THROTTLE(1.0, "update flash ui"); } void CoverFlashUi::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 18badf57..f82a3c41 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -28,10 +28,16 @@ void TimeChangeUi::updateForQueue() updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) + if ( !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) { - graph_queue_->push(*graph_); + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*graph_); + else + graph_queue_->push_back(*graph_); + last_send_ = ros::Time::now(); + } } @@ -42,9 +48,14 @@ void TimeChangeGroupUi::updateForQueue() for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph.second->isRepeated()) + if ( !graph.second->isRepeated() ) { - graph_queue_->push(*graph.second); + std::string characters = graph.second->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*graph.second); + else + graph_queue_->push_back(*graph.second); + last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 90a6ebac..ca5d2c38 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -24,9 +24,9 @@ void TriggerChangeUi::updateForQueue(bool check_repeat) std::string characters = graph_->getCharacters(); if (!characters.empty()) - character_queue_->push(*graph_); + character_queue_->push_back(*graph_); else - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); } void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) @@ -43,11 +43,11 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) if (!characters.empty()) { for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); + character_queue_->push_back(*graph_); } else{ for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); } } @@ -82,9 +82,9 @@ void TriggerChangeGroupUi::updateForQueue(bool check_repeat) return; for (auto it : character_vector_) - character_queue_->push(*graph_); + character_queue_->push_back(*it.second); for (auto it : graph_vector_) - graph_queue_->push(*graph_); + graph_queue_->push_back(*it.second); } @@ -113,10 +113,10 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) for (auto it : character_vector_) for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); + character_queue_->push_back(*it.second); for (auto it : graph_vector_) for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); + graph_queue_->push_back(*it.second); } diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 91c16be6..3f221634 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -18,7 +18,7 @@ void UiBase::addForQueue(int add_times) for (int i = 0; i < add_times; i++) { graph_->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); last_send_ = ros::Time::now(); } } @@ -51,7 +51,7 @@ void GroupUiBase::addForQueue(int add_times) for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push(*graph.second); + graph_queue_->push_back(*graph.second); last_send_ = ros::Time::now(); } } @@ -352,9 +352,10 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr tx_data.header.receiver_id = base_.client_id_; tx_data.config[0] = graph0->getConfig(); tx_data.config[1] = graph1->getConfig(); - tx_data.config[3] = graph2->getConfig(); - tx_data.config[4] = graph3->getConfig(); - tx_data.config[5] = graph4->getConfig(); + tx_data.config[2] = graph2->getConfig(); + tx_data.config[3] = graph3->getConfig(); + tx_data.config[4] = graph4->getConfig(); + tx_data.config[5] = graph5->getConfig(); tx_data.config[6] = graph5->getConfig(); tx_data.header.data_cmd_id = rm_referee::DataCmdId::CLIENT_GRAPH_SEVEN_CMD; @@ -364,16 +365,25 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr void FixedUi::updateForQueue() { - if (ros::Time::now() - last_send_ > delay_) - for (auto graph : graph_vector_) - { - graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph.second->isRepeated()) - { - graph_queue_->push(*graph.second); - last_send_ = ros::Time::now(); - } - } + while(update_fixed_ui_times < 1) + { + for (auto it : graph_vector_) + it.second->updateLastConfig(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : graph_vector_) + { + std::string characters = it.second->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*it.second); + else + graph_queue_->push_back(*it.second); + } + ROS_INFO_THROTTLE(1.0, "update fixed ui"); + update_fixed_ui_times++; + } } void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const From b50b718de2f3fcc77d392e9d57a2d3288330a7dc Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 11 Sep 2023 23:03:32 +0800 Subject: [PATCH 06/29] modify ui queue --- rm_referee/include/rm_referee/referee_base.h | 11 +++- rm_referee/include/rm_referee/ui/flash_ui.h | 6 +- .../include/rm_referee/ui/time_change_ui.h | 22 +++---- .../include/rm_referee/ui/trigger_change_ui.h | 15 +++-- rm_referee/include/rm_referee/ui/ui_base.h | 11 ++-- rm_referee/src/referee_base.cpp | 66 +++++++++++-------- rm_referee/src/ui/time_change_ui.cpp | 4 +- rm_referee/src/ui/trigger_change_ui.cpp | 16 +++++ rm_referee/src/ui/ui_base.cpp | 4 +- 9 files changed, 99 insertions(+), 56 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 4be31103..055d7a4d 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -13,6 +13,7 @@ #include "rm_referee/ui/time_change_ui.h" #include "rm_referee/ui/flash_ui.h" + namespace rm_referee { class RefereeBase @@ -97,7 +98,15 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; - std::vector graph_queue_; + std::queue graph_queue_; + //std::queue character_queue_; + Graph returnGraph(std::queue& queue_) +{ +Graph first_element = queue_.front(); +queue_.pop(); +return first_element; +} +std::vector ui_buffer; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 016bb5c4..d3a49f82 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,7 +12,7 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -24,7 +24,7 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : FlashUi(rpc_value, base, "cover", graph_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; @@ -36,7 +36,7 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : FlashUi(rpc_value, base, "spin", graph_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); 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 2d8020f6..a9c1b375 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,7 +12,7 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -26,7 +26,7 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : GroupUiBase(rpc_value, base, graph_queue) { graph_name_ = graph_name; @@ -42,7 +42,7 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "capacitor", graph_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,7 +55,7 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "effort", graph_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -68,7 +68,7 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "progress", graph_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); @@ -81,7 +81,7 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "dart", graph_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); @@ -93,7 +93,7 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "rotation", graph_queue) { if (rpc_value.hasMember("data")) @@ -125,7 +125,7 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue) { if (rpc_value.hasMember("data")) @@ -171,7 +171,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue) { XmlRpc::XmlRpcValue config; @@ -232,7 +232,7 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,7 +245,7 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::string name) : TimeChangeUi(rpc_value, base, name, graph_queue) { 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 82abac28..055b432d 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,7 +13,7 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) + std::queue * graph_queue) : UiBase(rpc_value, base, graph_queue) { if (graph_name == "chassis") @@ -23,11 +23,12 @@ class TriggerChangeUi : public UiBase }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + void updateForQueue(); }; class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "chassis", graph_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) @@ -58,7 +59,7 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "shooter", graph_queue) { graph_->setContent("0"); @@ -76,7 +77,7 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue) { graph_->setContent("0"); @@ -94,7 +95,7 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "target", graph_queue) { graph_->setContent("armor"); @@ -116,7 +117,7 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue) { } @@ -177,7 +178,7 @@ class PolygonTriggerChangeGroupUi : public GroupUiBase class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) : TriggerChangeUi(rpc_value, base, "camera", graph_queue) { if (rpc_value.hasMember("camera_name")) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 03725530..c59f1ea4 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -7,6 +7,7 @@ #include #include #include +#include #include #include #include @@ -18,13 +19,14 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) if (rpc_value["config"].hasMember("delay")) delay_ = ros::Duration(static_cast(rpc_value["config"]["delay"])); graph_queue_ = graph_queue; + //character_queue_ = character_queue; }; ~UiBase() = default; virtual void add(); @@ -57,7 +59,8 @@ class UiBase Base& base_; Graph* graph_; static int id_; - std::vector* graph_queue_; + std::queue * graph_queue_; + std::queue * character_queue_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -69,7 +72,7 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : UiBase(rpc_value, base, graph_queue){}; ~GroupUiBase() = default; void add() override; @@ -93,7 +96,7 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) : GroupUiBase(rpc_value, base, graph_queue) { for (int i = 0; i < static_cast(rpc_value.size()); i++) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index f39e89e1..f7737096 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -113,7 +113,7 @@ void RefereeBase::addUi() add_ui_timer_.stop(); if (!graph_queue_.empty()) while (graph_queue_.size() > 0) - graph_queue_.pop_back(); + graph_queue_.pop(); is_adding_ = false; send_graph_ui_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; @@ -159,6 +159,7 @@ void RefereeBase::addUi() add_ui_times_++; } + void RefereeBase::sendGraphQueueCallback() { if (graph_queue_.empty()) @@ -169,48 +170,61 @@ void RefereeBase::sendGraphQueueCallback() ROS_WARN_THROTTLE(2.0, "Sending UI too frequently, please modify the configuration file or code to " "reduce the frequency"); while (graph_queue_.size() > 50) - graph_queue_.pop_back(); + graph_queue_.pop(); } - int index = graph_queue_.size() - 1; if (!is_adding_) { + +/* + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + sendCharacter(time, graph_); +*/ if (graph_queue_.size() >= 7) { - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1), - &graph_queue_.at(index - 2), &graph_queue_.at(index - 3), - &graph_queue_.at(index - 4), &graph_queue_.at(index - 5), - &graph_queue_.at(index - 6)); - for (int i = 0; i < 7; i++) - graph_queue_.pop_back(); - } + for(int i=0;i<7;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), + &ui_buffer.at(2), &ui_buffer.at(3), + &ui_buffer.at(4), &ui_buffer.at(5), + &ui_buffer.at(6)); + ui_buffer.clear(); + } else if (graph_queue_.size() >= 5) { - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1), - &graph_queue_.at(index - 2), &graph_queue_.at(index - 3), - &graph_queue_.at(index - 4)); - for (int i = 0; i < 5; i++) - graph_queue_.pop_back(); + for(int i=0;i<5;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), + &ui_buffer.at(2), &ui_buffer.at(3), + &ui_buffer.at(4)); + ui_buffer.clear(); } else if (graph_queue_.size() >= 2) { - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(index), &graph_queue_.at(index - 1)); - for (int i = 0; i < 2; i++) - graph_queue_.pop_back(); + for(int i=0;i<2;i++) + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1)); + ui_buffer.clear(); } else if (graph_queue_.size() == 1) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); + ui_buffer.clear(); } + } - else - { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); - } + else{ + ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); + ui_buffer.clear(); + } send_graph_ui_timer_.start(); } @@ -270,7 +284,7 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) is_adding_ = true; if (!graph_queue_.empty()) while (graph_queue_.size() > 0) - graph_queue_.pop_back(); + graph_queue_.pop(); send_graph_ui_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 98f80c55..9503b3c6 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -30,7 +30,7 @@ void TimeChangeUi::updateForQueue() if (graph_queue_ && !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) { - graph_queue_->push_back(*graph_); + graph_queue_->push(*graph_); last_send_ = ros::Time::now(); } } @@ -44,7 +44,7 @@ void TimeChangeGroupUi::updateForQueue() graph.second->setOperation(rm_referee::GraphOperation::UPDATE); if (graph_queue_ && !graph.second->isRepeated()) { - graph_queue_->push_back(*graph.second); + graph_queue_->push(*graph.second); last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index f0355b2a..bf04e83f 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -11,7 +11,23 @@ void TriggerChangeUi::setContent(const std::string& content) graph_->setContent(content); display(); } +/* +void TriggerChangeUi::updateForQueue() +{ + + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_.push(characters); + else + sendSingleGraph(time, graph_); +} +*/ void ChassisTriggerChangeUi::update() { if (s_l_ == rm_msgs::DbusData::MID && s_r_ == rm_msgs::DbusData::UP) diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index dd0f427f..6b82f985 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -18,7 +18,7 @@ void UiBase::addForQueue(int add_times) for (int i = 0; i < add_times; i++) { graph_->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push_back(*graph_); + graph_queue_->push(*graph_); last_send_ = ros::Time::now(); } } @@ -51,7 +51,7 @@ void GroupUiBase::addForQueue(int add_times) for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push_back(*graph.second); + graph_queue_->push(*graph.second); last_send_ = ros::Time::now(); } } From 0d6c68330a7f23fe4f8b9404908143fe800d693e Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 12 Sep 2023 22:59:08 +0800 Subject: [PATCH 07/29] modify ui sender --- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/include/rm_referee/ui/flash_ui.h | 10 +- .../include/rm_referee/ui/time_change_ui.h | 44 +++--- .../include/rm_referee/ui/trigger_change_ui.h | 58 +++++--- rm_referee/include/rm_referee/ui/ui_base.h | 12 +- rm_referee/src/referee_base.cpp | 72 +++++----- rm_referee/src/ui/trigger_change_ui.cpp | 132 +++++++++++++++--- 7 files changed, 226 insertions(+), 104 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 055d7a4d..32d5f8cd 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -99,7 +99,7 @@ class RefereeBase GroupUiBase* graph_queue_sender_{}; std::queue graph_queue_; - //std::queue character_queue_; + std::queue character_queue_; Graph returnGraph(std::queue& queue_) { Graph first_element = queue_.front(); diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index d3a49f82..256ddfb5 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,7 +12,7 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) + std::queue * graph_queue, std::queue * character_queue) : UiBase(rpc_value, base, graph_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -24,8 +24,8 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : FlashUi(rpc_value, base, "cover", graph_queue){}; + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; private: @@ -36,8 +36,8 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : FlashUi(rpc_value, base, "spin", graph_queue){}; + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : FlashUi(rpc_value, base, "spin", graph_queue, character_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); private: 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 a9c1b375..a132edfd 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,8 +12,8 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : UiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); } @@ -26,8 +26,8 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : GroupUiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; } @@ -42,8 +42,8 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "capacitor", graph_queue){}; + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "capacitor", graph_queue, character_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,8 +55,8 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "effort", graph_queue){}; + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "effort", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); private: @@ -68,8 +68,8 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "progress", graph_queue){}; + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "progress", graph_queue, character_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); private: @@ -81,8 +81,8 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "dart", graph_queue){}; + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "dart", graph_queue, character_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); private: @@ -93,8 +93,8 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "rotation", graph_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "rotation", graph_queue, character_queue) { if (rpc_value.hasMember("data")) { @@ -125,8 +125,8 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue, character_queue) { if (rpc_value.hasMember("data")) { @@ -171,8 +171,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue, character_queue) { XmlRpc::XmlRpcValue config; @@ -232,8 +232,8 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TimeChangeUi(rpc_value, base, "pitch", graph_queue){}; + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,9 +245,9 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue, std::string name) - : TimeChangeUi(rpc_value, base, name, graph_queue) + : TimeChangeUi(rpc_value, base, name, graph_queue,character_queue) { if (rpc_value.hasMember("data")) { 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 055b432d..c0282a48 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,8 +13,8 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue) - : UiBase(rpc_value, base, graph_queue) + std::queue * graph_queue, std::queue * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { if (graph_name == "chassis") graph_ = new Graph(rpc_value["config"], base_, 1); @@ -23,13 +23,34 @@ class TriggerChangeUi : public UiBase }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; - void updateForQueue(); + void updateForQueue(bool check_repeat = true); + void updateTwiceForQueue(bool check_repeat = true); }; + + +class TriggerChangeGroupUi : public GroupUiBase +{ +public: + explicit TriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, + std::queue * graph_queue, std::queue * character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) + { + if (graph_name == "chassis") + graph_ = new Graph(rpc_value["config"], base_, 1); + else + graph_ = new Graph(rpc_value["config"], base_, id_++); + }; + virtual void setContent(const std::string& content); + virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + void updateForQueue(bool check_repeat = true); + void updateTwiceForQueue(bool check_repeat = true); +}; + class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "chassis", graph_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "chassis", graph_queue, character_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) graph_->setContent("raw"); @@ -59,8 +80,8 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "shooter", graph_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "shooter", graph_queue, character_queue) { graph_->setContent("0"); } @@ -77,8 +98,8 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue, character_queue) { graph_->setContent("0"); } @@ -95,8 +116,8 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "target", graph_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "target", graph_queue, character_queue) { graph_->setContent("armor"); if (base_.robot_color_ == "red") @@ -117,8 +138,8 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue, character_queue) { } void updateTrackID(int id); @@ -129,10 +150,11 @@ class TargetViewAngleTriggerChangeUi : public TriggerChangeUi int track_id_; }; -class PolygonTriggerChangeGroupUi : public GroupUiBase +class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi { public: - explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base) : GroupUiBase(rpc_value, base) + explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); XmlRpc::XmlRpcValue config; @@ -172,14 +194,14 @@ class PolygonTriggerChangeGroupUi : public GroupUiBase } } void update() override; - virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; + }; class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue) - : TriggerChangeUi(rpc_value, base, "camera", graph_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + : TriggerChangeUi(rpc_value, base, "camera", graph_queue, character_queue) { if (rpc_value.hasMember("camera_name")) { diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index c59f1ea4..6403f7f3 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -19,14 +19,14 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr ) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) if (rpc_value["config"].hasMember("delay")) delay_ = ros::Duration(static_cast(rpc_value["config"]["delay"])); graph_queue_ = graph_queue; - //character_queue_ = character_queue; + character_queue_ = character_queue; }; ~UiBase() = default; virtual void add(); @@ -72,8 +72,8 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) - : UiBase(rpc_value, base, graph_queue){}; + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + : UiBase(rpc_value, base, graph_queue, character_queue){}; ~GroupUiBase() = default; void add() override; void update() override; @@ -96,8 +96,8 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr) - : GroupUiBase(rpc_value, base, graph_queue) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { for (int i = 0; i < static_cast(rpc_value.size()); i++) graph_vector_.insert( diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index f7737096..bd2a213c 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -47,44 +47,44 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "chassis") - chassis_trigger_change_ui_ = new ChassisTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + chassis_trigger_change_ui_ = new ChassisTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "shooter") - shooter_trigger_change_ui_ = new ShooterTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + shooter_trigger_change_ui_ = new ShooterTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gimbal") - gimbal_trigger_change_ui_ = new GimbalTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + gimbal_trigger_change_ui_ = new GimbalTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target") - target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target_view_angle") - target_view_angle_trigger_change_ui_ = new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + target_view_angle_trigger_change_ui_ = new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "camera") - camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_); + camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("time_change", rpc_value); for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "capacitor") - capacitor_time_change_ui_ = new CapacitorTimeChangeUi(rpc_value[i], base_, &graph_queue_); + capacitor_time_change_ui_ = new CapacitorTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "effort") - effort_time_change_ui_ = new EffortTimeChangeUi(rpc_value[i], base_, &graph_queue_); + effort_time_change_ui_ = new EffortTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "progress") - progress_time_change_ui_ = new ProgressTimeChangeUi(rpc_value[i], base_, &graph_queue_); + progress_time_change_ui_ = new ProgressTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "dart_status") - dart_status_time_change_ui_ = new DartStatusTimeChangeUi(rpc_value[i], base_, &graph_queue_); + dart_status_time_change_ui_ = new DartStatusTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "rotation") - rotation_time_change_ui_ = new RotationTimeChangeUi(rpc_value[i], base_, &graph_queue_); + rotation_time_change_ui_ = new RotationTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "lane_line") - lane_line_time_change_ui_ = new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_); + lane_line_time_change_ui_ = new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "pitch") - pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_); + pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "balance_pitch") - balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_); + balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "engineer_joint1") - engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint1"); + engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint1"); if (rpc_value[i]["name"] == "engineer_joint2") - engineer_joint2_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint2"); + engineer_joint2_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint2"); if (rpc_value[i]["name"] == "engineer_joint3") - engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, "joint3"); + engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint3"); } ui_nh.getParam("fixed", rpc_value); @@ -94,9 +94,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) for (int i = 0; i < rpc_value.size(); i++) { if (rpc_value[i]["name"] == "cover") - cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_); + 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_); + spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } @@ -162,27 +162,35 @@ void RefereeBase::addUi() void RefereeBase::sendGraphQueueCallback() { - if (graph_queue_.empty()) + if (graph_queue_.empty() || character_queue_.empty()) return; if (graph_queue_.size() > 50) { - ROS_WARN_THROTTLE(2.0, "Sending UI too frequently, please modify the configuration file or code to " + ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " "reduce the frequency"); while (graph_queue_.size() > 50) graph_queue_.pop(); } + if (character_queue_.size() > 7) + { + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); + while (character_queue_.size() > 7) + character_queue_.pop(); + } + if (!is_adding_) { + if(!character_queue_.empty()) + { + graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); + character_queue_.pop(); + } -/* - std::string characters = graph_->getCharacters(); - if (!characters.empty()) - sendCharacter(time, graph_); -*/ - if (graph_queue_.size() >= 7) + else if (graph_queue_.size() >= 7) { for(int i=0;i<7;i++) ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); @@ -213,16 +221,14 @@ void RefereeBase::sendGraphQueueCallback() } else if (graph_queue_.size() == 1) { - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); - ui_buffer.clear(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); + graph_queue_.pop(); } } else{ - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &ui_buffer.at(0)); - ui_buffer.clear(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); + graph_queue_.pop(); } send_graph_ui_timer_.start(); diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index bf04e83f..18f0611d 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -11,23 +11,116 @@ void TriggerChangeUi::setContent(const std::string& content) graph_->setContent(content); display(); } -/* -void TriggerChangeUi::updateForQueue() + +void TriggerChangeUi::updateForQueue(bool check_repeat) { + if (check_repeat) + if (graph_->isRepeated()) + return; + graph_->updateLastConfig(); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_->push(*graph_); + else + graph_queue_->push(*graph_); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) +} +void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) +{ + if (check_repeat) + if (graph_->isRepeated()) return; + graph_->updateLastConfig(); - std::string characters = graph_->getCharacters(); - if (!characters.empty()) - character_queue_.push(characters); - else - sendSingleGraph(time, graph_); + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + { + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + } + else{ + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + } + +} + +void TriggerChangeGroupUi::setContent(const std::string& content) +{ + graph_->setContent(content); + display(); +} + +void TriggerChangeGroupUi::updateForQueue(bool check_repeat) +{ + if (check_repeat) + { + 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(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : character_vector_) + character_queue_->push(*graph_); + for (auto it : graph_vector_) + graph_queue_->push(*graph_); } -*/ + +void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) +{ + if (check_repeat) + { + 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(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : character_vector_) + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + for (auto it : graph_vector_) + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + +} + + void ChassisTriggerChangeUi::update() { if (s_l_ == rm_msgs::DbusData::MID && s_r_ == rm_msgs::DbusData::UP) @@ -37,7 +130,7 @@ void ChassisTriggerChangeUi::update() power_limit_state_ == rm_common::PowerLimit::CHARGE); graph_->setOperation(rm_referee::GraphOperation::UPDATE); checkModeChange(); - displayTwice(); + updateTwiceForQueue(); } void ChassisTriggerChangeUi::displayInCapacity() @@ -47,7 +140,7 @@ void ChassisTriggerChangeUi::displayInCapacity() updateConfig(254, 0); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void ChassisTriggerChangeUi::checkModeChange() @@ -70,7 +163,7 @@ void ChassisTriggerChangeUi::checkModeChange() else if ((ros::Time::now() - trigger_time).toSec() > mode_change_threshold_) { is_different = false; - display(false); + updateForQueue(false); } } } @@ -174,7 +267,7 @@ void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - TriggerChangeUi::display(); + updateForQueue(); } void ShooterTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -218,7 +311,7 @@ void GimbalTriggerChangeUi::update() updateConfig(gimbal_mode_, gimbal_eject_); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void GimbalTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -261,7 +354,7 @@ void TargetTriggerChangeUi::update() else updateConfig(gimbal_eject_, shoot_frequency_, det_armor_target_, det_color_ == rm_msgs::StatusChangeRequest::RED); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - TriggerChangeUi::display(); + updateForQueue(); } void TargetTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -322,7 +415,7 @@ void TargetViewAngleTriggerChangeUi::update() { updateConfig(track_id_ == 0, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(); } void TargetViewAngleTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -343,7 +436,8 @@ void PolygonTriggerChangeGroupUi::update() { for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(); + } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) @@ -367,6 +461,6 @@ void CameraTriggerChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(); } } // namespace rm_referee From b7c89c0c0eb83e2d22da201785f0e4fdf92d854d Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 17 Sep 2023 21:02:26 +0800 Subject: [PATCH 08/29] modify the ui and interactive data sender --- rm_referee/include/rm_referee/referee_base.h | 17 +++--- rm_referee/include/rm_referee/ui/flash_ui.h | 2 + rm_referee/include/rm_referee/ui/ui_base.h | 6 +- rm_referee/src/referee_base.cpp | 58 +++++++++++++------- rm_referee/src/ui/flash_ui.cpp | 40 +++++++++++++- rm_referee/src/ui/time_change_ui.cpp | 6 +- rm_referee/src/ui/ui_base.cpp | 26 +++++++-- 7 files changed, 116 insertions(+), 39 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 32d5f8cd..fee8f335 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -52,8 +52,8 @@ class RefereeBase virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data); virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); - // send graph_type ui - void sendGraphQueueCallback(); + // send ui + void sendSerialDataCallback(); ros::Subscriber joint_state_sub_; ros::Subscriber actuator_state_sub_; @@ -102,16 +102,19 @@ class RefereeBase std::queue character_queue_; Graph returnGraph(std::queue& queue_) { -Graph first_element = queue_.front(); -queue_.pop(); -return first_element; + Graph first_element = queue_.front(); + queue_.pop(); + return first_element; } -std::vector ui_buffer; + std::vector ui_buffer; + rm_referee::ClientMapReceiveData radar_receive_data; + rm_referee::MapSentryData map_sentry_data; + bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; Base& base_; - ros::Timer add_ui_timer_, send_graph_ui_timer_; + ros::Timer add_ui_timer_, send_serial_data_timer_; int add_ui_times_, add_ui_max_times_, add_ui_frequency_; double send_ui_queue_delay_; bool add_ui_flag_ = false, is_adding_ = false; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 256ddfb5..7a084919 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -19,6 +19,7 @@ class FlashUi : public UiBase } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; + void updateForQueue(const ros::Time& time, bool state, bool once); }; class CoverFlashUi : public FlashUi @@ -30,6 +31,7 @@ class CoverFlashUi : public FlashUi private: void display(const ros::Time& time) override; + uint8_t cover_state_; }; diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 6403f7f3..21e4ac42 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -41,7 +41,7 @@ class UiBase void sendSingleGraph(const ros::Time& time, Graph* graph); void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); void sendRadarInteractiveData(rm_referee::ClientMapReceiveData& data); - void sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data); + void sendMapSentryData(rm_referee::MapSentryData& data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); @@ -67,6 +67,7 @@ class UiBase ros::Time last_send_; ros::Duration delay_ = ros::Duration(0.); const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; + }; class GroupUiBase : public UiBase @@ -103,6 +104,9 @@ class FixedUi : public GroupUiBase graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; + ~FixedUi() = default; + void updateForQueue(); }; + } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index bd2a213c..4d491ce9 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -102,8 +102,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) add_ui_timer_ = nh.createTimer(ros::Duration(1. / add_ui_frequency_), std::bind(&RefereeBase::addUi, this), false, false); - send_graph_ui_timer_ = nh.createTimer(ros::Duration(send_ui_queue_delay_), - std::bind(&RefereeBase::sendGraphQueueCallback, this), false, true); + send_serial_data_timer_ = nh.createTimer(ros::Duration(send_ui_queue_delay_), + std::bind(&RefereeBase::sendSerialDataCallback, this), false, true); } void RefereeBase::addUi() { @@ -115,7 +115,7 @@ void RefereeBase::addUi() while (graph_queue_.size() > 0) graph_queue_.pop(); is_adding_ = false; - send_graph_ui_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); + send_serial_data_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; } @@ -160,7 +160,7 @@ void RefereeBase::addUi() } -void RefereeBase::sendGraphQueueCallback() +void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() || character_queue_.empty()) return; @@ -173,18 +173,28 @@ void RefereeBase::sendGraphQueueCallback() graph_queue_.pop(); } - if (character_queue_.size() > 7) + if (character_queue_.size() > 6) { - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently"); while (character_queue_.size() > 7) character_queue_.pop(); } - if (!is_adding_) { - if(!character_queue_.empty()) + if(send_radar_receive_data_) + { + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + send_radar_receive_data_ = false; + } + + else if (send_map_sentry_data_) + { + interactive_data_sender_->sendMapSentryData(map_sentry_data); + send_map_sentry_data_ = false; + } + + else if(!character_queue_.empty()) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); character_queue_.pop(); @@ -226,18 +236,19 @@ void RefereeBase::sendGraphQueueCallback() } } - else{ + else + { graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); graph_queue_.pop(); - } - send_graph_ui_timer_.start(); + } + send_serial_data_timer_.start(); } void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, const ros::Time& last_get_data_time) { if (fixed_ui_ && !is_adding_) - fixed_ui_->update(); + fixed_ui_->updateForQueue(); } void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { @@ -291,7 +302,7 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) if (!graph_queue_.empty()) while (graph_queue_.size() > 0) graph_queue_.pop(); - send_graph_ui_timer_.setPeriod(ros::Duration(0.05)); + send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; } @@ -366,15 +377,22 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data } void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { - rm_referee::ClientMapReceiveData send_data; - send_data.target_position_x = data->target_position_x; - send_data.target_position_y = data->target_position_y; - send_data.target_robot_ID = data->target_robot_ID; + radar_receive_data.target_position_x = data->target_position_x; + radar_receive_data.target_position_y = data->target_position_y; + radar_receive_data.target_robot_ID = data->target_robot_ID; - interactive_data_sender_->sendRadarInteractiveData(send_data); + send_radar_receive_data_ = true; } void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) { - interactive_data_sender_->sendMapSentryData(data); + map_sentry_data.intention = data->intention; + map_sentry_data.start_position_x = data->start_position_x; + map_sentry_data.start_position_y = data->start_position_y; + for (int i = 0; i < 49; i++) + { + map_sentry_data.delta_x[i] = data->delta_x[i]; + map_sentry_data.delta_y[i] = data->delta_y[i]; + } + send_map_sentry_data_ = true; } } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ad897bab..a40f6bfc 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -6,11 +6,47 @@ namespace rm_referee { + +void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) +{ + if (once) + { + if (state) + graph_->setOperation(rm_referee::GraphOperation::ADD); + else + graph_->setOperation(rm_referee::GraphOperation::DELETE); + } + else if (state && 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 (graph_->isRepeated()) + return; + graph_->updateLastConfig(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + { + for(int i = 0; i < 2; i++) + character_queue_->push(*graph_); + } + else{ + for(int i = 0; i < 2; i++) + graph_queue_->push(*graph_); + } +} + void CoverFlashUi::display(const ros::Time& time) { if (!cover_state_) graph_->setOperation(rm_referee::GraphOperation::DELETE); - UiBase::display(time, cover_state_, true); + FlashUi::updateForQueue(time, cover_state_, true); } void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, @@ -24,7 +60,7 @@ void SpinFlashUi::display(const ros::Time& time) { if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) graph_->setOperation(rm_referee::GraphOperation::DELETE); - UiBase::display(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); + FlashUi::updateForQueue(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); } void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 9503b3c6..d27bf654 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -129,7 +129,7 @@ void ProgressTimeChangeUi::updateEngineerUiData(const rm_msgs::EngineerUi::Const { total_steps_ = data->total_steps; finished_data_ = data->finished_step; - TimeChangeUi::update(); + TimeChangeUi::updateForQueue(); } void DartStatusTimeChangeUi::updateConfig() @@ -157,7 +157,7 @@ void DartStatusTimeChangeUi::updateDartClientCmd(const rm_msgs::DartClientCmd::C const ros::Time& last_get_data_time) { dart_launch_opening_status_ = data->dart_launch_opening_status; - TimeChangeUi::update(); + TimeChangeUi::updateForQueue(); } void RotationTimeChangeUi::updateConfig() @@ -284,7 +284,7 @@ void PitchAngleTimeChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(ros::Time::now()); + updateForQueue(); } void PitchAngleTimeChangeUi::updateConfig() diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 6b82f985..91c16be6 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -150,20 +150,20 @@ void UiBase::sendUi(const ros::Time& time) sendSingleGraph(time, graph_); } -void UiBase::sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data) +void UiBase::sendMapSentryData(rm_referee::MapSentryData& data) { uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; for (int i = 0; i < 128; i++) tx_buffer_[i] = 0; - map_sentry_data->intention = data->intention; - map_sentry_data->start_position_x = data->start_position_x; - map_sentry_data->start_position_y = data->start_position_y; + map_sentry_data->intention = data.intention; + map_sentry_data->start_position_x = data.start_position_x; + map_sentry_data->start_position_y = data.start_position_y; for (int i = 0; i < 49; i++) { - map_sentry_data->delta_x[i] = data->delta_x[i]; - map_sentry_data->delta_y[i] = data->delta_y[i]; + map_sentry_data->delta_x[i] = data.delta_x[i]; + map_sentry_data->delta_y[i] = data.delta_y[i]; } pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::MAP_SENTRY_CMD, sizeof(rm_referee::MapSentryData)); tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::MapSentryData) + k_tail_length_); @@ -362,6 +362,20 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr sendSerial(time, data_len); } +void FixedUi::updateForQueue() +{ + if (ros::Time::now() - last_send_ > delay_) + for (auto graph : graph_vector_) + { + graph.second->setOperation(rm_referee::GraphOperation::UPDATE); + if (graph_queue_ && !graph.second->isRepeated()) + { + graph_queue_->push(*graph.second); + last_send_ = ros::Time::now(); + } + } +} + void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const { memset(tx_buffer, 0, k_frame_length_); From e3e58f7fe07a4ac116c720291366076632296780 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 17 Sep 2023 21:39:49 +0800 Subject: [PATCH 09/29] Modify the ui and robot interactive data sender --- rm_referee/src/ui/time_change_ui.cpp | 2 -- rm_referee/src/ui/trigger_change_ui.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index d27bf654..18badf57 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -282,8 +282,6 @@ void PitchAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState: void PitchAngleTimeChangeUi::update() { - updateConfig(); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(); } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 18f0611d..90a6ebac 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -437,7 +437,6 @@ void PolygonTriggerChangeGroupUi::update() for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(); - } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) From dcc40236ef74d81edb18643c7d28db3bc2e8245b Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 19 Sep 2023 20:31:50 +0800 Subject: [PATCH 10/29] Modify the ui and robot interactive data sender --- rm_referee/include/rm_referee/referee_base.h | 12 +- rm_referee/include/rm_referee/ui/flash_ui.h | 8 +- .../include/rm_referee/ui/time_change_ui.h | 22 ++-- .../include/rm_referee/ui/trigger_change_ui.h | 28 ++--- rm_referee/include/rm_referee/ui/ui_base.h | 18 ++- rm_referee/src/referee_base.cpp | 107 ++++++++++-------- rm_referee/src/ui/flash_ui.cpp | 14 +-- rm_referee/src/ui/time_change_ui.cpp | 19 +++- rm_referee/src/ui/trigger_change_ui.cpp | 16 +-- rm_referee/src/ui/ui_base.cpp | 40 ++++--- 10 files changed, 155 insertions(+), 129 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index fee8f335..81d1b97e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -98,18 +98,12 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; - std::queue graph_queue_; - std::queue character_queue_; - Graph returnGraph(std::queue& queue_) -{ - Graph first_element = queue_.front(); - queue_.pop(); - return first_element; -} - std::vector ui_buffer; + std::deque graph_queue_; + std::deque character_queue_; rm_referee::ClientMapReceiveData radar_receive_data; rm_referee::MapSentryData map_sentry_data; + ros::Time radar_receive_last_send, map_sentry_last_send; bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 7a084919..6c84fd32 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,8 +12,8 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) - : UiBase(rpc_value, base, graph_queue) + std::deque * graph_queue, std::deque * character_queue) + : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); } @@ -25,7 +25,7 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; @@ -38,7 +38,7 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : FlashUi(rpc_value, base, "spin", graph_queue, character_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); 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 a132edfd..017eff7f 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,7 +12,7 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -26,7 +26,7 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; @@ -42,7 +42,7 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "capacitor", graph_queue, character_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,7 +55,7 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "effort", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -68,7 +68,7 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "progress", graph_queue, character_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); @@ -81,7 +81,7 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "dart", graph_queue, character_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); @@ -93,7 +93,7 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "rotation", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -125,7 +125,7 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -171,7 +171,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue, character_queue) { XmlRpc::XmlRpcValue config; @@ -232,7 +232,7 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -245,7 +245,7 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue, + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue, std::string name) : TimeChangeUi(rpc_value, base, name, graph_queue,character_queue) { 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 c0282a48..2a7fa677 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,7 +13,7 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque* graph_queue, std::deque* character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { if (graph_name == "chassis") @@ -32,24 +32,24 @@ class TriggerChangeGroupUi : public GroupUiBase { public: explicit TriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::queue * graph_queue, std::queue * character_queue) + std::deque * graph_queue, std::deque * character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - if (graph_name == "chassis") - graph_ = new Graph(rpc_value["config"], base_, 1); - else - graph_ = new Graph(rpc_value["config"], base_, id_++); + graph_name_ = graph_name; }; virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; void updateForQueue(bool check_repeat = true); void updateTwiceForQueue(bool check_repeat = true); + +protected: + std::string graph_name_; }; class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "chassis", graph_queue, character_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) @@ -80,7 +80,7 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "shooter", graph_queue, character_queue) { graph_->setContent("0"); @@ -98,7 +98,7 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue, character_queue) { graph_->setContent("0"); @@ -116,7 +116,7 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "target", graph_queue, character_queue) { graph_->setContent("armor"); @@ -138,7 +138,7 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue, character_queue) { } @@ -153,7 +153,7 @@ class TargetViewAngleTriggerChangeUi : public TriggerChangeUi class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi { public: - explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); @@ -194,13 +194,13 @@ class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi } } void update() override; - + //virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){} void setContent(const std::string &content) override; }; class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue, std::queue * character_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TriggerChangeUi(rpc_value, base, "camera", graph_queue, character_queue) { if (rpc_value.hasMember("camera_name")) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 21e4ac42..70cf2d6c 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -7,7 +7,7 @@ #include #include #include -#include +#include #include #include #include @@ -19,7 +19,7 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr ) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr ) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) @@ -59,21 +59,20 @@ class UiBase Base& base_; Graph* graph_; static int id_; - std::queue * graph_queue_; - std::queue * character_queue_; + std::deque* graph_queue_; + std::deque* character_queue_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; ros::Time last_send_; ros::Duration delay_ = ros::Duration(0.); const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; - }; class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue = nullptr, std::deque * character_queue = nullptr) : UiBase(rpc_value, base, graph_queue, character_queue){}; ~GroupUiBase() = default; void add() override; @@ -97,16 +96,15 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::queue * graph_queue = nullptr, std::queue * character_queue = nullptr) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - for (int i = 0; i < static_cast(rpc_value.size()); i++) + for(int i = 0; i < static_cast(rpc_value.size()); i++) graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; - ~FixedUi() = default; void updateForQueue(); + int update_fixed_ui_times = 0; }; - } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 4d491ce9..3892e676 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -88,7 +88,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) } ui_nh.getParam("fixed", rpc_value); - fixed_ui_ = new FixedUi(rpc_value, base_, &graph_queue_); + fixed_ui_ = new FixedUi(rpc_value, base_, &graph_queue_, &character_queue_); ui_nh.getParam("flash", rpc_value); for (int i = 0; i < rpc_value.size(); i++) @@ -109,11 +109,10 @@ void RefereeBase::addUi() { if (add_ui_times_ > add_ui_max_times_) { - ROS_INFO("End add"); + ROS_INFO_THROTTLE(2.0, "End adding"); add_ui_timer_.stop(); if (!graph_queue_.empty()) - while (graph_queue_.size() > 0) - graph_queue_.pop(); + graph_queue_.clear(); is_adding_ = false; send_serial_data_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; @@ -162,85 +161,101 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { - if (graph_queue_.empty() || character_queue_.empty()) + if (graph_queue_.empty() && character_queue_.empty()){ + ROS_INFO_THROTTLE(1.0, "No data to send"); return; + } if (graph_queue_.size() > 50) - { ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " "reduce the frequency"); - while (graph_queue_.size() > 50) - graph_queue_.pop(); - } - if (character_queue_.size() > 6) - { - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently"); - while (character_queue_.size() > 7) - character_queue_.pop(); - } + if (character_queue_.size() > 20) + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); if (!is_adding_) { + if (graph_queue_.size() > 50) + { + ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, now reduce the queue"); + while (graph_queue_.size() > 50) + graph_queue_.pop_front(); + } + + if (character_queue_.size() > 8) + { + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, now reduce the queue"); + while (character_queue_.size() > 7) + character_queue_.pop_front(); + } + if(send_radar_receive_data_) { + if(ros::Time::now() - radar_receive_last_send < ros::Duration(0.2)) + ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + radar_receive_last_send = ros::Time::now(); send_radar_receive_data_ = false; } else if (send_map_sentry_data_) { + if(ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) + ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); interactive_data_sender_->sendMapSentryData(map_sentry_data); + radar_receive_last_send = ros::Time::now(); send_map_sentry_data_ = false; } - else if(!character_queue_.empty()) + else if(!character_queue_.empty() && graph_queue_.size() <= 10) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); - character_queue_.pop(); + character_queue_.pop_front(); + ROS_INFO_THROTTLE(1.0, " send character"); } else if (graph_queue_.size() >= 7) { - for(int i=0;i<7;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), - &ui_buffer.at(2), &ui_buffer.at(3), - &ui_buffer.at(4), &ui_buffer.at(5), - &ui_buffer.at(6)); - ui_buffer.clear(); - } + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4), &graph_queue_.at(5), + &graph_queue_.at(6)); + ROS_INFO_THROTTLE(1.0, " send 7 graph"); + + for (int i = 0; i < 7; i++) + graph_queue_.pop_front(); + } else if (graph_queue_.size() >= 5) { - for(int i=0;i<5;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1), - &ui_buffer.at(2), &ui_buffer.at(3), - &ui_buffer.at(4)); - ui_buffer.clear(); + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4)); + ROS_INFO_THROTTLE(1.0, " send 5 graph"); + for (int i = 0; i < 5; i++) + graph_queue_.pop_front(); } else if (graph_queue_.size() >= 2) { - for(int i=0;i<2;i++) - ui_buffer.push_back(RefereeBase::returnGraph(graph_queue_)); - - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &ui_buffer.at(0), &ui_buffer.at(1)); - ui_buffer.clear(); + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); + for (int i = 0; i < 2; i++) + graph_queue_.pop_front(); + + ROS_INFO_THROTTLE(1.0, " send 2 graph"); } else if (graph_queue_.size() == 1) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); - graph_queue_.pop(); + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); + graph_queue_.pop_front(); } - } else { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.front()); - graph_queue_.pop(); - + if(!graph_queue_.empty()) + { + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); + graph_queue_.pop_front(); + } } send_serial_data_timer_.start(); } @@ -300,11 +315,11 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) add_ui_flag_ = false; is_adding_ = true; if (!graph_queue_.empty()) - while (graph_queue_.size() > 0) - graph_queue_.pop(); + graph_queue_.clear(); send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; + } if (data->s_r != rm_msgs::DbusData::UP) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index a40f6bfc..b4a178b8 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -25,6 +25,7 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) } if (graph_->isRepeated()) return; + graph_->updateLastConfig(); if (base_.robot_id_ == 0 || base_.client_id_ == 0) @@ -32,14 +33,11 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) std::string characters = graph_->getCharacters(); if (!characters.empty()) - { - for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); - } - else{ - for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); - } + character_queue_->push_back(*graph_); + else + graph_queue_->push_back(*graph_); + + ROS_INFO_THROTTLE(1.0, "update flash ui"); } void CoverFlashUi::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 18badf57..f82a3c41 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -28,10 +28,16 @@ void TimeChangeUi::updateForQueue() updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) + if ( !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) { - graph_queue_->push(*graph_); + std::string characters = graph_->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*graph_); + else + graph_queue_->push_back(*graph_); + last_send_ = ros::Time::now(); + } } @@ -42,9 +48,14 @@ void TimeChangeGroupUi::updateForQueue() for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph.second->isRepeated()) + if ( !graph.second->isRepeated() ) { - graph_queue_->push(*graph.second); + std::string characters = graph.second->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*graph.second); + else + graph_queue_->push_back(*graph.second); + last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 90a6ebac..ca5d2c38 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -24,9 +24,9 @@ void TriggerChangeUi::updateForQueue(bool check_repeat) std::string characters = graph_->getCharacters(); if (!characters.empty()) - character_queue_->push(*graph_); + character_queue_->push_back(*graph_); else - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); } void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) @@ -43,11 +43,11 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) if (!characters.empty()) { for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); + character_queue_->push_back(*graph_); } else{ for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); } } @@ -82,9 +82,9 @@ void TriggerChangeGroupUi::updateForQueue(bool check_repeat) return; for (auto it : character_vector_) - character_queue_->push(*graph_); + character_queue_->push_back(*it.second); for (auto it : graph_vector_) - graph_queue_->push(*graph_); + graph_queue_->push_back(*it.second); } @@ -113,10 +113,10 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) for (auto it : character_vector_) for(int i = 0; i < 2; i++) - character_queue_->push(*graph_); + character_queue_->push_back(*it.second); for (auto it : graph_vector_) for(int i = 0; i < 2; i++) - graph_queue_->push(*graph_); + graph_queue_->push_back(*it.second); } diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 91c16be6..3f221634 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -18,7 +18,7 @@ void UiBase::addForQueue(int add_times) for (int i = 0; i < add_times; i++) { graph_->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push(*graph_); + graph_queue_->push_back(*graph_); last_send_ = ros::Time::now(); } } @@ -51,7 +51,7 @@ void GroupUiBase::addForQueue(int add_times) for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::ADD); - graph_queue_->push(*graph.second); + graph_queue_->push_back(*graph.second); last_send_ = ros::Time::now(); } } @@ -352,9 +352,10 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr tx_data.header.receiver_id = base_.client_id_; tx_data.config[0] = graph0->getConfig(); tx_data.config[1] = graph1->getConfig(); - tx_data.config[3] = graph2->getConfig(); - tx_data.config[4] = graph3->getConfig(); - tx_data.config[5] = graph4->getConfig(); + tx_data.config[2] = graph2->getConfig(); + tx_data.config[3] = graph3->getConfig(); + tx_data.config[4] = graph4->getConfig(); + tx_data.config[5] = graph5->getConfig(); tx_data.config[6] = graph5->getConfig(); tx_data.header.data_cmd_id = rm_referee::DataCmdId::CLIENT_GRAPH_SEVEN_CMD; @@ -364,16 +365,25 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr void FixedUi::updateForQueue() { - if (ros::Time::now() - last_send_ > delay_) - for (auto graph : graph_vector_) - { - graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph.second->isRepeated()) - { - graph_queue_->push(*graph.second); - last_send_ = ros::Time::now(); - } - } + while(update_fixed_ui_times < 1) + { + for (auto it : graph_vector_) + it.second->updateLastConfig(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + for (auto it : graph_vector_) + { + std::string characters = it.second->getCharacters(); + if (!characters.empty()) + character_queue_->push_back(*it.second); + else + graph_queue_->push_back(*it.second); + } + ROS_INFO_THROTTLE(1.0, "update fixed ui"); + update_fixed_ui_times++; + } } void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const From d507dcdd84348176f2d9e6c55a8ac7be3040aac4 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 1 Oct 2023 19:12:38 +0800 Subject: [PATCH 11/29] Add forced calibration. --- rm_referee/src/referee_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 3892e676..9849a27c 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -162,7 +162,7 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() && character_queue_.empty()){ - ROS_INFO_THROTTLE(1.0, "No data to send"); + ROS_INFO_THROTTLE(0.1, "No data to send"); return; } @@ -222,7 +222,6 @@ void RefereeBase::sendSerialDataCallback() &graph_queue_.at(4), &graph_queue_.at(5), &graph_queue_.at(6)); ROS_INFO_THROTTLE(1.0, " send 7 graph"); - for (int i = 0; i < 7; i++) graph_queue_.pop_front(); } @@ -247,6 +246,7 @@ void RefereeBase::sendSerialDataCallback() { graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); graph_queue_.pop_front(); + ROS_INFO_THROTTLE(1.0, " send 1 graph"); } } else From 8754bb7b26c7c79ace6d892a662f1c02cae9d9ce Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sat, 7 Oct 2023 20:07:47 +0800 Subject: [PATCH 12/29] Add some test code. --- rm_referee/src/referee_base.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 9849a27c..5e409bfc 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -162,7 +162,7 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() && character_queue_.empty()){ - ROS_INFO_THROTTLE(0.1, "No data to send"); + ROS_INFO_THROTTLE(1.0, "No ui to send"); return; } From 5d736c7bffcafb3b1d3f3ebdc1ea5aab7fd05f7c Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Fri, 20 Oct 2023 20:21:42 +0800 Subject: [PATCH 13/29] Modify ui send logic. --- rm_referee/include/rm_referee/ui/graph.h | 3 +++ rm_referee/src/ui/graph.cpp | 12 ++++++++---- rm_referee/src/ui/time_change_ui.cpp | 11 +++-------- rm_referee/src/ui/trigger_change_ui.cpp | 6 ++---- rm_referee/src/ui/ui_base.cpp | 4 ++-- 5 files changed, 18 insertions(+), 18 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 5ca2902b..1900af17 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -38,6 +38,7 @@ class Graph void setContent(const std::string& content) { content_ = content; + config_.end_angle = static_cast(content_.length()); } void setEndX(int end_x) { @@ -78,6 +79,7 @@ class Graph last_config_ = config_; } + bool isString; private: void initPosition(XmlRpc::XmlRpcValue value, std::vector>& positions); rm_referee::GraphColor getColor(const std::string& color); @@ -87,6 +89,7 @@ class Graph std::vector> start_positions_{}, end_positions_{}; rm_referee::GraphConfig config_{}, last_config_{}; std::string title_{}, content_{}, last_title_{}, last_content_{}; + }; } // namespace rm_referee diff --git a/rm_referee/src/ui/graph.cpp b/rm_referee/src/ui/graph.cpp index 14a3acab..7868dac5 100644 --- a/rm_referee/src/ui/graph.cpp +++ b/rm_referee/src/ui/graph.cpp @@ -15,11 +15,19 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base config_.graphic_type = rm_referee::GraphType::STRING; if (config_.graphic_type == getType("string")) { + this->isString = true; if (config.hasMember("size")) config_.start_angle = static_cast(config["size"]); + if (config.hasMember("title")) + title_ = static_cast(config["title"]); + if (config.hasMember("content")) + content_ = static_cast(config["content"]); + + config_.end_angle = static_cast((title_ + content_).length()); } else { + this->isString = false; if (config.hasMember("start_angle")) config_.start_angle = static_cast(config["start_angle"]); } @@ -53,10 +61,6 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base config_.radius = static_cast(config["radius"]); if (config.hasMember("width")) config_.width = static_cast(config["width"]); - if (config.hasMember("title")) - title_ = static_cast(config["title"]); - if (config.hasMember("content")) - content_ = static_cast(config["content"]); config_.operate_type = rm_referee::GraphOperation::DELETE; last_config_ = config_; last_title_ = title_; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index f82a3c41..9de83a8a 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -30,8 +30,7 @@ void TimeChangeUi::updateForQueue() if ( !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) { - std::string characters = graph_->getCharacters(); - if (!characters.empty()) + if (graph_->isString) character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); @@ -48,17 +47,13 @@ void TimeChangeGroupUi::updateForQueue() for (auto graph : graph_vector_) { graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if ( !graph.second->isRepeated() ) - { - std::string characters = graph.second->getCharacters(); - if (!characters.empty()) + if(graph_->isString) character_queue_->push_back(*graph.second); - else + else graph_queue_->push_back(*graph.second); last_send_ = ros::Time::now(); } - } } void CapacitorTimeChangeUi::add() diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index ca5d2c38..116172ac 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -22,8 +22,7 @@ void TriggerChangeUi::updateForQueue(bool check_repeat) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - std::string characters = graph_->getCharacters(); - if (!characters.empty()) + if (graph_->isString) character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); @@ -39,8 +38,7 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - std::string characters = graph_->getCharacters(); - if (!characters.empty()) + if (graph_->isString) { for(int i = 0; i < 2; i++) character_queue_->push_back(*graph_); diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 3f221634..650a9473 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -375,8 +375,7 @@ void FixedUi::updateForQueue() for (auto it : graph_vector_) { - std::string characters = it.second->getCharacters(); - if (!characters.empty()) + if (graph_->isString) character_queue_->push_back(*it.second); else graph_queue_->push_back(*it.second); @@ -411,6 +410,7 @@ void UiBase::sendSerial(const ros::Time& time, int data_len) { } clearTxBuffer(); + } void UiBase::clearTxBuffer() From 8c608f73a5ed4258bdc6a3ea8e69353fe77d5770 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Fri, 20 Oct 2023 20:24:27 +0800 Subject: [PATCH 14/29] Modify ui sender --- rm_referee/src/referee_base.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 5e409bfc..c0606d42 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -166,16 +166,16 @@ void RefereeBase::sendSerialDataCallback() return; } - if (graph_queue_.size() > 50) - ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); - - if (character_queue_.size() > 20) - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); - if (!is_adding_) { + if (graph_queue_.size() > 50) + ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); + + if (character_queue_.size() > 8) + ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " + "reduce the frequency"); + if (graph_queue_.size() > 50) { ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, now reduce the queue"); From 535fa547a8c7be42758742105e8d4a3fbfb464c5 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Fri, 20 Oct 2023 22:27:15 +0800 Subject: [PATCH 15/29] Add test code. --- rm_referee/src/referee_base.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index c0606d42..03b64a69 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -112,7 +112,10 @@ void RefereeBase::addUi() ROS_INFO_THROTTLE(2.0, "End adding"); add_ui_timer_.stop(); if (!graph_queue_.empty()) - graph_queue_.clear(); + { + graph_queue_.clear(); + ROS_WARN_THROTTLE(0.5, "Some UI is not add completely, now clear the queue"); + } is_adding_ = false; send_serial_data_timer_.setPeriod(ros::Duration(send_ui_queue_delay_)); return; @@ -168,13 +171,6 @@ void RefereeBase::sendSerialDataCallback() if (!is_adding_) { - if (graph_queue_.size() > 50) - ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); - - if (character_queue_.size() > 8) - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, please modify the configuration file or code to " - "reduce the frequency"); if (graph_queue_.size() > 50) { @@ -197,6 +193,7 @@ void RefereeBase::sendSerialDataCallback() interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); radar_receive_last_send = ros::Time::now(); send_radar_receive_data_ = false; + ROS_INFO_THROTTLE(1.0, " send radar receive data"); } else if (send_map_sentry_data_) @@ -206,9 +203,10 @@ void RefereeBase::sendSerialDataCallback() interactive_data_sender_->sendMapSentryData(map_sentry_data); radar_receive_last_send = ros::Time::now(); send_map_sentry_data_ = false; + ROS_INFO_THROTTLE(1.0, " send map sentry data"); } - else if(!character_queue_.empty() && graph_queue_.size() <= 10) + else if(!character_queue_.empty()) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); character_queue_.pop_front(); @@ -255,6 +253,7 @@ void RefereeBase::sendSerialDataCallback() { graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); graph_queue_.pop_front(); + ROS_INFO_THROTTLE(1.0, " adding send 1 graph"); } } send_serial_data_timer_.start(); From 0fd8d6dbaed0ceb069717f4d5ae488be380ffd09 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 16:20:16 +0800 Subject: [PATCH 16/29] Optimization code. --- rm_referee/include/rm_referee/ui/graph.h | 11 ++--- rm_referee/src/referee_base.cpp | 53 +++++++++++++++++++----- rm_referee/src/ui/flash_ui.cpp | 8 ++-- rm_referee/src/ui/graph.cpp | 21 +++++----- rm_referee/src/ui/time_change_ui.cpp | 31 +++++++------- rm_referee/src/ui/trigger_change_ui.cpp | 5 ++- rm_referee/src/ui/ui_base.cpp | 8 ++-- 7 files changed, 85 insertions(+), 52 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 1900af17..5e93b7c2 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -38,7 +38,8 @@ class Graph void setContent(const std::string& content) { content_ = content; - config_.end_angle = static_cast(content_.length()); + if (!title_.empty() || !content_.empty()) + config_.end_angle = static_cast((title_ + content_).size()); } void setEndX(int end_x) { @@ -70,16 +71,17 @@ class Graph { return config_ == last_config_ && title_ == last_title_ && content_ == last_content_; } + bool isString() + { + return config_.graphic_type == rm_referee::GraphType::STRING; + } void updateLastConfig() { - if (!title_.empty() && !content_.empty()) - config_.end_angle = static_cast((title_ + content_).size()); last_content_ = content_; last_title_ = title_; last_config_ = config_; } - bool isString; private: void initPosition(XmlRpc::XmlRpcValue value, std::vector>& positions); rm_referee::GraphColor getColor(const std::string& color); @@ -89,7 +91,6 @@ class Graph std::vector> start_positions_{}, end_positions_{}; rm_referee::GraphConfig config_{}, last_config_{}; std::string title_{}, content_{}, last_title_{}, last_content_{}; - }; } // namespace rm_referee diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 03b64a69..b511bda1 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -161,28 +161,28 @@ void RefereeBase::addUi() add_ui_times_++; } - void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() && character_queue_.empty()){ - ROS_INFO_THROTTLE(1.0, "No ui to send"); + ROS_INFO_THROTTLE(1.0, "No UI to send"); return; } if (!is_adding_) { - if (graph_queue_.size() > 50) { - ROS_WARN_THROTTLE(2.0, "Sending graph UI too frequently, now reduce the queue"); + ROS_WARN_THROTTLE(0.5, "Sending graph UI too frequently, please modify the configuration file or code to" + "reduce the frequency . Now pop the queue"); while (graph_queue_.size() > 50) graph_queue_.pop_front(); } if (character_queue_.size() > 8) { - ROS_WARN_THROTTLE(2.0, "Sending character UI too frequently, now reduce the queue"); - while (character_queue_.size() > 7) + ROS_WARN_THROTTLE(0.5, "Sending character UI too frequently, please modify the configuration file or code to" + "reduce the frequency . Now pop the queue"); + while (character_queue_.size() > 8) character_queue_.pop_front(); } @@ -206,9 +206,9 @@ void RefereeBase::sendSerialDataCallback() ROS_INFO_THROTTLE(1.0, " send map sentry data"); } - else if(!character_queue_.empty()) + else if(!character_queue_.empty() && graph_queue_.size() <= 14) { - graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.front()); + graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.at(0)); character_queue_.pop_front(); ROS_INFO_THROTTLE(1.0, " send character"); } @@ -249,13 +249,46 @@ void RefereeBase::sendSerialDataCallback() } else { - if(!graph_queue_.empty()) + if (graph_queue_.size() >= 7) + { + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4), &graph_queue_.at(5), + &graph_queue_.at(6)); + ROS_INFO_THROTTLE(1.0, " add 7 graph"); + for (int i = 0; i < 7; i++) + graph_queue_.pop_front(); + } + else if (graph_queue_.size() >= 5) + { + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4)); + ROS_INFO_THROTTLE(1.0, " add 5 graph"); + for (int i = 0; i < 5; i++) + graph_queue_.pop_front(); + } + else if (graph_queue_.size() >= 2) + { + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); + for (int i = 0; i < 2; i++) + graph_queue_.pop_front(); + + ROS_INFO_THROTTLE(1.0, " add 2 graph"); + } + else if (graph_queue_.size() == 1) { graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); graph_queue_.pop_front(); - ROS_INFO_THROTTLE(1.0, " adding send 1 graph"); + ROS_INFO_THROTTLE(1.0, " add 1 graph"); } } + if (base_.robot_id_ == 0 ) + { + ROS_WARN_THROTTLE(1.0, "robot base id = 0, the serial or referee system may not be connected"); + } + if ( base_.client_id_ == 0) + ROS_WARN_THROTTLE(1.0, "client base id = 0, the serial or referee system may not be connected\""); send_serial_data_timer_.start(); } diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index b4a178b8..da18ff52 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -28,16 +28,14 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) graph_->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) + if(base_.robot_id_ == 0 || base_.client_id_ == 0) return; - std::string characters = graph_->getCharacters(); - if (!characters.empty()) + if (graph_->isString()) character_queue_->push_back(*graph_); else - graph_queue_->push_back(*graph_); + graph_queue_->push_back(*graph_); - ROS_INFO_THROTTLE(1.0, "update flash ui"); } void CoverFlashUi::display(const ros::Time& time) diff --git a/rm_referee/src/ui/graph.cpp b/rm_referee/src/ui/graph.cpp index 7868dac5..7ba5148e 100644 --- a/rm_referee/src/ui/graph.cpp +++ b/rm_referee/src/ui/graph.cpp @@ -12,22 +12,16 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base if (config.hasMember("type")) config_.graphic_type = getType(config["type"]); else + { config_.graphic_type = rm_referee::GraphType::STRING; + } if (config_.graphic_type == getType("string")) { - this->isString = true; if (config.hasMember("size")) config_.start_angle = static_cast(config["size"]); - if (config.hasMember("title")) - title_ = static_cast(config["title"]); - if (config.hasMember("content")) - content_ = static_cast(config["content"]); - - config_.end_angle = static_cast((title_ + content_).length()); } else { - this->isString = false; if (config.hasMember("start_angle")) config_.start_angle = static_cast(config["start_angle"]); } @@ -61,10 +55,15 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base config_.radius = static_cast(config["radius"]); if (config.hasMember("width")) config_.width = static_cast(config["width"]); + if (config.hasMember("title")) + title_ = static_cast(config["title"]); + if (config.hasMember("content")) + { + content_ = static_cast(config["content"]); + if (!title_.empty()|| !content_.empty()) + config_.end_angle = static_cast((title_ + content_).size()); + } config_.operate_type = rm_referee::GraphOperation::DELETE; - last_config_ = config_; - last_title_ = title_; - last_content_ = content_; } void Graph::updatePosition(int index) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 9de83a8a..e3a8f484 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -28,32 +28,34 @@ void TimeChangeUi::updateForQueue() updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - if ( !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) + if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) { - if (graph_->isString) + if (graph_->isString()) character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); last_send_ = ros::Time::now(); - } } void TimeChangeGroupUi::updateForQueue() { updateConfig(); - if (ros::Time::now() - last_send_ > delay_) - for (auto graph : graph_vector_) + if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) + { + for (auto it : character_vector_) { - graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - if(graph_->isString) - character_queue_->push_back(*graph.second); - else - graph_queue_->push_back(*graph.second); - - last_send_ = ros::Time::now(); - } + it.second->setOperation(rm_referee::GraphOperation::UPDATE); + character_queue_->push_back(*it.second); + } + for (auto it : graph_vector_) + { + it.second->setOperation(rm_referee::GraphOperation::UPDATE); + graph_queue_->push_back(*it.second); + } + last_send_ = ros::Time::now(); + } } void CapacitorTimeChangeUi::add() @@ -133,8 +135,9 @@ void ProgressTimeChangeUi::updateConfig() void ProgressTimeChangeUi::updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time) { - total_steps_ = data->total_steps; + /*total_steps_ = data->total_steps; finished_data_ = data->finished_step; + */ TimeChangeUi::updateForQueue(); } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 116172ac..92039aa7 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -22,7 +22,7 @@ void TriggerChangeUi::updateForQueue(bool check_repeat) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - if (graph_->isString) + if (graph_->isString()) character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); @@ -38,7 +38,7 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - if (graph_->isString) + if (graph_->isString()) { for(int i = 0; i < 2; i++) character_queue_->push_back(*graph_); @@ -109,6 +109,7 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; + for (auto it : character_vector_) for(int i = 0; i < 2; i++) character_queue_->push_back(*it.second); diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 650a9473..cb5773e4 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -143,8 +143,7 @@ void UiBase::sendUi(const ros::Time& time) if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - std::string characters = graph_->getCharacters(); - if (!characters.empty()) + if (graph_->isString()) sendCharacter(time, graph_); else sendSingleGraph(time, graph_); @@ -296,7 +295,7 @@ void GroupUiBase::display(const ros::Time& time) void GroupUiBase::sendUi(const ros::Time& time) { - if (base_.robot_id_ == 0 || base_.client_id_ == 0) + if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; for (auto it : character_vector_) @@ -375,7 +374,7 @@ void FixedUi::updateForQueue() for (auto it : graph_vector_) { - if (graph_->isString) + if (graph_->isString()) character_queue_->push_back(*it.second); else graph_queue_->push_back(*it.second); @@ -410,7 +409,6 @@ void UiBase::sendSerial(const ros::Time& time, int data_len) { } clearTxBuffer(); - } void UiBase::clearTxBuffer() From 634507be364b7a105db4c43ff4e979bfb17d26e1 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 16:55:14 +0800 Subject: [PATCH 17/29] Optimize code. --- rm_referee/include/rm_referee/referee_base.h | 1 + .../include/rm_referee/ui/time_change_ui.h | 1 - .../include/rm_referee/ui/trigger_change_ui.h | 3 - rm_referee/src/referee_base.cpp | 116 ++++++------------ rm_referee/src/ui/time_change_ui.cpp | 11 +- rm_referee/src/ui/trigger_change_ui.cpp | 26 ---- 6 files changed, 43 insertions(+), 115 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 81d1b97e..72e68507 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -54,6 +54,7 @@ class RefereeBase // send ui void sendSerialDataCallback(); + void queueSend(); ros::Subscriber joint_state_sub_; ros::Subscriber actuator_state_sub_; 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 017eff7f..cbf42cb9 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -234,7 +234,6 @@ class PitchAngleTimeChangeUi : public TimeChangeUi public: explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; - void update() override; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); private: 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 2a7fa677..1f92d7f0 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -21,13 +21,11 @@ class TriggerChangeUi : public UiBase else graph_ = new Graph(rpc_value["config"], base_, id_++); }; - virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; void updateForQueue(bool check_repeat = true); void updateTwiceForQueue(bool check_repeat = true); }; - class TriggerChangeGroupUi : public GroupUiBase { public: @@ -37,7 +35,6 @@ class TriggerChangeGroupUi : public GroupUiBase { graph_name_ = graph_name; }; - virtual void setContent(const std::string& content); virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; void updateForQueue(bool check_repeat = true); void updateTwiceForQueue(bool check_repeat = true); diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index b511bda1..362f2583 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -195,7 +195,6 @@ void RefereeBase::sendSerialDataCallback() send_radar_receive_data_ = false; ROS_INFO_THROTTLE(1.0, " send radar receive data"); } - else if (send_map_sentry_data_) { if(ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) @@ -205,84 +204,12 @@ void RefereeBase::sendSerialDataCallback() send_map_sentry_data_ = false; ROS_INFO_THROTTLE(1.0, " send map sentry data"); } - - else if(!character_queue_.empty() && graph_queue_.size() <= 14) - { - graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.at(0)); - character_queue_.pop_front(); - ROS_INFO_THROTTLE(1.0, " send character"); - } - - else if (graph_queue_.size() >= 7) - { - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4), &graph_queue_.at(5), - &graph_queue_.at(6)); - ROS_INFO_THROTTLE(1.0, " send 7 graph"); - for (int i = 0; i < 7; i++) - graph_queue_.pop_front(); - } - else if (graph_queue_.size() >= 5) - { - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4)); - ROS_INFO_THROTTLE(1.0, " send 5 graph"); - for (int i = 0; i < 5; i++) - graph_queue_.pop_front(); - } - else if (graph_queue_.size() >= 2) - { - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); - for (int i = 0; i < 2; i++) - graph_queue_.pop_front(); - - ROS_INFO_THROTTLE(1.0, " send 2 graph"); - } - else if (graph_queue_.size() == 1) - { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); - graph_queue_.pop_front(); - ROS_INFO_THROTTLE(1.0, " send 1 graph"); - } + else + queueSend(); } else - { - if (graph_queue_.size() >= 7) - { - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4), &graph_queue_.at(5), - &graph_queue_.at(6)); - ROS_INFO_THROTTLE(1.0, " add 7 graph"); - for (int i = 0; i < 7; i++) - graph_queue_.pop_front(); - } - else if (graph_queue_.size() >= 5) - { - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4)); - ROS_INFO_THROTTLE(1.0, " add 5 graph"); - for (int i = 0; i < 5; i++) - graph_queue_.pop_front(); - } - else if (graph_queue_.size() >= 2) - { - graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); - for (int i = 0; i < 2; i++) - graph_queue_.pop_front(); + queueSend(); - ROS_INFO_THROTTLE(1.0, " add 2 graph"); - } - else if (graph_queue_.size() == 1) - { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); - graph_queue_.pop_front(); - ROS_INFO_THROTTLE(1.0, " add 1 graph"); - } - } if (base_.robot_id_ == 0 ) { ROS_WARN_THROTTLE(1.0, "robot base id = 0, the serial or referee system may not be connected"); @@ -292,6 +219,43 @@ void RefereeBase::sendSerialDataCallback() send_serial_data_timer_.start(); } +void RefereeBase::queueSend() +{ + if(!character_queue_.empty() && graph_queue_.size() <= 14) + { + graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.at(0)); + character_queue_.pop_front(); + } + else if (graph_queue_.size() >= 7) + { + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4), &graph_queue_.at(5), + &graph_queue_.at(6)); + for (int i = 0; i < 7; i++) + graph_queue_.pop_front(); + } + else if (graph_queue_.size() >= 5) + { + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), + &graph_queue_.at(2), &graph_queue_.at(3), + &graph_queue_.at(4)); + for (int i = 0; i < 5; i++) + graph_queue_.pop_front(); + } + else if (graph_queue_.size() >= 2) + { + graph_queue_sender_->sendDoubleGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1)); + for (int i = 0; i < 2; i++) + graph_queue_.pop_front(); + } + else if (graph_queue_.size() == 1) + { + graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(0)); + graph_queue_.pop_front(); + } +} + void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, const ros::Time& last_get_data_time) { if (fixed_ui_ && !is_adding_) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index e3a8f484..4fb5d9a2 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -34,7 +34,6 @@ void TimeChangeUi::updateForQueue() character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); - last_send_ = ros::Time::now(); } } @@ -117,7 +116,7 @@ void EffortTimeChangeUi::updateJointStateData(const sensor_msgs::JointState::Con { joint_effort_ = data->effort[max_index]; joint_name_ = data->name[max_index]; - TimeChangeUi::update(); + updateForQueue(); } } } @@ -135,9 +134,8 @@ void ProgressTimeChangeUi::updateConfig() void ProgressTimeChangeUi::updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time) { - /*total_steps_ = data->total_steps; + total_steps_ = data->total_steps; finished_data_ = data->finished_step; - */ TimeChangeUi::updateForQueue(); } @@ -286,11 +284,6 @@ void PitchAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState: for (unsigned int i = 0; i < data->name.size(); i++) if (data->name[i] == "pitch_joint") pitch_angle_ = data->position[i]; - update(); -} - -void PitchAngleTimeChangeUi::update() -{ updateForQueue(); } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 92039aa7..82215733 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -6,11 +6,6 @@ namespace rm_referee { -void TriggerChangeUi::setContent(const std::string& content) -{ - graph_->setContent(content); - display(); -} void TriggerChangeUi::updateForQueue(bool check_repeat) { @@ -19,14 +14,10 @@ void TriggerChangeUi::updateForQueue(bool check_repeat) return; graph_->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) - return; - if (graph_->isString()) character_queue_->push_back(*graph_); else graph_queue_->push_back(*graph_); - } void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) { @@ -35,9 +26,6 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) return; graph_->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) - return; - if (graph_->isString()) { for(int i = 0; i < 2; i++) @@ -47,13 +35,6 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) for(int i = 0; i < 2; i++) graph_queue_->push_back(*graph_); } - -} - -void TriggerChangeGroupUi::setContent(const std::string& content) -{ - graph_->setContent(content); - display(); } void TriggerChangeGroupUi::updateForQueue(bool check_repeat) @@ -76,9 +57,6 @@ void TriggerChangeGroupUi::updateForQueue(bool check_repeat) for (auto it : character_vector_) it.second->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) - return; - for (auto it : character_vector_) character_queue_->push_back(*it.second); for (auto it : graph_vector_) @@ -106,10 +84,6 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) for (auto it : character_vector_) it.second->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) - return; - - for (auto it : character_vector_) for(int i = 0; i < 2; i++) character_queue_->push_back(*it.second); From 553386a9da274b645a993b3527edcb1133438035 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 17:33:47 +0800 Subject: [PATCH 18/29] Optimize the code. --- .../include/rm_referee/ui/time_change_ui.h | 4 +- .../include/rm_referee/ui/trigger_change_ui.h | 12 ++-- rm_referee/include/rm_referee/ui/ui_base.h | 4 +- rm_referee/src/ui/time_change_ui.cpp | 16 +---- rm_referee/src/ui/trigger_change_ui.cpp | 60 ++++++++----------- rm_referee/src/ui/ui_base.cpp | 29 ++++++--- 6 files changed, 60 insertions(+), 65 deletions(-) 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 cbf42cb9..1a0f4d1b 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -18,7 +18,7 @@ class TimeChangeUi : public UiBase graph_ = new Graph(rpc_value["config"], base_, id_++); } void update() override; - void updateForQueue(); + void updateForQueue() override; virtual void updateConfig(){}; }; @@ -32,7 +32,7 @@ class TimeChangeGroupUi : public GroupUiBase graph_name_ = graph_name; } void update() override; - void updateForQueue(); + void updateForQueue() override; virtual void updateConfig(){}; protected: 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 1f92d7f0..60fb3a6f 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -22,7 +22,8 @@ class TriggerChangeUi : public UiBase graph_ = new Graph(rpc_value["config"], base_, id_++); }; virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; - void updateForQueue(bool check_repeat = true); + void updateForQueue() override; + void updateForQueue(bool check_repeat); void updateTwiceForQueue(bool check_repeat = true); }; @@ -36,7 +37,8 @@ class TriggerChangeGroupUi : public GroupUiBase graph_name_ = graph_name; }; virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){}; - void updateForQueue(bool check_repeat = true); + void updateForQueue() override; + void updateForQueue(bool check_repeat); void updateTwiceForQueue(bool check_repeat = true); protected: @@ -59,9 +61,9 @@ class ChassisTriggerChangeUi : public TriggerChangeUi else mode_change_threshold_ = 0.7; } - void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data); + void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data); void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) override; - void updateDbusData(const rm_msgs::DbusData::ConstPtr data); + void updateDbusData(const rm_msgs::DbusData::ConstPtr& data); void updateCapacityResetStatus(); void checkModeChange(); @@ -100,7 +102,7 @@ class GimbalTriggerChangeUi : public TriggerChangeUi { graph_->setContent("0"); } - void updateGimbalCmdData(const rm_msgs::GimbalCmd ::ConstPtr data); + void updateGimbalCmdData(const rm_msgs::GimbalCmd ::ConstPtr& data); void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) override; private: diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 70cf2d6c..f178f462 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -31,6 +31,7 @@ class UiBase ~UiBase() = default; virtual void add(); virtual void update(); + virtual void updateForQueue(); virtual void erasure(); virtual void addForQueue(int add_times = 1); virtual void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data){}; @@ -77,6 +78,7 @@ class GroupUiBase : public UiBase ~GroupUiBase() = default; void add() override; void update() override; + void updateForQueue() override; void erasure() override; void addForQueue(int add_times = 1) override; void sendUi(const ros::Time& time) override; @@ -103,7 +105,7 @@ class FixedUi : public GroupUiBase graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; - void updateForQueue(); + void updateForQueue() override; int update_fixed_ui_times = 0; }; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 4fb5d9a2..9fd110e0 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -30,10 +30,7 @@ void TimeChangeUi::updateForQueue() if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) { - if (graph_->isString()) - character_queue_->push_back(*graph_); - else - graph_queue_->push_back(*graph_); + UiBase::updateForQueue(); last_send_ = ros::Time::now(); } } @@ -43,16 +40,7 @@ void TimeChangeGroupUi::updateForQueue() updateConfig(); if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) { - for (auto it : character_vector_) - { - it.second->setOperation(rm_referee::GraphOperation::UPDATE); - character_queue_->push_back(*it.second); - } - for (auto it : graph_vector_) - { - it.second->setOperation(rm_referee::GraphOperation::UPDATE); - graph_queue_->push_back(*it.second); - } + GroupUiBase::updateForQueue(); last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 82215733..9420d848 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -6,35 +6,38 @@ namespace rm_referee { +void TriggerChangeUi::updateForQueue() +{ + graph_->updateLastConfig(); + UiBase::updateForQueue(); +} void TriggerChangeUi::updateForQueue(bool check_repeat) { if (check_repeat) if (graph_->isRepeated()) return; - graph_->updateLastConfig(); - - if (graph_->isString()) - character_queue_->push_back(*graph_); - else - graph_queue_->push_back(*graph_); + TriggerChangeUi::updateForQueue(); } + void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) { if (check_repeat) if (graph_->isRepeated()) return; graph_->updateLastConfig(); + for(int i = 0; i < 2; i++) + UiBase::updateForQueue(); +} - if (graph_->isString()) - { - for(int i = 0; i < 2; i++) - character_queue_->push_back(*graph_); - } - else{ - for(int i = 0; i < 2; i++) - graph_queue_->push_back(*graph_); - } +void TriggerChangeGroupUi::updateForQueue() +{ + for (auto it : graph_vector_) + it.second->updateLastConfig(); + for (auto it : character_vector_) + it.second->updateLastConfig(); + + GroupUiBase::updateForQueue(); } void TriggerChangeGroupUi::updateForQueue(bool check_repeat) @@ -51,17 +54,7 @@ void TriggerChangeGroupUi::updateForQueue(bool check_repeat) if (is_repeat) return; } - - for (auto it : graph_vector_) - it.second->updateLastConfig(); - for (auto it : character_vector_) - it.second->updateLastConfig(); - - for (auto it : character_vector_) - character_queue_->push_back(*it.second); - for (auto it : graph_vector_) - graph_queue_->push_back(*it.second); - + TriggerChangeGroupUi::updateForQueue(); } void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) @@ -84,13 +77,8 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) for (auto it : character_vector_) it.second->updateLastConfig(); - for (auto it : character_vector_) - for(int i = 0; i < 2; i++) - character_queue_->push_back(*it.second); - for (auto it : graph_vector_) - for(int i = 0; i < 2; i++) - graph_queue_->push_back(*it.second); - + for(int i = 0; i < 2; i++) + GroupUiBase::updateForQueue(); } @@ -211,7 +199,7 @@ std::string ChassisTriggerChangeUi::getChassisState(uint8_t mode) return "error"; } -void ChassisTriggerChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data) +void ChassisTriggerChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data) { chassis_mode_ = data->mode; update(); @@ -222,7 +210,7 @@ void ChassisTriggerChangeUi::updateManualCmdData(const rm_msgs::ManualToReferee: power_limit_state_ = data->power_limit_state; } -void ChassisTriggerChangeUi::updateDbusData(const rm_msgs::DbusData::ConstPtr data) +void ChassisTriggerChangeUi::updateDbusData(const rm_msgs::DbusData::ConstPtr& data) { s_l_ = data->s_l; s_r_ = data->s_r; @@ -308,7 +296,7 @@ std::string GimbalTriggerChangeUi::getGimbalState(uint8_t mode) return "error"; } -void GimbalTriggerChangeUi::updateGimbalCmdData(const rm_msgs::GimbalCmd::ConstPtr data) +void GimbalTriggerChangeUi::updateGimbalCmdData(const rm_msgs::GimbalCmd::ConstPtr& data) { gimbal_mode_ = data->mode; update(); diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index cb5773e4..9a00a524 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -35,6 +35,13 @@ void UiBase::erasure() displayTwice(false); } +void UiBase::updateForQueue() +{ + if (graph_->isString()) + character_queue_->push_back(*graph_); + else + graph_queue_->push_back(*graph_); +} void GroupUiBase::add() { for (auto graph : graph_vector_) @@ -75,6 +82,20 @@ void GroupUiBase::erasure() displayTwice(false); } +void GroupUiBase::updateForQueue() +{ + for (auto it : character_vector_) + { + it.second->setOperation(rm_referee::GraphOperation::UPDATE); + character_queue_->push_back(*it.second); + } + for (auto it : graph_vector_) + { + it.second->setOperation(rm_referee::GraphOperation::UPDATE); + graph_queue_->push_back(*it.second); + } +} + void UiBase::display(bool check_repeat) { if (check_repeat) @@ -372,13 +393,7 @@ void FixedUi::updateForQueue() if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; - for (auto it : graph_vector_) - { - if (graph_->isString()) - character_queue_->push_back(*it.second); - else - graph_queue_->push_back(*it.second); - } + GroupUiBase::updateForQueue(); ROS_INFO_THROTTLE(1.0, "update fixed ui"); update_fixed_ui_times++; } From 59cab1d4e844f5ec5012dede8511e719565f3e83 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 17:48:35 +0800 Subject: [PATCH 19/29] Optimize the code. --- rm_referee/include/rm_referee/ui/flash_ui.h | 2 +- rm_referee/src/ui/flash_ui.cpp | 17 ++++------------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 6c84fd32..d79b1b19 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -19,7 +19,7 @@ class FlashUi : public UiBase } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; - void updateForQueue(const ros::Time& time, bool state, bool once); + void updateFlashUiForQueue(const ros::Time& time, bool state, bool once); }; class CoverFlashUi : public FlashUi diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index da18ff52..184d0f28 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -7,7 +7,7 @@ namespace rm_referee { -void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) +void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once) { if (once) { @@ -25,24 +25,15 @@ void FlashUi::updateForQueue(const ros::Time& time, bool state, bool once) } if (graph_->isRepeated()) return; - graph_->updateLastConfig(); - - if(base_.robot_id_ == 0 || base_.client_id_ == 0) - return; - - if (graph_->isString()) - character_queue_->push_back(*graph_); - else - graph_queue_->push_back(*graph_); - + UiBase::updateForQueue(); } void CoverFlashUi::display(const ros::Time& time) { if (!cover_state_) graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateForQueue(time, cover_state_, true); + FlashUi::updateFlashUiForQueue(time, cover_state_, true); } void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, @@ -56,7 +47,7 @@ void SpinFlashUi::display(const ros::Time& time) { if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateForQueue(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); + FlashUi::updateFlashUiForQueue(time, chassis_mode_ != rm_msgs::ChassisCmd::RAW, true); } void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time) From ec229c2d4e56f2467d41f292d5ac5dfbe99681dc Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 17:57:40 +0800 Subject: [PATCH 20/29] modify the format --- rm_referee/include/rm_referee/ui/flash_ui.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index d79b1b19..6251fe1e 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,7 +12,7 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::deque * graph_queue, std::deque * character_queue) + std::deque* graph_queue, std::deque* character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); From c0d7a7221135f6d15030ee2fc71d06f40d7e974e Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 22 Oct 2023 18:07:03 +0800 Subject: [PATCH 21/29] Optimize the code. --- rm_referee/include/rm_referee/referee_base.h | 1 - rm_referee/include/rm_referee/ui/flash_ui.h | 6 ++- .../include/rm_referee/ui/time_change_ui.h | 34 +++++++----- .../include/rm_referee/ui/trigger_change_ui.h | 26 ++++++---- rm_referee/include/rm_referee/ui/ui_base.h | 11 ++-- rm_referee/src/referee_base.cpp | 52 ++++++++++--------- rm_referee/src/ui/flash_ui.cpp | 1 - rm_referee/src/ui/graph.cpp | 2 +- rm_referee/src/ui/time_change_ui.cpp | 2 +- rm_referee/src/ui/trigger_change_ui.cpp | 5 +- rm_referee/src/ui/ui_base.cpp | 18 +++---- 11 files changed, 90 insertions(+), 68 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 72e68507..e526d2fd 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -13,7 +13,6 @@ #include "rm_referee/ui/time_change_ui.h" #include "rm_referee/ui/flash_ui.h" - namespace rm_referee { class RefereeBase diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 6251fe1e..7be7495e 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -25,7 +25,8 @@ class FlashUi : public UiBase class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; @@ -38,7 +39,8 @@ class CoverFlashUi : public FlashUi class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : FlashUi(rpc_value, base, "spin", graph_queue, character_queue){}; void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, const ros::Time& last_get_data_time); 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 1a0f4d1b..a249a44b 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -12,7 +12,7 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::deque * graph_queue, std::deque * character_queue) + std::deque* graph_queue, std::deque* character_queue) : UiBase(rpc_value, base, graph_queue, character_queue) { graph_ = new Graph(rpc_value["config"], base_, id_++); @@ -26,7 +26,7 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::deque * graph_queue, std::deque * character_queue) + std::deque* graph_queue, std::deque* character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; @@ -42,7 +42,8 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "capacitor", graph_queue, character_queue){}; void add() override; void updateRemainCharge(const double remain_charge, const ros::Time& time); @@ -55,7 +56,8 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "effort", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -68,7 +70,8 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "progress", graph_queue, character_queue){}; void updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); @@ -81,7 +84,8 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "dart", graph_queue, character_queue){}; void updateDartClientCmd(const rm_msgs::DartClientCmd::ConstPtr data, const ros::Time& last_get_data_time); @@ -93,7 +97,8 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "rotation", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -125,7 +130,8 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_queue, character_queue) { if (rpc_value.hasMember("data")) @@ -171,7 +177,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_queue, character_queue) { XmlRpc::XmlRpcValue config; @@ -232,7 +239,8 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TimeChangeUi(rpc_value, base, "pitch", graph_queue, character_queue){}; void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); @@ -244,9 +252,9 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue, - std::string name) - : TimeChangeUi(rpc_value, base, name, graph_queue,character_queue) + explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue, std::string name) + : TimeChangeUi(rpc_value, base, name, graph_queue, character_queue) { if (rpc_value.hasMember("data")) { 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 60fb3a6f..2e882cf4 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -31,7 +31,7 @@ class TriggerChangeGroupUi : public GroupUiBase { public: explicit TriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::deque * graph_queue, std::deque * character_queue) + std::deque* graph_queue, std::deque* character_queue) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; @@ -48,7 +48,8 @@ class TriggerChangeGroupUi : public GroupUiBase class ChassisTriggerChangeUi : public TriggerChangeUi { public: - explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit ChassisTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "chassis", graph_queue, character_queue) { if (base.robot_id_ == rm_referee::RobotId::RED_ENGINEER || base.robot_id_ == rm_referee::RobotId::BLUE_ENGINEER) @@ -79,7 +80,8 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "shooter", graph_queue, character_queue) { graph_->setContent("0"); @@ -97,7 +99,8 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "gimbal", graph_queue, character_queue) { graph_->setContent("0"); @@ -115,7 +118,8 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "target", graph_queue, character_queue) { graph_->setContent("armor"); @@ -137,7 +141,8 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "target_scale", graph_queue, character_queue) { } @@ -152,7 +157,8 @@ class TargetViewAngleTriggerChangeUi : public TriggerChangeUi class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi { public: - explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit PolygonTriggerChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); @@ -193,13 +199,15 @@ class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi } } void update() override; - //virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){} void setContent(const std::string &content) override; + // virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){} void + // setContent(const std::string &content) override; }; class CameraTriggerChangeUi : public TriggerChangeUi { public: - explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue, std::deque * character_queue) + explicit CameraTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) : TriggerChangeUi(rpc_value, base, "camera", graph_queue, character_queue) { if (rpc_value.hasMember("camera_name")) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index f178f462..e5b5a1a4 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -19,7 +19,8 @@ namespace rm_referee class UiBase { public: - explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr ) + explicit UiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) : base_(base), tf_listener_(tf_buffer_) { if (rpc_value.hasMember("config")) @@ -73,7 +74,8 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque * graph_queue = nullptr, std::deque * character_queue = nullptr) + explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) : UiBase(rpc_value, base, graph_queue, character_queue){}; ~GroupUiBase() = default; void add() override; @@ -98,10 +100,11 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, std::deque* character_queue = nullptr) + explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - for(int i = 0; i < static_cast(rpc_value.size()); i++) + for (int i = 0; i < static_cast(rpc_value.size()); i++) graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 362f2583..3fed1374 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -55,7 +55,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "target") target_trigger_change_ui_ = new TargetTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "target_view_angle") - target_view_angle_trigger_change_ui_ = new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + target_view_angle_trigger_change_ui_ = + new TargetViewAngleTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "camera") camera_trigger_change_ui_ = new CameraTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } @@ -74,17 +75,22 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "rotation") rotation_time_change_ui_ = new RotationTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "lane_line") - lane_line_time_change_ui_ = new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + lane_line_time_change_ui_ = + new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "pitch") pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "balance_pitch") - balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + balance_pitch_time_change_group_ui_ = + new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "engineer_joint1") - engineer_joint1_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint1"); + engineer_joint1_time_change_ui = + new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint1"); if (rpc_value[i]["name"] == "engineer_joint2") - engineer_joint2_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint2"); + engineer_joint2_time_change_ui = + new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint2"); if (rpc_value[i]["name"] == "engineer_joint3") - engineer_joint3_time_change_ui = new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_,"joint3"); + engineer_joint3_time_change_ui = + new JointPositionTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "joint3"); } ui_nh.getParam("fixed", rpc_value); @@ -103,7 +109,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) add_ui_timer_ = nh.createTimer(ros::Duration(1. / add_ui_frequency_), std::bind(&RefereeBase::addUi, this), false, false); send_serial_data_timer_ = nh.createTimer(ros::Duration(send_ui_queue_delay_), - std::bind(&RefereeBase::sendSerialDataCallback, this), false, true); + std::bind(&RefereeBase::sendSerialDataCallback, this), false, true); } void RefereeBase::addUi() { @@ -163,7 +169,8 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { - if (graph_queue_.empty() && character_queue_.empty()){ + if (graph_queue_.empty() && character_queue_.empty()) + { ROS_INFO_THROTTLE(1.0, "No UI to send"); return; } @@ -186,9 +193,9 @@ void RefereeBase::sendSerialDataCallback() character_queue_.pop_front(); } - if(send_radar_receive_data_) + if (send_radar_receive_data_) { - if(ros::Time::now() - radar_receive_last_send < ros::Duration(0.2)) + if (ros::Time::now() - radar_receive_last_send < ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); radar_receive_last_send = ros::Time::now(); @@ -197,49 +204,47 @@ void RefereeBase::sendSerialDataCallback() } else if (send_map_sentry_data_) { - if(ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) - ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); + if (ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) + ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); interactive_data_sender_->sendMapSentryData(map_sentry_data); radar_receive_last_send = ros::Time::now(); send_map_sentry_data_ = false; ROS_INFO_THROTTLE(1.0, " send map sentry data"); } - else + else queueSend(); } else queueSend(); - if (base_.robot_id_ == 0 ) + if (base_.robot_id_ == 0) { ROS_WARN_THROTTLE(1.0, "robot base id = 0, the serial or referee system may not be connected"); } - if ( base_.client_id_ == 0) + if (base_.client_id_ == 0) ROS_WARN_THROTTLE(1.0, "client base id = 0, the serial or referee system may not be connected\""); send_serial_data_timer_.start(); } void RefereeBase::queueSend() { - if(!character_queue_.empty() && graph_queue_.size() <= 14) + if (!character_queue_.empty() && graph_queue_.size() <= 14) { graph_queue_sender_->sendCharacter(ros::Time::now(), &character_queue_.at(0)); character_queue_.pop_front(); } else if (graph_queue_.size() >= 7) { - graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4), &graph_queue_.at(5), + graph_queue_sender_->sendSevenGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), &graph_queue_.at(2), + &graph_queue_.at(3), &graph_queue_.at(4), &graph_queue_.at(5), &graph_queue_.at(6)); for (int i = 0; i < 7; i++) graph_queue_.pop_front(); } else if (graph_queue_.size() >= 5) { - graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), - &graph_queue_.at(2), &graph_queue_.at(3), - &graph_queue_.at(4)); + graph_queue_sender_->sendFiveGraph(ros::Time::now(), &graph_queue_.at(0), &graph_queue_.at(1), &graph_queue_.at(2), + &graph_queue_.at(3), &graph_queue_.at(4)); for (int i = 0; i < 5; i++) graph_queue_.pop_front(); } @@ -311,11 +316,10 @@ void RefereeBase::dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) add_ui_flag_ = false; is_adding_ = true; if (!graph_queue_.empty()) - graph_queue_.clear(); + graph_queue_.clear(); send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; - } if (data->s_r != rm_msgs::DbusData::UP) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 184d0f28..3b8421b8 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -6,7 +6,6 @@ namespace rm_referee { - void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once) { if (once) diff --git a/rm_referee/src/ui/graph.cpp b/rm_referee/src/ui/graph.cpp index 7ba5148e..1a6061f6 100644 --- a/rm_referee/src/ui/graph.cpp +++ b/rm_referee/src/ui/graph.cpp @@ -60,7 +60,7 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base if (config.hasMember("content")) { content_ = static_cast(config["content"]); - if (!title_.empty()|| !content_.empty()) + if (!title_.empty() || !content_.empty()) config_.end_angle = static_cast((title_ + content_).size()); } config_.operate_type = rm_referee::GraphOperation::DELETE; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 9fd110e0..a1533c62 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -40,7 +40,7 @@ void TimeChangeGroupUi::updateForQueue() updateConfig(); if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) { - GroupUiBase::updateForQueue(); + GroupUiBase::updateForQueue(); last_send_ = ros::Time::now(); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 9420d848..8811a9c8 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -26,7 +26,7 @@ void TriggerChangeUi::updateTwiceForQueue(bool check_repeat) if (graph_->isRepeated()) return; graph_->updateLastConfig(); - for(int i = 0; i < 2; i++) + for (int i = 0; i < 2; i++) UiBase::updateForQueue(); } @@ -77,11 +77,10 @@ void TriggerChangeGroupUi::updateTwiceForQueue(bool check_repeat) for (auto it : character_vector_) it.second->updateLastConfig(); - for(int i = 0; i < 2; i++) + for (int i = 0; i < 2; i++) GroupUiBase::updateForQueue(); } - void ChassisTriggerChangeUi::update() { if (s_l_ == rm_msgs::DbusData::MID && s_r_ == rm_msgs::DbusData::UP) diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 9a00a524..30ff6ce1 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -316,7 +316,7 @@ void GroupUiBase::display(const ros::Time& time) void GroupUiBase::sendUi(const ros::Time& time) { - if (base_.robot_id_ == 0 || base_.client_id_ == 0) + if (base_.robot_id_ == 0 || base_.client_id_ == 0) return; for (auto it : character_vector_) @@ -385,17 +385,17 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr void FixedUi::updateForQueue() { - while(update_fixed_ui_times < 1) + while (update_fixed_ui_times < 1) { - for (auto it : graph_vector_) - it.second->updateLastConfig(); + for (auto it : graph_vector_) + it.second->updateLastConfig(); - if (base_.robot_id_ == 0 || base_.client_id_ == 0) - return; + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; - GroupUiBase::updateForQueue(); - ROS_INFO_THROTTLE(1.0, "update fixed ui"); - update_fixed_ui_times++; + GroupUiBase::updateForQueue(); + ROS_INFO_THROTTLE(1.0, "update fixed ui"); + update_fixed_ui_times++; } } From dc6bf0496210e6dd99436bebfd7d833073ffa7cd Mon Sep 17 00:00:00 2001 From: Mahoushoujyo-FurudoErika <115915981+GuraMumei@users.noreply.github.com> Date: Mon, 23 Oct 2023 20:21:42 +0800 Subject: [PATCH 22/29] Update trigger_change_ui.h --- rm_referee/include/rm_referee/ui/trigger_change_ui.h | 2 -- 1 file changed, 2 deletions(-) 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 2e882cf4..92875904 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -199,8 +199,6 @@ class PolygonTriggerChangeGroupUi : public TriggerChangeGroupUi } } void update() override; - // virtual void updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode = 0, bool sub_flag = false){} void - // setContent(const std::string &content) override; }; class CameraTriggerChangeUi : public TriggerChangeUi From deb18583a5ff484e1b664e0fbed775c1cddc5bd0 Mon Sep 17 00:00:00 2001 From: Mahoushoujyo-FurudoErika <115915981+GuraMumei@users.noreply.github.com> Date: Mon, 23 Oct 2023 20:24:57 +0800 Subject: [PATCH 23/29] Update referee_base.h --- rm_referee/include/rm_referee/referee_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index e526d2fd..382e5b10 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -103,7 +103,7 @@ class RefereeBase rm_referee::ClientMapReceiveData radar_receive_data; rm_referee::MapSentryData map_sentry_data; - ros::Time radar_receive_last_send, map_sentry_last_send; + ros::Time radar_receive_data_last_send, map_sentry_data_last_send; bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; From 70ac554f4222d53cb4498d4ef3d3ef725b4aebf8 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 23 Oct 2023 20:31:31 +0800 Subject: [PATCH 24/29] Optimize the code. --- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/src/referee_base.cpp | 11 +++++------ 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 382e5b10..034aebf6 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -103,7 +103,7 @@ class RefereeBase rm_referee::ClientMapReceiveData radar_receive_data; rm_referee::MapSentryData map_sentry_data; - ros::Time radar_receive_data_last_send, map_sentry_data_last_send; + ros::Time interactive_data_last_send; bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 3fed1374..6072fbf5 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -195,19 +195,19 @@ void RefereeBase::sendSerialDataCallback() if (send_radar_receive_data_) { - if (ros::Time::now() - radar_receive_last_send < ros::Duration(0.2)) + if (ros::Time::now() - interactive_data_last_send <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); - radar_receive_last_send = ros::Time::now(); + interactive_data_last_send = ros::Time::now(); send_radar_receive_data_ = false; ROS_INFO_THROTTLE(1.0, " send radar receive data"); } else if (send_map_sentry_data_) { - if (ros::Time::now() - map_sentry_last_send < ros::Duration(0.2)) + if (ros::Time::now() - interactive_data_last_send <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); interactive_data_sender_->sendMapSentryData(map_sentry_data); - radar_receive_last_send = ros::Time::now(); + interactive_data_last_send = ros::Time::now(); send_map_sentry_data_ = false; ROS_INFO_THROTTLE(1.0, " send map sentry data"); } @@ -218,9 +218,8 @@ void RefereeBase::sendSerialDataCallback() queueSend(); if (base_.robot_id_ == 0) - { ROS_WARN_THROTTLE(1.0, "robot base id = 0, the serial or referee system may not be connected"); - } + if (base_.client_id_ == 0) ROS_WARN_THROTTLE(1.0, "client base id = 0, the serial or referee system may not be connected\""); send_serial_data_timer_.start(); From ec6bf8313b935eb943f6fab9048adf422dbe9bb6 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 24 Oct 2023 15:59:45 +0800 Subject: [PATCH 25/29] Optimize the code. --- rm_referee/include/rm_referee/referee_base.h | 6 ++--- rm_referee/src/referee_base.cpp | 28 ++++++++++---------- 2 files changed, 17 insertions(+), 17 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 034aebf6..f00b722e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -101,9 +101,9 @@ class RefereeBase std::deque graph_queue_; std::deque character_queue_; - rm_referee::ClientMapReceiveData radar_receive_data; - rm_referee::MapSentryData map_sentry_data; - ros::Time interactive_data_last_send; + rm_referee::ClientMapReceiveData radar_receive_data_; + rm_referee::MapSentryData map_sentry_data_; + ros::Time interactive_data_last_send_; bool send_radar_receive_data_ = false, send_map_sentry_data_ = false; UiBase* interactive_data_sender_{}; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 6072fbf5..859138ed 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -195,19 +195,19 @@ void RefereeBase::sendSerialDataCallback() if (send_radar_receive_data_) { - if (ros::Time::now() - interactive_data_last_send <= ros::Duration(0.2)) + if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); - interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); - interactive_data_last_send = ros::Time::now(); + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); + interactive_data_last_send_ = ros::Time::now(); send_radar_receive_data_ = false; ROS_INFO_THROTTLE(1.0, " send radar receive data"); } else if (send_map_sentry_data_) { - if (ros::Time::now() - interactive_data_last_send <= ros::Duration(0.2)) + if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); - interactive_data_sender_->sendMapSentryData(map_sentry_data); - interactive_data_last_send = ros::Time::now(); + interactive_data_sender_->sendMapSentryData(map_sentry_data_); + interactive_data_last_send_ = ros::Time::now(); send_map_sentry_data_ = false; ROS_INFO_THROTTLE(1.0, " send map sentry data"); } @@ -391,21 +391,21 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data } void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { - radar_receive_data.target_position_x = data->target_position_x; - radar_receive_data.target_position_y = data->target_position_y; - radar_receive_data.target_robot_ID = data->target_robot_ID; + radar_receive_data_.target_position_x = data->target_position_x; + radar_receive_data_.target_position_y = data->target_position_y; + radar_receive_data_.target_robot_ID = data->target_robot_ID; send_radar_receive_data_ = true; } void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) { - map_sentry_data.intention = data->intention; - map_sentry_data.start_position_x = data->start_position_x; - map_sentry_data.start_position_y = data->start_position_y; + map_sentry_data_.intention = data->intention; + map_sentry_data_.start_position_x = data->start_position_x; + map_sentry_data_.start_position_y = data->start_position_y; for (int i = 0; i < 49; i++) { - map_sentry_data.delta_x[i] = data->delta_x[i]; - map_sentry_data.delta_y[i] = data->delta_y[i]; + map_sentry_data_.delta_x[i] = data->delta_x[i]; + map_sentry_data_.delta_y[i] = data->delta_y[i]; } send_map_sentry_data_ = true; } From bfc6d7146e5fcfd169368b11817e4f43dff2ba57 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 26 Nov 2023 20:48:17 +0800 Subject: [PATCH 26/29] Optimize the code. --- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/src/referee_base.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index f00b722e..2ac49f5b 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -53,7 +53,7 @@ class RefereeBase // send ui void sendSerialDataCallback(); - void queueSend(); + void sendQueue(); ros::Subscriber joint_state_sub_; ros::Subscriber actuator_state_sub_; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 859138ed..c365013b 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -212,10 +212,10 @@ void RefereeBase::sendSerialDataCallback() ROS_INFO_THROTTLE(1.0, " send map sentry data"); } else - queueSend(); + sendQueue(); } else - queueSend(); + sendQueue(); if (base_.robot_id_ == 0) ROS_WARN_THROTTLE(1.0, "robot base id = 0, the serial or referee system may not be connected"); @@ -225,7 +225,7 @@ void RefereeBase::sendSerialDataCallback() send_serial_data_timer_.start(); } -void RefereeBase::queueSend() +void RefereeBase::sendQueue() { if (!character_queue_.empty() && graph_queue_.size() <= 14) { From 2b3c02ab08b11b28ec0d27abe08ba81a370493e2 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Sun, 26 Nov 2023 22:21:57 +0800 Subject: [PATCH 27/29] Modify code for engineer ui. --- rm_referee/include/rm_referee/ui/ui_base.h | 4 ++-- rm_referee/src/referee.cpp | 12 ------------ rm_referee/src/ui/ui_base.cpp | 4 ++-- 3 files changed, 4 insertions(+), 16 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index d5ad66fa..0296faf8 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -42,8 +42,8 @@ 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(rm_referee::ClientMapReceiveData& data); - void sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data); + void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); + void sendMapSentryData(const rm_referee::MapSentryData& data); void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); void sendSerial(const ros::Time& time, int data_len); diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index cd1662e2..e99daff5 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -473,18 +473,6 @@ int Referee::unpack(uint8_t* rx_data) client_map_send_data_pub_.publish(client_map_send_data); break; } - /* - m_msgs::GameRobotPos game_robot_pos_data; - memcpy(&game_robot_pos_ref, rx_data + 7, sizeof(rm_referee::GameRobotPos)); - -game_robot_pos_data.x = game_robot_pos_ref.x; -game_robot_pos_data.y = game_robot_pos_ref.y; -game_robot_pos_data.z = game_robot_pos_ref.z; -game_robot_pos_data.yaw = game_robot_pos_ref.yaw; - -game_robot_pos_pub_.publish(game_robot_pos_data); - - */ case rm_referee::POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD: { rm_msgs::PowerManagementSampleAndStatusData sample_and_status_pub_data; diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 2e6bacd9..ec3af275 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -192,7 +192,7 @@ void UiBase::sendUi(const ros::Time& time) sendSingleGraph(time, graph_); } -void UiBase::sendMapSentryData(rm_referee::MapSentryData& data) +void UiBase::sendMapSentryData(const rm_referee::MapSentryData& data) { uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; @@ -221,7 +221,7 @@ void UiBase::sendMapSentryData(rm_referee::MapSentryData& data) clearTxBuffer(); } -void UiBase::sendRadarInteractiveData(rm_referee::ClientMapReceiveData& data) +void UiBase::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) { uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; auto radar_interactive_data = (rm_referee::ClientMapReceiveData*)tx_data; From 311595b7b47074b296231636f1e48c0db5351ebe Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 27 Nov 2023 18:42:39 +0800 Subject: [PATCH 28/29] Modify code for engineer ui. --- rm_referee/src/referee_base.cpp | 25 ++++++++++++++++--------- 1 file changed, 16 insertions(+), 9 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 84a29f2d..1362f0db 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -35,7 +35,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::radar_receive_sub_ = nh.subscribe("/rm_radar", 10, &RefereeBase::radarReceiveCallback, this); RefereeBase::sentry_deviate_sub_ = - nh.subscribe("/deviate", 10, &RefereeBase::sentryDeviateCallback, this); + nh.subscribe("/odometry", 10, &RefereeBase::sentryDeviateCallback, this); RefereeBase::radar_to_sentry_sub_ = nh.subscribe( "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); @@ -202,19 +202,25 @@ void RefereeBase::sendSerialDataCallback() { if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); - interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); - interactive_data_last_send_ = ros::Time::now(); - send_radar_receive_data_ = false; - ROS_INFO_THROTTLE(1.0, " send radar receive data"); + else + { + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); + interactive_data_last_send_ = ros::Time::now(); + send_radar_receive_data_ = false; + ROS_INFO_THROTTLE(1.0, " send radar receive data"); + } } else if (send_map_sentry_data_) { if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); - interactive_data_sender_->sendMapSentryData(map_sentry_data_); - interactive_data_last_send_ = ros::Time::now(); - send_map_sentry_data_ = false; - ROS_INFO_THROTTLE(1.0, " send map sentry data"); + else + { + interactive_data_sender_->sendMapSentryData(map_sentry_data_); + interactive_data_last_send_ = ros::Time::now(); + send_map_sentry_data_ = false; + ROS_INFO_THROTTLE(1.0, " send map sentry data"); + } } else sendQueue(); @@ -397,6 +403,7 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data void RefereeBase::sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data) { } + void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { radar_receive_data_.target_position_x = data->target_position_x; From 5e62fa5fc4919024e2052c11a325c2cda919083c Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 14 Dec 2023 17:59:42 +0800 Subject: [PATCH 29/29] Fixed some bug. --- rm_referee/src/referee_base.cpp | 13 ++++--------- rm_referee/src/ui/time_change_ui.cpp | 4 ++-- rm_referee/src/ui/trigger_change_ui.cpp | 16 ++++++++-------- 3 files changed, 14 insertions(+), 19 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 1362f0db..a47f2ae5 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -175,10 +175,7 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { if (graph_queue_.empty() && character_queue_.empty()) - { - ROS_INFO_THROTTLE(1.0, "No UI to send"); return; - } if (!is_adding_) { @@ -201,25 +198,23 @@ void RefereeBase::sendSerialDataCallback() if (send_radar_receive_data_) { if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) - ROS_WARN_THROTTLE(2.0, "Sending radar receive data too frequently"); + return; else { interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); interactive_data_last_send_ = ros::Time::now(); send_radar_receive_data_ = false; - ROS_INFO_THROTTLE(1.0, " send radar receive data"); } } else if (send_map_sentry_data_) { if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) - ROS_WARN_THROTTLE(2.0, "Sending map sentry data too frequently"); + return; else { interactive_data_sender_->sendMapSentryData(map_sentry_data_); interactive_data_last_send_ = ros::Time::now(); send_map_sentry_data_ = false; - ROS_INFO_THROTTLE(1.0, " send map sentry data"); } } else @@ -366,8 +361,8 @@ 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 (progress_time_change_ui_ && !is_adding_) + progress_time_change_ui_->updateEngineerUiData(data, ros::Time::now());*/ } void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data) { diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index a1533c62..30360ff7 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -122,8 +122,8 @@ void ProgressTimeChangeUi::updateConfig() void ProgressTimeChangeUi::updateEngineerUiData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time) { - total_steps_ = data->total_steps; - finished_data_ = data->finished_step; + /*total_steps_ = data->total_steps; + finished_data_ = data->finished_step;*/ TimeChangeUi::updateForQueue(); } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 8811a9c8..0e9feb87 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -90,7 +90,7 @@ void ChassisTriggerChangeUi::update() power_limit_state_ == rm_common::PowerLimit::CHARGE); graph_->setOperation(rm_referee::GraphOperation::UPDATE); checkModeChange(); - updateTwiceForQueue(); + updateTwiceForQueue(true); } void ChassisTriggerChangeUi::displayInCapacity() @@ -100,7 +100,7 @@ void ChassisTriggerChangeUi::displayInCapacity() updateConfig(254, 0); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateTwiceForQueue(); + updateTwiceForQueue(true); } void ChassisTriggerChangeUi::checkModeChange() @@ -227,7 +227,7 @@ void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(); + updateForQueue(true); } void ShooterTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -271,7 +271,7 @@ void GimbalTriggerChangeUi::update() updateConfig(gimbal_mode_, gimbal_eject_); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateTwiceForQueue(); + updateTwiceForQueue(true); } void GimbalTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -314,7 +314,7 @@ void TargetTriggerChangeUi::update() else updateConfig(gimbal_eject_, shoot_frequency_, det_armor_target_, det_color_ == rm_msgs::StatusChangeRequest::RED); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(); + updateForQueue(true); } void TargetTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -375,7 +375,7 @@ void TargetViewAngleTriggerChangeUi::update() { updateConfig(track_id_ == 0, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateTwiceForQueue(); + updateTwiceForQueue(true); } void TargetViewAngleTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -396,7 +396,7 @@ void PolygonTriggerChangeGroupUi::update() { for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(); + updateForQueue(true); } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) @@ -420,6 +420,6 @@ void CameraTriggerChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(); + updateForQueue(true); } } // namespace rm_referee