diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 4a67e255..0dccd5df 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -68,7 +68,7 @@ add_message_files( SupplyProjectileAction.msg DartRemainingTime.msg RobotHurt.msg - CurrentSentryPosData.msg + SentryAttackingTarget.msg ShootData.msg BulletAllowance.msg RfidStatus.msg diff --git a/rm_msgs/msg/referee/CurrentSentryPosData.msg b/rm_msgs/msg/referee/CurrentSentryPosData.msg deleted file mode 100644 index 9354c5f7..00000000 --- a/rm_msgs/msg/referee/CurrentSentryPosData.msg +++ /dev/null @@ -1,4 +0,0 @@ -float32 x -float32 y -float32 z -float32 yaw diff --git a/rm_msgs/msg/referee/EngineerUi.msg b/rm_msgs/msg/referee/EngineerUi.msg index 277d30e8..0407eb26 100644 --- a/rm_msgs/msg/referee/EngineerUi.msg +++ b/rm_msgs/msg/referee/EngineerUi.msg @@ -4,5 +4,5 @@ string ex_arm_state uint32 stone_num string step_queue_name -bool symbol +uint32 symbol time stamp diff --git a/rm_msgs/msg/referee/EventData.msg b/rm_msgs/msg/referee/EventData.msg index fe1b9bb8..dc39fcba 100644 --- a/rm_msgs/msg/referee/EventData.msg +++ b/rm_msgs/msg/referee/EventData.msg @@ -1,3 +1,15 @@ -uint32 event_data +bool forward_supply_station_state +bool inside_supply_station_state +bool supplier_zone_state +bool power_rune_activation_point_state +bool small_power_rune_state +bool large_power_rune_state +uint8 ring_elevated_ground_state +uint8 r3_state +uint8 r4_state +uint8 base_shield_value +uint16 be_hit_time +uint8 be_hit_target +uint8 central_point_state time stamp diff --git a/rm_msgs/msg/referee/RfidStatus.msg b/rm_msgs/msg/referee/RfidStatus.msg index 657fb12a..b37bc4b6 100644 --- a/rm_msgs/msg/referee/RfidStatus.msg +++ b/rm_msgs/msg/referee/RfidStatus.msg @@ -1,3 +1,23 @@ -uint32 rfid_status +bool base_buff_point_state +bool own_ring_elevated_ground_state +bool enemy_ring_elevated_ground_state +bool own_r3_state +bool enemy_r3_state +bool own_r4_state +bool enemy_r4_state +bool power_rune_activation_point_state +bool forward_own_launch_ramp_buff_point_state +bool behind_own_launch_ramp_buff_point_state +bool forward_enemy_launch_ramp_buff_point_state +bool behind_enemy_launch_ramp_buff_point_state +bool own_outpost_buff_point +bool own_side_restoration_zone +bool own_sentry_patrol_zones +bool enemy_sentry_patrol_zones +bool own_large_resource_island_point +bool enemy_large_resource_island_point +bool own_exchange_zone +bool central_buff_point +uint32 reverse time stamp diff --git a/rm_msgs/msg/referee/SentryAttackingTarget.msg b/rm_msgs/msg/referee/SentryAttackingTarget.msg new file mode 100644 index 00000000..abc0f29f --- /dev/null +++ b/rm_msgs/msg/referee/SentryAttackingTarget.msg @@ -0,0 +1,3 @@ +uint8 target_robot_ID +float32 target_position_x +float32 target_position_y diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index e37e21a2..ad3403cb 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -81,13 +81,12 @@ #include #include #include -#include -#include #include #include "rm_msgs/SentryInfo.h" #include "rm_msgs/RadarInfo.h" #include "rm_msgs/Buff.h" #include "rm_msgs/TrackData.h" +#include "rm_msgs/SentryAttackingTarget.h" #include #include #include diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 5c7ecbbe..deed1d9b 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -94,7 +94,9 @@ typedef enum CLIENT_CHARACTER_CMD = 0x0110, SENTRY_CMD = 0x0120, RADAR_CMD = 0x0121, - CURRENT_SENTRY_POSITION_CMD = 0x0200 // send radar->sentry + BULLET_NUM_SHARE_CMD = 0x0200, // send robot->aerial + SENTRY_TO_RADAR_CMD = 0x0201 // send sentry->radar + // send radar->sentry } DataCmdId; typedef enum @@ -282,7 +284,19 @@ typedef struct typedef struct { - uint32_t event_type; + uint8_t forward_supply_station_state : 1; + uint8_t inside_supply_station_state : 1; + uint8_t supplier_zone_state : 1; + uint8_t power_rune_activation_point_state : 1; + uint8_t small_power_rune_state : 1; + uint8_t large_power_rune_state : 1; + uint8_t ring_elevated_ground_state : 2; + uint8_t r3_state : 2; + uint8_t r4_state : 2; + uint8_t base_shield_value : 7; + uint16_t be_hit_time : 9; + uint8_t be_hit_target : 2; + uint8_t central_point_state : 2; } __packed EventData; typedef struct @@ -371,7 +385,27 @@ typedef struct typedef struct { - uint32_t rfid_status; + uint8_t base_buff_point_state : 1; + uint8_t own_ring_elevated_ground_state : 1; + uint8_t enemy_ring_elevated_ground_state : 1; + uint8_t own_r3_state : 1; + uint8_t enemy_r3_state : 1; + uint8_t own_r4_state : 1; + uint8_t enemy_r4_state : 1; + uint8_t power_rune_activation_point_state : 1; + uint8_t forward_own_launch_ramp_buff_point_state : 1; + uint8_t behind_own_launch_ramp_buff_point_state : 1; + uint8_t forward_enemy_launch_ramp_buff_point_state : 1; + uint8_t behind_enemy_launch_ramp_buff_point_state : 1; + uint8_t own_outpost_buff_point : 1; + uint8_t own_side_restoration_zone : 1; + uint8_t own_sentry_patrol_zones : 1; + uint8_t enemy_sentry_patrol_zones : 1; + uint8_t own_large_resource_island_point : 1; + uint8_t enemy_large_resource_island_point : 1; + uint8_t own_exchange_zone : 1; + uint8_t central_buff_point : 1; + uint32_t reverse : 12; } __packed RfidStatus; typedef struct @@ -535,6 +569,14 @@ typedef struct float target_position_y; } __packed ClientMapReceiveData; +typedef struct +{ + InteractiveDataHeader header_data; + uint8_t target_robot_ID; + float target_position_x; + float target_position_y; +} __packed SentryAttackingTargetData; + typedef struct { int16_t mouse_x; @@ -559,11 +601,9 @@ typedef struct typedef struct { InteractiveDataHeader header_data; - float position_x; - float position_y; - float position_z; - float position_yaw; -} __packed CurrentSentryPosData; + uint8_t bullet_42_mm_num; + uint8_t bullet_17_mm_num; +} __packed BulletNumData; typedef struct { diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index 1895bf04..800fad9b 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -55,7 +55,6 @@ class Referee game_status_pub_ = nh.advertise("game_status", 1); power_heat_data_pub_ = nh.advertise("power_heat_data", 1); game_robot_hp_pub_ = nh.advertise("game_robot_hp", 1); - current_sentry_pos_pub_ = nh.advertise("current_sentry_pos", 1); buff_pub_ = nh.advertise("robot_buff", 1); event_data_pub_ = nh.advertise("event_data", 1); dart_status_pub_ = nh.advertise("dart_status_data", 1); @@ -75,6 +74,7 @@ class Referee game_robot_pos_pub_ = nh.advertise("game_robot_pos", 1); sentry_info_pub_ = nh.advertise("sentry_info", 1); radar_info_pub_ = nh.advertise("radar_info", 1); + sentry_to_radar_pub_ = nh.advertise("sentry_target_to_radar", 1); ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management"); power_management_sample_and_status_data_pub_ = @@ -102,7 +102,7 @@ class Referee ros::Publisher game_status_pub_; ros::Publisher power_heat_data_pub_; ros::Publisher game_robot_hp_pub_; - ros::Publisher current_sentry_pos_pub_; + ros::Publisher sentry_to_radar_pub_; ros::Publisher event_data_pub_; ros::Publisher dart_status_pub_; ros::Publisher buff_pub_; diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index f174c9cd..7372e910 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -12,6 +12,7 @@ #include "rm_referee/ui/trigger_change_ui.h" #include "rm_referee/ui/time_change_ui.h" #include "rm_referee/ui/flash_ui.h" +#include "rm_referee/ui/interactive_data.h" namespace rm_referee { @@ -38,6 +39,7 @@ class RefereeBase virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data); virtual void supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data); virtual void updateShootDataDataCallBack(const rm_msgs::ShootData& msg); + virtual void updateBulletRemainData(const rm_referee::BulletNumData& data); // sub call back virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state); @@ -56,12 +58,12 @@ class RefereeBase virtual void balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data); virtual void radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data); virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); - virtual void sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data); - virtual void sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data); + virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data); virtual void sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data); virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data); virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); + virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); // send ui void sendSerialDataCallback(); @@ -85,11 +87,13 @@ class RefereeBase ros::Subscriber balance_state_sub_; ros::Subscriber radar_receive_sub_; ros::Subscriber map_sentry_sub_; + ros::Subscriber sentry_to_radar_sub_; ros::Subscriber radar_to_sentry_sub_; ros::Subscriber sentry_cmd_sub_; ros::Subscriber radar_cmd_sub_; ros::Subscriber sentry_state_sub_; ros::Subscriber drone_pose_sub_; + ros::Subscriber shoot_cmd_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; @@ -97,6 +101,7 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; + FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{}; BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; @@ -110,6 +115,7 @@ class RefereeBase JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{}, *engineer_joint3_time_change_ui{}; TargetDistanceTimeChangeUi* target_distance_time_change_ui_{}; + FriendBulletsTimeChangeGroupUi* friend_bullets_time_change_group_ui_{}; DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{}; StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, @@ -121,14 +127,18 @@ class RefereeBase SpinFlashUi* spin_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; + EngineerActionFlashUi* engineer_action_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; - InteractiveSender* enemy_hero_state_sender_{}; - InteractiveSender* sentry_state_sender_{}; + CustomInfoSender* enemy_hero_state_sender_{}; + CustomInfoSender* sentry_state_sender_{}; + BulletNumShare* bullet_num_share_{}; + SentryToRadar* sentry_to_radar_{}; GroupUiBase* graph_queue_sender_{}; std::deque graph_queue_; std::deque character_queue_; + // std::deque> interactive_data_queue_; ros::Time radar_interactive_data_last_send_; ros::Time sentry_interactive_data_last_send_; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 98603ec3..ce3937f1 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -22,6 +22,47 @@ class FlashUi : public UiBase void updateFlashUiForQueue(const ros::Time& time, bool state, bool once); }; +class FlashGroupUi : public GroupUiBase +{ +public: + explicit FlashGroupUi(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 display(const ros::Time& time){}; + virtual void updateConfig(){}; + void updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph); + +protected: + std::string graph_name_; +}; + +class EngineerActionFlashUi : public FlashGroupUi +{ +public: + explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue) + { + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + for (int i = 0; i < static_cast(rpc_value["data"].size()); i++) + { + graph_vector_.insert(std::pair(std::to_string(static_cast(data[i]["flag"])), + new Graph(data[i]["config"], base_, id_++))); + } + } + } + void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); + +private: + void display(const ros::Time& time) override; + uint32_t symbol_; +}; + class CoverFlashUi : public FlashUi { public: diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index eba8cbab..23a9979e 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -53,6 +53,25 @@ class Graph { config_.radius = radius; } + void setIntNum(int num) + { + int a = num & 1023; + int b = (num >> 10) & 2047; + int c = num >> 21; + config_.radius = a; + config_.end_x = b; + config_.end_y = c; + } + void setFloatNum(float data) + { + int num = static_cast(data * 1000); + int a = num & 1023; + int b = (num >> 10) & 2047; + int c = num >> 21; + config_.radius = a; + config_.end_x = b; + config_.end_y = c; + } void setStartX(int start_x) { config_.start_x = start_x; diff --git a/rm_referee/include/rm_referee/ui/interactive_data.h b/rm_referee/include/rm_referee/ui/interactive_data.h new file mode 100644 index 00000000..0a822e14 --- /dev/null +++ b/rm_referee/include/rm_referee/ui/interactive_data.h @@ -0,0 +1,63 @@ +// +// Created by gura on 24-5-29. +// + +#pragma once + +#include "rm_referee/ui/ui_base.h" + +namespace rm_referee +{ +class InteractiveSender : public UiBase +{ +public: + explicit InteractiveSender(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) + : UiBase(rpc_value, base, graph_queue, character_queue){}; + + void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); + void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); + void sendMapSentryData(const rm_referee::MapSentryData& data); + void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data); + void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); + virtual bool needSendInteractiveData(); + ros::Time last_send_time_; +}; + +class CustomInfoSender : public InteractiveSender +{ +public: + explicit CustomInfoSender(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) + : InteractiveSender(rpc_value, base, graph_queue, character_queue){}; + void sendCustomInfoData(std::wstring data); + +protected: + std::wstring last_custom_info_; +}; + +class BulletNumShare : public InteractiveSender +{ +public: + explicit BulletNumShare(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) + : InteractiveSender(rpc_value, base, graph_queue, character_queue){}; + void sendBulletData(); + void updateBulletRemainData(const rm_msgs::BulletAllowance& data); + int bullet_42_mm_num_, bullet_17_mm_num_, count_receive_time_; +}; + +class SentryToRadar : public InteractiveSender +{ +public: + explicit SentryToRadar(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, + std::deque* character_queue = nullptr) + : InteractiveSender(rpc_value, base, graph_queue, character_queue){}; + void updateSentryAttackingTargetData(const rm_msgs::SentryAttackingTargetConstPtr& data); + bool needSendInteractiveData() override; + void sendSentryToRadarData(); + ros::Time last_get_data_time_; + float target_position_x_, target_position_y_, robot_id_; +}; + +} // namespace rm_referee 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 911d9264..7211515a 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -120,11 +120,13 @@ class RotationTimeChangeUi : public TimeChangeUi else ROS_WARN("RotationTimeChangeUi config 's member 'data' not defined."); }; + void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data); private: void updateConfig() override; int arc_scale_; std::string gimbal_reference_frame_, chassis_reference_frame_; + uint8_t chassis_mode_; }; class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi @@ -334,4 +336,34 @@ class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi right_line_y2_; }; +class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi +{ +public: + explicit FriendBulletsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "friend_bullets", graph_queue, character_queue) + { + graph_vector_.insert(std::pair("hero", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard3", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard4", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert(std::pair("standard5", new Graph(rpc_value["config"], base_, id_++))); + int ui_start_y; + for (auto it = graph_vector_.begin(); it != graph_vector_.end(); ++it) + { + if (it == graph_vector_.begin()) + ui_start_y = it->second->getConfig().start_y; + else + { + ui_start_y += 40; + it->second->setStartY(ui_start_y); + } + } + }; + void updateBulletsData(const rm_referee::BulletNumData& data); + +private: + void updateConfig() override; + int hero_bullets_, standard3_bullets_, standard4_bullets_, standard5_bullets_; +}; + } // namespace rm_referee 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 90ae3381..7fd04bfc 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -239,4 +239,17 @@ class StringTriggerChangeUi : public TriggerChangeUi std::string data_; }; +class FrictionSpeedTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit FrictionSpeedTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "friction_speed", graph_queue, character_queue){}; + void updateFrictionSpeedUiData(const rm_msgs::ShootCmdConstPtr& data); + +private: + void update() override; + double wheel_speed_; +}; + } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 2f9fe030..bf51f44c 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -41,7 +41,6 @@ class UiBase void sendCharacter(const ros::Time& time, Graph* graph); void sendSingleGraph(const ros::Time& time, Graph* graph); - void transferInt(const int data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); @@ -52,7 +51,7 @@ class UiBase void display(const ros::Time& time, bool state, bool once = false); void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const; - uint8_t tx_buffer_[128]; + uint8_t tx_buffer_[127]; int tx_len_; protected: @@ -69,25 +68,6 @@ class UiBase const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; }; -class InteractiveSender : public UiBase -{ -public: - explicit InteractiveSender(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue = nullptr, - std::deque* character_queue = nullptr) - : UiBase(rpc_value, base, graph_queue, character_queue){}; - - void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); - void sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data); - void sendMapSentryData(const rm_referee::MapSentryData& data); - void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); - void sendCustomInfoData(std::wstring data); - void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data); - void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); - -protected: - std::wstring last_custom_info_; -}; - class GroupUiBase : public UiBase { public: diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 33ae97c0..2a150638 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -197,7 +197,19 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::EventData event_data; memcpy(&event_ref, rx_data + 7, sizeof(rm_referee::EventData)); - event_data.event_data = event_ref.event_type; + event_data.forward_supply_station_state = event_ref.forward_supply_station_state; + event_data.inside_supply_station_state = event_ref.inside_supply_station_state; + event_data.supplier_zone_state = event_ref.supplier_zone_state; + event_data.power_rune_activation_point_state = event_ref.power_rune_activation_point_state; + event_data.small_power_rune_state = event_ref.small_power_rune_state; + event_data.large_power_rune_state = event_ref.small_power_rune_state; + event_data.ring_elevated_ground_state = event_ref.ring_elevated_ground_state; + event_data.r3_state = event_ref.r3_state; + event_data.r4_state = event_ref.r4_state; + event_data.base_shield_value = event_ref.base_shield_value; + event_data.be_hit_time = event_ref.be_hit_time; + event_data.be_hit_target = event_ref.be_hit_target; + event_data.central_point_state = event_ref.central_point_state; event_data.stamp = last_get_data_time_; event_data_pub_.publish(event_data); @@ -357,7 +369,31 @@ int Referee::unpack(uint8_t* rx_data) rm_msgs::RfidStatus rfid_status_data; memcpy(&rfid_status_ref, rx_data + 7, sizeof(rm_referee::RfidStatus)); - rfid_status_data.rfid_status = rfid_status_ref.rfid_status; + rfid_status_data.base_buff_point_state = rfid_status_ref.base_buff_point_state; + rfid_status_data.own_ring_elevated_ground_state = rfid_status_ref.own_ring_elevated_ground_state; + rfid_status_data.enemy_ring_elevated_ground_state = rfid_status_ref.enemy_ring_elevated_ground_state; + rfid_status_data.own_r3_state = rfid_status_ref.own_r3_state; + rfid_status_data.enemy_r3_state = rfid_status_ref.enemy_r3_state; + rfid_status_data.own_r4_state = rfid_status_ref.own_r4_state; + rfid_status_data.enemy_r4_state = rfid_status_ref.enemy_r4_state; + rfid_status_data.power_rune_activation_point_state = rfid_status_ref.power_rune_activation_point_state; + rfid_status_data.forward_own_launch_ramp_buff_point_state = + rfid_status_ref.forward_own_launch_ramp_buff_point_state; + rfid_status_data.behind_own_launch_ramp_buff_point_state = + rfid_status_ref.behind_own_launch_ramp_buff_point_state; + rfid_status_data.forward_enemy_launch_ramp_buff_point_state = + rfid_status_ref.forward_enemy_launch_ramp_buff_point_state; + rfid_status_data.behind_enemy_launch_ramp_buff_point_state = + rfid_status_ref.behind_enemy_launch_ramp_buff_point_state; + rfid_status_data.own_outpost_buff_point = rfid_status_ref.own_outpost_buff_point; + rfid_status_data.own_side_restoration_zone = rfid_status_ref.own_side_restoration_zone; + rfid_status_data.own_sentry_patrol_zones = rfid_status_ref.own_sentry_patrol_zones; + rfid_status_data.enemy_sentry_patrol_zones = rfid_status_ref.enemy_sentry_patrol_zones; + rfid_status_data.own_large_resource_island_point = rfid_status_ref.own_large_resource_island_point; + rfid_status_data.enemy_large_resource_island_point = rfid_status_ref.enemy_large_resource_island_point; + rfid_status_data.own_exchange_zone = rfid_status_ref.own_exchange_zone; + rfid_status_data.central_buff_point = rfid_status_ref.central_buff_point; + rfid_status_data.reverse = rfid_status_ref.reverse; rfid_status_data.stamp = last_get_data_time_; rfid_status_pub_.publish(rfid_status_data); @@ -425,17 +461,21 @@ int Referee::unpack(uint8_t* rx_data) rm_referee::InteractiveData interactive_data_ref; // local variable temporarily before moving referee data memcpy(&interactive_data_ref, rx_data + 7, sizeof(rm_referee::InteractiveData)); // TODO: case cmd_id - if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::CURRENT_SENTRY_POSITION_CMD) + if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::BULLET_NUM_SHARE_CMD) { - rm_referee::CurrentSentryPosData current_sentry_pos_ref; - rm_msgs::CurrentSentryPosData current_sentry_pos_data; - memcpy(¤t_sentry_pos_ref, rx_data + 7, sizeof(rm_referee::CurrentSentryPosData)); - current_sentry_pos_data.x = current_sentry_pos_ref.position_x; - current_sentry_pos_data.y = current_sentry_pos_ref.position_y; - current_sentry_pos_data.z = current_sentry_pos_ref.position_z; - current_sentry_pos_data.yaw = current_sentry_pos_ref.position_yaw; - - current_sentry_pos_pub_.publish(current_sentry_pos_data); + rm_referee::BulletNumData bullet_num_data_ref; + memcpy(&bullet_num_data_ref, rx_data + 7, sizeof(rm_referee::BulletNumData)); + referee_ui_.updateBulletRemainData(bullet_num_data_ref); + } + else if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::SENTRY_TO_RADAR_CMD) + { + rm_referee::SentryAttackingTargetData sentry_attacking_target_data_ref; + rm_msgs::SentryAttackingTarget sentry_attacking_target_data_data; + memcpy(&sentry_attacking_target_data_ref, rx_data + 7, sizeof(rm_referee::SentryAttackingTargetData)); + sentry_attacking_target_data_data.target_robot_ID = sentry_attacking_target_data_ref.target_robot_ID; + sentry_attacking_target_data_data.target_position_x = sentry_attacking_target_data_ref.target_position_x; + sentry_attacking_target_data_data.target_position_y = sentry_attacking_target_data_ref.target_position_y; + sentry_to_radar_pub_.publish(sentry_attacking_target_data_data); } break; } diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index de66c445..30568935 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -36,8 +36,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/map_sentry_data", 10, &RefereeBase::mapSentryCallback, this); RefereeBase::radar_receive_sub_ = nh.subscribe("/rm_radar", 10, &RefereeBase::radarReceiveCallback, this); - RefereeBase::radar_to_sentry_sub_ = nh.subscribe( - "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); RefereeBase::sentry_cmd_sub_ = nh.subscribe("/sentry_cmd", 1, &RefereeBase::sendSentryCmdCallback, this); RefereeBase::radar_cmd_sub_ = @@ -46,6 +44,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this); RefereeBase::drone_pose_sub_ = nh.subscribe("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this); + RefereeBase::shoot_cmd_sub_ = nh.subscribe("/controllers/shooter_controller/command", 1, + &RefereeBase::shootCmdCallBack, this); + RefereeBase::sentry_to_radar_sub_ = nh.subscribe( + "/sentry_target_to_referee", 1, &RefereeBase::sentryAttackingTargetCallback, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -72,6 +74,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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_); + if (rpc_value[i]["name"] == "friction_speed") + friction_speed_trigger_change_ui_ = + new FrictionSpeedTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "gripper") gripper_state_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "gripper", &graph_queue_, &character_queue_); @@ -121,6 +126,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "drone_towards") drone_towards_time_change_group_ui_ = new DroneTowardsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "friend_bullets") + friend_bullets_time_change_group_ui_ = + new FriendBulletsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -138,6 +146,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "exceed_bullet_speed") exceed_bullet_speed_flash_ui_ = new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "engineer_action") + engineer_action_flash_ui_ = new EngineerActionFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -146,9 +156,13 @@ 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"] == "enemy_hero_state") - enemy_hero_state_sender_ = new InteractiveSender(rpc_value[i], base_); + enemy_hero_state_sender_ = new CustomInfoSender(rpc_value[i], base_); if (rpc_value[i]["name"] == "sentry_state") - sentry_state_sender_ = new InteractiveSender(rpc_value[i], base_); + sentry_state_sender_ = new CustomInfoSender(rpc_value[i], base_); + if (rpc_value[i]["name"] == "bullet_num_share") + bullet_num_share_ = new BulletNumShare(rpc_value[i], base_); + if (rpc_value[i]["name"] == "sentry_to_radar") + sentry_to_radar_ = new SentryToRadar(rpc_value[i], base_); } } @@ -212,14 +226,14 @@ void RefereeBase::addUi() engineer_joint3_time_change_ui->addForQueue(); if (drone_towards_time_change_group_ui_) drone_towards_time_change_group_ui_->addForQueue(); - // if (drag_state_trigger_change_ui_) - // drag_state_trigger_change_ui_->addForQueue(); if (gripper_state_trigger_change_ui_) gripper_state_trigger_change_ui_->addForQueue(); if (stone_num_trigger_change_ui_) stone_num_trigger_change_ui_->addForQueue(); if (servo_mode_trigger_change_ui_) servo_mode_trigger_change_ui_->addForQueue(); + if (friction_speed_trigger_change_ui_) + friction_speed_trigger_change_ui_->addForQueue(); if (bullet_time_change_ui_) { bullet_time_change_ui_->reset(); @@ -227,6 +241,8 @@ void RefereeBase::addUi() } if (target_distance_time_change_ui_) target_distance_time_change_ui_->addForQueue(); + if (friend_bullets_time_change_group_ui_) + friend_bullets_time_change_group_ui_->addForQueue(); add_ui_times_++; } @@ -252,7 +268,12 @@ void RefereeBase::sendSerialDataCallback() while (character_queue_.size() > 8) character_queue_.pop_front(); } - sendQueue(); + if (bullet_num_share_ && bullet_num_share_->needSendInteractiveData()) + bullet_num_share_->sendBulletData(); + else if (sentry_to_radar_ && sentry_to_radar_->needSendInteractiveData()) + sentry_to_radar_->sendSentryToRadarData(); + else + sendQueue(); } else sendQueue(); @@ -308,25 +329,25 @@ void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, void RefereeBase::updateEnemyHeroState(const rm_msgs::GameRobotHp& game_robot_hp_data, const ros::Time& last_get_data_time) { - if (enemy_hero_state_sender_) - { - std::wstring data; - if (base_.robot_id_ < 100) - { - if (game_robot_hp_data.blue_1_robot_hp > 0) - data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.blue_1_robot_hp); - else - data = L"敌方英雄死亡"; - } - else if (base_.robot_id_ >= 100) - { - if (game_robot_hp_data.red_1_robot_hp > 0) - data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.red_1_robot_hp); - else - data = L"敌方英雄死亡"; - } - enemy_hero_state_sender_->sendCustomInfoData(data); - } + // if (enemy_hero_state_sender_) + // { + // std::wstring data; + // if (base_.robot_id_ < 100) + // { + // if (game_robot_hp_data.blue_1_robot_hp > 0) + // data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.blue_1_robot_hp); + // else + // data = L"敌方英雄死亡"; + // } + // else if (base_.robot_id_ >= 100) + // { + // if (game_robot_hp_data.red_1_robot_hp > 0) + // data = L"敌方英雄存活:" + std::to_wstring(game_robot_hp_data.red_1_robot_hp); + // else + // data = L"敌方英雄死亡"; + // } + // enemy_hero_state_sender_->sendCustomInfoData(data); + // } } void RefereeBase::updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data) @@ -356,6 +377,8 @@ void RefereeBase::bulletRemainDataCallBack(const rm_msgs::BulletAllowance& bulle { if (bullet_time_change_ui_ && !is_adding_) bullet_time_change_ui_->updateBulletData(bullet_allowance, last_get_data_time); + if (bullet_num_share_ && !is_adding_) + bullet_num_share_->updateBulletRemainData(bullet_allowance); } void RefereeBase::interactiveDataCallBack(const rm_referee::InteractiveData& data, const ros::Time& last_get_data_time) { @@ -409,6 +432,8 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da chassis_trigger_change_ui_->updateChassisCmdData(data); if (spin_flash_ui_ && !is_adding_) spin_flash_ui_->updateChassisCmdData(data, ros::Time::now()); + if (rotation_time_change_ui_ && !is_adding_) + rotation_time_change_ui_->updateChassisCmdData(data); } void RefereeBase::vel2DCmdDataCallback(const geometry_msgs::Twist::ConstPtr& data) { @@ -440,6 +465,8 @@ void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& da stone_num_trigger_change_ui_->updateStringUiData(std::to_string(data->stone_num)); if (servo_mode_trigger_change_ui_ && !is_adding_) servo_mode_trigger_change_ui_->updateStringUiData(data->control_mode); + if (engineer_action_flash_ui_ && !is_adding_) + engineer_action_flash_ui_->updateEngineerUiCmdData(data, ros::Time::now()); } void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data) { @@ -474,10 +501,11 @@ void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data if (balance_pitch_time_change_group_ui_) balance_pitch_time_change_group_ui_->calculatePointPosition(data, ros::Time::now()); } -void RefereeBase::sentryDeviateCallback(const rm_msgs::SentryDeviateConstPtr& data) +void RefereeBase::sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data) { + if (sentry_to_radar_) + sentry_to_radar_->updateSentryAttackingTargetData(data); } - void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { rm_referee::ClientMapReceiveData radar_receive_data; @@ -511,11 +539,6 @@ void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) sentry_interactive_data_last_send_ = ros::Time::now(); } } - -void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data) -{ - interactive_data_sender_->sendCurrentSentryData(data); -} void RefereeBase::sendSentryCmdCallback(const rm_msgs::SentryInfoConstPtr& data) { if (ros::Time::now() - sentry_cmd_data_last_send_ <= ros::Duration(0.15)) @@ -560,4 +583,16 @@ void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg) exceed_bullet_speed_flash_ui_->updateShootData(msg); } +void RefereeBase::shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data) +{ + if (friction_speed_trigger_change_ui_ && !is_adding_) + friction_speed_trigger_change_ui_->updateFrictionSpeedUiData(data); +} + +void RefereeBase::updateBulletRemainData(const rm_referee::BulletNumData& data) +{ + if (friend_bullets_time_change_group_ui_ && !is_adding_) + friend_bullets_time_change_group_ui_->updateBulletsData(data); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index c0adbafc..d31f39a5 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -10,6 +10,7 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once { if (once) { + // ROS_INFO("test"); if (state) graph_->setOperation(rm_referee::GraphOperation::ADD); else @@ -17,7 +18,7 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once } else if (time - last_send_ > delay_) { - ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); + // ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); if (state) graph_->setOperation(rm_referee::GraphOperation::ADD); else @@ -25,6 +26,7 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once // graph_->setOperation(graph_->getOperation() == rm_referee::GraphOperation::ADD ? // rm_referee::GraphOperation::DELETE : // rm_referee::GraphOperation::ADD); + // ROS_INFO("delay"); } if (graph_->isRepeated()) return; @@ -33,6 +35,51 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once UiBase::updateForQueue(); } +void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph) +{ + if (once) + { + if (state) + graph->setOperation(rm_referee::GraphOperation::ADD); + else + graph->setOperation(rm_referee::GraphOperation::DELETE); + } + else if (time - last_send_ > delay_) + { + ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); + if (state) + graph->setOperation(rm_referee::GraphOperation::ADD); + else + graph->setOperation(rm_referee::GraphOperation::DELETE); + } + if (graph->isRepeated()) + return; + graph->updateLastConfig(); + last_send_ = time; + if (graph->isString()) + character_queue_->push_back(*graph); + else + graph_queue_->push_back(*graph); +} + +void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, + const ros::Time& last_get_data_time) +{ + symbol_ = data->symbol; + display(last_get_data_time); +} + +void EngineerActionFlashUi::display(const ros::Time& time) +{ + for (auto graph : graph_vector_) + { + bool state = false; + if (std::to_string(static_cast(symbol_)) == graph.first) + state = true; + FlashGroupUi::updateFlashUiForQueue(time, state, true, graph.second); + } +} + void CoverFlashUi::display(const ros::Time& time) { if (!cover_state_) @@ -65,25 +112,23 @@ void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) if (base_.robot_id_ > 100) { hitted_ = - (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 175 || last_hp_msg_.red_base_hp - msg.red_base_hp > 175); + (last_hp_msg_.red_outpost_hp - msg.red_outpost_hp > 190 || last_hp_msg_.red_base_hp - msg.red_base_hp > 190); } else { - hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 175 || - last_hp_msg_.blue_base_hp - msg.blue_base_hp > 175); + hitted_ = (last_hp_msg_.blue_outpost_hp - msg.blue_outpost_hp > 190 || + last_hp_msg_.blue_base_hp - msg.blue_base_hp > 190); } last_hp_msg_ = msg; + display(ros::Time::now()); } void HeroHitFlashUi::display(const ros::Time& time) { if (hitted_) - { - graph_->setOperation(rm_referee::GraphOperation::ADD); FlashUi::updateFlashUiForQueue(time, true, true); - } else - FlashUi::updateFlashUiForQueue(time, hitted_, false); + FlashUi::updateFlashUiForQueue(time, false, false); } void ExceedBulletSpeedFlashUi::display(const ros::Time& time) diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp new file mode 100644 index 00000000..69e4738d --- /dev/null +++ b/rm_referee/src/ui/interactive_data.cpp @@ -0,0 +1,215 @@ +// +// Created by gura on 24-5-29. +// + +#include "rm_referee/ui/interactive_data.h" + +namespace rm_referee +{ +void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data) +{ + uint8_t tx_data[sizeof(InteractiveData)] = { 0 }; + auto student_interactive_data = (InteractiveData*)tx_data; + + for (int i = 0; i < 127; i++) + tx_buffer_[i] = 0; + student_interactive_data->header_data.data_cmd_id = data_cmd_id; + student_interactive_data->header_data.sender_id = base_.robot_id_; + student_interactive_data->header_data.receiver_id = receiver_id; + student_interactive_data->data = data; + pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(InteractiveData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(InteractiveData) + k_tail_length_); + + sendSerial(ros::Time::now(), sizeof(InteractiveData)); +} + +bool InteractiveSender::needSendInteractiveData() +{ + return ros::Time::now() - last_send_time_ > delay_; +} + +void InteractiveSender::sendMapSentryData(const rm_referee::MapSentryData& data) +{ + uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; + auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; + + for (int i = 0; i < 127; 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; + 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->sender_id = base_.robot_id_; + 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_); + + try + { + base_.serial_.write(tx_buffer_, tx_len_); + } + catch (serial::PortNotOpenedException& e) + { + ROS_ERROR_STREAM(e.what()); + } + clearTxBuffer(); +} + +void CustomInfoSender::sendCustomInfoData(std::wstring data) +{ + if (data == last_custom_info_ || ros::Time::now() - last_send_ < ros::Duration(0.35)) + return; + else + last_custom_info_ = data; + + int data_len; + rm_referee::CustomInfo tx_data; + data_len = static_cast(sizeof(rm_referee::CustomInfo)); + + tx_data.sender_id = base_.robot_id_; + tx_data.receiver_id = base_.client_id_; + + uint16_t characters[15]; + for (int i = 0; i < 15; i++) + { + if (i < static_cast(data.size())) + characters[i] = static_cast(data[i]); + else + characters[i] = static_cast(L' '); + } + for (int i = 0; i < 15; i++) + { + tx_data.user_data[2 * i] = characters[i] & 0xFF; + tx_data.user_data[2 * i + 1] = (characters[i] >> 8) & 0xFF; + } + pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::CUSTOM_INFO_CMD, data_len); + last_send_ = ros::Time::now(); + sendSerial(ros::Time::now(), data_len); +} + +void InteractiveSender::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) +{ + uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; + auto radar_interactive_data = (rm_referee::ClientMapReceiveData*)tx_data; + + for (int i = 0; i < 127; i++) + tx_buffer_[i] = 0; + radar_interactive_data->target_robot_ID = data.target_robot_ID; + radar_interactive_data->target_position_x = data.target_position_x; + radar_interactive_data->target_position_y = data.target_position_y; + pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CLIENT_MAP_CMD, sizeof(rm_referee::ClientMapReceiveData)); + tx_len_ = + k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::ClientMapReceiveData) + k_tail_length_); + try + { + base_.serial_.write(tx_buffer_, tx_len_); + } + catch (serial::PortNotOpenedException& e) + { + ROS_ERROR_STREAM(e.what()); + } + clearTxBuffer(); +} + +void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data) +{ + int data_len; + rm_referee::SentryInfo tx_data; + data_len = static_cast(sizeof(rm_referee::SentryInfo)); + + tx_data.header.sender_id = base_.robot_id_; + tx_data.header.receiver_id = REFEREE_SERVER; + tx_data.sentry_info = data->sentry_info; + + tx_data.header.data_cmd_id = rm_referee::DataCmdId::SENTRY_CMD; + pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); + sendSerial(ros::Time::now(), data_len); +} + +void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) +{ + int data_len; + rm_referee::RadarInfo tx_data; + data_len = static_cast(sizeof(rm_referee::RadarInfo)); + + tx_data.header.sender_id = base_.robot_id_; + tx_data.header.receiver_id = REFEREE_SERVER; + tx_data.radar_info = data->radar_info; + + tx_data.header.data_cmd_id = rm_referee::DataCmdId::RADAR_CMD; + pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); + sendSerial(ros::Time::now(), data_len); +} + +void BulletNumShare::sendBulletData() +{ + uint16_t receiver_id; + if (base_.robot_color_ == "red") + receiver_id = RED_HERO; + else + receiver_id = BLUE_HERO; + receiver_id += (4 - (count_receive_time_ % 3)); + + uint8_t tx_data[sizeof(BulletNumData)] = { 0 }; + auto bullet_num_data = (BulletNumData*)tx_data; + + for (int i = 0; i < 127; i++) + tx_buffer_[i] = 0; + bullet_num_data->header_data.data_cmd_id = rm_referee::DataCmdId::BULLET_NUM_SHARE_CMD; + bullet_num_data->header_data.sender_id = base_.robot_id_; + bullet_num_data->header_data.receiver_id = receiver_id; + bullet_num_data->bullet_42_mm_num = bullet_42_mm_num_; + bullet_num_data->bullet_17_mm_num = bullet_17_mm_num_; + pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(BulletNumData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(BulletNumData) + k_tail_length_); + sendSerial(ros::Time::now(), sizeof(BulletNumData)); + last_send_time_ = ros::Time::now(); + count_receive_time_++; +} + +void BulletNumShare::updateBulletRemainData(const rm_msgs::BulletAllowance& data) +{ + if (data.bullet_allowance_num_42_mm > 5096 || data.bullet_allowance_num_17_mm > 5096) + return; + bullet_17_mm_num_ = data.bullet_allowance_num_17_mm; + bullet_42_mm_num_ = data.bullet_allowance_num_42_mm; +} + +void SentryToRadar::updateSentryAttackingTargetData(const rm_msgs::SentryAttackingTargetConstPtr& data) +{ + robot_id_ = data->target_robot_ID; + target_position_x_ = data->target_position_x; + target_position_y_ = data->target_position_y; + last_get_data_time_ = ros::Time::now(); +} + +void SentryToRadar::sendSentryToRadarData() +{ + uint8_t tx_data[sizeof(SentryAttackingTargetData)] = { 0 }; + auto sentry_attacking_target_data = (SentryAttackingTargetData*)tx_data; + + for (int i = 0; i < 127; i++) + tx_buffer_[i] = 0; + sentry_attacking_target_data->header_data.data_cmd_id = rm_referee::DataCmdId::SENTRY_TO_RADAR_CMD; + sentry_attacking_target_data->header_data.sender_id = base_.robot_id_; + if (base_.robot_color_ == "red") + sentry_attacking_target_data->header_data.receiver_id = RED_RADAR; + else + sentry_attacking_target_data->header_data.receiver_id = BLUE_RADAR; + sentry_attacking_target_data->target_robot_ID = robot_id_; + sentry_attacking_target_data->target_position_x = target_position_x_; + sentry_attacking_target_data->target_position_y = target_position_y_; + pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(SentryAttackingTargetData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(SentryAttackingTargetData) + k_tail_length_); + sendSerial(ros::Time::now(), sizeof(SentryAttackingTargetData)); + last_send_time_ = ros::Time::now(); +} + +bool SentryToRadar::needSendInteractiveData() +{ + return InteractiveSender::needSendInteractiveData() && ros::Time::now() - last_get_data_time_ < ros::Duration(0.5); +} +} // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 5a1b1ab1..7b07d9aa 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -157,6 +157,10 @@ void DartStatusTimeChangeUi::updateDartClientCmd(const rm_msgs::DartClientCmd::C void RotationTimeChangeUi::updateConfig() { + if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) + graph_->setColor(rm_referee::GraphColor::PINK); + else + graph_->setColor(rm_referee::GraphColor::GREEN); if (!tf_buffer_.canTransform(gimbal_reference_frame_, chassis_reference_frame_, ros::Time(0))) return; try @@ -179,6 +183,11 @@ void RotationTimeChangeUi::updateConfig() } } +void RotationTimeChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data) +{ + chassis_mode_ = data->mode; +} + void LaneLineTimeChangeGroupUi::updateConfig() { double spacing_x_a = robot_radius_ * screen_y_ / 2 * tan(M_PI / 2 - camera_range_ / 2) / @@ -326,12 +335,14 @@ void BulletTimeChangeUi::updateBulletData(const rm_msgs::BulletAllowance& data, { if (data.bullet_allowance_num_17_mm >= 0 && data.bullet_allowance_num_17_mm < 1000) { - bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm); + if (bullet_allowance_num_17_mm_ > data.bullet_allowance_num_17_mm) + bullet_num_17_mm_ += (bullet_allowance_num_17_mm_ - data.bullet_allowance_num_17_mm); bullet_allowance_num_17_mm_ = data.bullet_allowance_num_17_mm; } if (data.bullet_allowance_num_42_mm >= 0 && data.bullet_allowance_num_42_mm < 1000) { - bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm); + if (bullet_allowance_num_42_mm_ > data.bullet_allowance_num_42_mm) + bullet_num_42_mm_ += (bullet_allowance_num_42_mm_ - data.bullet_allowance_num_42_mm); bullet_allowance_num_42_mm_ = data.bullet_allowance_num_42_mm; } updateForQueue(); @@ -348,7 +359,7 @@ void BulletTimeChangeUi::updateConfig() std::string bullet_allowance_num; if (base_.robot_id_ == RED_HERO || base_.robot_id_ == BLUE_HERO) { - graph_->setRadius(bullet_num_42_mm_); + graph_->setIntNum(bullet_num_42_mm_); if (bullet_allowance_num_42_mm_ > 5) graph_->setColor(rm_referee::GraphColor::GREEN); else if (bullet_allowance_num_42_mm_ < 3) @@ -358,7 +369,7 @@ void BulletTimeChangeUi::updateConfig() } else { - graph_->setRadius(bullet_num_17_mm_); // TODO:need use uint32, now only < 1024 + graph_->setIntNum(bullet_num_17_mm_); if (bullet_allowance_num_17_mm_ > 50) graph_->setColor(rm_referee::GraphColor::GREEN); else if (bullet_allowance_num_17_mm_ < 10) @@ -395,7 +406,7 @@ void TargetDistanceTimeChangeUi::updateTargetDistanceData(const rm_msgs::TrackDa void TargetDistanceTimeChangeUi::updateConfig() { - UiBase::transferInt(std::floor(target_distance_ * 1000)); + graph_->setFloatNum(target_distance_); } void DroneTowardsTimeChangeGroupUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) @@ -440,4 +451,35 @@ void DroneTowardsTimeChangeGroupUi::updateConfig() } } +void FriendBulletsTimeChangeGroupUi::updateBulletsData(const rm_referee::BulletNumData& data) +{ + if (data.header_data.sender_id == rm_referee::RobotId::RED_HERO || + data.header_data.sender_id == rm_referee::RobotId::BLUE_HERO) + hero_bullets_ = data.bullet_42_mm_num; + else if (data.header_data.sender_id == rm_referee::RobotId::RED_STANDARD_3 || + data.header_data.sender_id == rm_referee::RobotId::BLUE_STANDARD_3) + standard3_bullets_ = data.bullet_17_mm_num; + else if (data.header_data.sender_id == rm_referee::RobotId::RED_STANDARD_4 || + data.header_data.sender_id == rm_referee::RobotId::BLUE_STANDARD_4) + standard4_bullets_ = data.bullet_17_mm_num; + else if (data.header_data.sender_id == rm_referee::RobotId::RED_STANDARD_5 || + data.header_data.sender_id == rm_referee::RobotId::BLUE_STANDARD_5) + standard5_bullets_ = data.bullet_17_mm_num; + updateForQueue(); +} + +void FriendBulletsTimeChangeGroupUi::updateConfig() +{ + for (auto it : graph_vector_) + { + if (it.first == "hero") + it.second->setIntNum(hero_bullets_); + else if (it.first == "standard3") + it.second->setIntNum(standard3_bullets_); + else if (it.first == "standard4") + it.second->setIntNum(standard4_bullets_); + else if (it.first == "standard5") + it.second->setIntNum(standard5_bullets_); + } +} } // namespace rm_referee diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 15c39700..b91865de 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -437,4 +437,17 @@ void StringTriggerChangeUi::update() updateForQueue(true); } +void FrictionSpeedTriggerChangeUi::updateFrictionSpeedUiData(const rm_msgs::ShootCmdConstPtr& data) +{ + wheel_speed_ = data->wheel_speed; + update(); +} + +void FrictionSpeedTriggerChangeUi::update() +{ + graph_->setIntNum(std::floor(wheel_speed_)); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 073d6ab9..920cc6c1 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -148,45 +148,6 @@ void UiBase::display(const ros::Time& time, bool state, bool once) displayTwice(); } -void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data) -{ - uint8_t tx_data[sizeof(InteractiveData)] = { 0 }; - auto student_interactive_data = (InteractiveData*)tx_data; - - for (int i = 0; i < 128; i++) - tx_buffer_[i] = 0; - student_interactive_data->header_data.data_cmd_id = data_cmd_id; - student_interactive_data->header_data.sender_id = base_.robot_id_; - student_interactive_data->header_data.receiver_id = receiver_id; - student_interactive_data->data = data; - pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(InteractiveData)); - tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(InteractiveData) + k_tail_length_); - - sendSerial(ros::Time::now(), sizeof(InteractiveData)); -} - -void InteractiveSender::sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data) -{ - int data_len; - uint8_t tx_data[sizeof(CurrentSentryPosData)] = { 0 }; - auto current_sentry_pos_data = (CurrentSentryPosData*)tx_data; - data_len = static_cast(sizeof(rm_referee::CurrentSentryPosData)); - - for (int i = 0; i < 128; i++) - tx_buffer_[i] = 0; - - current_sentry_pos_data->header_data.data_cmd_id = DataCmdId::CURRENT_SENTRY_POSITION_CMD; - current_sentry_pos_data->header_data.sender_id = base_.robot_id_; - current_sentry_pos_data->header_data.receiver_id = base_.robot_id_ < 100 ? RobotId::RED_SENTRY : RobotId::BLUE_SENTRY; - current_sentry_pos_data->position_x = data->x; - current_sentry_pos_data->position_y = data->y; - current_sentry_pos_data->position_z = data->z; - current_sentry_pos_data->position_yaw = data->yaw; - - pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(InteractiveData)); - sendSerial(ros::Time::now(), data_len); -} - void UiBase::sendUi(const ros::Time& time) { if (base_.robot_id_ == 0 || base_.client_id_ == 0) @@ -198,91 +159,6 @@ void UiBase::sendUi(const ros::Time& time) sendSingleGraph(time, graph_); } -void InteractiveSender::sendMapSentryData(const rm_referee::MapSentryData& data) -{ - uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; - auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; - - 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; - 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->sender_id = base_.robot_id_; - 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_); - - try - { - base_.serial_.write(tx_buffer_, tx_len_); - } - catch (serial::PortNotOpenedException& e) - { - } - - clearTxBuffer(); -} - -void InteractiveSender::sendCustomInfoData(std::wstring data) -{ - if (data == last_custom_info_ || ros::Time::now() - last_send_ < ros::Duration(0.35)) - return; - else - last_custom_info_ = data; - - int data_len; - rm_referee::CustomInfo tx_data; - data_len = static_cast(sizeof(rm_referee::CustomInfo)); - - tx_data.sender_id = base_.robot_id_; - tx_data.receiver_id = base_.client_id_; - - uint16_t characters[15]; - for (int i = 0; i < 15; i++) - { - if (i < static_cast(data.size())) - characters[i] = static_cast(data[i]); - else - characters[i] = static_cast(L' '); - } - for (int i = 0; i < 15; i++) - { - tx_data.user_data[2 * i] = characters[i] & 0xFF; - tx_data.user_data[2 * i + 1] = (characters[i] >> 8) & 0xFF; - } - pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::CUSTOM_INFO_CMD, data_len); - last_send_ = ros::Time::now(); - sendSerial(ros::Time::now(), data_len); -} - -void InteractiveSender::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) -{ - uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; - auto radar_interactive_data = (rm_referee::ClientMapReceiveData*)tx_data; - - for (int i = 0; i < 128; i++) - tx_buffer_[i] = 0; - radar_interactive_data->target_robot_ID = data.target_robot_ID; - radar_interactive_data->target_position_x = data.target_position_x; - radar_interactive_data->target_position_y = data.target_position_y; - pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CLIENT_MAP_CMD, sizeof(rm_referee::ClientMapReceiveData)); - tx_len_ = - k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(rm_referee::ClientMapReceiveData) + k_tail_length_); - try - { - base_.serial_.write(tx_buffer_, tx_len_); - } - catch (serial::PortNotOpenedException& e) - { - } - clearTxBuffer(); -} - void UiBase::sendCharacter(const ros::Time& time, rm_referee::Graph* graph) { int data_len; @@ -321,36 +197,6 @@ void UiBase::sendSingleGraph(const ros::Time& time, Graph* graph) sendSerial(time, data_len); } -void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data) -{ - int data_len; - rm_referee::SentryInfo tx_data; - data_len = static_cast(sizeof(rm_referee::SentryInfo)); - - tx_data.header.sender_id = base_.robot_id_; - tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.sentry_info = data->sentry_info; - - tx_data.header.data_cmd_id = rm_referee::DataCmdId::SENTRY_CMD; - pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); - sendSerial(ros::Time::now(), data_len); -} - -void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) -{ - int data_len; - rm_referee::RadarInfo tx_data; - data_len = static_cast(sizeof(rm_referee::RadarInfo)); - - tx_data.header.sender_id = base_.robot_id_; - tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.radar_info = data->radar_info; - - tx_data.header.data_cmd_id = rm_referee::DataCmdId::RADAR_CMD; - pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); - sendSerial(ros::Time::now(), data_len); -} - void GroupUiBase::display(bool check_repeat) { if (check_repeat) @@ -490,14 +336,6 @@ void FixedUi::updateForQueue() } } -void UiBase::transferInt(const int data) -{ - int a = data & 1023; - int b = data >> 10; - graph_->setRadius(a); - graph_->setEndX(b); -} - void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const { memset(tx_buffer, 0, k_frame_length_); @@ -527,7 +365,7 @@ void UiBase::sendSerial(const ros::Time& time, int data_len) void UiBase::clearTxBuffer() { - for (int i = 0; i < 128; i++) + for (int i = 0; i < 127; i++) tx_buffer_[i] = 0; tx_len_ = 0; }