diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 58d731d8..a64f7f7e 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -27,6 +27,7 @@ add_message_files( MultiDofCmd.msg TagMsg.msg TagMsgArray.msg + SentryDeviate.msg ) add_service_files( @@ -63,6 +64,7 @@ add_message_files( SupplyProjectileAction.msg DartRemainingTime.msg RobotHurt.msg + CurrentSentryPosData.msg ShootData.msg BulletAllowance.msg RfidStatus.msg diff --git a/rm_msgs/msg/SentryDeviate.msg b/rm_msgs/msg/SentryDeviate.msg new file mode 100644 index 00000000..77079b16 --- /dev/null +++ b/rm_msgs/msg/SentryDeviate.msg @@ -0,0 +1,3 @@ +float32 deviate_x +float32 deviate_y +bool isDeviate diff --git a/rm_msgs/msg/referee/CurrentSentryPosData.msg b/rm_msgs/msg/referee/CurrentSentryPosData.msg new file mode 100644 index 00000000..9354c5f7 --- /dev/null +++ b/rm_msgs/msg/referee/CurrentSentryPosData.msg @@ -0,0 +1,4 @@ +float32 x +float32 y +float32 z +float32 yaw diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index c256d98d..daff6734 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -80,6 +80,8 @@ #include #include #include +#include +#include #include #include #include diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 6f9aabd0..f309e110 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -76,7 +76,7 @@ typedef enum POWER_MANAGEMENT_INITIALIZATION_EXCEPTION_CMD = 0X8302, POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD = 0X8303, POWER_MANAGEMENT_PROCESS_STACK_OVERFLOW_CMD = 0X8304, - POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD = 0X8305 + POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD = 0X8305, } RefereeCmdId; typedef enum @@ -89,6 +89,7 @@ typedef enum CLIENT_GRAPH_FIVE_CMD = 0x0103, CLIENT_GRAPH_SEVEN_CMD = 0x0104, CLIENT_CHARACTER_CMD = 0x0110, + CURRENT_SENTRY_POSITION_CMD = 0x0200 // send radar->sentry } DataCmdId; typedef enum @@ -540,6 +541,15 @@ typedef struct int8_t delta_y[49]; } __packed MapSentryData; +typedef struct +{ + InteractiveDataHeader header_data; + float position_x; + float position_y; + float position_z; + float position_yaw; +} __packed CurrentSentryPosData; + typedef struct { uint8_t chassis_power_high_8_bit; diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index 79000439..23f8fab4 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -55,6 +55,7 @@ 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); event_data_pub_ = nh.advertise("event_data", 1); dart_status_pub_ = nh.advertise("dart_status_data", 1); icra_buff_debuff_zone_status_pub_ = @@ -97,6 +98,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 event_data_pub_; ros::Publisher dart_status_pub_; ros::Publisher icra_buff_debuff_zone_status_pub_; diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 4be31103..0fd0fcd7 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -50,6 +50,8 @@ 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); // send graph_type ui void sendGraphQueueCallback(); @@ -72,6 +74,8 @@ class RefereeBase ros::Subscriber balance_state_sub_; ros::Subscriber radar_receive_sub_; ros::Subscriber map_sentry_sub_; + ros::Subscriber sentry_deviate_sub_; + ros::Subscriber radar_to_sentry_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index 03725530..dc60a1a5 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -40,6 +40,7 @@ class UiBase void sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data); void sendRadarInteractiveData(rm_referee::ClientMapReceiveData& data); void sendMapSentryData(const rm_msgs::MapSentryDataConstPtr& data); + void sendCurrentSentryData(const rm_msgs::CurrentSentryPosDataConstPtr& data); void sendSerial(const ros::Time& time, int data_len); void clearTxBuffer(); diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 6b2aa6d8..cd1662e2 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -425,6 +425,19 @@ 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) + { + 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); + } break; } case rm_referee::CLIENT_MAP_CMD: @@ -460,6 +473,18 @@ int Referee::unpack(uint8_t* rx_data) client_map_send_data_pub_.publish(client_map_send_data); break; } + /* + m_msgs::GameRobotPos game_robot_pos_data; + memcpy(&game_robot_pos_ref, rx_data + 7, sizeof(rm_referee::GameRobotPos)); + +game_robot_pos_data.x = game_robot_pos_ref.x; +game_robot_pos_data.y = game_robot_pos_ref.y; +game_robot_pos_data.z = game_robot_pos_ref.z; +game_robot_pos_data.yaw = game_robot_pos_ref.yaw; + +game_robot_pos_pub_.publish(game_robot_pos_data); + + */ case rm_referee::POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD: { rm_msgs::PowerManagementSampleAndStatusData sample_and_status_pub_data; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index f39e89e1..2b1868f9 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -34,6 +34,11 @@ 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::sentry_deviate_sub_ = + nh.subscribe("/deviate", 10, &RefereeBase::sentryDeviateCallback, this); + RefereeBase::radar_to_sentry_sub_ = nh.subscribe( + "/radar_to_sentry", 10, &RefereeBase::sendCurrentSentryCallback, this); + XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); add_ui_frequency_ = getParam(nh, "add_ui_frequency", 5); @@ -344,6 +349,9 @@ 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::radarReceiveCallback(const rm_msgs::ClientMapReceiveData::ConstPtr& data) { rm_referee::ClientMapReceiveData send_data; @@ -357,4 +365,10 @@ void RefereeBase::mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data) { interactive_data_sender_->sendMapSentryData(data); } + +void RefereeBase::sendCurrentSentryCallback(const rm_msgs::CurrentSentryPosDataConstPtr& data) +{ + interactive_data_sender_->sendCurrentSentryData(data); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index dd0f427f..25ea95f4 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -138,6 +138,28 @@ void UiBase::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char sendSerial(ros::Time::now(), sizeof(InteractiveData)); } +void UiBase::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)