diff --git a/rm_common/include/rm_common/decision/invincible_detect.h b/rm_common/include/rm_common/decision/invincible_detect.h new file mode 100644 index 00000000..3eaa9fbe --- /dev/null +++ b/rm_common/include/rm_common/decision/invincible_detect.h @@ -0,0 +1,54 @@ +// +// Created by ray on 25-1-11. +// + +#pragma once + +#include +#include +#include + +namespace rm_common +{ +class InvincibleDetect +{ +public: + InvincibleDetect(ros::NodeHandle& nh) + { + if (!nh.getParam("invincible_check", invincible_check_)) + ROS_ERROR("Invincible_check no defined (namespace: %s)", nh.getNamespace().c_str()); + } + typedef enum + { + INJURABLE = 0, + IN_SUPPLY_BASE = 1, + NORMAL_FRESHLY_RESURRECTED = 2, + MONEY_FRESHLY_RESURRECTED = 3, + } InvincibleState; + + // void updateEnemyStatus(int robot_id) + // 调用上面的update函数,需要输入敌方id,然后函数中直接更新对应id机器人的无敌状态 + // InvincibleState getInvincibleState(int robot_id) + // 用这个函数来get对应机器人的无敌状态,然后用这个状态做判断,如果不是INJURABLE,就把shooter设为READY + +private: + // updateState() + void updateRobotInfo(int track_id, rm_msgs::GameRobotStatus& data) + { + robot_id = data.robot_id; + if (robot_id < 100) + track_id = track_id + 100; + } + + void updateInvincibleState(int track_id, bool is_red) + { + } + + int last_hp; + float last_revive_time; + float last_heal_time; + int robot_id; + InvincibleState invincible_state = INJURABLE; + XmlRpc::XmlRpcValue invincible_check_; +}; +} // namespace rm_common diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index a50f4aa0..19d85691 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -75,14 +76,12 @@ class PowerLimit ROS_ERROR("Burst power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("gyro_power", gyro_power_)) ROS_ERROR("Gyro power no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("upstairs_power", upstairs_power_)) - ROS_ERROR("Upstairs power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("max_power_limit", max_power_limit_)) ROS_ERROR("max power limit no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("power_gain", power_gain_)) - ROS_ERROR("power gain no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("total_burst_time", total_burst_time_)) - ROS_ERROR("total burst time no defined (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("robot_type", robot_type_)) + ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str()); + if (!nh.getParam("upstairs_power", upstairs_power_)) + ROS_ERROR("Upstairs power no defined (namespace: %s)", nh.getNamespace().c_str()); default_max_power_limit_ = max_power_limit_; default_burst_power_ = burst_power_; } @@ -97,37 +96,55 @@ class PowerLimit void updateSafetyPower(int safety_power) { - if (safety_power > 0) - safety_power_ = safety_power; - ROS_INFO("update safety power: %d", safety_power); + if (robot_type_ == "standard") + safety_power_ = 40 + robot_id_ * 5; + else if (robot_type_ == "hero") + safety_power_ = 45 + robot_id_ * 5; + else + safety_power_ = safety_power > 0 ? safety_power : safety_power_; + ROS_WARN("update safety power: %.0f", safety_power_); } + + void updateBurstPower(int burst_power) + { + burst_power_ = burst_power > 0 ? burst_power : burst_power_; + ROS_INFO("update burst power: %.0f", burst_power_); + } + void updateState(uint8_t state) { expect_state_ = state; } + void setGameRobotData(const rm_msgs::GameRobotStatus data) { + robot_level_ = data.robot_level; robot_id_ = data.robot_id; chassis_power_limit_ = data.chassis_power_limit; } + void setChassisPowerBuffer(const rm_msgs::PowerHeatData data) { chassis_power_buffer_ = data.chassis_power_buffer; } + void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { - capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3); + capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(5); cap_energy_ = data.capacity_remain_charge; cap_state_ = data.state_machine_running_state; } + void setRefereeStatus(bool status) { referee_is_online_ = status; } + void setStartBurstTime(const ros::Time start_burst_time) { start_burst_time_ = start_burst_time; } + ros::Time getStartBurstTime() const { return start_burst_time_; @@ -188,7 +205,7 @@ class PowerLimit if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER) chassis_cmd.power_limit = 400; else - { // standard and hero + { if (referee_is_online_) { if (capacity_is_online_ && expect_state_ != ALLOFF) @@ -218,7 +235,10 @@ class PowerLimit normal(chassis_cmd); } else + { + updateSafetyPower(safety_power_); chassis_cmd.power_limit = safety_power_; + } } applyPosturePowerScale(chassis_cmd); } @@ -279,8 +299,10 @@ class PowerLimit uint8_t expect_state_{}, cap_state_{}; + std::string robot_type_{}; int chassis_power_buffer_{}; - int robot_id_{}, chassis_power_limit_{}; + int robot_id_{}, robot_level_{}; + int chassis_power_limit_{}; double max_power_limit_{ 70.0 }; float cap_energy_{}; double safety_power_{}; @@ -291,13 +313,11 @@ class PowerLimit double disable_normal_cap_threshold_{}; double extra_power_{}, burst_power_{}, gyro_power_{}, upstairs_power_{}; double default_max_power_limit_{}, default_burst_power_{}; - double power_gain_{}; bool allow_gyro_cap_{ false }, allow_use_cap_{ false }; double posture_power_scale_{ 1.0 }; ros::Time start_burst_time_{}; - int total_burst_time_{}; bool referee_is_online_{ false }; bool capacity_is_online_{ false }; diff --git a/rm_common/include/rm_common/math_utilities.h b/rm_common/include/rm_common/math_utilities.h index 841a5674..08aac317 100644 --- a/rm_common/include/rm_common/math_utilities.h +++ b/rm_common/include/rm_common/math_utilities.h @@ -76,3 +76,13 @@ T alpha(T cutoff, double freq) T te = 1.0 / freq; return 1.0 / (1.0 + tau / te); } + +template +T limit(T val, T min, T max) +{ + if (val > max) + return max; + if (val < min) + return min; + return val; +} diff --git a/rm_common/include/rm_common/rls.h b/rm_common/include/rm_common/rls.h new file mode 100644 index 00000000..cf0f2030 --- /dev/null +++ b/rm_common/include/rm_common/rls.h @@ -0,0 +1,165 @@ +// +// Created by xiezd on 3/25/26. +// + +#pragma once + +#include +#include +#include + +#include +#include "eigen_types.h" + +#define RLS_ASSERT(condition, message) \ + if (!(condition)) \ + throw std::invalid_argument(message) + +template +class Rls +{ +public: + Rls(int xSize, int ySize, T lambda, T pInit) : xSize_(xSize), ySize_(ySize), lambda_(lambda) + { + RLS_ASSERT(xSize_ > 0, "rls: xSize should be positive"); + RLS_ASSERT(ySize_ > 0, "rls: ySize should be positive"); + RLS_ASSERT(std::abs(lambda_) > std::numeric_limits::epsilon(), "rls: lambda should be non-zero"); + + x_.resize(xSize_, 1); + x_.setZero(); + xt_.resize(1, xSize_); + xt_.setZero(); + + w_.resize(xSize_, ySize_); + w_.setZero(); + output_.resize(xSize_, ySize_); + output_.setZero(); + k_.resize(xSize_, 1); + k_.setZero(); + kNumerator_.resize(xSize_, 1); + kNumerator_.setZero(); + + p_.resize(xSize_, xSize_); + p_.setIdentity(); + p_ *= pInit; + + y_.resize(ySize_, 1); + y_.setZero(); + u_.resize(ySize_, 1); + u_.setZero(); + e_.resize(ySize_, 1); + e_.setZero(); + } + + template + bool setX(const Eigen::MatrixBase& x) + { + static_assert(TX::ColsAtCompileTime == 1 || TX::ColsAtCompileTime == Eigen::Dynamic, + "rls: X should be a column vector"); + RLS_ASSERT(x.rows() == xSize_ && x.cols() == 1, "rls: X size mismatch"); + x_ = x; + return true; + } + + template + bool setY(const Eigen::MatrixBase& y) + { + static_assert(TY::ColsAtCompileTime == 1 || TY::ColsAtCompileTime == Eigen::Dynamic, + "rls: Y should be a column vector"); + RLS_ASSERT(y.rows() == ySize_ && y.cols() == 1, "rls: Y size mismatch"); + y_ = y; + return true; + } + + bool setY(T y) + { + RLS_ASSERT(ySize_ == 1, "rls: setY(scalar) requires ySize==1"); + y_(0, 0) = y; + return true; + } + + template + bool setW(const Eigen::MatrixBase& w) + { + RLS_ASSERT(w.rows() == xSize_ && w.cols() == ySize_, "rls: W size mismatch"); + w_ = w; + return true; + } + + template + bool setU(const Eigen::MatrixBase& u) + { + static_assert(TU::ColsAtCompileTime == 1 || TU::ColsAtCompileTime == Eigen::Dynamic, + "rls: U should be a column vector"); + RLS_ASSERT(u.rows() == ySize_ && u.cols() == 1, "rls: U size mismatch"); + u_ = u; + return true; + } + + bool setU(T u) + { + RLS_ASSERT(ySize_ == 1, "rls: setU(scalar) requires ySize==1"); + u_(0, 0) = u; + return true; + } + + bool update() + { + xt_ = x_.transpose(); + // Compute predicted output using current weights: u_pred = x^T * w + // Compute prediction error: e = y - u_pred + e_ = y_ - u_; + + kNumerator_ = p_ * x_; + T denominator = lambda_ + (xt_ * p_ * x_)(0, 0); + RLS_ASSERT(std::abs(denominator) > std::numeric_limits::epsilon(), + "rls: denominator too small, numerical instability"); + + // Compute gain vector: k = P*x / (lambda + x^T*P*x) + k_ = kNumerator_ / denominator; + // Update parameters: w = w + k * e^T + output_ = w_ + k_ * e_.transpose(); + w_ = output_; + + // Update covariance matrix: P = (P - k*x^T*P) / lambda + p_ = (p_ - k_ * xt_ * p_) / lambda_; + return true; + } + + DMat getW() const + { + return w_; + } + + DMat getP() const + { + return p_; + } + + DVec getError() const + { + return e_; + } + + DVec getEstimate() const + { + return u_; + } + +private: + int xSize_; + int ySize_; + T lambda_; + + DVec x_; + DMat xt_; + DMat w_; + DMat output_; + DVec k_; + DVec kNumerator_; + DMat p_; + + DVec y_; + DVec u_; + DVec e_; +}; diff --git a/rm_common/include/rm_common/traj_gen.h b/rm_common/include/rm_common/traj_gen.h index 6d00c022..1d3e065c 100644 --- a/rm_common/include/rm_common/traj_gen.h +++ b/rm_common/include/rm_common/traj_gen.h @@ -199,3 +199,41 @@ class MinTimeTraj } } }; + +template +class NonlinearTrackingDifferentiator +{ +public: + NonlinearTrackingDifferentiator(T r, T h) : r_(r), h_(h) + { + } + void update(T v, T v_dot, T af) + { + if ((v > 3. && x1_ < -3.) || (v < -3 && x1_ > 3)) + { + x1_ = v; + x2_ = 0; + } + else + { + T y = x1_ - v + h_ * x2_; + T a0 = sqrt(h_ * h_ * r_ * r_ + 8 * r_ * fabs(y)); + T a = x2_ + af * (a0 - h_ * r_) * (y > 0 ? 1 : -1); + T u = fabs(a) > h_ * r_ ? -r_ * (a > 0 ? 1 : -1) : -r_ * a / (h_ * r_); + x1_ = x1_ + h_ * (x2_ + v_dot); + x2_ = x2_ + h_ * u; + } + } + T getX1() const + { + return x1_; + } + T getX2() const + { + return x2_; + } + +private: + T r_{}, h_{}; + T x1_{}, x2_{}; +}; diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 51ddc3ff..c9bbcc1c 100755 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -128,6 +128,7 @@ add_message_files( add_action_files( FILES Engineer.action + QueryGlobalLocalization.action ) # Generate added messages and services with any dependencies listed here generate_messages( diff --git a/rm_msgs/action/QueryGlobalLocalization.action b/rm_msgs/action/QueryGlobalLocalization.action new file mode 100644 index 00000000..952fe423 --- /dev/null +++ b/rm_msgs/action/QueryGlobalLocalization.action @@ -0,0 +1,9 @@ +# Goal (Empty) +--- +# Result +bool success +float64 coarse_align_inlier_fraction +float64 fine_align_inlier_fraction +--- +# Feedback +float64 progress diff --git a/rm_msgs/msg/detection/TrackData.msg b/rm_msgs/msg/detection/TrackData.msg index 276929d6..4a749245 100644 --- a/rm_msgs/msg/detection/TrackData.msg +++ b/rm_msgs/msg/detection/TrackData.msg @@ -10,4 +10,5 @@ float64 v_yaw float64 radius_1 float64 radius_2 float64 dz +float64 za float64 accel diff --git a/rm_msgs/msg/referee/ManualToReferee.msg b/rm_msgs/msg/referee/ManualToReferee.msg index a51c9af6..9e1d83bc 100644 --- a/rm_msgs/msg/referee/ManualToReferee.msg +++ b/rm_msgs/msg/referee/ManualToReferee.msg @@ -3,7 +3,7 @@ time start_burst_time #standard uint8 shoot_frequency -bool cover_state +bool zip_state #hero bool gimbal_eject diff --git a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg index 9946e9aa..ff9b2baa 100644 --- a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg +++ b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg @@ -1,9 +1,10 @@ float32 chassis_power -float32 chassis_expect_power -float32 capacity_recent_charge_power +float32 chassis_error_code +float32 capacity_receive_message float32 capacity_remain_charge uint8 capacity_discharge_power uint8 state_machine_running_state + uint8 power_management_protection_info uint8 power_management_topology diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index 83e162e6..3c6eb716 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -37,6 +37,8 @@ #pragma once +#include +#include #include #include #include diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index 1b3af4d1..af62db2e 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -36,6 +36,7 @@ // #pragma once +#include #include #include #include diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index 6217da4c..ff8c77ef 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -106,6 +106,8 @@ class RefereeBase TargetViewAngleTriggerChangeUi* target_view_angle_trigger_change_ui_{}; CameraTriggerChangeUi* camera_trigger_change_ui_{}; FrictionSpeedTriggerChangeUi* friction_speed_trigger_change_ui_{}; + GyroTriggerChangeUi* gyro_trigger_change_ui_{}; + ZipTriggerChangeUi* zip_trigger_change_ui_{}; BulletTimeChangeUi* bullet_time_change_ui_{}; CapacitorTimeChangeUi* capacitor_time_change_ui_{}; @@ -130,7 +132,6 @@ class RefereeBase FixedUi* fixed_ui_{}; - CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; DeployFlashUi* deploy_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index a61a592c..ec9c326d 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -64,20 +64,6 @@ class CustomizeDisplayFlashUi : public FlashGroupUi uint32_t symbol_; }; -class CoverFlashUi : public FlashUi -{ -public: - explicit CoverFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashUi(rpc_value, base, "cover", graph_queue, character_queue){}; - void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, const ros::Time& last_get_data_time) override; - -private: - void display(const ros::Time& time) override; - - uint8_t cover_state_; -}; - class SpinFlashUi : public FlashUi { public: diff --git a/rm_referee/include/rm_referee/ui/graph.h b/rm_referee/include/rm_referee/ui/graph.h index 23a9979e..27485ad8 100644 --- a/rm_referee/include/rm_referee/ui/graph.h +++ b/rm_referee/include/rm_referee/ui/graph.h @@ -53,6 +53,11 @@ class Graph { config_.radius = radius; } + void setWidth(int width) + { + if (width > 0) + config_.width = width; + } void setIntNum(int num) { int a = num & 1023; 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 b66d9edf..ae7deb47 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -159,6 +159,8 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi robot_height_ = data["height"]; camera_range_ = data["camera_range"]; surface_coefficient_ = data["surface_coefficient"]; + if (data.hasMember("x_offset")) + x_offset_ = data["x_offset"]; } else ROS_WARN("LaneLineTimeChangeGroupUi config 's member 'data' not defined."); @@ -183,6 +185,7 @@ class LaneLineTimeChangeGroupUi : public TimeChangeGroupUi protected: std::string reference_frame_; double robot_radius_, robot_height_, camera_range_, surface_coefficient_ = 0.5; + double x_offset_ = 0.0; double pitch_angle_ = 0., screen_x_ = 1920, screen_y_ = 1080; double end_point_a_angle_, end_point_b_angle_; 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 4c7d3342..2efe329d 100644 --- a/rm_referee/include/rm_referee/ui/trigger_change_ui.h +++ b/rm_referee/include/rm_referee/ui/trigger_change_ui.h @@ -3,6 +3,9 @@ // #pragma once +#include +#include + #include "rm_referee/ui/ui_base.h" #include #include "std_msgs/String.h" @@ -78,6 +81,49 @@ class ChassisTriggerChangeUi : public TriggerChangeUi double mode_change_threshold_; }; +class GyroTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit GyroTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "gyro", graph_queue, character_queue) + { + outline_width_ = graph_->getConfig().width; + const auto config = graph_->getConfig(); + const int rect_w = std::abs(config.end_x - config.start_x); + const int rect_h = std::abs(config.end_y - config.start_y); + fill_width_ = std::max(outline_width_ + 1, std::min(rect_w, rect_h) / 1); + } + void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data); + +private: + void update() override; + uint8_t chassis_mode_{}; + int outline_width_{}; + int fill_width_{}; +}; + +class ZipTriggerChangeUi : public TriggerChangeUi +{ +public: + explicit ZipTriggerChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, + std::deque* character_queue) + : TriggerChangeUi(rpc_value, base, "zip", graph_queue, character_queue) + { + outline_width_ = graph_->getConfig().width; + const auto config = graph_->getConfig(); + const int rect_w = std::abs(config.end_x - config.start_x); + const int rect_h = std::abs(config.end_y - config.start_y); + fill_width_ = std::max(outline_width_ + 1, std::min(rect_w, rect_h) / 1); + } + void updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) override; + +private: + void update() override; + uint8_t zip_mode_{}; + int outline_width_{}; + int fill_width_{}; + class HeroLegTriggerChangeUi : public TriggerChangeUi { public: diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index 2d98e329..df5083cf 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -699,8 +699,8 @@ int Referee::unpack(uint8_t* rx_data) uint8_t data[sizeof(rm_referee::PowerManagementSampleAndStatusData)]; memcpy(&data, rx_data + 7, sizeof(rm_referee::PowerManagementSampleAndStatusData)); sample_and_status_pub_data.chassis_power = (static_cast((data[0] << 8) | data[1]) / 100.); - sample_and_status_pub_data.chassis_expect_power = (static_cast((data[2] << 8) | data[3]) / 100.); - sample_and_status_pub_data.capacity_recent_charge_power = + sample_and_status_pub_data.chassis_error_code = (static_cast((data[2] << 8) | data[3]) / 100.); + sample_and_status_pub_data.capacity_receive_message = (static_cast((data[4] << 8) | data[5]) / 100.); sample_and_status_pub_data.capacity_remain_charge = (static_cast((data[6] << 8) | data[7]) / 10000.); diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 7de785a6..2c11ecd6 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -91,6 +91,10 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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"] == "gyro") + gyro_trigger_change_ui_ = new GyroTriggerChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + if (rpc_value[i]["name"] == "zip") + zip_trigger_change_ui_ = new ZipTriggerChangeUi(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_); @@ -167,8 +171,8 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) ui_nh.getParam("flash", rpc_value); for (int i = 0; i < rpc_value.size(); i++) { - if (rpc_value[i]["name"] == "cover") - cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); + // if (rpc_value[i]["name"] == "cover") + // cover_flash_ui_ = new CoverFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "spin") spin_flash_ui_ = new SpinFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "deploy") @@ -275,6 +279,10 @@ void RefereeBase::addUi() servo_mode_trigger_change_ui_->addForQueue(); if (friction_speed_trigger_change_ui_) friction_speed_trigger_change_ui_->addForQueue(); + if (gyro_trigger_change_ui_) + gyro_trigger_change_ui_->addForQueue(); + if (zip_trigger_change_ui_) + zip_trigger_change_ui_->addForQueue(); if (bullet_time_change_ui_) { bullet_time_change_ui_->reset(); @@ -483,6 +491,8 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da { if (chassis_trigger_change_ui_) chassis_trigger_change_ui_->updateChassisCmdData(data); + if (gyro_trigger_change_ui_ && !is_adding_) + gyro_trigger_change_ui_->updateChassisCmdData(data); if (spin_flash_ui_ && !is_adding_) spin_flash_ui_->updateChassisCmdData(data, ros::Time::now()); if (deploy_flash_ui_ && !is_adding_) @@ -531,10 +541,8 @@ void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& d gimbal_trigger_change_ui_->updateManualCmdData(data); if (target_trigger_change_ui_ && !is_adding_) target_trigger_change_ui_->updateManualCmdData(data); - if (cover_flash_ui_ && !is_adding_) - cover_flash_ui_->updateManualCmdData(data, ros::Time::now()); - // if (burst_flash_ui_ && !is_adding_) - // burst_flash_ui_->updateBurstTimeData(data); + if (zip_trigger_change_ui_ && !is_adding_) + zip_trigger_change_ui_->updateManualCmdData(data); } void RefereeBase::radarDataCallBack(const std_msgs::Int8MultiArrayConstPtr& data) { diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index 3ebdb597..3e7c8ef5 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -139,20 +139,6 @@ void CustomizeDisplayFlashUi::display(const ros::Time& time) } } -void CoverFlashUi::display(const ros::Time& time) -{ - if (!cover_state_) - graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateFlashUiForQueue(time, cover_state_, true); -} - -void CoverFlashUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data, - const ros::Time& last_get_data_time) -{ - cover_state_ = data->cover_state; - display(last_get_data_time); -} - void SpinFlashUi::display(const ros::Time& time) { if (chassis_mode_ != rm_msgs::ChassisCmd::RAW) diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index f4b2d2b4..0aa4a210 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -238,16 +238,16 @@ void LaneLineTimeChangeGroupUi::updateConfig() { if (it.first == "lane_line_left") { - it.second->setStartX(screen_x_ / 2 - spacing_x_a); + it.second->setStartX(screen_x_ / 2 + x_offset_ - spacing_x_a); it.second->setStartY(screen_y_ / 2 - spacing_y_a); - it.second->setEndX(screen_x_ / 2 - spacing_x_b * surface_coefficient_); + it.second->setEndX(screen_x_ / 2 + x_offset_ - spacing_x_b * surface_coefficient_); it.second->setEndY(screen_y_ / 2 - spacing_y_b); } else if (it.first == "lane_line_right") { - it.second->setStartX(screen_x_ / 2 + spacing_x_a); + it.second->setStartX(screen_x_ / 2 + x_offset_ + spacing_x_a); it.second->setStartY(screen_y_ / 2 - spacing_y_a); - it.second->setEndX(screen_x_ / 2 + spacing_x_b * surface_coefficient_); + it.second->setEndX(screen_x_ / 2 + x_offset_ + spacing_x_b * surface_coefficient_); it.second->setEndY(screen_y_ / 2 - spacing_y_b); } } diff --git a/rm_referee/src/ui/trigger_change_ui.cpp b/rm_referee/src/ui/trigger_change_ui.cpp index 2e350296..07434549 100644 --- a/rm_referee/src/ui/trigger_change_ui.cpp +++ b/rm_referee/src/ui/trigger_change_ui.cpp @@ -229,6 +229,36 @@ void ChassisTriggerChangeUi::updateCapacityResetStatus() displayInCapacity(); } +void GyroTriggerChangeUi::update() +{ + if (chassis_mode_ == rm_msgs::ChassisCmd::RAW) + graph_->setWidth(fill_width_); + else + graph_->setWidth(outline_width_); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(true); +} + +void GyroTriggerChangeUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data) +{ + chassis_mode_ = data->mode; + update(); +} + +void ZipTriggerChangeUi::update() +{ + if (zip_mode_) + graph_->setWidth(fill_width_); + else + graph_->setWidth(outline_width_); + graph_->setOperation(rm_referee::GraphOperation::UPDATE); + updateForQueue(false); +} + +void ZipTriggerChangeUi::updateManualCmdData(const rm_msgs::ManualToReferee::ConstPtr data) +{ + zip_mode_ = data->zip_state; + update(); void HeroLegTriggerChangeUi::updateMode(uint8_t mode) { leg_mode_ = mode;