diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 0fd0fcd7..b6799a69 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -53,8 +53,9 @@ class RefereeBase virtual void sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data); virtual void sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data); - // send graph_type ui - void sendGraphQueueCallback(); + // send ui + void sendSerialDataCallback(); + void sendQueue(); ros::Subscriber joint_state_sub_; ros::Subscriber actuator_state_sub_; @@ -101,12 +102,17 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; GroupUiBase* graph_queue_sender_{}; - std::vector graph_queue_; + 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_; + 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 016bb5c4..7be7495e 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -12,32 +12,36 @@ class FlashUi : public UiBase { public: explicit FlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_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_++); } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; + void updateFlashUiForQueue(const ros::Time& time, bool state, bool once); }; class CoverFlashUi : public FlashUi { public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : FlashUi(rpc_value, base, "cover", graph_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; private: void display(const ros::Time& time) override; + uint8_t cover_state_; }; class SpinFlashUi : public FlashUi { public: - explicit SpinFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : FlashUi(rpc_value, base, "spin", graph_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); private: diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 5ca2902b..5e93b7c2 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -38,6 +38,8 @@ class Graph void setContent(const std::string& content) { content_ = content; + if (!title_.empty() || !content_.empty()) + config_.end_angle = static_cast((title_ + content_).size()); } void setEndX(int end_x) { @@ -69,10 +71,12 @@ 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_; 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..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,13 +12,13 @@ class TimeChangeUi : public UiBase { public: explicit TimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_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_++); } void update() override; - void updateForQueue(); + void updateForQueue() override; virtual void updateConfig(){}; }; @@ -26,13 +26,13 @@ class TimeChangeGroupUi : public GroupUiBase { public: explicit TimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) - : GroupUiBase(rpc_value, base, graph_queue) + std::deque* graph_queue, std::deque* character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { graph_name_ = graph_name; } void update() override; - void updateForQueue(); + void updateForQueue() override; virtual void updateConfig(){}; protected: @@ -42,8 +42,9 @@ class TimeChangeGroupUi : public GroupUiBase class CapacitorTimeChangeUi : public TimeChangeUi { public: - explicit CapacitorTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "capacitor", graph_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,8 +56,9 @@ class CapacitorTimeChangeUi : public TimeChangeUi class EffortTimeChangeUi : public TimeChangeUi { public: - explicit EffortTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "effort", graph_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); private: @@ -68,8 +70,9 @@ class EffortTimeChangeUi : public TimeChangeUi class ProgressTimeChangeUi : public TimeChangeUi { public: - explicit ProgressTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "progress", graph_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); private: @@ -81,8 +84,9 @@ class ProgressTimeChangeUi : public TimeChangeUi class DartStatusTimeChangeUi : public TimeChangeUi { public: - explicit DartStatusTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "dart", graph_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); private: @@ -93,8 +97,9 @@ class DartStatusTimeChangeUi : public TimeChangeUi class RotationTimeChangeUi : public TimeChangeUi { public: - explicit RotationTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "rotation", graph_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,8 +130,9 @@ class RotationTimeChangeUi : public TimeChangeUi class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit LaneLineTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeGroupUi(rpc_value, base, "lane_line", graph_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,8 +177,9 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit BalancePitchTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeGroupUi(rpc_value, base, "balance_pitch", graph_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,9 +239,9 @@ class BalancePitchTimeChangeGroupUi : public TimeChangeGroupUi class PitchAngleTimeChangeUi : public TimeChangeUi { public: - explicit PitchAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TimeChangeUi(rpc_value, base, "pitch", graph_queue){}; - void update() override; + 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); private: @@ -245,9 +252,9 @@ class PitchAngleTimeChangeUi : public TimeChangeUi class JointPositionTimeChangeUi : public TimeChangeUi { public: - explicit JointPositionTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue, - std::string name) - : TimeChangeUi(rpc_value, base, name, graph_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 82abac28..92875904 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -13,22 +13,44 @@ class TriggerChangeUi : public UiBase { public: explicit TriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, - std::vector* graph_queue) - : UiBase(rpc_value, base, graph_queue) + std::deque* graph_queue, std::deque* character_queue) + : UiBase(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() override; + void updateForQueue(bool check_repeat); + 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::deque* graph_queue, std::deque* character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) + { + 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() override; + void updateForQueue(bool check_repeat); + 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::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "chassis", graph_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) graph_->setContent("raw"); @@ -40,9 +62,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(); @@ -58,8 +80,9 @@ class ChassisTriggerChangeUi : public TriggerChangeUi class ShooterTriggerChangeUi : public TriggerChangeUi { public: - explicit ShooterTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "shooter", graph_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"); } @@ -76,12 +99,13 @@ class ShooterTriggerChangeUi : public TriggerChangeUi class GimbalTriggerChangeUi : public TriggerChangeUi { public: - explicit GimbalTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "gimbal", graph_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"); } - 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: @@ -94,8 +118,9 @@ class GimbalTriggerChangeUi : public TriggerChangeUi class TargetTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "target", graph_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"); if (base_.robot_color_ == "red") @@ -116,8 +141,9 @@ class TargetTriggerChangeUi : public TriggerChangeUi class TargetViewAngleTriggerChangeUi : public TriggerChangeUi { public: - explicit TargetViewAngleTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "target_scale", graph_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) { } void updateTrackID(int id); @@ -128,10 +154,12 @@ 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::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeGroupUi(rpc_value, base, "Polygon", graph_queue, character_queue) { ROS_ASSERT(rpc_value.hasMember("points")); XmlRpc::XmlRpcValue config; @@ -171,14 +199,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::vector* graph_queue) - : TriggerChangeUi(rpc_value, base, "camera", graph_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 dc60a1a5..0296faf8 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,17 +19,20 @@ 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::deque* graph_queue = nullptr, + std::deque* 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; }; ~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){}; @@ -38,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); @@ -58,7 +62,8 @@ class UiBase Base& base_; Graph* graph_; static int id_; - std::vector* graph_queue_; + std::deque* graph_queue_; + std::deque* character_queue_; tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -70,11 +75,13 @@ class UiBase class GroupUiBase : public UiBase { public: - explicit GroupUiBase(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) - : UiBase(rpc_value, base, graph_queue){}; + 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; void update() override; + void updateForQueue() override; void erasure() override; void addForQueue(int add_times = 1) override; void sendUi(const ros::Time& time) override; @@ -94,13 +101,16 @@ class GroupUiBase : public UiBase class FixedUi : public GroupUiBase { public: - explicit FixedUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::vector* graph_queue = nullptr) - : GroupUiBase(rpc_value, base, graph_queue) + 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++) graph_vector_.insert( std::pair(rpc_value[i]["name"], new Graph(rpc_value[i]["config"], base_, id_++))); }; + void updateForQueue() override; + int update_fixed_ui_times = 0; }; } // namespace rm_referee 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/referee_base.cpp b/rm_referee/src/referee_base.cpp index 2b1868f9..a47f2ae5 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); @@ -52,75 +52,83 @@ 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); - 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++) { 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_); } } 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() { 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_back(); + { + graph_queue_.clear(); + ROS_WARN_THROTTLE(0.5, "Some UI is not add completely, now clear the queue"); + } 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; } @@ -164,65 +172,104 @@ void RefereeBase::addUi() add_ui_times_++; } -void RefereeBase::sendGraphQueueCallback() +void RefereeBase::sendSerialDataCallback() { - 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 " - "reduce the frequency"); - while (graph_queue_.size() > 50) - graph_queue_.pop_back(); - } - - int index = graph_queue_.size() - 1; - if (!is_adding_) { - if (graph_queue_.size() >= 7) + if (graph_queue_.size() > 50) { - 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(); + 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(); } - else if (graph_queue_.size() >= 5) + + if (character_queue_.size() > 8) { - 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(); + 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(); } - else if (graph_queue_.size() >= 2) + + if (send_radar_receive_data_) { - 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(); + if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) + return; + else + { + interactive_data_sender_->sendRadarInteractiveData(radar_receive_data_); + interactive_data_last_send_ = ros::Time::now(); + send_radar_receive_data_ = false; + } } - else if (graph_queue_.size() == 1) + else if (send_map_sentry_data_) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); + if (ros::Time::now() - interactive_data_last_send_ <= ros::Duration(0.2)) + return; + else + { + interactive_data_sender_->sendMapSentryData(map_sentry_data_); + interactive_data_last_send_ = ros::Time::now(); + send_map_sentry_data_ = false; + } } + else + sendQueue(); } else + sendQueue(); + + 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(); +} + +void RefereeBase::sendQueue() +{ + if (!character_queue_.empty() && graph_queue_.size() <= 14) { - graph_queue_sender_->sendSingleGraph(ros::Time::now(), &graph_queue_.at(index)); - graph_queue_.pop_back(); + 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(); } - - send_graph_ui_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) { @@ -274,9 +321,8 @@ 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_back(); - send_graph_ui_timer_.setPeriod(ros::Duration(0.05)); + graph_queue_.clear(); + send_serial_data_timer_.setPeriod(ros::Duration(0.05)); add_ui_timer_.start(); add_ui_times_ = 0; } @@ -315,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) { @@ -352,18 +398,26 @@ 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) { - 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; } void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data) diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index ad897bab..3b8421b8 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -6,11 +6,33 @@ namespace rm_referee { +void FlashUi::updateFlashUiForQueue(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(); + UiBase::updateForQueue(); +} + void CoverFlashUi::display(const ros::Time& time) { if (!cover_state_) graph_->setOperation(rm_referee::GraphOperation::DELETE); - UiBase::display(time, cover_state_, true); + FlashUi::updateFlashUiForQueue(time, cover_state_, true); } void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, @@ -24,7 +46,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::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) diff --git a/rm_referee/src/ui/graph.cpp b/rm_referee/src/ui/graph.cpp index 14a3acab..1a6061f6 100644 --- a/rm_referee/src/ui/graph.cpp +++ b/rm_referee/src/ui/graph.cpp @@ -12,7 +12,9 @@ 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")) { if (config.hasMember("size")) @@ -56,11 +58,12 @@ Graph::Graph(const XmlRpc::XmlRpcValue& config, Base& base, int id) : base_(base 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 98f80c55..30360ff7 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -28,9 +28,9 @@ void TimeChangeUi::updateForQueue() updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - if (graph_queue_ && !graph_->isRepeated() && ros::Time::now() - last_send_ > delay_) + if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) { - graph_queue_->push_back(*graph_); + UiBase::updateForQueue(); last_send_ = ros::Time::now(); } } @@ -38,16 +38,11 @@ void TimeChangeUi::updateForQueue() void TimeChangeGroupUi::updateForQueue() { updateConfig(); - 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_back(*graph.second); - last_send_ = ros::Time::now(); - } - } + if (graph_queue_ && character_queue_ && ros::Time::now() - last_send_ > delay_) + { + GroupUiBase::updateForQueue(); + last_send_ = ros::Time::now(); + } } void CapacitorTimeChangeUi::add() @@ -109,7 +104,7 @@ void EffortTimeChangeUi::updateJointStateData(const sensor_msgs::JointState::Con { joint_effort_ = data->effort[max_index]; joint_name_ = data->name[max_index]; - TimeChangeUi::update(); + updateForQueue(); } } } @@ -127,9 +122,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; - finished_data_ = data->finished_step; - TimeChangeUi::update(); + /*total_steps_ = data->total_steps; + finished_data_ = data->finished_step;*/ + TimeChangeUi::updateForQueue(); } void DartStatusTimeChangeUi::updateConfig() @@ -157,7 +152,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() @@ -277,14 +272,7 @@ 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() -{ - updateConfig(); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(ros::Time::now()); + updateForQueue(); } void PitchAngleTimeChangeUi::updateConfig() diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index f0355b2a..0e9feb87 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -6,10 +6,79 @@ namespace rm_referee { -void TriggerChangeUi::setContent(const std::string& content) +void TriggerChangeUi::updateForQueue() { - graph_->setContent(content); - display(); + graph_->updateLastConfig(); + UiBase::updateForQueue(); +} + +void TriggerChangeUi::updateForQueue(bool check_repeat) +{ + if (check_repeat) + if (graph_->isRepeated()) + return; + 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(); +} + +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) +{ + 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; + } + TriggerChangeGroupUi::updateForQueue(); +} + +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(); + + for (int i = 0; i < 2; i++) + GroupUiBase::updateForQueue(); } void ChassisTriggerChangeUi::update() @@ -21,7 +90,7 @@ void ChassisTriggerChangeUi::update() power_limit_state_ == rm_common::PowerLimit::CHARGE); graph_->setOperation(rm_referee::GraphOperation::UPDATE); checkModeChange(); - displayTwice(); + updateTwiceForQueue(true); } void ChassisTriggerChangeUi::displayInCapacity() @@ -31,7 +100,7 @@ void ChassisTriggerChangeUi::displayInCapacity() updateConfig(254, 0); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(true); } void ChassisTriggerChangeUi::checkModeChange() @@ -54,7 +123,7 @@ void ChassisTriggerChangeUi::checkModeChange() else if ((ros::Time::now() - trigger_time).toSec() > mode_change_threshold_) { is_different = false; - display(false); + updateForQueue(false); } } } @@ -129,7 +198,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(); @@ -140,7 +209,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; @@ -158,7 +227,7 @@ void ShooterTriggerChangeUi::update() { updateConfig(shooter_mode_, 0, shoot_frequency_, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - TriggerChangeUi::display(); + updateForQueue(true); } void ShooterTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -202,7 +271,7 @@ void GimbalTriggerChangeUi::update() updateConfig(gimbal_mode_, gimbal_eject_); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(true); } void GimbalTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -226,7 +295,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(); @@ -245,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); - TriggerChangeUi::display(); + updateForQueue(true); } void TargetTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -306,7 +375,7 @@ void TargetViewAngleTriggerChangeUi::update() { updateConfig(track_id_ == 0, false); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - displayTwice(); + updateTwiceForQueue(true); } void TargetViewAngleTriggerChangeUi::updateConfig(uint8_t main_mode, bool main_flag, uint8_t sub_mode, bool sub_flag) @@ -327,7 +396,7 @@ void PolygonTriggerChangeGroupUi::update() { for (auto graph : graph_vector_) graph.second->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(true); } void CameraTriggerChangeUi::updateCameraName(const std_msgs::StringConstPtr& data) @@ -351,6 +420,6 @@ void CameraTriggerChangeUi::update() { updateConfig(); graph_->setOperation(rm_referee::GraphOperation::UPDATE); - display(); + updateForQueue(true); } } // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 25ea95f4..ec3af275 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) @@ -165,27 +186,26 @@ 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_); } -void UiBase::sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& 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; 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_); @@ -201,7 +221,7 @@ void UiBase::sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& 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; @@ -374,9 +394,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; @@ -384,6 +405,22 @@ void GroupUiBase::sendSevenGraph(const ros::Time& time, Graph* graph0, Graph* gr sendSerial(time, data_len); } +void FixedUi::updateForQueue() +{ + while (update_fixed_ui_times < 1) + { + for (auto it : graph_vector_) + it.second->updateLastConfig(); + + if (base_.robot_id_ == 0 || base_.client_id_ == 0) + return; + + GroupUiBase::updateForQueue(); + 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 { memset(tx_buffer, 0, k_frame_length_);