From 7975407dc86234b4e1d43cc5a88f436392b61894 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 25 Apr 2024 20:22:32 +0800 Subject: [PATCH 1/7] Add new ui. --- rm_referee/include/rm_referee/referee_base.h | 4 ++- .../include/rm_referee/ui/trigger_change_ui.h | 13 ++++++++ rm_referee/src/referee.cpp | 1 + rm_referee/src/referee_base.cpp | 11 +++++++ rm_referee/src/ui/trigger_change_ui.cpp | 30 +++++++++++++++++++ 5 files changed, 58 insertions(+), 1 deletion(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index ece8bafd..8123dec8 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -36,6 +36,7 @@ class RefereeBase const ros::Time& last_get_data_time); virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time); virtual void updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data); + virtual void supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data); // sub call back virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state); @@ -93,8 +94,9 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; - BulletTimeChangeUi* bullet_time_change_ui_{}; + EnemySupplyBulletTriggerChangeUi* enemy_supply_bullet_trigger_change_ui_{}; + BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; EffortTimeChangeUi* effort_time_change_ui_{}; ProgressTimeChangeUi* progress_time_change_ui_{}; 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 1d3064f6..914a7ed5 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -238,4 +238,17 @@ class StringTriggerChangeUi : public TriggerChangeUi void update() override; std::string data_; }; + +class EnemySupplyBulletTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit EnemySupplyBulletTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "enemy_supply_bullet", graph_queue, character_queue){}; + void updateSupplyInfo(const rm_msgs::SupplyProjectileAction& data); + +private: + void update() override; + int supply_bullet_num_; +}; } // namespace rm_referee diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 4d7c1242..ee92b6c2 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -215,6 +215,7 @@ int Referee::unpack(uint8_t* rx_data) supply_projectile_action_data.supply_robot_id = supply_projectile_action_ref.supply_robot_id; supply_projectile_action_data.stamp = last_get_data_time_; + referee_ui_.supplyBulletDataCallBack(supply_projectile_action_data); supply_projectile_action_pub_.publish(supply_projectile_action_data); break; } diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 9bed0804..6919b3bb 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -70,6 +70,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"] == "enemy_supply_bullet") + enemy_supply_bullet_trigger_change_ui_ = + new EnemySupplyBulletTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); // if (rpc_value[i]["name"] == "drag") // drag_state_trigger_change_ui_ = // new StringTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "drag"); @@ -209,6 +212,8 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); + if (enemy_supply_bullet_trigger_change_ui_) + enemy_supply_bullet_trigger_change_ui_->addForQueue(); // if (drag_state_trigger_change_ui_) // drag_state_trigger_change_ui_->addForQueue(); if (gripper_state_trigger_change_ui_) @@ -533,4 +538,10 @@ void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data) sentry_state_sender_->sendCustomInfoData(converter.from_bytes(data->data)); } +void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data) +{ + if (enemy_supply_bullet_trigger_change_ui_ && !is_adding_) + enemy_supply_bullet_trigger_change_ui_->updateSupplyInfo(data); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index eafb1d89..c36d768d 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -436,4 +436,34 @@ void StringTriggerChangeUi::update() graph_->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(true); } + +void EnemySupplyBulletTriggerChangeUi::updateSupplyInfo(const rm_msgs::SupplyProjectileAction& data) +{ + if (base_.robot_id_ > 100) + { + if (data.supply_robot_id == rm_referee::RobotId::RED_HERO) + { + supply_bullet_num_ = data.supply_projectile_num; + update(); + } + } + else + { + if (data.supply_robot_id == rm_referee::RobotId::BLUE_HERO) + { + supply_bullet_num_ = data.supply_projectile_num; + update(); + } + } +} + +void EnemySupplyBulletTriggerChangeUi::update() +{ + graph_->setRadius(supply_bullet_num_); + graph_->setColor(graph_->getConfig().color == rm_referee::GraphColor::YELLOW ? rm_referee::GraphColor::PINK : + rm_referee::GraphColor::YELLOW); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + } // namespace rm_referee From a2dd1fe0abe3ec045cc3a77b4c33e2f3f111a387 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Tue, 7 May 2024 16:42:03 +0800 Subject: [PATCH 2/7] Add drone ui. --- .../include/rm_referee/ui/time_change_ui.h | 33 +++++++++++++++ rm_referee/src/ui/time_change_ui.cpp | 41 +++++++++++++++++++ 2 files changed, 74 insertions(+) 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 9364a182..da8d5936 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -300,4 +300,37 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi void updateConfig() override; double target_distance_; }; + +class DroneTowardsTimeChangeUi : public TimeChangeGroupUi +{ +public: + explicit DroneTowardsTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) + { + if (rpc_value.hasMember("data")) + { + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + ori_x_ = data["ori_x"]; + ori_y_ = data["ori_y"]; + } + else + ROS_WARN("DroneTowardsTimeChangeUi config 's member 'data' not defined."); + + graph_vector_.insert( + std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_left", new Graph(rpc_value["config"], base_, id_++))); + graph_vector_.insert( + std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); + }; + void updateTowardsData(const rm_msgs::TrackData::ConstPtr& data); + +private: + void updateConfig() override; + double ori_x_, ori_y_, angle_; + int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, + right_line_y2_; +}; + } // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index fdc54823..8392b802 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -398,4 +398,45 @@ void TargetDistanceTimeChangeUi::updateConfig() UiBase::transferInt(std::floor(target_distance_ * 1000)); } +void DroneTowardsTimeChangeUi::updateTowardsData(const rm_msgs::TrackData::ConstPtr& data) +{ + mid_line_x2_ = ori_x_ + 30 * cos(angle_ - M_PI / 2); + mid_line_y2_ = ori_y_ + 30 * sin(angle_ - M_PI / 2); + mid_line_x1_ = ori_x_ + 30 * cos(angle_ + M_PI / 2); + mid_line_y1_ = ori_y_ + 30 * sin(angle_ + M_PI / 2); + left_line_x2_ = ori_x_ + 20 * cos(angle_ + (7 * M_PI) / 6); + left_line_y2_ = ori_y_ + 20 * sin(angle_ + (7 * M_PI) / 6); + right_line_x2_ = ori_x_ + 20 * cos(angle_ + M_PI / 3); + right_line_y2_ = ori_y_ + 20 * sin(angle_ + M_PI / 3); + updateForQueue(); +} + +void DroneTowardsTimeChangeUi::updateConfig() +{ + for (auto it : graph_vector_) + { + if (it.first == "drone_towards_mid") + { + it.second->setStartX(mid_line_x2_); + it.second->setStartY(mid_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + else if (it.first == "drone_towards_left") + { + it.second->setStartX(left_line_x2_); + it.second->setStartY(left_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + else if (it.first == "drone_towards_right") + { + it.second->setStartX(right_line_x2_); + it.second->setStartY(right_line_y2_); + it.second->setEndX(mid_line_x1_); + it.second->setEndY(mid_line_y1_); + } + } +} + } // namespace rm_referee From 41cf9dace04ff971f1e10079cc0bbcb926b9b81a Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 9 May 2024 00:45:15 +0800 Subject: [PATCH 3/7] Update drone ui. --- rm_referee/include/rm_referee/referee_base.h | 6 +++- rm_referee/include/rm_referee/ui/flash_ui.h | 16 ++++++++++ .../include/rm_referee/ui/time_change_ui.h | 9 +++--- rm_referee/src/referee.cpp | 1 + rm_referee/src/referee_base.cpp | 29 ++++++++++++++----- rm_referee/src/ui/flash_ui.cpp | 12 ++++++++ rm_referee/src/ui/time_change_ui.cpp | 19 ++++++------ 7 files changed, 71 insertions(+), 21 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 8123dec8..e1e5cf4d 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -37,6 +37,7 @@ class RefereeBase virtual void eventDataCallBack(const rm_msgs::EventData& event_data, const ros::Time& last_get_data_time); 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); // sub call back virtual void jointStateCallback(const sensor_msgs::JointState::ConstPtr& joint_state); @@ -60,6 +61,7 @@ class RefereeBase 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); // send ui void sendSerialDataCallback(); @@ -87,6 +89,7 @@ class RefereeBase ros::Subscriber sentry_cmd_sub_; ros::Subscriber radar_cmd_sub_; ros::Subscriber sentry_state_sub_; + ros::Subscriber drone_pose_sub_; ChassisTriggerChangeUi* chassis_trigger_change_ui_{}; ShooterTriggerChangeUi* shooter_trigger_change_ui_{}; @@ -94,7 +97,6 @@ class RefereeBase TargetTriggerChangeUi* target_trigger_change_ui_{}; TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; - EnemySupplyBulletTriggerChangeUi* enemy_supply_bullet_trigger_change_ui_{}; BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; @@ -108,6 +110,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_{}; + DroneTowardsTimeChangeUi* drone_towards_time_change_ui_{}; StringTriggerChangeUi *step_name_trigger_change_ui_{}, *servo_mode_trigger_change_ui_{}, *reversal_state_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, *joint_temperature_trigger_change_ui_{}, *drag_state_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{}; @@ -117,6 +120,7 @@ class RefereeBase CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; + ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; InteractiveSender* enemy_hero_state_sender_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index b0a39ebd..98603ec3 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -64,4 +64,20 @@ class HeroHitFlashUi : public FlashUi bool hitted_; rm_msgs::GameRobotHp last_hp_msg_; }; + +class ExceedBulletSpeedFlashUi : public FlashUi +{ +public: + explicit ExceedBulletSpeedFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : FlashUi(rpc_value, base, "exceed_bullet_speed", graph_queue, character_queue) + { + } + void updateShootData(const rm_msgs::ShootData& msg); + +private: + void display(const ros::Time& time) override; + rm_msgs::ShootData shoot_data_; +}; + } // 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 da8d5936..fdfd0f04 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -311,8 +311,8 @@ class DroneTowardsTimeChangeUi : public TimeChangeGroupUi if (rpc_value.hasMember("data")) { XmlRpc::XmlRpcValue& data = rpc_value["data"]; - ori_x_ = data["ori_x"]; - ori_y_ = data["ori_y"]; + ori_x_ = static_cast(data["ori_x"]); + ori_y_ = static_cast(data["ori_y"]); } else ROS_WARN("DroneTowardsTimeChangeUi config 's member 'data' not defined."); @@ -324,11 +324,12 @@ class DroneTowardsTimeChangeUi : public TimeChangeGroupUi graph_vector_.insert( std::pair(graph_name_ + "_right", new Graph(rpc_value["config"], base_, id_++))); }; - void updateTowardsData(const rm_msgs::TrackData::ConstPtr& data); + void updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data); private: void updateConfig() override; - double ori_x_, ori_y_, angle_; + int ori_x_, ori_y_; + double angle_; int mid_line_x1_, mid_line_y1_, mid_line_x2_, mid_line_y2_, left_line_x2_, left_line_y2_, right_line_x2_, right_line_y2_; }; diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index b3c7767d..33ae97c0 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -332,6 +332,7 @@ int Referee::unpack(uint8_t* rx_data) shoot_data.shooter_id = shoot_data_ref.shooter_id; shoot_data.stamp = last_get_data_time_; + referee_ui_.updateShootDataDataCallBack(shoot_data); shoot_data_pub_.publish(shoot_data); break; } diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 6919b3bb..bc1f6ec2 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -44,6 +44,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); RefereeBase::sentry_state_sub_ = nh.subscribe("/sentry_state", 1, &RefereeBase::sendSentryStateCallback, this); + RefereeBase::drone_pose_sub_ = + nh.subscribe("/mavros/vision_pose/pose", 1, &RefereeBase::dronePoseCallBack, this); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -70,9 +72,6 @@ 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"] == "enemy_supply_bullet") - enemy_supply_bullet_trigger_change_ui_ = - new EnemySupplyBulletTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); // if (rpc_value[i]["name"] == "drag") // drag_state_trigger_change_ui_ = // new StringTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "drag"); @@ -126,6 +125,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "target_distance") target_distance_time_change_ui_ = new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "drone_towards") + drone_towards_time_change_ui_ = + new DroneTowardsTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -140,6 +142,9 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "hero_hit") hero_hit_flash_ui_ = new HeroHitFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "exceed_bullet_speed") + exceed_bullet_speed_flash_ui_ = + new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -212,8 +217,8 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); - if (enemy_supply_bullet_trigger_change_ui_) - enemy_supply_bullet_trigger_change_ui_->addForQueue(); + if (drone_towards_time_change_ui_) + drone_towards_time_change_ui_->addForQueue(); // if (drag_state_trigger_change_ui_) // drag_state_trigger_change_ui_->addForQueue(); if (gripper_state_trigger_change_ui_) @@ -540,8 +545,18 @@ void RefereeBase::sendSentryStateCallback(const std_msgs::StringConstPtr& data) void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction& data) { - if (enemy_supply_bullet_trigger_change_ui_ && !is_adding_) - enemy_supply_bullet_trigger_change_ui_->updateSupplyInfo(data); +} + +void RefereeBase::dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data) +{ + if (drone_towards_time_change_ui_ && !is_adding_) + drone_towards_time_change_ui_->updateTowardsData(data); +} + +void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg) +{ + if (exceed_bullet_speed_flash_ui_ && !is_adding_) + exceed_bullet_speed_flash_ui_->updateShootData(msg); } } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index f4574694..c0adbafc 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -86,5 +86,17 @@ void HeroHitFlashUi::display(const ros::Time& time) FlashUi::updateFlashUiForQueue(time, hitted_, false); } +void ExceedBulletSpeedFlashUi::display(const ros::Time& time) +{ + if (shoot_data_.bullet_speed <= 30) + graph_->setOperation(rm_referee::GraphOperation::DELETE); + FlashUi::updateFlashUiForQueue(time, shoot_data_.bullet_speed > 30, true); +} + +void ExceedBulletSpeedFlashUi::updateShootData(const rm_msgs::ShootData& msg) +{ + shoot_data_ = msg; +} + } // namespace rm_referee // namespace rm_referee diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 8392b802..62069604 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -398,16 +398,17 @@ void TargetDistanceTimeChangeUi::updateConfig() UiBase::transferInt(std::floor(target_distance_ * 1000)); } -void DroneTowardsTimeChangeUi::updateTowardsData(const rm_msgs::TrackData::ConstPtr& data) +void DroneTowardsTimeChangeUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) { - mid_line_x2_ = ori_x_ + 30 * cos(angle_ - M_PI / 2); - mid_line_y2_ = ori_y_ + 30 * sin(angle_ - M_PI / 2); - mid_line_x1_ = ori_x_ + 30 * cos(angle_ + M_PI / 2); - mid_line_y1_ = ori_y_ + 30 * sin(angle_ + M_PI / 2); - left_line_x2_ = ori_x_ + 20 * cos(angle_ + (7 * M_PI) / 6); - left_line_y2_ = ori_y_ + 20 * sin(angle_ + (7 * M_PI) / 6); - right_line_x2_ = ori_x_ + 20 * cos(angle_ + M_PI / 3); - right_line_y2_ = ori_y_ + 20 * sin(angle_ + M_PI / 3); + angle_ = yawFromQuat(data->pose.orientation) - M_PI / 2; + mid_line_x2_ = ori_x_ + 60 * cos(angle_ - M_PI / 2); + mid_line_y2_ = ori_y_ + 60 * sin(angle_ - M_PI / 2); + mid_line_x1_ = ori_x_ + 60 * cos(angle_ + M_PI / 2); + mid_line_y1_ = ori_y_ + 60 * sin(angle_ + M_PI / 2); + left_line_x2_ = ori_x_ + 40 * cos(angle_ + (5 * M_PI) / 6); + left_line_y2_ = ori_y_ + 40 * sin(angle_ + (5 * M_PI) / 6); + right_line_x2_ = ori_x_ + 40 * cos(angle_ + M_PI / 6); + right_line_y2_ = ori_y_ + 40 * sin(angle_ + M_PI / 6); updateForQueue(); } From 8ec94fb5304474670a7e89633e5e9f0fa3d2cad9 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 9 May 2024 00:49:21 +0800 Subject: [PATCH 4/7] Update drone ui. --- rm_referee/include/rm_referee/referee_base.h | 2 +- rm_referee/include/rm_referee/ui/time_change_ui.h | 6 +++--- rm_referee/src/referee_base.cpp | 12 ++++++------ rm_referee/src/ui/time_change_ui.cpp | 4 ++-- 4 files changed, 12 insertions(+), 12 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index e1e5cf4d..c704f22e 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -110,7 +110,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_{}; - DroneTowardsTimeChangeUi* drone_towards_time_change_ui_{}; + DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{}; StringTriggerChangeUi *step_name_trigger_change_ui_{}, *servo_mode_trigger_change_ui_{}, *reversal_state_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, *joint_temperature_trigger_change_ui_{}, *drag_state_trigger_change_ui_{}, *gripper_state_trigger_change_ui_{}; 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 fdfd0f04..2045be4e 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -301,10 +301,10 @@ class TargetDistanceTimeChangeUi : public TimeChangeUi double target_distance_; }; -class DroneTowardsTimeChangeUi : public TimeChangeGroupUi +class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi { public: - explicit DroneTowardsTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue) : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) { @@ -315,7 +315,7 @@ class DroneTowardsTimeChangeUi : public TimeChangeGroupUi ori_y_ = static_cast(data["ori_y"]); } else - ROS_WARN("DroneTowardsTimeChangeUi config 's member 'data' not defined."); + ROS_WARN("DroneTowardsTimeChangeGroupUi config 's member 'data' not defined."); graph_vector_.insert( std::pair(graph_name_ + "_mid", new Graph(rpc_value["config"], base_, id_++))); diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index bc1f6ec2..898ec405 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -126,8 +126,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) target_distance_time_change_ui_ = new TargetDistanceTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "drone_towards") - drone_towards_time_change_ui_ = - new DroneTowardsTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + drone_towards_time_change_group_ui_ = + new DroneTowardsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -217,8 +217,8 @@ void RefereeBase::addUi() engineer_joint2_time_change_ui->addForQueue(); if (engineer_joint3_time_change_ui) engineer_joint3_time_change_ui->addForQueue(); - if (drone_towards_time_change_ui_) - drone_towards_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_) @@ -549,8 +549,8 @@ void RefereeBase::supplyBulletDataCallBack(const rm_msgs::SupplyProjectileAction void RefereeBase::dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data) { - if (drone_towards_time_change_ui_ && !is_adding_) - drone_towards_time_change_ui_->updateTowardsData(data); + if (drone_towards_time_change_group_ui_ && !is_adding_) + drone_towards_time_change_group_ui_->updateTowardsData(data); } void RefereeBase::updateShootDataDataCallBack(const rm_msgs::ShootData& msg) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 62069604..5a1b1ab1 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -398,7 +398,7 @@ void TargetDistanceTimeChangeUi::updateConfig() UiBase::transferInt(std::floor(target_distance_ * 1000)); } -void DroneTowardsTimeChangeUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) +void DroneTowardsTimeChangeGroupUi::updateTowardsData(const geometry_msgs::PoseStampedConstPtr& data) { angle_ = yawFromQuat(data->pose.orientation) - M_PI / 2; mid_line_x2_ = ori_x_ + 60 * cos(angle_ - M_PI / 2); @@ -412,7 +412,7 @@ void DroneTowardsTimeChangeUi::updateTowardsData(const geometry_msgs::PoseStampe updateForQueue(); } -void DroneTowardsTimeChangeUi::updateConfig() +void DroneTowardsTimeChangeGroupUi::updateConfig() { for (auto it : graph_vector_) { From 5b50e7daa2493c0775df7c3e41da782780f27799 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 9 May 2024 00:53:18 +0800 Subject: [PATCH 5/7] Delete some ui. --- .../include/rm_referee/ui/trigger_change_ui.h | 12 -------- rm_referee/src/ui/trigger_change_ui.cpp | 29 ------------------- 2 files changed, 41 deletions(-) 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 914a7ed5..90ae3381 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -239,16 +239,4 @@ class StringTriggerChangeUi : public TriggerChangeUi std::string data_; }; -class EnemySupplyBulletTriggerChangeUi : public TriggerChangeUi -{ -public: - explicit EnemySupplyBulletTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TriggerChangeUi(rpc_value, base, "enemy_supply_bullet", graph_queue, character_queue){}; - void updateSupplyInfo(const rm_msgs::SupplyProjectileAction& data); - -private: - void update() override; - int supply_bullet_num_; -}; } // namespace rm_referee diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index c36d768d..15c39700 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -437,33 +437,4 @@ void StringTriggerChangeUi::update() updateForQueue(true); } -void EnemySupplyBulletTriggerChangeUi::updateSupplyInfo(const rm_msgs::SupplyProjectileAction& data) -{ - if (base_.robot_id_ > 100) - { - if (data.supply_robot_id == rm_referee::RobotId::RED_HERO) - { - supply_bullet_num_ = data.supply_projectile_num; - update(); - } - } - else - { - if (data.supply_robot_id == rm_referee::RobotId::BLUE_HERO) - { - supply_bullet_num_ = data.supply_projectile_num; - update(); - } - } -} - -void EnemySupplyBulletTriggerChangeUi::update() -{ - graph_->setRadius(supply_bullet_num_); - graph_->setColor(graph_->getConfig().color == rm_referee::GraphColor::YELLOW ? rm_referee::GraphColor::PINK : - rm_referee::GraphColor::YELLOW); - graph_->setOperation(rm_referee::GraphOperation::UPDATE); - updateForQueue(true); -} - } // namespace rm_referee From 3f7c65afdad39789b3b1ae528e17f2cf31625622 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 9 May 2024 00:54:52 +0800 Subject: [PATCH 6/7] Update msg for sentry. --- rm_msgs/msg/referee/ClientMapSendData.msg | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/rm_msgs/msg/referee/ClientMapSendData.msg b/rm_msgs/msg/referee/ClientMapSendData.msg index 628adb38..591fc67f 100644 --- a/rm_msgs/msg/referee/ClientMapSendData.msg +++ b/rm_msgs/msg/referee/ClientMapSendData.msg @@ -1,14 +1,17 @@ uint8 KEY_A = 65 +uint8 KEY_E = 69 uint8 KEY_H = 72 uint8 KEY_K = 75 uint8 KEY_L = 76 uint8 KEY_N = 78 uint8 KEY_O = 79 +uint8 KEY_Q = 81 +uint8 KEY_R = 82 +uint8 KEY_W = 87 uint8 KEY_U = 85 float32 target_position_x float32 target_position_y -float32 target_position_z uint8 command_keyboard uint16 target_robot_ID uint8 cmd_source From a2070924f4576ae36b4659166ecef760faab9c9c Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Thu, 9 May 2024 00:56:44 +0800 Subject: [PATCH 7/7] Update format. --- rm_referee/include/rm_referee/ui/time_change_ui.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 2045be4e..911d9264 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -305,7 +305,7 @@ class DroneTowardsTimeChangeGroupUi : public TimeChangeGroupUi { public: explicit DroneTowardsTimeChangeGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) + std::deque* character_queue) : TimeChangeGroupUi(rpc_value, base, "drone_towards", graph_queue, character_queue) { if (rpc_value.hasMember("data"))