From 7e638d0185be95a6421171d2d428da9084f65b89 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Mon, 15 Jul 2024 16:47:37 +0800 Subject: [PATCH 1/8] Update interactive data sender. --- .../include/rm_referee/ui/interactive_data.h | 1 - rm_referee/src/referee_base.cpp | 16 +++++++++++----- rm_referee/src/ui/interactive_data.cpp | 3 +-- 3 files changed, 12 insertions(+), 8 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/interactive_data.h b/rm_referee/include/rm_referee/ui/interactive_data.h index 0a822e14..252b1726 100644 --- a/rm_referee/include/rm_referee/ui/interactive_data.h +++ b/rm_referee/include/rm_referee/ui/interactive_data.h @@ -56,7 +56,6 @@ class SentryToRadar : public InteractiveSender 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_; }; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 30568935..c560f81e 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -248,6 +248,17 @@ void RefereeBase::addUi() void RefereeBase::sendSerialDataCallback() { + if (bullet_num_share_ && bullet_num_share_->needSendInteractiveData()) + { + bullet_num_share_->sendBulletData(); + return; + } + else if (sentry_to_radar_ && sentry_to_radar_->needSendInteractiveData()) + { + sentry_to_radar_->sendSentryToRadarData(); + return; + } + if (graph_queue_.empty() && character_queue_.empty()) return; @@ -268,11 +279,6 @@ void RefereeBase::sendSerialDataCallback() while (character_queue_.size() > 8) character_queue_.pop_front(); } - 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 diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp index 69e4738d..59be156b 100644 --- a/rm_referee/src/ui/interactive_data.cpp +++ b/rm_referee/src/ui/interactive_data.cpp @@ -183,7 +183,6 @@ void SentryToRadar::updateSentryAttackingTargetData(const rm_msgs::SentryAttacki 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() @@ -210,6 +209,6 @@ void SentryToRadar::sendSentryToRadarData() bool SentryToRadar::needSendInteractiveData() { - return InteractiveSender::needSendInteractiveData() && ros::Time::now() - last_get_data_time_ < ros::Duration(0.5); + return InteractiveSender::needSendInteractiveData() && (robot_id_ != 0); } } // namespace rm_referee From d306a8259048023c7729e5384ec18e685475aa43 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 15:34:54 +0800 Subject: [PATCH 2/8] Update referee and add new interactive data sender. --- rm_referee/include/rm_referee/common/data.h | 1 + .../include/rm_referee/common/protocol.h | 34 +++++++++-- rm_referee/include/rm_referee/referee.h | 2 + rm_referee/include/rm_referee/referee_base.h | 6 +- .../include/rm_referee/ui/interactive_data.h | 19 ++++++- rm_referee/src/referee.cpp | 29 ++++++---- rm_referee/src/referee_base.cpp | 29 +++++++--- rm_referee/src/ui/interactive_data.cpp | 56 +++++++++++++++++-- 8 files changed, 142 insertions(+), 34 deletions(-) diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index ad3403cb..632bd286 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -87,6 +87,7 @@ #include "rm_msgs/Buff.h" #include "rm_msgs/TrackData.h" #include "rm_msgs/SentryAttackingTarget.h" +#include "rm_msgs/RadarToSentry.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 deed1d9b..8c334315 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -95,8 +95,8 @@ typedef enum SENTRY_CMD = 0x0120, RADAR_CMD = 0x0121, BULLET_NUM_SHARE_CMD = 0x0200, // send robot->aerial - SENTRY_TO_RADAR_CMD = 0x0201 // send sentry->radar - // send radar->sentry + SENTRY_TO_RADAR_CMD = 0x0201, // send sentry->radar + RADAR_TO_SENTRY_CMD = 0x0202 // send radar->sentry } DataCmdId; typedef enum @@ -354,7 +354,11 @@ typedef struct typedef struct { - uint8_t power_rune_buff; + uint8_t recovery_buff; + uint8_t cooling_buff; + uint8_t defence_buff; + uint8_t vulnerability_buff; + uint16_t attack_buff; } __packed Buff; typedef struct @@ -564,9 +568,18 @@ typedef struct typedef struct { - uint16_t target_robot_ID; - float target_position_x; - float target_position_y; + uint16_t hero_position_x; + uint16_t hero_position_y; + uint16_t engineer_position_x; + uint16_t engineer_position_y; + uint16_t infantry_3_position_x; + uint16_t infantry_3_position_y; + uint16_t infantry_4_position_x; + uint16_t infantry_4_position_y; + uint16_t infantry_5_position_x; + uint16_t infantry_5_position_y; + uint16_t sentry_position_x; + uint16_t sentry_position_y; } __packed ClientMapReceiveData; typedef struct @@ -577,6 +590,15 @@ typedef struct float target_position_y; } __packed SentryAttackingTargetData; +typedef struct +{ + InteractiveDataHeader header_data; + uint8_t robot_ID; + float position_x; + float position_y; + bool engineer_marked; +} __packed RadarToSentryData; + typedef struct { int16_t mouse_x; diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index 800fad9b..c96663df 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -75,6 +75,7 @@ class Referee 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); + radar_to_sentry_pub_ = nh.advertise("radar_to_sentry", 1); ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management"); power_management_sample_and_status_data_pub_ = @@ -121,6 +122,7 @@ class Referee ros::Publisher sentry_info_pub_; ros::Publisher radar_info_pub_; ros::Publisher client_map_send_data_pub_; + ros::Publisher radar_to_sentry_pub_; ros::Publisher power_management_sample_and_status_data_pub_; ros::Publisher power_management_initialization_exception_pub_; ros::Publisher power_management_system_exception_data_; diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 7372e910..ed19a853 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -64,6 +64,7 @@ class RefereeBase virtual void sendSentryStateCallback(const std_msgs::StringConstPtr& data); virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); + virtual void radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data); // send ui void sendSerialDataCallback(); @@ -87,8 +88,8 @@ 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_to_referee_sub_; + ros::Subscriber radar_to_referee_sub_; ros::Subscriber sentry_cmd_sub_; ros::Subscriber radar_cmd_sub_; ros::Subscriber sentry_state_sub_; @@ -134,6 +135,7 @@ class RefereeBase CustomInfoSender* sentry_state_sender_{}; BulletNumShare* bullet_num_share_{}; SentryToRadar* sentry_to_radar_{}; + RadarToSentry* radar_to_sentry_{}; GroupUiBase* graph_queue_sender_{}; std::deque graph_queue_; diff --git a/rm_referee/include/rm_referee/ui/interactive_data.h b/rm_referee/include/rm_referee/ui/interactive_data.h index 252b1726..1a548406 100644 --- a/rm_referee/include/rm_referee/ui/interactive_data.h +++ b/rm_referee/include/rm_referee/ui/interactive_data.h @@ -16,7 +16,7 @@ class InteractiveSender : public UiBase : 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 sendRadarInteractiveData(const rm_msgs::ClientMapReceiveData::ConstPtr& data); void sendMapSentryData(const rm_referee::MapSentryData& data); void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data); void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); @@ -56,7 +56,22 @@ class SentryToRadar : public InteractiveSender void updateSentryAttackingTargetData(const rm_msgs::SentryAttackingTargetConstPtr& data); bool needSendInteractiveData() override; void sendSentryToRadarData(); - float target_position_x_, target_position_y_, robot_id_; + int robot_id_; + float target_position_x_, target_position_y_; +}; + +class RadarToSentry : public InteractiveSender +{ +public: + explicit RadarToSentry(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 updateRadarToSentryData(const rm_msgs::RadarToSentryConstPtr& data); + bool needSendInteractiveData() override; + void sendRadarToSentryData(); + int robot_id_; + float position_x_, position_y_; + bool engineer_marked_{ false }, has_new_data_{ false }; }; } // namespace rm_referee diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 2a150638..b181a12e 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -307,7 +307,14 @@ int Referee::unpack(uint8_t* rx_data) case rm_referee::RefereeCmdId::BUFF_CMD: { rm_referee::Buff referee_buff; + rm_msgs::Buff robot_buff; memcpy(&referee_buff, rx_data + 7, sizeof(rm_referee::Buff)); + robot_buff.attack_buff = referee_buff.attack_buff; + robot_buff.defence_buff = referee_buff.defence_buff; + robot_buff.vulnerability_buff = referee_buff.vulnerability_buff; + robot_buff.cooling_buff = referee_buff.cooling_buff; + robot_buff.recovery_buff = referee_buff.recovery_buff; + buff_pub_.publish(robot_buff); break; } case rm_referee::RefereeCmdId::AERIAL_ROBOT_ENERGY_CMD: @@ -460,7 +467,6 @@ 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::BULLET_NUM_SHARE_CMD) { rm_referee::BulletNumData bullet_num_data_ref; @@ -477,6 +483,17 @@ int Referee::unpack(uint8_t* rx_data) 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); } + else if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::RADAR_TO_SENTRY_CMD) + { + rm_referee::RadarToSentryData radar_to_sentry_data_ref; + rm_msgs::RadarToSentry radar_to_sentry_data; + memcpy(&radar_to_sentry_data_ref, rx_data + 7, sizeof(rm_referee::RadarToSentryData)); + radar_to_sentry_data.robot_ID = radar_to_sentry_data_ref.robot_ID; + radar_to_sentry_data.position_x = radar_to_sentry_data_ref.position_x; + radar_to_sentry_data.position_y = radar_to_sentry_data_ref.position_y; + radar_to_sentry_data.engineer_marked = radar_to_sentry_data_ref.engineer_marked; + radar_to_sentry_pub_.publish(radar_to_sentry_data); + } break; } case rm_referee::CLIENT_MAP_CMD: @@ -484,16 +501,6 @@ int Referee::unpack(uint8_t* rx_data) rm_referee::ClientMapReceiveData client_map_receive_ref; rm_msgs::ClientMapReceiveData client_map_receive_data; memcpy(&client_map_receive_ref, rx_data + 7, sizeof(rm_referee::ClientMapReceiveData)); - - if (client_map_receive_ref.target_robot_ID == base_.robot_id_) - { - client_map_receive_data.target_robot_ID = client_map_receive_ref.target_robot_ID; - client_map_receive_data.target_position_x = client_map_receive_ref.target_position_x; - client_map_receive_data.target_position_y = client_map_receive_ref.target_position_y; - client_map_receive_data.stamp = last_get_data_time_; - - client_map_receive_pub_.publish(client_map_receive_data); - } break; } case rm_referee::CUSTOM_INFO_CMD: diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index c560f81e..a5c7d5f8 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -46,8 +46,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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( + RefereeBase::sentry_to_referee_sub_ = nh.subscribe( "/sentry_target_to_referee", 1, &RefereeBase::sentryAttackingTargetCallback, this); + RefereeBase::radar_to_referee_sub_ = + nh.subscribe("/radar_to_referee", 1, &RefereeBase::radarToRefereeCallBack, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -163,6 +165,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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_); + if (rpc_value[i]["name"] == "radar_to_sentry") + radar_to_sentry_ = new RadarToSentry(rpc_value[i], base_); } } @@ -253,11 +257,16 @@ void RefereeBase::sendSerialDataCallback() bullet_num_share_->sendBulletData(); return; } - else if (sentry_to_radar_ && sentry_to_radar_->needSendInteractiveData()) + if (sentry_to_radar_ && sentry_to_radar_->needSendInteractiveData()) { sentry_to_radar_->sendSentryToRadarData(); return; } + if (radar_to_sentry_ && radar_to_sentry_->needSendInteractiveData()) + { + radar_to_sentry_->sendRadarToSentryData(); + return; + } if (graph_queue_.empty() && character_queue_.empty()) return; @@ -279,7 +288,7 @@ void RefereeBase::sendSerialDataCallback() while (character_queue_.size() > 8) character_queue_.pop_front(); } - sendQueue(); + sendQueue(); } else sendQueue(); @@ -514,15 +523,11 @@ void RefereeBase::sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTa } void RefereeBase::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { - rm_referee::ClientMapReceiveData radar_receive_data; - radar_receive_data.target_position_x = data->target_position_x; - radar_receive_data.target_position_y = data->target_position_y; - radar_receive_data.target_robot_ID = data->target_robot_ID; - if (ros::Time::now() - radar_interactive_data_last_send_ <= ros::Duration(0.1)) + if (ros::Time::now() - radar_interactive_data_last_send_ <= ros::Duration(0.2)) return; else { - interactive_data_sender_->sendRadarInteractiveData(radar_receive_data); + interactive_data_sender_->sendRadarInteractiveData(data); radar_interactive_data_last_send_ = ros::Time::now(); } } @@ -601,4 +606,10 @@ void RefereeBase::updateBulletRemainData(const rm_referee::BulletNumData& data) friend_bullets_time_change_group_ui_->updateBulletsData(data); } +void RefereeBase::radarToRefereeCallBack(const rm_msgs::RadarToSentryConstPtr& data) +{ + if (radar_to_sentry_) + radar_to_sentry_->updateRadarToSentryData(data); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp index 59be156b..9c87ab28 100644 --- a/rm_referee/src/ui/interactive_data.cpp +++ b/rm_referee/src/ui/interactive_data.cpp @@ -90,16 +90,25 @@ void CustomInfoSender::sendCustomInfoData(std::wstring data) sendSerial(ros::Time::now(), data_len); } -void InteractiveSender::sendRadarInteractiveData(const rm_referee::ClientMapReceiveData& data) +void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceiveData::ConstPtr& 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; + radar_interactive_data->hero_position_x = data->hero_position_x; + radar_interactive_data->hero_position_y = data->hero_position_y; + radar_interactive_data->engineer_position_x = data->engineer_position_x; + radar_interactive_data->engineer_position_y = data->engineer_position_y; + radar_interactive_data->infantry_3_position_x = data->infantry_3_position_x; + radar_interactive_data->infantry_3_position_y = data->infantry_3_position_y; + radar_interactive_data->infantry_4_position_x = data->infantry_4_position_x; + radar_interactive_data->infantry_4_position_y = data->infantry_4_position_y; + radar_interactive_data->infantry_5_position_x = data->infantry_5_position_x; + radar_interactive_data->infantry_5_position_y = data->infantry_5_position_y; + radar_interactive_data->sentry_position_x = data->sentry_position_x; + radar_interactive_data->sentry_position_y = data->sentry_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_); @@ -211,4 +220,43 @@ bool SentryToRadar::needSendInteractiveData() { return InteractiveSender::needSendInteractiveData() && (robot_id_ != 0); } + +void RadarToSentry::updateRadarToSentryData(const rm_msgs::RadarToSentryConstPtr& data) +{ + robot_id_ = data->robot_ID; + position_x_ = data->position_x; + position_y_ = data->position_y; + engineer_marked_ = data->engineer_marked; + has_new_data_ = true; +} + +void RadarToSentry::sendRadarToSentryData() +{ + uint8_t tx_data[sizeof(RadarToSentryData)] = { 0 }; + auto radar_to_sentry_data = (RadarToSentryData*)tx_data; + + for (int i = 0; i < 127; i++) + tx_buffer_[i] = 0; + radar_to_sentry_data->header_data.data_cmd_id = rm_referee::DataCmdId::RADAR_TO_SENTRY_CMD; + radar_to_sentry_data->header_data.sender_id = base_.robot_id_; + if (base_.robot_color_ == "red") + radar_to_sentry_data->header_data.receiver_id = RED_SENTRY; + else + radar_to_sentry_data->header_data.receiver_id = BLUE_SENTRY; + radar_to_sentry_data->robot_ID = robot_id_; + radar_to_sentry_data->position_x = position_x_; + radar_to_sentry_data->position_y = position_y_; + radar_to_sentry_data->engineer_marked = engineer_marked_; + pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(RadarToSentryData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(RadarToSentryData) + k_tail_length_); + sendSerial(ros::Time::now(), sizeof(RadarToSentryData)); + last_send_time_ = ros::Time::now(); + has_new_data_ = false; +} + +bool RadarToSentry::needSendInteractiveData() +{ + return (InteractiveSender::needSendInteractiveData() && has_new_data_); +} + } // namespace rm_referee From 4497516e482ed0b8f36501805b705772d499abbe Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 15:50:14 +0800 Subject: [PATCH 3/8] Update referee. --- rm_referee/include/rm_referee/referee_base.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index ed19a853..2d3b89f9 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -131,7 +131,7 @@ class RefereeBase EngineerActionFlashUi* engineer_action_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; - CustomInfoSender* enemy_hero_state_sender_{}; + // CustomInfoSender* enemy_hero_state_sender_{}; CustomInfoSender* sentry_state_sender_{}; BulletNumShare* bullet_num_share_{}; SentryToRadar* sentry_to_radar_{}; From b2fd335e50f0925536fc4c2a1a39c95e4ae39dd2 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 15:55:47 +0800 Subject: [PATCH 4/8] Update referee. --- rm_msgs/CMakeLists.txt | 1 + rm_msgs/msg/referee/ClientMapReceiveData.msg | 15 ++++++++++++--- rm_msgs/msg/referee/RadarToSentry.msg | 4 ++++ 3 files changed, 17 insertions(+), 3 deletions(-) create mode 100644 rm_msgs/msg/referee/RadarToSentry.msg diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 0dccd5df..c37a445a 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -84,6 +84,7 @@ add_message_files( SentryInfo.msg RadarInfo.msg Buff.msg + RadarToSentry.msg PowerManagementSampleAndStatusData.msg PowerManagementInitializationExceptionData.msg PowerManagementProcessStackOverflowData.msg diff --git a/rm_msgs/msg/referee/ClientMapReceiveData.msg b/rm_msgs/msg/referee/ClientMapReceiveData.msg index 29a3ba34..d78a250d 100644 --- a/rm_msgs/msg/referee/ClientMapReceiveData.msg +++ b/rm_msgs/msg/referee/ClientMapReceiveData.msg @@ -1,5 +1,14 @@ -uint16 target_robot_ID -float32 target_position_x -float32 target_position_y +uint16 hero_position_x +uint16 hero_position_y +uint16 engineer_position_x +uint16 engineer_position_y +uint16 infantry_3_position_x +uint16 infantry_3_position_y +uint16 infantry_4_position_x +uint16 infantry_4_position_y +uint16 infantry_5_position_x +uint16 infantry_5_position_y +uint16 sentry_position_x +uint16 sentry_position_y time stamp diff --git a/rm_msgs/msg/referee/RadarToSentry.msg b/rm_msgs/msg/referee/RadarToSentry.msg new file mode 100644 index 00000000..7adeea52 --- /dev/null +++ b/rm_msgs/msg/referee/RadarToSentry.msg @@ -0,0 +1,4 @@ +uint8 robot_ID +float32 position_x +float32 position_y +bool engineer_marked \ No newline at end of file From 4bc9a371c5e0b3606690591b7680c263d988139c Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 15:59:56 +0800 Subject: [PATCH 5/8] Update referee msg. --- rm_msgs/msg/referee/SentryInfo.msg | 1 + rm_referee/include/rm_referee/common/protocol.h | 1 + rm_referee/src/ui/interactive_data.cpp | 2 +- 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg index 5295edf2..30d0524c 100644 --- a/rm_msgs/msg/referee/SentryInfo.msg +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -1 +1,2 @@ uint32 sentry_info +uint16 sentry_info_2; diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 8c334315..5027749d 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -544,6 +544,7 @@ typedef struct { InteractiveDataHeader header; uint32_t sentry_info; + uint16_t sentry_info_2; } __packed SentryInfo; typedef struct diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp index 9c87ab28..083edd26 100644 --- a/rm_referee/src/ui/interactive_data.cpp +++ b/rm_referee/src/ui/interactive_data.cpp @@ -132,7 +132,7 @@ void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& dat tx_data.header.sender_id = base_.robot_id_; tx_data.header.receiver_id = REFEREE_SERVER; tx_data.sentry_info = data->sentry_info; - + tx_data.sentry_info_2 = data->sentry_info_2; 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); From 6d18fa8ca375228117c5a967dfe7d9cae5716e12 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 16:00:38 +0800 Subject: [PATCH 6/8] Update referee msg. --- rm_msgs/msg/referee/SentryInfo.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg index 30d0524c..af5a4377 100644 --- a/rm_msgs/msg/referee/SentryInfo.msg +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -1,2 +1,2 @@ uint32 sentry_info -uint16 sentry_info_2; +uint16 sentry_info_2 From 199e33294f4bfb0769038e8ff83c8a7d84e82176 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 16:01:28 +0800 Subject: [PATCH 7/8] Update referee. --- rm_referee/src/referee_base.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index a5c7d5f8..96ff5800 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -157,8 +157,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.getParam("interactive_data", rpc_value); for (int i = 0; i < rpc_value.size(); i++) { - if (rpc_value[i]["name"] == "enemy_hero_state") - enemy_hero_state_sender_ = new CustomInfoSender(rpc_value[i], base_); + // if (rpc_value[i]["name"] == "enemy_hero_state") + // enemy_hero_state_sender_ = new CustomInfoSender(rpc_value[i], base_); if (rpc_value[i]["name"] == "sentry_state") sentry_state_sender_ = new CustomInfoSender(rpc_value[i], base_); if (rpc_value[i]["name"] == "bullet_num_share") From bb99d58142adeb06f7017ca1f27ebc64d951a2c1 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 17 Jul 2024 16:05:34 +0800 Subject: [PATCH 8/8] Fix format. --- rm_msgs/msg/referee/RadarToSentry.msg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_msgs/msg/referee/RadarToSentry.msg b/rm_msgs/msg/referee/RadarToSentry.msg index 7adeea52..9a16f884 100644 --- a/rm_msgs/msg/referee/RadarToSentry.msg +++ b/rm_msgs/msg/referee/RadarToSentry.msg @@ -1,4 +1,4 @@ uint8 robot_ID float32 position_x float32 position_y -bool engineer_marked \ No newline at end of file +bool engineer_marked