From 1454a787a3ef1030e067498b6ea63ed18004d185 Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 29 May 2024 17:17:38 +0800 Subject: [PATCH 1/3] Add wheel speed ui and fix bug. --- rm_referee/include/rm_referee/referee_base.h | 3 +++ .../include/rm_referee/ui/trigger_change_ui.h | 13 ++++++++++ rm_referee/src/referee_base.cpp | 24 +++++++++---------- rm_referee/src/ui/flash_ui.cpp | 9 ++++--- rm_referee/src/ui/time_change_ui.cpp | 6 +++-- rm_referee/src/ui/trigger_change_ui.cpp | 12 ++++++++++ 6 files changed, 49 insertions(+), 18 deletions(-) diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index c704f22e..5246f89b 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -62,6 +62,7 @@ class RefereeBase 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(); @@ -90,6 +91,7 @@ class RefereeBase 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 +99,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_{}; 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/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 898ec405..d4fb3b77 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -46,6 +46,8 @@ 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); XmlRpc::XmlRpcValue rpc_value; send_ui_queue_delay_ = getParam(nh, "send_ui_queue_delay", 0.15); @@ -72,22 +74,12 @@ 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"] == "drag") - // drag_state_trigger_change_ui_ = - // new StringTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_, "drag"); + 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_); - // if (rpc_value[i]["name"] == "step") - // step_name_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "step"); - // if (rpc_value[i]["name"] == "reversal") - // reversal_state_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "reversal"); - // if (rpc_value[i]["name"] == "stone") - // stone_num_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "stone"); - // if (rpc_value[i]["name"] == "temperature") - // joint_temperature_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "temperature"); - // if (rpc_value[i]["name"] == "servo_mode") - // servo_mode_trigger_change_ui_ = new StringTriggerChangeUi(rpc_value[i], base_, "mode"); } ui_nh.getParam("time_change", rpc_value); @@ -559,4 +551,10 @@ 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); +} + } // namespace rm_referee diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index c0adbafc..5068a129 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 @@ -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; @@ -65,14 +67,15 @@ 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) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 5a1b1ab1..ba8e9a29 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -326,12 +326,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(); diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 15c39700..45f8df08 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -437,4 +437,16 @@ void StringTriggerChangeUi::update() updateForQueue(true); } +void FrictionSpeedTriggerChangeUi::updateFrictionSpeedUiData(const rm_msgs::ShootCmdConstPtr& data) +{ + wheel_speed_ = data->wheel_speed; + update(); +} + +void FrictionSpeedTriggerChangeUi::update() +{ + UiBase::transferInt(std::floor(wheel_speed_ * 1000)); + updateForQueue(true); +} + } // namespace rm_referee From f4866238ae863a2fec8687498c79ebdf7c0da504 Mon Sep 17 00:00:00 2001 From: FurudoErika <115915981+GuraMumei@users.noreply.github.com> Date: Wed, 29 May 2024 18:22:27 +0800 Subject: [PATCH 2/3] Update referee_base.cpp --- rm_referee/src/referee_base.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 25129c34..9409aa34 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -223,6 +223,8 @@ void RefereeBase::addUi() 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(); From 3ccc86d18977d249b9bac8b1dfa7234eb30524bd Mon Sep 17 00:00:00 2001 From: Guramumei <1421887449@qq.com> Date: Wed, 12 Jun 2024 17:15:04 +0800 Subject: [PATCH 3/3] Update hero ui. --- rm_referee/src/ui/flash_ui.cpp | 7 ++----- rm_referee/src/ui/trigger_change_ui.cpp | 3 ++- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 5068a129..b9dab75e 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -18,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 @@ -81,12 +81,9 @@ void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) 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/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 45f8df08..2a7923a4 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -445,7 +445,8 @@ void FrictionSpeedTriggerChangeUi::updateFrictionSpeedUiData(const rm_msgs::Shoo void FrictionSpeedTriggerChangeUi::update() { - UiBase::transferInt(std::floor(wheel_speed_ * 1000)); + graph_->setRadius(std::floor(wheel_speed_)); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); updateForQueue(true); }