Skip to content

Commit

Permalink
Update referee protocol and sentry to radar communicate.
Browse files Browse the repository at this point in the history
  • Loading branch information
GuraMumei committed Jul 9, 2024
1 parent 9901197 commit baea838
Show file tree
Hide file tree
Showing 10 changed files with 195 additions and 22 deletions.
14 changes: 13 additions & 1 deletion rm_msgs/msg/referee/EventData.msg
Original file line number Diff line number Diff line change
@@ -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
22 changes: 21 additions & 1 deletion rm_msgs/msg/referee/RfidStatus.msg
Original file line number Diff line number Diff line change
@@ -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
2 changes: 1 addition & 1 deletion rm_referee/include/rm_referee/common/data.h
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,12 @@
#include <rm_msgs/ClientMapReceiveData.h>
#include <rm_msgs/SupplyProjectileAction.h>
#include <rm_msgs/IcraBuffDebuffZoneStatus.h>
#include <rm_msgs/SentryDeviate.h>
#include <rm_msgs/GameRobotPosData.h>
#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 <rm_msgs/PowerManagementSampleAndStatusData.h>
#include <rm_msgs/PowerManagementSystemExceptionData.h>
#include <rm_msgs/PowerManagementInitializationExceptionData.h>
Expand Down
45 changes: 43 additions & 2 deletions rm_referee/include/rm_referee/common/protocol.h
Original file line number Diff line number Diff line change
Expand Up @@ -95,6 +95,7 @@ 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
} DataCmdId;

Expand Down Expand Up @@ -283,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
Expand Down Expand Up @@ -372,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
Expand Down Expand Up @@ -536,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;
Expand Down
3 changes: 2 additions & 1 deletion rm_referee/include/rm_referee/referee.h
Original file line number Diff line number Diff line change
Expand Up @@ -74,6 +74,7 @@ class Referee
game_robot_pos_pub_ = nh.advertise<rm_msgs::GameRobotPosData>("game_robot_pos", 1);
sentry_info_pub_ = nh.advertise<rm_msgs::SentryInfo>("sentry_info", 1);
radar_info_pub_ = nh.advertise<rm_msgs::RadarInfo>("radar_info", 1);
sentry_to_radar_pub_ = nh.advertise<rm_msgs::SentryAttackingTarget>("sentry_target_to_radar", 1);

ros::NodeHandle power_management_nh = ros::NodeHandle(nh, "power_management");
power_management_sample_and_status_data_pub_ =
Expand Down Expand Up @@ -101,7 +102,7 @@ class Referee
ros::Publisher game_status_pub_;
ros::Publisher power_heat_data_pub_;
ros::Publisher game_robot_hp_pub_;
ros::Publisher bullet_num_pub_;
ros::Publisher sentry_to_radar_pub_;
ros::Publisher event_data_pub_;
ros::Publisher dart_status_pub_;
ros::Publisher buff_pub_;
Expand Down
4 changes: 3 additions & 1 deletion rm_referee/include/rm_referee/referee_base.h
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ 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 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);
Expand Down Expand Up @@ -87,6 +87,7 @@ 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_;
Expand Down Expand Up @@ -132,6 +133,7 @@ class RefereeBase
CustomInfoSender* enemy_hero_state_sender_{};
CustomInfoSender* sentry_state_sender_{};
BulletNumShare* bullet_num_share_{};
SentryToRadar* sentry_to_radar_{};

GroupUiBase* graph_queue_sender_{};
std::deque<Graph> graph_queue_;
Expand Down
17 changes: 15 additions & 2 deletions rm_referee/include/rm_referee/ui/interactive_data.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,8 @@ class InteractiveSender : public UiBase
void sendMapSentryData(const rm_referee::MapSentryData& data);
void sendSentryCmdData(const rm_msgs::SentryInfoConstPtr& data);
void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data);
ros::Duration getDelayTime();
virtual bool needSendInteractiveData();
ros::Time last_send_time_;
};

class CustomInfoSender : public InteractiveSender
Expand All @@ -43,8 +44,20 @@ class BulletNumShare : public InteractiveSender
: InteractiveSender(rpc_value, base, graph_queue, character_queue){};
void sendBulletData();
void updateBulletRemainData(const rm_msgs::BulletAllowance& data);
ros::Time last_send_time_;
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>* graph_queue = nullptr,
std::deque<Graph>* 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_;
};

} // namespace rm_referee
50 changes: 48 additions & 2 deletions rm_referee/src/referee.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -431,6 +467,16 @@ int Referee::unpack(uint8_t* rx_data)
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;
}
case rm_referee::CLIENT_MAP_CMD:
Expand Down
19 changes: 13 additions & 6 deletions rm_referee/src/referee_base.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh)
nh.subscribe<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this);
RefereeBase::shoot_cmd_sub_ = nh.subscribe<rm_msgs::ShootCmd>("/controllers/shooter_controller/command", 1,
&RefereeBase::shootCmdCallBack, this);
RefereeBase::sentry_to_radar_sub_ = nh.subscribe<rm_msgs::SentryAttackingTarget>(
"/sentry_target_to_referee", 1, &RefereeBase::sentryAttackingTargetCallback, this);

XmlRpc::XmlRpcValue rpc_value;
send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15);
Expand Down Expand Up @@ -156,11 +158,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 CustomInfoSender(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 CustomInfoSender(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_);
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_);
}
}

Expand Down Expand Up @@ -266,8 +270,10 @@ void RefereeBase::sendSerialDataCallback()
while (character_queue_.size() > 8)
character_queue_.pop_front();
}
if (bullet_num_share_ && ros::Time::now() - bullet_num_share_->last_send_time_ > bullet_num_share_->getDelayTime())
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();
}
Expand Down Expand Up @@ -497,10 +503,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;
Expand Down
41 changes: 36 additions & 5 deletions rm_referee/src/ui/interactive_data.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,11 @@ void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, un
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 };
Expand Down Expand Up @@ -139,11 +144,6 @@ void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data)
sendSerial(ros::Time::now(), data_len);
}

ros::Duration InteractiveSender::getDelayTime()
{
return delay_;
}

void BulletNumShare::sendBulletData()
{
uint16_t receiver_id;
Expand Down Expand Up @@ -178,4 +178,35 @@ void BulletNumShare::updateBulletRemainData(const rm_msgs::BulletAllowance& data
bullet_42_mm_num_ = data.bullet_allowance_num_42_mm;
}

void SentryToRadar::updateSentryAttackingTargetData(const rm_msgs::SentryAttackingTargetConstPtr& data)
{
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;
pack(tx_buffer_, tx_data, RefereeCmdId::INTERACTIVE_DATA_CMD, sizeof(SentryAttackingTargetData));
tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast<int>(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

0 comments on commit baea838

Please sign in to comment.