From f85e1407d2f44940034bd20fe58b6ada13f511c5 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 10 Jul 2024 15:00:35 +0800 Subject: [PATCH 1/7] Add gimbal_buff_error when tracking buff. --- .../include/rm_common/decision/command_sender.h | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 537f3918..83238997 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -352,8 +352,10 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || (track_data_.accel > target_acceleration_tolerance_)) || (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) @@ -491,6 +497,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Wed, 10 Jul 2024 15:04:05 +0800 Subject: [PATCH 2/7] Modift something unreasonable. --- rm_common/include/rm_common/decision/command_sender.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 83238997..7e965c06 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -354,8 +354,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Wed, 10 Jul 2024 15:25:29 +0800 Subject: [PATCH 3/7] Modify something wrong. --- rm_referee/include/rm_referee/ui/flash_ui.h | 34 +++++------ rm_referee/src/referee_base.cpp | 8 +-- rm_referee/src/ui/flash_ui.cpp | 67 +++++++++++---------- 3 files changed, 54 insertions(+), 55 deletions(-) diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 8338ef17..ce3937f1 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -27,40 +27,40 @@ class FlashGroupUi : public GroupUiBase public: explicit FlashGroupUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, const std::string& graph_name, std::deque* graph_queue, std::deque* character_queue) - : GroupUiBase(rpc_value, base ,graph_queue,character_queue) + : GroupUiBase(rpc_value, base, graph_queue, character_queue) { - graph_name_ = graph_name; + graph_name_ = graph_name; } virtual void display(const ros::Time& time){}; virtual void updateConfig(){}; void updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph); protected: - std::string graph_name_; + std::string graph_name_; }; class EngineerActionFlashUi : public FlashGroupUi { public: - explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + explicit EngineerActionFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, std::deque* character_queue) - : FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue) + : FlashGroupUi(rpc_value, base, "engineer_action", graph_queue, character_queue) + { + if (rpc_value.hasMember("data")) { - if(rpc_value.hasMember("data")) - { - XmlRpc::XmlRpcValue& data = rpc_value["data"]; - for (int i = 0; i < static_cast(rpc_value["data"].size()); i++) - { - graph_vector_.insert( - std::pair(std::to_string(static_cast(data[i]["flag"])), new Graph(data[i]["config"], base_, id_++))); - } - } + XmlRpc::XmlRpcValue& data = rpc_value["data"]; + for (int i = 0; i < static_cast(rpc_value["data"].size()); i++) + { + graph_vector_.insert(std::pair(std::to_string(static_cast(data[i]["flag"])), + new Graph(data[i]["config"], base_, id_++))); + } } - void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); + } + void updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time& last_get_data_time); private: - void display(const ros::Time& time) override; - uint32_t symbol_; + void display(const ros::Time& time) override; + uint32_t symbol_; }; class CoverFlashUi : public FlashUi diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 284f0e3f..618f7023 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -144,9 +144,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) exceed_bullet_speed_flash_ui_ = new ExceedBulletSpeedFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "engineer_action") - engineer_action_flash_ui_ = - new EngineerActionFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - + engineer_action_flash_ui_ = new EngineerActionFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -449,8 +447,8 @@ void RefereeBase::engineerUiDataCallback(const rm_msgs::EngineerUi::ConstPtr& da stone_num_trigger_change_ui_->updateStringUiData(std::to_string(data->stone_num)); if (servo_mode_trigger_change_ui_ && !is_adding_) servo_mode_trigger_change_ui_->updateStringUiData(data->control_mode); - if(engineer_action_flash_ui_ && !is_adding_) - engineer_action_flash_ui_->updateEngineerUiCmdData(data,ros::Time::now()); + if (engineer_action_flash_ui_ && !is_adding_) + engineer_action_flash_ui_->updateEngineerUiCmdData(data, ros::Time::now()); } void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& data) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 8173fabc..d31f39a5 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -37,46 +37,47 @@ void FlashUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once void FlashGroupUi::updateFlashUiForQueue(const ros::Time& time, bool state, bool once, Graph* graph) { - if (once) - { - if (state) - graph->setOperation(rm_referee::GraphOperation::ADD); - else - graph->setOperation(rm_referee::GraphOperation::DELETE); - } - else if (time - last_send_ > delay_) - { - ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); - if (state) - graph->setOperation(rm_referee::GraphOperation::ADD); - else - graph->setOperation(rm_referee::GraphOperation::DELETE); - } - if (graph->isRepeated()) - return; - graph->updateLastConfig(); - last_send_ = time; - if (graph->isString()) - character_queue_->push_back(*graph); + if (once) + { + if (state) + graph->setOperation(rm_referee::GraphOperation::ADD); + else + graph->setOperation(rm_referee::GraphOperation::DELETE); + } + else if (time - last_send_ > delay_) + { + ROS_INFO("%f %.3f", last_send_.toSec(), delay_.toSec()); + if (state) + graph->setOperation(rm_referee::GraphOperation::ADD); else - graph_queue_->push_back(*graph); + graph->setOperation(rm_referee::GraphOperation::DELETE); + } + if (graph->isRepeated()) + return; + graph->updateLastConfig(); + last_send_ = time; + if (graph->isString()) + character_queue_->push_back(*graph); + else + graph_queue_->push_back(*graph); } -void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, const ros::Time &last_get_data_time) +void EngineerActionFlashUi::updateEngineerUiCmdData(const rm_msgs::EngineerUi::ConstPtr data, + const ros::Time& last_get_data_time) { - symbol_ = data->symbol; - display(last_get_data_time); + symbol_ = data->symbol; + display(last_get_data_time); } -void EngineerActionFlashUi::display(const ros::Time &time) +void EngineerActionFlashUi::display(const ros::Time& time) { - for (auto graph : graph_vector_) - { - bool state = false; - if (std::to_string(static_cast(symbol_)) == graph.first) - state = true; - FlashGroupUi::updateFlashUiForQueue(time, state, true, graph.second); - } + for (auto graph : graph_vector_) + { + bool state = false; + if (std::to_string(static_cast(symbol_)) == graph.first) + state = true; + FlashGroupUi::updateFlashUiForQueue(time, state, true, graph.second); + } } void CoverFlashUi::display(const ros::Time& time) From df3661954a0e25bb0e45351508a102f5f45e1daa Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Thu, 11 Jul 2024 09:39:57 +0800 Subject: [PATCH 4/7] Modify for clean. --- .../rm_common/decision/command_sender.h | 19 ++++++++----------- 1 file changed, 8 insertions(+), 11 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 7e965c06..e044f535 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -352,9 +352,9 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || + double gimbal_error_tolerance; + gimbal_error_tolerance = track_data_.id == 7 ? track_buff_error_ : track_armor_error_; + if (((gimbal_des_error_.error > gimbal_error_tolerance && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || (track_data_.accel > target_acceleration_tolerance_)) || (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) if (msg_.mode == rm_msgs::ShootCmd::PUSH) @@ -495,9 +493,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Mon, 15 Jul 2024 10:48:06 +0800 Subject: [PATCH 5/7] Modify wrong id. --- rm_common/include/rm_common/decision/command_sender.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index e044f535..fcac5936 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -407,7 +407,7 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || (track_data_.accel > target_acceleration_tolerance_)) || (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) From 20efc7c3423ce173c70fd77ec4ac7610e759c718 Mon Sep 17 00:00:00 2001 From: liyixin135 <421159734@qq.com> Date: Wed, 17 Jul 2024 11:58:34 +0800 Subject: [PATCH 6/7] Modify param's name. --- rm_common/include/rm_common/decision/command_sender.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index fcac5936..5c1f869e 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -352,9 +352,9 @@ class ShooterCommandSender : public TimeStampCommandSenderBase gimbal_error_tolerance && time - gimbal_des_error_.stamp < ros::Duration(0.1)) || (track_data_.accel > target_acceleration_tolerance_)) || (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE)) @@ -493,8 +492,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase Date: Fri, 19 Jul 2024 01:54:41 +0800 Subject: [PATCH 7/7] Modify code position for clean. --- rm_common/include/rm_common/decision/command_sender.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 660e3508..3e77e0a0 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -356,14 +356,14 @@ class ShooterCommandSender : public TimeStampCommandSenderBase