From f42f6dea3bbf2cfd0180011399309f4954a57553 Mon Sep 17 00:00:00 2001 From: Whathelp233 <3351730846@qq.com> Date: Tue, 3 Mar 2026 15:46:51 +0000 Subject: [PATCH] Add RMU 2026 support. --- .github/workflows/format.yml | 26 +- .pre-commit-config.yaml | 9 +- .../rm_common/decision/command_sender.h | 143 ++---- .../include/rm_common/decision/heat_limit.h | 41 +- .../include/rm_common/decision/power_limit.h | 113 ++--- rm_common/test/LqrTest.cpp | 74 --- rm_common/test/test_kalman_filter.cpp | 116 ----- rm_common/test/test_one_euro_filter.cpp | 93 ---- rm_common/test/test_traj.cpp | 127 ------ rm_common/test/unit_test_lqr.cpp | 46 -- rm_gazebo/launch/step.launch | 32 -- rm_gazebo/worlds/place/step.stl | Bin 1084 -> 0 bytes rm_gazebo/worlds/step.world | 73 --- rm_hw/src/control_loop.cpp | 1 - rm_hw/test/test_imu.launch | 12 - rm_hw/test/test_imu.yaml | 24 - rm_hw/test/test_motor.launch | 29 -- rm_hw/test/test_motor.xacro | 24 - rm_hw/test/test_motor.yaml | 39 -- rm_msgs/CMakeLists.txt | 16 +- rm_msgs/msg/BusState.msg | 2 +- rm_msgs/msg/ChassisCmd.msg | 2 - rm_msgs/msg/CustomControllerData.msg | 2 +- rm_msgs/msg/Dart.msg | 4 - rm_msgs/msg/GimbalCmd.msg | 1 - rm_msgs/msg/GimbalPosState.msg | 2 - rm_msgs/msg/LocalHeatData.msg | 2 - rm_msgs/msg/LocalHeatState.msg | 4 +- rm_msgs/msg/ShootCmd.msg | 2 - rm_msgs/msg/SuperCapacitor.msg | 6 - rm_msgs/msg/VTKeyboardMouseData.msg | 2 +- rm_msgs/msg/VTReceiverControlData.msg | 2 +- rm_msgs/msg/detection/TargetDetection.msg | 3 - rm_msgs/msg/referee/Buff.msg | 2 +- rm_msgs/msg/referee/BulletAllowance.msg | 1 + rm_msgs/msg/referee/BulletNumShare.msg | 2 - rm_msgs/msg/referee/BulletRemaining.msg | 5 - rm_msgs/msg/referee/ClientMapReceiveData.msg | 4 +- rm_msgs/msg/referee/ClientMapSendData.msg | 2 +- rm_msgs/msg/referee/CurrentSentryPosData.msg | 0 rm_msgs/msg/referee/DartClientCmd.msg | 1 + rm_msgs/msg/referee/DartRemainingTime.msg | 4 + rm_msgs/msg/referee/EnemyHpInfo.msg | 6 + rm_msgs/msg/referee/GameRobotHp.msg | 21 +- rm_msgs/msg/referee/GameRobotStatus.msg | 2 + rm_msgs/msg/referee/ManualToReferee.msg | 1 - rm_msgs/msg/referee/PowerHeatData.msg | 1 - .../PowerManagementSampleAndStatusData.msg | 4 +- rm_msgs/msg/referee/RadarCmd.msg | 8 + rm_msgs/msg/referee/RadarInfo.msg | 4 + rm_msgs/msg/referee/RadarMarkData.msg | 13 + .../referee/RadarWirelessEnemyCallSign.msg | 3 + .../RadarWirelessEnemyCoinAndFieldStatus.msg | 5 + .../RadarWirelessEnemyProjectileAllowance.msg | 7 + .../referee/RadarWirelessEnemyRobotBuff.msg | 32 ++ .../msg/referee/RadarWirelessEnemyRobotHp.msg | 8 + .../referee/RadarWirelessEnemyRobotPos.msg | 14 + rm_msgs/msg/referee/RfidStatus.msg | 31 +- rm_msgs/msg/referee/RobotCustomData2.msg | 3 + rm_msgs/msg/referee/SentryCmd.msg | 9 +- rm_msgs/msg/referee/SentryInfo.msg | 9 + rm_msgs/srv/CloseToTarget.srv | 2 +- rm_msgs/srv/ExtraFrictionWheelSpeed.srv | 3 - rm_msgs/srv/ShooterSpeed.srv | 3 - rm_referee/CMakeLists.txt | 1 + rm_referee/include/rm_referee/common/data.h | 1 + .../include/rm_referee/common/protocol.h | 229 ++++++---- rm_referee/include/rm_referee/referee.h | 27 +- rm_referee/include/rm_referee/referee_base.h | 8 +- rm_referee/include/rm_referee/ui/flash_ui.h | 29 -- .../include/rm_referee/ui/interactive_data.h | 2 +- .../include/rm_referee/ui/time_change_ui.h | 40 -- rm_referee/include/rm_referee/ui/ui_base.h | 2 +- rm_referee/src/referee.cpp | 424 +++++++++++++----- rm_referee/src/referee_base.cpp | 31 +- rm_referee/src/referee_benchmark.cpp | 206 +++++++++ rm_referee/src/ui/flash_ui.cpp | 49 +- rm_referee/src/ui/interactive_data.cpp | 57 ++- rm_referee/src/ui/time_change_ui.cpp | 61 --- rm_referee/src/ui/ui_base.cpp | 7 +- rm_vt/CMakeLists.txt | 23 +- rm_vt/include/rm_vt/common/data.h | 1 + rm_vt/include/rm_vt/common/protocol.h | 29 +- rm_vt/include/rm_vt/video_transmission.h | 25 +- rm_vt/launch/load.launch | 0 rm_vt/package.xml | 1 + rm_vt/src/main.cpp | 0 rm_vt/src/video_transmission.cpp | 258 +++++++++-- 88 files changed, 1298 insertions(+), 1495 deletions(-) delete mode 100644 rm_common/test/LqrTest.cpp delete mode 100644 rm_common/test/test_kalman_filter.cpp delete mode 100644 rm_common/test/test_one_euro_filter.cpp delete mode 100644 rm_common/test/test_traj.cpp delete mode 100644 rm_common/test/unit_test_lqr.cpp delete mode 100644 rm_gazebo/launch/step.launch delete mode 100644 rm_gazebo/worlds/place/step.stl delete mode 100644 rm_gazebo/worlds/step.world delete mode 100644 rm_hw/test/test_imu.launch delete mode 100644 rm_hw/test/test_imu.yaml delete mode 100644 rm_hw/test/test_motor.launch delete mode 100644 rm_hw/test/test_motor.xacro delete mode 100644 rm_hw/test/test_motor.yaml mode change 100644 => 100755 rm_msgs/msg/BusState.msg mode change 100644 => 100755 rm_msgs/msg/CustomControllerData.msg delete mode 100644 rm_msgs/msg/Dart.msg delete mode 100644 rm_msgs/msg/LocalHeatData.msg delete mode 100644 rm_msgs/msg/SuperCapacitor.msg mode change 100644 => 100755 rm_msgs/msg/VTKeyboardMouseData.msg mode change 100644 => 100755 rm_msgs/msg/VTReceiverControlData.msg delete mode 100644 rm_msgs/msg/referee/BulletNumShare.msg delete mode 100644 rm_msgs/msg/referee/BulletRemaining.msg mode change 100644 => 100755 rm_msgs/msg/referee/CurrentSentryPosData.msg create mode 100755 rm_msgs/msg/referee/DartRemainingTime.msg create mode 100644 rm_msgs/msg/referee/EnemyHpInfo.msg create mode 100755 rm_msgs/msg/referee/RadarCmd.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg create mode 100644 rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg create mode 100644 rm_msgs/msg/referee/RobotCustomData2.msg mode change 100644 => 100755 rm_msgs/srv/CloseToTarget.srv delete mode 100644 rm_msgs/srv/ExtraFrictionWheelSpeed.srv delete mode 100644 rm_msgs/srv/ShooterSpeed.srv create mode 100644 rm_referee/src/referee_benchmark.cpp mode change 100644 => 100755 rm_vt/CMakeLists.txt mode change 100644 => 100755 rm_vt/include/rm_vt/common/data.h mode change 100644 => 100755 rm_vt/include/rm_vt/common/protocol.h mode change 100644 => 100755 rm_vt/include/rm_vt/video_transmission.h mode change 100644 => 100755 rm_vt/launch/load.launch mode change 100644 => 100755 rm_vt/package.xml mode change 100644 => 100755 rm_vt/src/main.cpp diff --git a/.github/workflows/format.yml b/.github/workflows/format.yml index 7ee6d89b..d6232ae7 100644 --- a/.github/workflows/format.yml +++ b/.github/workflows/format.yml @@ -11,12 +11,24 @@ on: jobs: pre-commit: name: pre-commit - runs-on: ubuntu-22.04 + runs-on: ubuntu-20.04 steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v3 - - name: Install clang-format + - uses: actions/checkout@v2 + - uses: actions/setup-python@v2 + - name: Install clang-format-10 run: | - sudo apt update - sudo apt install clang-format - - uses: pre-commit/action@v3.0.0 + wget -O - https://apt.llvm.org/llvm-snapshot.gpg.key|sudo apt-key add - + sudo apt-get install clang-format-10 + - name: Install catkin-lint + run: | + lsb_release -sc + sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' + sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 + sudo apt-get -q update + sudo apt-get -q install python3-rosdep + sudo rm -rf /etc/ros/rosdep/sources.list.d/* + sudo rosdep init + rosdep update + sudo apt-get -q install catkin-lint + export ROS_DISTRO=noetic + - uses: pre-commit/action@v2.0.0 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 904f0457..fc7336c8 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -44,7 +44,14 @@ repos: - id: clang-format name: clang-format description: Format files with ClangFormat. - entry: clang-format + entry: clang-format-10 language: system files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ args: [ "-fallback-style=none", "-i" ] + - id: catkin_lint + name: catkin_lint + description: Check package.xml and cmake files + entry: catkin_lint . + language: system + always_run: true + pass_filenames: false diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 0b2e7130..2620971d 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -70,6 +70,21 @@ namespace rm_common { +namespace detail +{ +template +auto setTrajFrameIdIfSupported(TMsg& msg, const std::string& traj_frame_id, int) + -> decltype((void)(msg.traj_frame_id = traj_frame_id), void()) +{ + msg.traj_frame_id = traj_frame_id; +} + +template +void setTrajFrameIdIfSupported(TMsg&, const std::string&, long) +{ +} +} // namespace detail + template class CommandSenderBase { @@ -269,10 +284,6 @@ class ChassisCommandSender : public TimeStampCommandSenderBasesetLimitPower(msg_, is_gyro); @@ -297,10 +308,6 @@ class GimbalCommandSender : public TimeStampCommandSenderBase 0 ? 1 : -1; if (std::abs(scale_pitch) > 1) scale_pitch = scale_pitch > 0 ? 1 : -1; - double time_constant{}; - if (use_rc_) - time_constant = time_constant_rc_; - else - time_constant = time_constant_pc_; - msg_.rate_yaw = msg_.rate_yaw + (scale_yaw * max_yaw_vel_ - msg_.rate_yaw) * (0.001 / (time_constant + 0.001)); - msg_.rate_pitch = - msg_.rate_pitch + (scale_pitch * max_pitch_vel_ - msg_.rate_pitch) * (0.001 / (time_constant + 0.001)); + msg_.rate_yaw = scale_yaw * max_yaw_vel_; + msg_.rate_pitch = scale_pitch * max_pitch_vel_; if (eject_flag_) { msg_.rate_yaw *= eject_sensitivity_; @@ -332,9 +333,9 @@ class GimbalCommandSender : public TimeStampCommandSenderBase @@ -385,11 +381,8 @@ 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)) if (msg_.mode == rm_msgs::ShootCmd::PUSH) - { setMode(rm_msgs::ShootCmd::READY); - } } void sendCommand(const ros::Time& time) override { msg_.wheel_speed = getWheelSpeedDes(); - msg_.wheels_speed_offset_front = getFrontWheelSpeedOffset(); - msg_.wheels_speed_offset_back = getBackWheelSpeedOffset(); msg_.hz = heat_limit_->getShootFrequency(); TimeStampCommandSenderBase::sendCommand(time); } @@ -505,26 +482,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBasegetSpeedLimit()) @@ -566,29 +525,13 @@ class ShooterCommandSender : public TimeStampCommandSenderBase -{ -public: - explicit UseLioCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) - { - } - - void setUseLio(bool flag) - { - msg_.data = flag; - } - bool getUseLio() const - { - return msg_.data; - } - void setZero() override{}; -}; - class BalanceCommandSender : public CommandSenderBase { public: @@ -900,16 +813,12 @@ class CameraSwitchCommandSender : public CommandSenderBase public: explicit CameraSwitchCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) { - ROS_ASSERT(nh.getParam("camera_left_name", camera1_name_) && nh.getParam("camera_right_name", camera2_name_)); - msg_.data = camera2_name_; - } - void switchCameraLeft() - { + ROS_ASSERT(nh.getParam("camera1_name", camera1_name_) && nh.getParam("camera2_name", camera2_name_)); msg_.data = camera1_name_; } - void switchCameraRight() + void switchCamera() { - msg_.data = camera2_name_; + msg_.data = msg_.data == camera1_name_ ? camera2_name_ : camera1_name_; } void sendCommand(const ros::Time& time) override { diff --git a/rm_common/include/rm_common/decision/heat_limit.h b/rm_common/include/rm_common/decision/heat_limit.h index d4c3365b..7f871648 100644 --- a/rm_common/include/rm_common/decision/heat_limit.h +++ b/rm_common/include/rm_common/decision/heat_limit.h @@ -38,12 +38,10 @@ #pragma once #include -#include #include #include #include #include -#include #include namespace rm_common @@ -68,14 +66,12 @@ class HeatLimit ROS_ERROR("Heat coeff no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("type", type_)) ROS_ERROR("Shooter type no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("local_heat_protect_threshold", heat_protect_threshold_)) - ROS_ERROR("Local heat protect threshold no defined (namespace: %s)", nh.getNamespace().c_str()); nh.param("use_local_heat", use_local_heat_, true); if (type_ == "ID1_42MM") bullet_heat_ = 100.; else bullet_heat_ = 10.; - local_heat_pub_ = nh.advertise("/local_heat_state/local_cooling_heat", 10); + local_heat_pub_ = nh.advertise("/local_heat_state/local_cooling_heat", 10); shoot_state_sub_ = nh.subscribe("/local_heat_state/shooter_state", 50, &HeatLimit::heatCB, this); timer_ = nh.createTimer(ros::Duration(0.1), std::bind(&HeatLimit::timerCB, this)); @@ -91,34 +87,24 @@ class HeatLimit void heatCB(const rm_msgs::LocalHeatStateConstPtr& msg) { - std::lock_guard lock(heat_mutex_); - if (msg->has_shoot && last_shoot_state_ != msg->has_shoot) - { + if (msg->has_shoot) local_shooter_cooling_heat_ += bullet_heat_; - if (local_shooter_cooling_heat_ > 0) - once_shoot_num_++; - } - last_shoot_state_ = msg->has_shoot; } void timerCB() { - std::lock_guard lock(heat_mutex_); if (local_shooter_cooling_heat_ > 0.0) - local_shooter_cooling_heat_ -= shooter_cooling_rate_ * 0.1; + local_shooter_cooling_heat_ -= shooter_cooling_rate_ / 10.0; if (local_shooter_cooling_heat_ < 0.0) - { local_shooter_cooling_heat_ = 0.0; - once_shoot_num_ = 0; - } - local_heat_pub_data_.once_shoot_num = once_shoot_num_; - local_heat_pub_data_.local_shooter_cooling_heat = local_shooter_cooling_heat_; - local_heat_pub_.publish(local_heat_pub_data_); + std_msgs::Float64 msg; + msg.data = local_shooter_cooling_heat_; + local_heat_pub_.publish(msg); } void setStatusOfShooter(const rm_msgs::GameRobotStatus data) { - shooter_cooling_limit_ = data.shooter_cooling_limit - heat_protect_threshold_; + shooter_cooling_limit_ = data.shooter_cooling_limit; shooter_cooling_rate_ = data.shooter_cooling_rate; } @@ -130,7 +116,8 @@ class HeatLimit } else if (type_ == "ID2_17MM") { - shooter_cooling_heat_ = data.shooter_id_2_17_mm_cooling_heat; + // RoboMaster 2026 protocol only reports one 17mm barrel heat field. + shooter_cooling_heat_ = data.shooter_id_1_17_mm_cooling_heat; } else if (type_ == "ID1_42MM") { @@ -145,7 +132,6 @@ class HeatLimit double getShootFrequency() const { - std::lock_guard lock(heat_mutex_); if (state_ == BURST) return shoot_frequency_; double shooter_cooling_heat = @@ -230,18 +216,13 @@ class HeatLimit double bullet_heat_, safe_shoot_frequency_{}, heat_coeff_{}, shoot_frequency_{}, low_shoot_frequency_{}, high_shoot_frequency_{}, burst_shoot_frequency_{}, minimal_shoot_frequency_{}; - bool referee_is_online_, use_local_heat_, last_shoot_state_{}; + bool referee_is_online_, use_local_heat_; int shooter_cooling_limit_, shooter_cooling_rate_, shooter_cooling_heat_; - double local_shooter_cooling_heat_{}, heat_protect_threshold_{}; - int once_shoot_num_{}; + double local_shooter_cooling_heat_{}; ros::Publisher local_heat_pub_; ros::Subscriber shoot_state_sub_; ros::Timer timer_; - - rm_msgs::LocalHeatData local_heat_pub_data_; - - mutable std::mutex heat_mutex_; }; } // 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 c07b54ab..d87f42b8 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -56,32 +56,18 @@ class PowerLimit ROS_ERROR("Safety power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("capacitor_threshold", capacitor_threshold_)) ROS_ERROR("Capacitor threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("disable_cap_gyro_threshold", disable_cap_gyro_threshold_)) - ROS_ERROR("Disable cap gyro threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("enable_cap_gyro_threshold", enable_cap_gyro_threshold_)) - ROS_ERROR("Enable cap gyro threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("disable_use_cap_threshold", disable_use_cap_threshold_)) - ROS_ERROR("Disable use cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("enable_use_cap_threshold", enable_use_cap_threshold_)) - ROS_ERROR("Enable use cap threshold no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("charge_power", charge_power_)) ROS_ERROR("Charge power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("extra_power", extra_power_)) ROS_ERROR("Extra power no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("burst_power", burst_power_)) ROS_ERROR("Burst power no defined (namespace: %s)", nh.getNamespace().c_str()); - if (!nh.getParam("standard_power", standard_power_)) - ROS_ERROR("Standard 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("buffer_threshold", buffer_threshold_)) ROS_ERROR("buffer threshold no defined (namespace: %s)", nh.getNamespace().c_str()); if (!nh.getParam("is_new_capacitor", is_new_capacitor_)) ROS_ERROR("is_new_capacitor 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()); } typedef enum { @@ -117,7 +103,6 @@ class PowerLimit void setChassisPowerBuffer(const rm_msgs::PowerHeatData data) { chassis_power_buffer_ = data.chassis_power_buffer; - power_buffer_threshold_ = chassis_power_buffer_ * 0.8; } void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data) { @@ -129,47 +114,11 @@ class PowerLimit { 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_; - } + uint8_t getState() { return expect_state_; } - void setGyroPower(rm_msgs::ChassisCmd& chassis_cmd) - { - if (!allow_gyro_cap_ && cap_energy_ >= enable_cap_gyro_threshold_) - allow_gyro_cap_ = true; - if (allow_gyro_cap_ && cap_energy_ <= disable_cap_gyro_threshold_) - allow_gyro_cap_ = false; - if (allow_gyro_cap_ && chassis_power_limit_ < 80) - chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; - else - normal(chassis_cmd); - // expect_state_ = NORMAL; - } - void setBurstPower(rm_msgs::ChassisCmd& chassis_cmd) - { - if (!allow_use_cap_ && cap_energy_ >= enable_use_cap_threshold_) - allow_use_cap_ = true; - if (allow_use_cap_ && cap_energy_ <= disable_use_cap_threshold_) - allow_use_cap_ = false; - if (allow_use_cap_) - { - if (ros::Time::now() - start_burst_time_ < ros::Duration(total_burst_time_)) - chassis_cmd.power_limit = burst_power_; - else - chassis_cmd.power_limit = standard_power_; - } - else - normal(chassis_cmd); - // expect_state_ = NORMAL; - } void setLimitPower(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER) @@ -178,26 +127,31 @@ class PowerLimit { // standard and hero if (referee_is_online_) { - if (capacity_is_online_ && expect_state_ != ALLOFF) + if (capacity_is_online_) { - if (chassis_power_limit_ > burst_power_) - chassis_cmd.power_limit = burst_power_; + if (expect_state_ == ALLOFF) + normal(chassis_cmd); else { - switch (is_new_capacitor_ ? expect_state_ : cap_state_) + if (chassis_power_limit_ > burst_power_) + chassis_cmd.power_limit = burst_power_; + else { - case NORMAL: - normal(chassis_cmd); - break; - case BURST: - burst(chassis_cmd, is_gyro); - break; - case CHARGE: - charge(chassis_cmd); - break; - default: - zero(chassis_cmd); - break; + switch (is_new_capacitor_ ? expect_state_ : cap_state_) + { + case NORMAL: + normal(chassis_cmd); + break; + case BURST: + burst(chassis_cmd, is_gyro); + break; + case CHARGE: + charge(chassis_cmd); + break; + default: + zero(chassis_cmd); + break; + } } } } @@ -212,18 +166,14 @@ class PowerLimit private: void charge(rm_msgs::ChassisCmd& chassis_cmd) { - allow_use_cap_ = false; chassis_cmd.power_limit = chassis_power_limit_ * 0.70; } void normal(rm_msgs::ChassisCmd& chassis_cmd) { - allow_use_cap_ = false; double buffer_energy_error = chassis_power_buffer_ - buffer_threshold_; double plus_power = buffer_energy_error * power_gain_; chassis_cmd.power_limit = chassis_power_limit_ + plus_power; // TODO:Add protection when buffer<5 - if (chassis_cmd.power_limit > max_power_limit_) - chassis_cmd.power_limit = max_power_limit_; } void zero(rm_msgs::ChassisCmd& chassis_cmd) { @@ -231,37 +181,28 @@ class PowerLimit } void burst(rm_msgs::ChassisCmd& chassis_cmd, bool is_gyro) { - if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_ && chassis_power_buffer_ > power_buffer_threshold_) + if (cap_state_ != ALLOFF && cap_energy_ > capacitor_threshold_) { if (is_gyro) - setGyroPower(chassis_cmd); + chassis_cmd.power_limit = chassis_power_limit_ + extra_power_; else - setBurstPower(chassis_cmd); + chassis_cmd.power_limit = burst_power_; } else - normal(chassis_cmd); - // expect_state_ = NORMAL; + expect_state_ = NORMAL; } int chassis_power_buffer_; int robot_id_, chassis_power_limit_; - int max_power_limit_{ 70 }; float cap_energy_; double safety_power_{}; double capacitor_threshold_{}; - double power_buffer_threshold_{ 50.0 }; - double enable_cap_gyro_threshold_{}, disable_cap_gyro_threshold_{}, enable_use_cap_threshold_{}, - disable_use_cap_threshold_{}; - double charge_power_{}, extra_power_{}, burst_power_{}, standard_power_{}; + double charge_power_{}, extra_power_{}, burst_power_{}; double buffer_threshold_{}; double power_gain_{}; bool is_new_capacitor_{ false }; uint8_t expect_state_{}, cap_state_{}; bool capacitor_is_on_{ true }; - bool allow_gyro_cap_{ false }, allow_use_cap_{ false }; - - ros::Time start_burst_time_{}; - int total_burst_time_{}; bool referee_is_online_{ false }; bool capacity_is_online_{ false }; diff --git a/rm_common/test/LqrTest.cpp b/rm_common/test/LqrTest.cpp deleted file mode 100644 index a2f54eec..00000000 --- a/rm_common/test/LqrTest.cpp +++ /dev/null @@ -1,74 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by chenzheng on 3/20/21. -// - -#include "rm_common/lqr.h" -#include - -TEST(Lqr, lqr) -{ - static const size_t STATE_DIM = 6; - static const size_t CONTROL_DIM = 2; - - Eigen::Matrix A{}, Q{}; - Eigen::Matrix B{}; - Eigen::Matrix R{}; - Eigen::Matrix K{}; - Eigen::Matrix Nbar{}; - - A << 0, 1, 0, 0, 0, 0, 432.8077, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 73.9215, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, - 0, 0, 0; - - B << 0, 0, -49.6930, -49.6930, 0, 0, 7.3620, 7.3620, 0, 0, -26.3737, 26.3737; - - Q << 10, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 1; - - R << 1, 0, 0, 1; - - K << -36.4111581829, -1.5198212494, -2.2360679774, -2.4573417169, -0.7071067811, -0.7258175096, -36.4111581829, - -1.5198212494, -2.2360679774, -2.4573417169, 0.7071067811, 0.7258175096; - - Lqr lqr(A, B, Q, R); - lqr.computeK(); - Eigen::Matrix average = lqr.getK(); - for (uint j = 0; j < CONTROL_DIM; ++j) - { - for (uint i = 0; i < STATE_DIM; ++i) - { - EXPECT_NEAR(K(j, i), average(j, i), 1e-9); - } - } -} diff --git a/rm_common/test/test_kalman_filter.cpp b/rm_common/test/test_kalman_filter.cpp deleted file mode 100644 index 6a09c6f6..00000000 --- a/rm_common/test/test_kalman_filter.cpp +++ /dev/null @@ -1,116 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by qiayuan on 4/3/20. -// -#include "rm_common/filters/kalman_filter.h" -#include -#include -#include -#include - -#define SAMPLE_RATE 1000 -#define ACTUAL_RATE 10 -#define NOISE_MAG 1. - -int main(int argc, char** argv) -{ - // Set up filter - Mat2 A, B, H, Q, R; - Vec2 x, u; - - A << 1., 1. / SAMPLE_RATE, 0., 1.; - - B << 0., 0., 0, 0; - - H << 1., 0., 0., 1.; - - Q << 300., 0., 0., 300.; - - R << 2000., 0., 0., 2000.; - - x << 0., 0.; - u << 0., 0.; - KalmanFilter filter(A, B, H, Q, R); - filter.clear(x); - - // Set up normal distribution - unsigned seed = std::chrono::system_clock::now().time_since_epoch().count(); - std::default_random_engine generator(seed); - std::normal_distribution distribution(0.0, NOISE_MAG); - - // Set up ros - ros::init(argc, argv, "traj_test"); - ros::NodeHandle nh; - ros::Publisher pub; - pub = nh.advertise("test_cmd", 100); - geometry_msgs::Twist data{}; - ros::Rate loop_rate(SAMPLE_RATE); - - // Test - int i = 0; - double x_last = 0.; - double dis = 0.; - double t = 0.; - Vec2 x_hat{}; - while (ros::ok() && t < 2.) - { - if (i >= SAMPLE_RATE / ACTUAL_RATE) - { // oversampling - i = 0; - x[0] = 20. * sin(2. * M_PI * t) + distribution(generator); - x[1] = (x[0] - x_last) * (double)ACTUAL_RATE; - x_last = x[0]; - filter.predict(u); - filter.update(x); - } - else - { - filter.predict(u); - } - x_hat = filter.getState(); - data.linear.x = 20. * sin(2. * M_PI * t); - data.linear.y = x[0]; - data.linear.z = x_hat[0]; - dis += pow(data.linear.z - data.linear.x, 2); - // dis += pow(x[1], 2); - pub.publish(data); - ++i; - t += 1. / (double)SAMPLE_RATE; - loop_rate.sleep(); - } - - std::cout << sqrt(dis) << std::endl; - return 0; -} diff --git a/rm_common/test/test_one_euro_filter.cpp b/rm_common/test/test_one_euro_filter.cpp deleted file mode 100644 index ec22752e..00000000 --- a/rm_common/test/test_one_euro_filter.cpp +++ /dev/null @@ -1,93 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -/*(utf8) -1€ Filter, template-compliant version -Jonathan Aceituno - -25/04/14: fixed bug with last_time_ never updated on line 40 - -For details, see http://www.lifl.fr/~casiez/1euro -*/ - -//#include "one_euro_filter.h" -#include "filters.h" -#include -#include - -// if high speed lag is a problem, increase beta; if slow speed jitter is a problem, decrease mincutoff. -#define FREQUENCY 120. -#define MINCUTOFF 2.543785 -#define BETA 0.000001 -#define DCUTOFF 1. - -template -T rand01() -{ - return rand() / static_cast(RAND_MAX); -} - -int main(int argc, char** argv) -{ - srand((unsigned)time(0)); - OneEuroFilter filter(FREQUENCY, MINCUTOFF, BETA, DCUTOFF); - - std::cout << "#CFG {'beta':" << BETA << ", 'freq':" << FREQUENCY << ", 'dcutoff':" << DCUTOFF - << ", 'mincutoff':" << MINCUTOFF << "}" << std::endl; - - ros::init(argc, argv, "traj_test"); - ros::NodeHandle nh; - ros::Publisher pub; - pub = nh.advertise("test_cmd", 100); - geometry_msgs::Vector3 data{}; - ros::Rate loop_rate(FREQUENCY); - double t = 0; - while (ros::ok()) - { - t += 1 / FREQUENCY; - double signal = 10 * std::sin(t); - double noisy = signal + 10 * ((rand01() - 0.5) / 5); - - filter.input(noisy); - double filtered = filter.output(); - - data.x = signal; - data.y = filtered; - data.z = 0; - - pub.publish(data); - loop_rate.sleep(); - } - - return 0; -} diff --git a/rm_common/test/test_traj.cpp b/rm_common/test/test_traj.cpp deleted file mode 100644 index ac49f031..00000000 --- a/rm_common/test/test_traj.cpp +++ /dev/null @@ -1,127 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by qiayuan on 3/21/20. -// - -#include "rm_common/traj_gen.h" -#include -//#include - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "traj_test"); - - ros::NodeHandle nh; - ros::Publisher pub; - pub = nh.advertise("test_cmd", 100); - rm_msgs::Joint cmd{}; - - RampTraj traj; - traj.setLimit(1.); - traj.setState(0., 1., 0.); - ros::Rate loop_rate(100); - - if (!traj.calc(2.5)) - ROS_ERROR("Acc too small"); - else - { - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - - double t = -1.; - - while (ros::ok() && !traj.isReach(t)) - { - cmd.q_des[0] = traj.getPos(t); - cmd.qd_des[0] = traj.getVel(t); - cmd.ff[0] = traj.getAcc(t); - t += 0.01; - pub.publish(cmd); - - loop_rate.sleep(); - } - } - cmd.q_des[0] = 0.; - cmd.qd_des[0] = 0.; - cmd.ff[0] = 0.; - - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - pub.publish(cmd); - loop_rate.sleep(); - MinTimeTraj min_traj; - min_traj.setLimit(1., 1., 0.01); - min_traj.setTarget(1.); - double s[3]{}; - while (ros::ok() && !min_traj.isReach()) - { - s[2] = min_traj.getTau(s[0], s[1]) / 1.; - s[1] += 0.01 * s[2]; - s[0] += 0.01 * s[1]; - - cmd.q_des[0] = s[0]; - cmd.qd_des[0] = s[1]; - cmd.ff[0] = s[2]; - pub.publish(cmd); - loop_rate.sleep(); - } - return 0; -} diff --git a/rm_common/test/unit_test_lqr.cpp b/rm_common/test/unit_test_lqr.cpp deleted file mode 100644 index 7e138bd0..00000000 --- a/rm_common/test/unit_test_lqr.cpp +++ /dev/null @@ -1,46 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by chenzheng on 3/20/21. -// - -#include - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - - return RUN_ALL_TESTS(); - return 0; -} diff --git a/rm_gazebo/launch/step.launch b/rm_gazebo/launch/step.launch deleted file mode 100644 index 9ad879d9..00000000 --- a/rm_gazebo/launch/step.launch +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - r - - - - - - - diff --git a/rm_gazebo/worlds/place/step.stl b/rm_gazebo/worlds/place/step.stl deleted file mode 100644 index c0cc3ab1997d87fb6bd22fb8b1a807268b4347fa..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1084 zcmb7CyAi@L4Al?}kO^{p8XAs+a9j!w?to3`s5}LAHld+l6t;lK&o5FOC{U5u(tFnP zlTYW}x>%jJ#bj1a$K|-3%;!aQ*d30GrfJ^AzcCVxu#CUnr>?MlZbZ1&%&ofY6C(h@ z@9hgi*@sL*^6)A$LS6yNGpt9%>Oo6G(OsbO9yvqz f8H12EPj@xd&b`yC`}~zaA-(d9q}?-|)0h1Rb|$6U diff --git a/rm_gazebo/worlds/step.world b/rm_gazebo/worlds/step.world deleted file mode 100644 index 66cbd402..00000000 --- a/rm_gazebo/worlds/step.world +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - 0.001 - 1000 - - - - model://ground_plane - - - - 1 - 0 0 10 0 -0 0 - 0.8 0.8 0.8 1 - 0.2 0.2 0.2 1 - - 1000 - 0.9 - 0.01 - 0.001 - - -0.5 0.1 -0.9 - - - - 1 - 1 - - 2 0.8 0.0 0 0 3.14 - - - - model://rm_gazebo/worlds/place/step.stl - 0.001 0.001 0.001 - - - - - - - model://rm_gazebo/worlds/place/step.stl - 0.001 0.001 0.001 - - - 10 - - - - - - - - - - - - 0 - 0 - - 0 - 0 - 1 - - 1 - - - diff --git a/rm_hw/src/control_loop.cpp b/rm_hw/src/control_loop.cpp index 36aea177..79766a58 100644 --- a/rm_hw/src/control_loop.cpp +++ b/rm_hw/src/control_loop.cpp @@ -68,7 +68,6 @@ RmRobotHWLoop::RmRobotHWLoop(ros::NodeHandle& nh, std::shared_ptr har last_time_ = clock::now(); // Setup loop thread - loop_running_ = true; loop_thread_ = std::thread([&]() { while (loop_running_) { diff --git a/rm_hw/test/test_imu.launch b/rm_hw/test/test_imu.launch deleted file mode 100644 index ba2f0c32..00000000 --- a/rm_hw/test/test_imu.launch +++ /dev/null @@ -1,12 +0,0 @@ - - - - - - - - - diff --git a/rm_hw/test/test_imu.yaml b/rm_hw/test/test_imu.yaml deleted file mode 100644 index a5307c2b..00000000 --- a/rm_hw/test/test_imu.yaml +++ /dev/null @@ -1,24 +0,0 @@ -rm_hw: - bus: - - can0 - - can1 - loop_frequency: 1000 - cycle_time_error_threshold: 0.001 - - # Configurations of the actuators - imus: - base_imu: - frame_id: base_link - bus: can0 - id: 0x300 - orientation_covariance_diagonal: [ 0.0012, 0.0012, 0.0012 ] - angular_velocity_covariance: [ 0.0004, 0.0004, 0.0004 ] - linear_acceleration_covariance: [ 0.001, 0.001, 0.001 ] - angular_vel_coeff: 0.0010652644 - accel_coeff: 0.0008974358 - temp_coeff: 0.125 - temp_offset: 23.0 -controllers: - imu_sensor_controller: - type: imu_sensor_controller/ImuSensorController - publish_rate: 100 diff --git a/rm_hw/test/test_motor.launch b/rm_hw/test/test_motor.launch deleted file mode 100644 index c95565f5..00000000 --- a/rm_hw/test/test_motor.launch +++ /dev/null @@ -1,29 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - diff --git a/rm_hw/test/test_motor.xacro b/rm_hw/test/test_motor.xacro deleted file mode 100644 index da2aa1cb..00000000 --- a/rm_hw/test/test_motor.xacro +++ /dev/null @@ -1,24 +0,0 @@ - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - 1.0 - - - hardware_interface/EffortJointInterface - -0.004 - - - - diff --git a/rm_hw/test/test_motor.yaml b/rm_hw/test/test_motor.yaml deleted file mode 100644 index f30da576..00000000 --- a/rm_hw/test/test_motor.yaml +++ /dev/null @@ -1,39 +0,0 @@ -rm_hw: - bus: - - can0 - - can1 - loop_frequency: 1000 - cycle_time_error_threshold: 0.001 - - # Configurations of the actuators - actuators: - mit_joint_motor: - bus: can1 - id: 0x001 - type: cheetah - -controllers: - robot_state_controller: - type: robot_state_controller/RobotStateController - publish_rate: 100 - joint_state_controller: - type: joint_state_controller/JointStateController - publish_rate: 100 - joint_mit_cheetah_position_controller: - type: effort_controllers/JointEffortController - joint: mit_joint - pid: { p: 100.0, i: 5.0, d: 5.0 } - -#joint_limits: -# joint1: -# has_soft_limits: true -# k_position: 100 -# k_velocity: 1.5 -# soft_lower_limit: 0.0 -# soft_upper_limit: 2.0 -# has_velocity_limits: true -# max_velocity: 100.0 -# # has_acceleration_limits: true -# # max_acceleration: 5.0 -# has_effort_limits: true -# max_effort: 3 diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index aab8d4f5..26036fbf 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -15,9 +15,9 @@ add_message_files( BalanceState.msg BusState.msg DbusData.msg - Dart.msg ExchangerMsg.msg ChassisCmd.msg + CustomControllerData.msg ShootCmd.msg ShootState.msg ShootBeforehandCmd.msg @@ -26,7 +26,6 @@ add_message_files( GimbalPosState.msg LegCmd.msg LpData.msg - LocalHeatData.msg LocalHeatState.msg KalmanData.msg MovingAverageData.msg @@ -36,7 +35,6 @@ add_message_files( TagMsg.msg TagMsgArray.msg SentryDeviate.msg - CustomControllerData.msg VTKeyboardMouseData.msg VTReceiverControlData.msg ) @@ -49,6 +47,7 @@ add_service_files( CameraStatus.srv EnableImuTrigger.srv EnableGyro.srv + CloseToTarget.srv ) add_message_files( @@ -100,6 +99,17 @@ add_message_files( PowerManagementProcessStackOverflowData.msg PowerManagementSystemExceptionData.msg PowerManagementUnknownExceptionData.msg + CurrentSentryPosData.msg + DartRemainingTime.msg + EnemyHpInfo.msg + RadarCmd.msg + RadarWirelessEnemyCallSign.msg + RadarWirelessEnemyCoinAndFieldStatus.msg + RadarWirelessEnemyProjectileAllowance.msg + RadarWirelessEnemyRobotBuff.msg + RadarWirelessEnemyRobotHp.msg + RadarWirelessEnemyRobotPos.msg + RobotCustomData2.msg ) add_action_files( diff --git a/rm_msgs/msg/BusState.msg b/rm_msgs/msg/BusState.msg old mode 100644 new mode 100755 index 5e1efa98..e77bb4cd --- a/rm_msgs/msg/BusState.msg +++ b/rm_msgs/msg/BusState.msg @@ -1,4 +1,4 @@ time stamp string[] name -bool[] isOnline +bool[] isOnline \ No newline at end of file diff --git a/rm_msgs/msg/ChassisCmd.msg b/rm_msgs/msg/ChassisCmd.msg index 8d5bb0b1..8547573c 100644 --- a/rm_msgs/msg/ChassisCmd.msg +++ b/rm_msgs/msg/ChassisCmd.msg @@ -3,7 +3,6 @@ uint8 FOLLOW = 1 uint8 TWIST = 2 uint8 UP_SLOPE = 3 uint8 FALLEN = 4 -uint8 DEPLOY = 5 uint8 mode geometry_msgs/Accel accel @@ -11,5 +10,4 @@ float64 power_limit float64 follow_vel_des string follow_source_frame string command_source_frame -bool wireless_state time stamp diff --git a/rm_msgs/msg/CustomControllerData.msg b/rm_msgs/msg/CustomControllerData.msg old mode 100644 new mode 100755 index da14a8c4..c21fae30 --- a/rm_msgs/msg/CustomControllerData.msg +++ b/rm_msgs/msg/CustomControllerData.msg @@ -5,4 +5,4 @@ uint16 joystick_l_x_data uint16 joystick_r_y_data uint16 joystick_r_x_data -bool[4] button_data +bool[4] button_data \ No newline at end of file diff --git a/rm_msgs/msg/Dart.msg b/rm_msgs/msg/Dart.msg deleted file mode 100644 index deb18383..00000000 --- a/rm_msgs/msg/Dart.msg +++ /dev/null @@ -1,4 +0,0 @@ -int16 distance -int16 height -bool is_found -time stamp diff --git a/rm_msgs/msg/GimbalCmd.msg b/rm_msgs/msg/GimbalCmd.msg index aefab18a..c9f579d8 100644 --- a/rm_msgs/msg/GimbalCmd.msg +++ b/rm_msgs/msg/GimbalCmd.msg @@ -19,4 +19,3 @@ geometry_msgs/PointStamped target_pos #TRAJ float64 traj_yaw float64 traj_pitch -string traj_frame_id diff --git a/rm_msgs/msg/GimbalPosState.msg b/rm_msgs/msg/GimbalPosState.msg index 3eadaf49..2f70f8e9 100644 --- a/rm_msgs/msg/GimbalPosState.msg +++ b/rm_msgs/msg/GimbalPosState.msg @@ -1,8 +1,6 @@ std_msgs/Header header float64 set_point float64 set_point_dot -float64 traject_set_point float64 process_value float64 error float64 command -int16 shoot_number diff --git a/rm_msgs/msg/LocalHeatData.msg b/rm_msgs/msg/LocalHeatData.msg deleted file mode 100644 index 432322d5..00000000 --- a/rm_msgs/msg/LocalHeatData.msg +++ /dev/null @@ -1,2 +0,0 @@ -int64 once_shoot_num -float64 local_shooter_cooling_heat diff --git a/rm_msgs/msg/LocalHeatState.msg b/rm_msgs/msg/LocalHeatState.msg index 3fa778d8..bd338b5e 100644 --- a/rm_msgs/msg/LocalHeatState.msg +++ b/rm_msgs/msg/LocalHeatState.msg @@ -1,5 +1,3 @@ -float64 friction_speed -float64 friction_change_speed -float64 friction_change_speed_derivative +float64 friction_change_vel bool has_shoot time stamp diff --git a/rm_msgs/msg/ShootCmd.msg b/rm_msgs/msg/ShootCmd.msg index a4985032..9bd0016f 100644 --- a/rm_msgs/msg/ShootCmd.msg +++ b/rm_msgs/msg/ShootCmd.msg @@ -12,6 +12,4 @@ uint8 SPEED_30M_PER_SECOND = 5 uint8 mode float64 wheel_speed float64 hz -float64 wheels_speed_offset_front -float64 wheels_speed_offset_back time stamp diff --git a/rm_msgs/msg/SuperCapacitor.msg b/rm_msgs/msg/SuperCapacitor.msg deleted file mode 100644 index de1cfe4e..00000000 --- a/rm_msgs/msg/SuperCapacitor.msg +++ /dev/null @@ -1,6 +0,0 @@ -float32 capacity -float32 limit_power -float32 chassis_power -uint16 chassis_power_buffer - -time stamp diff --git a/rm_msgs/msg/VTKeyboardMouseData.msg b/rm_msgs/msg/VTKeyboardMouseData.msg old mode 100644 new mode 100755 index bd91451c..ad28df81 --- a/rm_msgs/msg/VTKeyboardMouseData.msg +++ b/rm_msgs/msg/VTKeyboardMouseData.msg @@ -19,4 +19,4 @@ bool key_z bool key_x bool key_c bool key_v -bool key_b +bool key_b \ No newline at end of file diff --git a/rm_msgs/msg/VTReceiverControlData.msg b/rm_msgs/msg/VTReceiverControlData.msg old mode 100644 new mode 100755 index d6c074c7..f2438ad4 --- a/rm_msgs/msg/VTReceiverControlData.msg +++ b/rm_msgs/msg/VTReceiverControlData.msg @@ -36,4 +36,4 @@ bool key_z bool key_x bool key_c bool key_v -bool key_b +bool key_b \ No newline at end of file diff --git a/rm_msgs/msg/detection/TargetDetection.msg b/rm_msgs/msg/detection/TargetDetection.msg index 03949865..cd14ca29 100644 --- a/rm_msgs/msg/detection/TargetDetection.msg +++ b/rm_msgs/msg/detection/TargetDetection.msg @@ -3,6 +3,3 @@ float64 distance_to_image_center float64 confidence bool is_large_armor geometry_msgs/Pose pose -geometry_msgs/Point[4] armor_points -uint32 x_offset -uint32 y_offset diff --git a/rm_msgs/msg/referee/Buff.msg b/rm_msgs/msg/referee/Buff.msg index eea334e3..60609a50 100644 --- a/rm_msgs/msg/referee/Buff.msg +++ b/rm_msgs/msg/referee/Buff.msg @@ -1,5 +1,5 @@ uint8 recovery_buff -uint8 cooling_buff +uint16 cooling_buff uint8 defence_buff uint8 vulnerability_buff uint16 attack_buff diff --git a/rm_msgs/msg/referee/BulletAllowance.msg b/rm_msgs/msg/referee/BulletAllowance.msg index ef09835a..ef488e51 100644 --- a/rm_msgs/msg/referee/BulletAllowance.msg +++ b/rm_msgs/msg/referee/BulletAllowance.msg @@ -1,5 +1,6 @@ uint16 bullet_allowance_num_17_mm uint16 bullet_allowance_num_42_mm uint16 coin_remaining_num +uint16 projectile_allowance_fortress time stamp diff --git a/rm_msgs/msg/referee/BulletNumShare.msg b/rm_msgs/msg/referee/BulletNumShare.msg deleted file mode 100644 index ee4b1724..00000000 --- a/rm_msgs/msg/referee/BulletNumShare.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 robot_id -uint8 bullet_num diff --git a/rm_msgs/msg/referee/BulletRemaining.msg b/rm_msgs/msg/referee/BulletRemaining.msg deleted file mode 100644 index d41079b5..00000000 --- a/rm_msgs/msg/referee/BulletRemaining.msg +++ /dev/null @@ -1,5 +0,0 @@ -uint16 bullet_remaining_num_17_mm -uint16 bullet_remaining_num_42_mm -uint16 coin_remaining_num - -time stamp diff --git a/rm_msgs/msg/referee/ClientMapReceiveData.msg b/rm_msgs/msg/referee/ClientMapReceiveData.msg index d78a250d..c3407e31 100644 --- a/rm_msgs/msg/referee/ClientMapReceiveData.msg +++ b/rm_msgs/msg/referee/ClientMapReceiveData.msg @@ -6,8 +6,8 @@ uint16 infantry_3_position_x uint16 infantry_3_position_y uint16 infantry_4_position_x uint16 infantry_4_position_y -uint16 infantry_5_position_x -uint16 infantry_5_position_y +uint16 reserved_1 +uint16 reserved_2 uint16 sentry_position_x uint16 sentry_position_y diff --git a/rm_msgs/msg/referee/ClientMapSendData.msg b/rm_msgs/msg/referee/ClientMapSendData.msg index 991c4c9a..7009a559 100644 --- a/rm_msgs/msg/referee/ClientMapSendData.msg +++ b/rm_msgs/msg/referee/ClientMapSendData.msg @@ -27,4 +27,4 @@ float32 target_position_x float32 target_position_y uint8 command_keyboard uint8 target_robot_ID -uint8 cmd_source +uint16 cmd_source diff --git a/rm_msgs/msg/referee/CurrentSentryPosData.msg b/rm_msgs/msg/referee/CurrentSentryPosData.msg old mode 100644 new mode 100755 diff --git a/rm_msgs/msg/referee/DartClientCmd.msg b/rm_msgs/msg/referee/DartClientCmd.msg index 2aa89c9e..5ae3188a 100644 --- a/rm_msgs/msg/referee/DartClientCmd.msg +++ b/rm_msgs/msg/referee/DartClientCmd.msg @@ -1,4 +1,5 @@ uint8 dart_launch_opening_status +uint8 reserved uint16 target_change_time uint16 latest_launch_cmd_time diff --git a/rm_msgs/msg/referee/DartRemainingTime.msg b/rm_msgs/msg/referee/DartRemainingTime.msg new file mode 100755 index 00000000..77fe568f --- /dev/null +++ b/rm_msgs/msg/referee/DartRemainingTime.msg @@ -0,0 +1,4 @@ +uint8 dart_remaining_time +uint8 dart_aim_state + +time stamp diff --git a/rm_msgs/msg/referee/EnemyHpInfo.msg b/rm_msgs/msg/referee/EnemyHpInfo.msg new file mode 100644 index 00000000..48dce2e7 --- /dev/null +++ b/rm_msgs/msg/referee/EnemyHpInfo.msg @@ -0,0 +1,6 @@ +uint16 enemy_1_HP +uint16 enemy_2_HP +uint16 enemy_3_HP +uint16 enemy_4_HP +uint16 enemy_5_HP +uint16 enemy_7_HP \ No newline at end of file diff --git a/rm_msgs/msg/referee/GameRobotHp.msg b/rm_msgs/msg/referee/GameRobotHp.msg index c93622cd..08a1adc2 100644 --- a/rm_msgs/msg/referee/GameRobotHp.msg +++ b/rm_msgs/msg/referee/GameRobotHp.msg @@ -1,16 +1,9 @@ -uint16 red_1_robot_hp -uint16 red_2_robot_hp -uint16 red_3_robot_hp -uint16 red_4_robot_hp -uint16 red_7_robot_hp -uint16 red_outpost_hp -uint16 red_base_hp -uint16 blue_1_robot_hp -uint16 blue_2_robot_hp -uint16 blue_3_robot_hp -uint16 blue_4_robot_hp -uint16 blue_7_robot_hp -uint16 blue_outpost_hp -uint16 blue_base_hp +uint16 ally_1_robot_hp +uint16 ally_2_robot_hp +uint16 ally_3_robot_hp +uint16 ally_4_robot_hp +uint16 ally_7_robot_hp +uint16 ally_outpost_hp +uint16 ally_base_hp time stamp diff --git a/rm_msgs/msg/referee/GameRobotStatus.msg b/rm_msgs/msg/referee/GameRobotStatus.msg index b09a90f1..91b5444c 100644 --- a/rm_msgs/msg/referee/GameRobotStatus.msg +++ b/rm_msgs/msg/referee/GameRobotStatus.msg @@ -7,6 +7,7 @@ uint8 RED_AERIAL = 6 uint8 RED_SENTRY = 7 uint8 RED_DART = 8 uint8 RED_RADAR = 9 +uint8 RED_OUTPOST = 10 uint8 RED_OUTPUT = 10 uint8 RED_BASE = 11 uint8 BLUE_HERO = 101 @@ -18,6 +19,7 @@ uint8 BLUE_AERIAL = 106 uint8 BLUE_SENTRY = 107 uint8 BLUE_DART = 108 uint8 BLUE_RADAR = 109 +uint8 BLUE_OUTPOST = 110 uint8 BLUE_OUTPUT = 110 uint8 BLUE_BASE = 111 diff --git a/rm_msgs/msg/referee/ManualToReferee.msg b/rm_msgs/msg/referee/ManualToReferee.msg index a51c9af6..0465343b 100644 --- a/rm_msgs/msg/referee/ManualToReferee.msg +++ b/rm_msgs/msg/referee/ManualToReferee.msg @@ -1,5 +1,4 @@ uint8 power_limit_state -time start_burst_time #standard uint8 shoot_frequency diff --git a/rm_msgs/msg/referee/PowerHeatData.msg b/rm_msgs/msg/referee/PowerHeatData.msg index 7981cd91..1d3ee282 100644 --- a/rm_msgs/msg/referee/PowerHeatData.msg +++ b/rm_msgs/msg/referee/PowerHeatData.msg @@ -1,6 +1,5 @@ uint16 chassis_power_buffer uint16 shooter_id_1_17_mm_cooling_heat -uint16 shooter_id_2_17_mm_cooling_heat uint16 shooter_id_1_42_mm_cooling_heat time stamp diff --git a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg index 11d38c39..9946e9aa 100644 --- a/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg +++ b/rm_msgs/msg/referee/PowerManagementSampleAndStatusData.msg @@ -1,6 +1,6 @@ float32 chassis_power -float32 cap_error_flag -float32 cap_received_msg +float32 chassis_expect_power +float32 capacity_recent_charge_power float32 capacity_remain_charge uint8 capacity_discharge_power uint8 state_machine_running_state diff --git a/rm_msgs/msg/referee/RadarCmd.msg b/rm_msgs/msg/referee/RadarCmd.msg new file mode 100755 index 00000000..ff94640e --- /dev/null +++ b/rm_msgs/msg/referee/RadarCmd.msg @@ -0,0 +1,8 @@ +uint8 radar_cmd +uint8 password_cmd +uint8 password_1 +uint8 password_2 +uint8 password_3 +uint8 password_4 +uint8 password_5 +uint8 password_6 \ No newline at end of file diff --git a/rm_msgs/msg/referee/RadarInfo.msg b/rm_msgs/msg/referee/RadarInfo.msg index ee153411..3a65f781 100644 --- a/rm_msgs/msg/referee/RadarInfo.msg +++ b/rm_msgs/msg/referee/RadarInfo.msg @@ -1 +1,5 @@ uint8 radar_info +uint8 double_vulnerability_chances +bool enemy_in_double_vulnerability +uint8 own_encryption_level +bool can_modify_key diff --git a/rm_msgs/msg/referee/RadarMarkData.msg b/rm_msgs/msg/referee/RadarMarkData.msg index 237e9393..dd8dcab7 100644 --- a/rm_msgs/msg/referee/RadarMarkData.msg +++ b/rm_msgs/msg/referee/RadarMarkData.msg @@ -1,3 +1,16 @@ +uint16 mark_progress +bool enemy_hero_vulnerable +bool enemy_engineer_vulnerable +bool enemy_standard_3_vulnerable +bool enemy_standard_4_vulnerable +bool enemy_aerial_special_mark +bool enemy_sentry_vulnerable +bool own_hero_special_mark +bool own_engineer_special_mark +bool own_standard_3_special_mark +bool own_standard_4_special_mark +bool own_aerial_special_mark +bool own_sentry_special_mark bool mark_hero_progress bool mark_engineer_progress bool mark_standard_3_progress diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg b/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg new file mode 100644 index 00000000..1c99ed1c --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyCallSign.msg @@ -0,0 +1,3 @@ +uint8[6] ascii_data + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg b/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg new file mode 100644 index 00000000..5ae7f573 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyCoinAndFieldStatus.msg @@ -0,0 +1,5 @@ +uint16 remaining_coin +uint16 total_coin +uint32 field_status + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg b/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg new file mode 100644 index 00000000..26b429b1 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyProjectileAllowance.msg @@ -0,0 +1,7 @@ +uint16 hero_projectile_allowance +uint16 infantry_3_projectile_allowance +uint16 infantry_4_projectile_allowance +uint16 aerial_projectile_allowance +uint16 sentry_projectile_allowance + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg new file mode 100644 index 00000000..108f4c78 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotBuff.msg @@ -0,0 +1,32 @@ +uint8 hero_recovery_buff +uint16 hero_cooling_buff +uint8 hero_defense_buff +uint8 hero_negative_defense_buff +uint16 hero_attack_buff + +uint8 engineer_recovery_buff +uint16 engineer_cooling_buff +uint8 engineer_defense_buff +uint8 engineer_negative_defense_buff +uint16 engineer_attack_buff + +uint8 infantry_3_recovery_buff +uint16 infantry_3_cooling_buff +uint8 infantry_3_defense_buff +uint8 infantry_3_negative_defense_buff +uint16 infantry_3_attack_buff + +uint8 infantry_4_recovery_buff +uint16 infantry_4_cooling_buff +uint8 infantry_4_defense_buff +uint8 infantry_4_negative_defense_buff +uint16 infantry_4_attack_buff + +uint8 sentry_recovery_buff +uint16 sentry_cooling_buff +uint8 sentry_defense_buff +uint8 sentry_negative_defense_buff +uint16 sentry_attack_buff +uint8 sentry_posture + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg new file mode 100644 index 00000000..55413df6 --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotHp.msg @@ -0,0 +1,8 @@ +uint16 hero_hp +uint16 engineer_hp +uint16 infantry_3_hp +uint16 infantry_4_hp +uint16 reserved +uint16 sentry_hp + +time stamp diff --git a/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg b/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg new file mode 100644 index 00000000..fa01ba1b --- /dev/null +++ b/rm_msgs/msg/referee/RadarWirelessEnemyRobotPos.msg @@ -0,0 +1,14 @@ +uint16 hero_position_x +uint16 hero_position_y +uint16 engineer_position_x +uint16 engineer_position_y +uint16 infantry_3_position_x +uint16 infantry_3_position_y +uint16 infantry_4_position_x +uint16 infantry_4_position_y +uint16 aerial_position_x +uint16 aerial_position_y +uint16 sentry_position_x +uint16 sentry_position_y + +time stamp diff --git a/rm_msgs/msg/referee/RfidStatus.msg b/rm_msgs/msg/referee/RfidStatus.msg index 0a984c0a..ce914640 100644 --- a/rm_msgs/msg/referee/RfidStatus.msg +++ b/rm_msgs/msg/referee/RfidStatus.msg @@ -1,3 +1,6 @@ +uint32 rfid_status +uint8 rfid_status_2 + bool base_buff_point_state bool own_central_elevated_ground_state bool enemy_central_elevated_ground_state @@ -15,12 +18,26 @@ bool below_road_own_terrain_span_buff_point_state bool upper_road_own_terrain_span_buff_point_state bool below_road_enemy_terrain_span_buff_point_state bool upper_road_enemy_terrain_span_buff_point_state -bool own_fort_buff_point -bool own_outpost_buff_point -bool nan_overlapping_supplier_zone -bool overlapping_supplier_zone -bool own_large_resource_island_point -bool enemy_large_resource_island_point -bool central_buff_point +bool own_fort_buff_point_state +bool own_outpost_buff_point_state +bool non_overlapping_supplier_zone_state +bool overlapping_supplier_zone_state +bool own_assembly_buff_point_state +bool enemy_assembly_buff_point_state +bool central_buff_point_state +bool enemy_fort_buff_point_state +bool enemy_outpost_buff_point_state +bool own_tunnel_road_lower_buff_point_state +bool own_tunnel_road_middle_buff_point_state +bool own_tunnel_road_upper_buff_point_state +bool own_tunnel_trapezoid_lower_buff_point_state +bool own_tunnel_trapezoid_middle_buff_point_state +bool own_tunnel_trapezoid_upper_buff_point_state +bool enemy_tunnel_road_lower_buff_point_state +bool enemy_tunnel_road_middle_buff_point_state +bool enemy_tunnel_road_upper_buff_point_state +bool enemy_tunnel_trapezoid_lower_buff_point_state +bool enemy_tunnel_trapezoid_middle_buff_point_state +bool enemy_tunnel_trapezoid_upper_buff_point_state time stamp diff --git a/rm_msgs/msg/referee/RobotCustomData2.msg b/rm_msgs/msg/referee/RobotCustomData2.msg new file mode 100644 index 00000000..070cdda6 --- /dev/null +++ b/rm_msgs/msg/referee/RobotCustomData2.msg @@ -0,0 +1,3 @@ +uint8[300] data + +time stamp diff --git a/rm_msgs/msg/referee/SentryCmd.msg b/rm_msgs/msg/referee/SentryCmd.msg index 5295edf2..1d90c706 100644 --- a/rm_msgs/msg/referee/SentryCmd.msg +++ b/rm_msgs/msg/referee/SentryCmd.msg @@ -1 +1,8 @@ -uint32 sentry_info +uint32 sentry_cmd +bool confirm_respawn +bool confirm_instant_respawn +uint16 bullet_exchange_target +uint8 remote_bullet_exchange_req_cnt +uint8 remote_hp_exchange_req_cnt +uint8 posture_cmd +bool confirm_rune_activating diff --git a/rm_msgs/msg/referee/SentryInfo.msg b/rm_msgs/msg/referee/SentryInfo.msg index f9319cbe..8ba91b2d 100644 --- a/rm_msgs/msg/referee/SentryInfo.msg +++ b/rm_msgs/msg/referee/SentryInfo.msg @@ -1,3 +1,12 @@ uint32 sentry_info +uint16 sentry_info_2 +uint16 exchanged_bullet_allowance +uint8 remote_bullet_exchange_success_cnt +uint8 remote_hp_exchange_success_cnt +bool can_confirm_free_respawn +bool can_exchange_instant_respawn +uint16 instant_respawn_cost bool is_out_of_war uint16 remaining_bullets_can_supply +uint8 sentry_mode +bool can_activate_energy_mechanism diff --git a/rm_msgs/srv/CloseToTarget.srv b/rm_msgs/srv/CloseToTarget.srv old mode 100644 new mode 100755 index 6e22dff2..64e0e2fb --- a/rm_msgs/srv/CloseToTarget.srv +++ b/rm_msgs/srv/CloseToTarget.srv @@ -1,3 +1,3 @@ geometry_msgs/PointStamped target_pos --- -geometry_msgs/PoseStamped move_point +geometry_msgs/PoseStamped move_point \ No newline at end of file diff --git a/rm_msgs/srv/ExtraFrictionWheelSpeed.srv b/rm_msgs/srv/ExtraFrictionWheelSpeed.srv deleted file mode 100644 index 8e41bda7..00000000 --- a/rm_msgs/srv/ExtraFrictionWheelSpeed.srv +++ /dev/null @@ -1,3 +0,0 @@ -float64 extra_speed ---- -bool is_success diff --git a/rm_msgs/srv/ShooterSpeed.srv b/rm_msgs/srv/ShooterSpeed.srv deleted file mode 100644 index d400a473..00000000 --- a/rm_msgs/srv/ShooterSpeed.srv +++ /dev/null @@ -1,3 +0,0 @@ -float64 shooter_speed ---- -bool is_success diff --git a/rm_referee/CMakeLists.txt b/rm_referee/CMakeLists.txt index 12239533..475e0509 100644 --- a/rm_referee/CMakeLists.txt +++ b/rm_referee/CMakeLists.txt @@ -66,6 +66,7 @@ include_directories( ## Declare cpp executables FILE(GLOB ALL_SOURCES "src/*.cpp" "src/common/*.cpp" "src/ui/*.cpp") +list(REMOVE_ITEM ALL_SOURCES "${CMAKE_CURRENT_SOURCE_DIR}/src/referee_benchmark.cpp") add_executable(${PROJECT_NAME} ${ALL_SOURCES}) ## Add dependencies to exported targets, like ROS msgs or srvs diff --git a/rm_referee/include/rm_referee/common/data.h b/rm_referee/include/rm_referee/common/data.h index d6ffba28..f2adff30 100644 --- a/rm_referee/include/rm_referee/common/data.h +++ b/rm_referee/include/rm_referee/common/data.h @@ -87,6 +87,7 @@ #include "rm_msgs/SentryInfo.h" #include "rm_msgs/SentryCmd.h" #include "rm_msgs/RadarInfo.h" +#include "rm_msgs/RadarCmd.h" #include "rm_msgs/Buff.h" #include "rm_msgs/TrackData.h" #include "rm_msgs/SentryAttackingTarget.h" diff --git a/rm_referee/include/rm_referee/common/protocol.h b/rm_referee/include/rm_referee/common/protocol.h index 48a95324..75260af4 100644 --- a/rm_referee/include/rm_referee/common/protocol.h +++ b/rm_referee/include/rm_referee/common/protocol.h @@ -47,8 +47,8 @@ typedef enum GAME_STATUS_CMD = 0x0001, GAME_RESULT_CMD = 0x0002, GAME_ROBOT_HP_CMD = 0x0003, - DART_STATUS_CMD = 0x0004, - ICRA_ZONE_STATUS_CMD = 0x0005, + DART_STATUS_CMD = 0x0004, // legacy, not listed in 2026 V1.2.0 command table + ICRA_ZONE_STATUS_CMD = 0x0005, // legacy, not listed in 2026 V1.2.0 command table FIELD_EVENTS_CMD = 0x0101, SUPPLY_PROJECTILE_ACTION_CMD = 0x0102, REFEREE_WARNING_CMD = 0x0104, @@ -57,7 +57,7 @@ typedef enum POWER_HEAT_DATA_CMD = 0x0202, ROBOT_POS_CMD = 0x0203, BUFF_CMD = 0x0204, - AERIAL_ROBOT_ENERGY_CMD = 0x0205, + AERIAL_ROBOT_ENERGY_CMD = 0x0205, // legacy, not listed in 2026 V1.2.0 command table ROBOT_HURT_CMD = 0x0206, SHOOT_DATA_CMD = 0x0207, BULLET_REMAINING_CMD = 0x0208, @@ -70,12 +70,18 @@ typedef enum INTERACTIVE_DATA_CMD = 0x0301, CUSTOM_CONTROLLER_CMD = 0x0302, // controller TARGET_POS_CMD = 0x0303, - ROBOT_COMMAND_CMD = 0x0304, // controller + ROBOT_COMMAND_CMD = 0x0304, // legacy keyboard/mouse command (deleted in 2026 V1.2.0) CLIENT_MAP_CMD = 0x0305, - CUSTOM_CLIENT_CMD = 0x0306, // controller - MAP_SENTRY_CMD = 0x0307, // send sentry->aerial - CUSTOM_TO_ROBOT_CMD = 0x0308, - ROBOT_TO_CUSTOM_CMD = 0x0309, + KEYBOARD_MOUSE_CMD = 0x0306, // 2026 table 1-42 + CUSTOM_CLIENT_CMD = KEYBOARD_MOUSE_CMD, // legacy alias + MAP_SENTRY_CMD = 0x0307, // send sentry->aerial + CUSTOM_INFO_CMD = 0x0308, // robot -> custom controller text/info + ROBOT_TO_CUSTOM_CONTROLLER_CMD = 0x0309, + ROBOT_TO_CUSTOM_CLIENT_CMD = 0x0310, + CUSTOM_CLIENT_TO_ROBOT_CMD = 0x0311, + CUSTOM_TO_ROBOT_CMD = CUSTOM_INFO_CMD, // legacy misnomer alias kept for compatibility + ROBOT_TO_CUSTOM_CMD = ROBOT_TO_CUSTOM_CONTROLLER_CMD, // legacy alias + ROBOT_TO_CUSTOM_CMD_2 = ROBOT_TO_CUSTOM_CLIENT_CMD, // legacy alias POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD = 0X8301, POWER_MANAGEMENT_INITIALIZATION_EXCEPTION_CMD = 0X8302, POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD = 0X8303, @@ -239,22 +245,14 @@ typedef struct typedef struct { - uint16_t red_1_robot_hp; - uint16_t red_2_robot_hp; - uint16_t red_3_robot_hp; - uint16_t red_4_robot_hp; - uint16_t reserved_1; - uint16_t red_7_robot_hp; - uint16_t red_outpost_hp; - uint16_t red_base_hp; - uint16_t blue_1_robot_hp; - uint16_t blue_2_robot_hp; - uint16_t blue_3_robot_hp; - uint16_t blue_4_robot_hp; - uint16_t reserved_2; - uint16_t blue_7_robot_hp; - uint16_t blue_outpost_hp; - uint16_t blue_base_hp; + uint16_t ally_1_robot_hp; + uint16_t ally_2_robot_hp; + uint16_t ally_3_robot_hp; + uint16_t ally_4_robot_hp; + uint16_t reserved; + uint16_t ally_7_robot_hp; + uint16_t ally_outpost_hp; + uint16_t ally_base_hp; } __packed GameRobotHp; typedef struct @@ -285,15 +283,15 @@ typedef struct typedef struct { - uint8_t nan_overlapping_supply_station_state : 1; uint8_t overlapping_supply_station_state : 1; + uint8_t nan_overlapping_supply_station_state : 1; uint8_t supplier_zone_state : 1; uint8_t small_power_rune_state : 1; uint8_t large_power_rune_state : 1; uint8_t central_elevated_ground_state : 2; uint8_t trapezoidal_elevated_ground_state : 2; uint16_t be_hit_time : 9; - uint8_t be_hit_target : 3; + uint8_t be_hit_target : 2; uint8_t central_point_state : 2; uint16_t reserved : 9; } __packed EventData; @@ -316,10 +314,7 @@ typedef struct typedef struct { uint8_t dart_remaining_time; - uint8_t dart_last_aim_state : 3; - uint8_t enemy_total_hit_received : 3; - uint8_t dart_current_target : 2; - uint8_t reserved; + uint16_t dart_info; } __packed DartInfo; typedef struct @@ -343,7 +338,6 @@ typedef struct float reserved_3; uint16_t chassis_power_buffer; uint16_t shooter_id_1_17_mm_cooling_heat; - uint16_t shooter_id_2_17_mm_cooling_heat; uint16_t shooter_id_1_42_mm_cooling_heat; } __packed PowerHeatData; @@ -357,7 +351,7 @@ typedef struct typedef struct { uint8_t recovery_buff; - uint8_t cooling_buff; + uint16_t cooling_buff; uint8_t defence_buff; uint8_t vulnerability_buff; uint16_t attack_buff; @@ -388,41 +382,19 @@ typedef struct uint16_t bullet_allowance_num_17_mm; uint16_t bullet_allowance_num_42_mm; uint16_t coin_remaining_num; + uint16_t projectile_allowance_fortress; } __packed BulletAllowance; typedef struct { - uint8_t base_buff_point_state : 1; - uint8_t own_central_elevated_ground_state : 1; - uint8_t enemy_central_elevated_ground_state : 1; - uint8_t own_trapezoidal_elevated_ground_state : 1; - uint8_t enemy_trapezoidal_elevated_ground_state : 1; - uint8_t forward_own_terrain_span_buff_point_state : 1; - uint8_t behind_own_terrain_span_buff_point_state : 1; - uint8_t forward_enemy_terrain_span_buff_point_state : 1; - uint8_t behind_enemy_terrain_span_buff_point_state : 1; - uint8_t below_central_own_terrain_span_buff_point_state : 1; - uint8_t upper_central_own_terrain_span_buff_point_state : 1; - uint8_t below_central_enemy_terrain_span_buff_point_state : 1; - uint8_t upper_central_enemy_terrain_span_buff_point_state : 1; - uint8_t below_road_own_terrain_span_buff_point_state : 1; - uint8_t upper_road_own_terrain_span_buff_point_state : 1; - uint8_t below_road_enemy_terrain_span_buff_point_state : 1; - uint8_t upper_road_enemy_terrain_span_buff_point_state : 1; - uint8_t own_fort_buff_point : 1; - uint8_t own_outpost_buff_point : 1; - uint8_t nan_overlapping_supplier_zone : 1; - uint8_t overlapping_supplier_zone : 1; - uint8_t own_large_resource_island_point : 1; - uint8_t enemy_large_resource_island_point : 1; - uint8_t central_buff_point : 1; - uint32_t reversed : 8; + uint32_t rfid_status; + uint8_t rfid_status_2; } __packed RfidStatus; typedef struct { uint8_t dart_launch_opening_status; - uint8_t reversed; + uint8_t reserved; uint16_t target_change_time; uint16_t latest_launch_cmd_time; } __packed DartClientCmd; @@ -495,15 +467,45 @@ typedef struct float reserved_2; } __packed RobotsPositionData; -typedef struct +// Table 1-21 (0x020C): radar_mark_data_t +typedef union { - uint8_t mark_hero_progress : 1; - uint8_t mark_engineer_progress : 1; - uint8_t mark_standard_3_progress : 1; - uint8_t mark_standard_4_progress : 1; - uint8_t mark_sentry_progress : 1; + uint16_t mark_progress; + struct + { + uint16_t enemy_hero_vulnerable : 1; // bit 0 + uint16_t enemy_engineer_vulnerable : 1; // bit 1 + uint16_t enemy_standard_3_vulnerable : 1; // bit 2 + uint16_t enemy_standard_4_vulnerable : 1; // bit 3 + uint16_t enemy_aerial_special_mark : 1; // bit 4 + uint16_t enemy_sentry_vulnerable : 1; // bit 5 + uint16_t own_hero_special_mark : 1; // bit 6 + uint16_t own_engineer_special_mark : 1; // bit 7 + uint16_t own_standard_3_special_mark : 1; // bit 8 + uint16_t own_standard_4_special_mark : 1; // bit 9 + uint16_t own_aerial_special_mark : 1; // bit 10 + uint16_t own_sentry_special_mark : 1; // bit 11 + uint16_t reserved : 4; // bit 12-15 + }; } __packed RadarMarkData; +// Table 1-32 (0x0120): sentry_cmd_t +typedef union +{ + uint32_t sentry_cmd; + struct + { + uint32_t confirm_respawn : 1; + uint32_t confirm_instant_respawn : 1; + uint32_t bullet_exchange_target : 11; + uint32_t remote_bullet_exchange_req_cnt : 4; + uint32_t remote_hp_exchange_req_cnt : 4; + uint32_t posture_cmd : 2; + uint32_t confirm_rune_activating : 1; + uint32_t reserved : 8; + }; +} __packed SentryCmd; + typedef struct { InteractiveDataHeader header; @@ -544,15 +546,48 @@ typedef struct typedef struct { InteractiveDataHeader header; - uint32_t sentry_info; -} __packed SentryCmd; + SentryCmd sentry_cmd; +} __packed SentryCmdInteractiveData; -typedef struct +// Table 1-23 (0x020E): radar_info_t +typedef union { - InteractiveDataHeader header; uint8_t radar_info; + struct + { + uint8_t double_vulnerability_chances : 2; // bit 0-1 + uint8_t enemy_in_double_vulnerability : 1; // bit 2 + uint8_t own_encryption_level : 2; // bit 3-4 + uint8_t can_modify_key : 1; // bit 5 + uint8_t reserved : 2; // bit 6-7 + }; } __packed RadarInfo; +typedef struct +{ + InteractiveDataHeader header; + RadarInfo radar_info; +} __packed RadarInfoInteractiveData; + +// Table 1-33 (0x0121): radar_cmd_t +typedef struct +{ + uint8_t radar_cmd; + uint8_t password_cmd; + uint8_t password_1; + uint8_t password_2; + uint8_t password_3; + uint8_t password_4; + uint8_t password_5; + uint8_t password_6; +} __packed RadarCmd; + +typedef struct +{ + InteractiveDataHeader header; + RadarCmd radar_cmd; +} __packed RadarCmdInteractiveData; + typedef struct { uint8_t data[30]; @@ -560,10 +595,32 @@ typedef struct typedef struct { - uint32_t sentry_info; - uint16_t is_out_of_war : 1; - uint16_t remaining_bullets_can_supply : 11; - uint16_t reverse : 4; + union + { + uint32_t sentry_info; + struct + { + uint32_t exchanged_bullet_allowance : 11; // bit 0-10 + uint32_t remote_bullet_exchange_success_cnt : 4; // bit 11-14 + uint32_t remote_hp_exchange_success_cnt : 4; // bit 15-18 + uint32_t can_confirm_free_respawn : 1; // bit 19 + uint32_t can_exchange_instant_respawn : 1; // bit 20 + uint32_t instant_respawn_cost : 10; // bit 21-30 + uint32_t reserved : 1; // bit 31 + }; + }; + union + { + uint16_t sentry_info_2; + struct + { + uint16_t is_out_of_war : 1; + uint16_t remaining_bullets_can_supply : 11; + uint16_t sentry_mode : 2; + uint16_t can_activate_energy_mechanism : 1; + uint16_t reserved_1 : 1; + }; + }; } __packed SentryInfo; typedef struct @@ -572,7 +629,7 @@ typedef struct float target_position_y; uint8_t command_keyboard; uint8_t target_robot_ID; - uint8_t cmd_source; + uint16_t cmd_source; } __packed ClientMapSendData; typedef struct @@ -585,8 +642,8 @@ typedef struct uint16_t infantry_3_position_y; uint16_t infantry_4_position_x; uint16_t infantry_4_position_y; - uint16_t infantry_5_position_x; - uint16_t infantry_5_position_y; + uint16_t reserved_1; + uint16_t reserved_2; uint16_t sentry_position_x; uint16_t sentry_position_y; } __packed ClientMapReceiveData; @@ -729,4 +786,26 @@ const uint16_t wCRC_table[256] = { 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 }; + +static_assert(sizeof(FrameHeader) == 5, "FrameHeader size must match protocol (5 bytes)."); +static_assert(sizeof(GameRobotHp) == 16, "GameRobotHp size must match protocol (16 bytes)."); +static_assert(sizeof(DartInfo) == 3, "DartInfo size must match protocol (3 bytes)."); +static_assert(sizeof(PowerHeatData) == 14, "PowerHeatData size must match protocol (14 bytes)."); +static_assert(sizeof(Buff) == 8, "Buff size must match protocol (8 bytes)."); +static_assert(sizeof(BulletAllowance) == 8, "BulletAllowance size must match protocol (8 bytes)."); +static_assert(sizeof(RfidStatus) == 5, "RfidStatus size must match protocol (5 bytes)."); +static_assert(sizeof(DartClientCmd) == 6, "DartClientCmd size must match protocol (6 bytes)."); +static_assert(sizeof(InteractiveDataHeader) == 6, "InteractiveDataHeader size must match protocol (6 bytes)."); +static_assert(sizeof(RadarMarkData) == 2, "RadarMarkData size must match protocol (2 bytes)."); +static_assert(sizeof(SentryCmd) == 4, "SentryCmd size must match protocol (4 bytes)."); +static_assert(sizeof(SentryCmdInteractiveData) == 10, "SentryCmdInteractiveData size must match protocol (10 bytes)."); +static_assert(sizeof(RadarInfo) == 1, "RadarInfo size must match protocol (1 byte)."); +static_assert(sizeof(RadarInfoInteractiveData) == 7, "RadarInfoInteractiveData size must match protocol (7 bytes)."); +static_assert(sizeof(RadarCmd) == 8, "RadarCmd size must match protocol (8 bytes)."); +static_assert(sizeof(RadarCmdInteractiveData) == 14, "RadarCmdInteractiveData size must match protocol (14 bytes)."); +static_assert(sizeof(SentryInfo) == 6, "SentryInfo size must match protocol (6 bytes)."); +static_assert(sizeof(ClientMapSendData) == 12, "ClientMapSendData size must match protocol (12 bytes)."); +static_assert(sizeof(ClientMapReceiveData) == 24, "ClientMapReceiveData size must match protocol (24 bytes)."); +static_assert(sizeof(MapSentryData) == 105, "MapSentryData size must match protocol (105 bytes)."); +static_assert(sizeof(CustomInfo) == 34, "CustomInfo size must match protocol (34 bytes)."); } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/referee.h b/rm_referee/include/rm_referee/referee.h index a75045d9..d4d1f1cb 100644 --- a/rm_referee/include/rm_referee/referee.h +++ b/rm_referee/include/rm_referee/referee.h @@ -50,6 +50,9 @@ class Referee Referee(ros::NodeHandle& nh) : referee_ui_(nh, base_), last_get_data_time_(ros::Time::now()) { ROS_INFO("New serial protocol loading."); + nh.param("enable_perf_stats", perf_stats_enabled_, false); + nh.param("perf_log_period", perf_log_period_, 1.0); + perf_window_start_ = ros::WallTime::now(); // pub game_robot_status_pub_ = nh.advertise("game_robot_status", 1); game_status_pub_ = nh.advertise("game_status", 1); @@ -135,14 +138,30 @@ class Referee int rx_len_; private: - int unpack(uint8_t* rx_data); + int unpack(uint8_t* rx_data, int rx_data_len); void getRobotInfo(); void publishCapacityData(); + void recordPerfStats(int rx_bytes, int unpack_calls, int frames_ok, double read_ms, double unpack_ms_sum, + double unpack_max_ms); ros::Time last_get_data_time_; - const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; - const int k_unpack_buffer_length_ = 256; - uint8_t unpack_buffer_[256]{}; + static constexpr int k_frame_length_ = 128; + static constexpr int k_header_length_ = 5; + static constexpr int k_cmd_id_length_ = 2; + static constexpr int k_tail_length_ = 2; + static constexpr int k_unpack_buffer_length_ = 512; + uint8_t unpack_buffer_[k_unpack_buffer_length_]{}; + bool perf_stats_enabled_{ false }; + double perf_log_period_{ 1.0 }; + ros::WallTime perf_window_start_; + uint64_t perf_read_cycles_{ 0 }; + uint64_t perf_rx_bytes_{ 0 }; + uint64_t perf_unpack_calls_{ 0 }; + uint64_t perf_frames_ok_{ 0 }; + double perf_read_ms_sum_{ 0.0 }; + double perf_unpack_ms_sum_{ 0.0 }; + double perf_read_ms_max_{ 0.0 }; + double perf_unpack_ms_max_{ 0.0 }; }; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/referee_base.h b/rm_referee/include/rm_referee/referee_base.h index dc750cac..9bdcd948 100644 --- a/rm_referee/include/rm_referee/referee_base.h +++ b/rm_referee/include/rm_referee/referee_base.h @@ -35,7 +35,7 @@ class RefereeBase virtual void interactiveDataCallBack(const rm_referee::InteractiveData& interactive_data, 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 updateGameRobotHpDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data); + 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); virtual void updateBulletRemainData(const rm_referee::BulletNumData& data); @@ -59,7 +59,7 @@ class RefereeBase virtual void mapSentryCallback(const rm_msgs::MapSentryDataConstPtr& data); virtual void sentryAttackingTargetCallback(const rm_msgs::SentryAttackingTargetConstPtr& data); virtual void sendSentryCmdCallback(const rm_msgs::SentryCmdConstPtr& data); - virtual void sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data); + virtual void sendRadarCmdCallback(const rm_msgs::RadarCmdConstPtr& data); virtual void sendCustomInfoCallback(const std_msgs::StringConstPtr& data); virtual void dronePoseCallBack(const geometry_msgs::PoseStampedConstPtr& data); virtual void shootCmdCallBack(const rm_msgs::ShootCmdConstPtr& data); @@ -116,12 +116,10 @@ class RefereeBase LaneLineTimeChangeGroupUi* lane_line_time_change_ui_{}; BalancePitchTimeChangeGroupUi* balance_pitch_time_change_group_ui_{}; PitchAngleTimeChangeUi* pitch_angle_time_change_ui_{}; - ImageTransmissionAngleTimeChangeUi* image_transmission_angle_time_change_ui_{}; JointPositionTimeChangeUi *engineer_joint1_time_change_ui{}, *engineer_joint2_time_change_ui{}, *engineer_joint3_time_change_ui{}; TargetDistanceTimeChangeUi* target_distance_time_change_ui_{}; FriendBulletsTimeChangeGroupUi* friend_bullets_time_change_group_ui_{}; - TargetHpTimeChangeUi* target_hp_time_change_ui_{}; DroneTowardsTimeChangeGroupUi* drone_towards_time_change_group_ui_{}; StringTriggerChangeUi *servo_mode_trigger_change_ui_{}, *stone_num_trigger_change_ui_{}, @@ -132,11 +130,9 @@ class RefereeBase CoverFlashUi* cover_flash_ui_{}; SpinFlashUi* spin_flash_ui_{}; - DeployFlashUi* deploy_flash_ui_{}; HeroHitFlashUi* hero_hit_flash_ui_{}; ExceedBulletSpeedFlashUi* exceed_bullet_speed_flash_ui_{}; CustomizeDisplayFlashUi* customize_display_flash_ui_{}; - BurstFlashUi* burst_flash_ui_{}; InteractiveSender* interactive_data_sender_{}; CustomInfoSender* custom_info_sender{}; diff --git a/rm_referee/include/rm_referee/ui/flash_ui.h b/rm_referee/include/rm_referee/ui/flash_ui.h index 4c9fe4c5..7ae978ee 100644 --- a/rm_referee/include/rm_referee/ui/flash_ui.h +++ b/rm_referee/include/rm_referee/ui/flash_ui.h @@ -91,21 +91,6 @@ class SpinFlashUi : public FlashUi uint8_t chassis_mode_; }; -class DeployFlashUi : public FlashUi -{ -public: - explicit DeployFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashUi(rpc_value, base, "deploy", graph_queue, character_queue){}; - void updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time); - void updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data); - -private: - void display(const ros::Time& time) override; - uint8_t chassis_mode_; - double angular_z_{ 0. }; -}; - class HeroHitFlashUi : public FlashGroupUi { public: @@ -156,18 +141,4 @@ class ExceedBulletSpeedFlashUi : public FlashUi rm_msgs::ShootData shoot_data_; }; -class BurstFlashUi : public FlashUi -{ -public: - explicit BurstFlashUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : FlashUi(rpc_value, base, "burst", graph_queue, character_queue) - { - } - void updateBurstTimeData(const rm_msgs::ManualToReferee::ConstPtr& data); - -private: - void display(const ros::Time& time) override; - ros::Time start_burst_time_; -}; } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/interactive_data.h b/rm_referee/include/rm_referee/ui/interactive_data.h index 6c3d6729..17c07e2a 100644 --- a/rm_referee/include/rm_referee/ui/interactive_data.h +++ b/rm_referee/include/rm_referee/ui/interactive_data.h @@ -19,7 +19,7 @@ class InteractiveSender : public UiBase void sendRadarInteractiveData(const rm_msgs::ClientMapReceiveData::ConstPtr& data); void sendMapSentryData(const rm_referee::MapSentryData& data); void sendSentryCmdData(const rm_msgs::SentryCmdConstPtr& data); - void sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data); + void sendRadarCmdData(const rm_msgs::RadarCmdConstPtr& data); virtual bool needSendInteractiveData(); ros::Time last_send_time_; }; 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 bc512091..e662af6d 100644 --- a/rm_referee/include/rm_referee/ui/time_change_ui.h +++ b/rm_referee/include/rm_referee/ui/time_change_ui.h @@ -251,19 +251,6 @@ class PitchAngleTimeChangeUi : public TimeChangeUi double pitch_angle_ = 0.; }; -class ImageTransmissionAngleTimeChangeUi : public TimeChangeUi -{ -public: - explicit ImageTransmissionAngleTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, - std::deque* graph_queue, std::deque* character_queue) - : TimeChangeUi(rpc_value, base, "image_transmission", graph_queue, character_queue){}; - void updateJointStateData(const sensor_msgs::JointState::ConstPtr data, const ros::Time& time); - -private: - void updateConfig() override; - double image_transmission_angle_ = 0.; -}; - class JointPositionTimeChangeUi : public TimeChangeUi { public: @@ -379,31 +366,4 @@ class FriendBulletsTimeChangeGroupUi : public TimeChangeGroupUi int hero_bullets_{ 1 }, standard3_bullets_{ 3 }, standard4_bullets_{ 4 }, standard5_bullets_{ 5 }; }; -class TargetHpTimeChangeUi : public TimeChangeUi -{ -public: - explicit TargetHpTimeChangeUi(XmlRpc::XmlRpcValue& rpc_value, Base& base, std::deque* graph_queue, - std::deque* character_queue) - : TimeChangeUi(rpc_value, base, "target_hp", graph_queue, character_queue) - { - if (rpc_value.hasMember("enemy_id")) - { - XmlRpc::XmlRpcValue& enemy_id = rpc_value["enemy_id"]; - for (int i = 0; i < enemy_id.size(); i++) - { - int id = static_cast(enemy_id[i]); - enemy_robot_hp_[id] = 0; - } - } - } - void setEnemyHp(const rm_msgs::GameRobotHp& data); - void updateTrackID(int id); - void updateTargeHptData(); - -private: - void updateConfig() override; - std::map enemy_robot_hp_; - int target_hp_{}, target_id_{}; -}; - } // namespace rm_referee diff --git a/rm_referee/include/rm_referee/ui/ui_base.h b/rm_referee/include/rm_referee/ui/ui_base.h index bf51f44c..9ffae534 100644 --- a/rm_referee/include/rm_referee/ui/ui_base.h +++ b/rm_referee/include/rm_referee/ui/ui_base.h @@ -51,7 +51,7 @@ class UiBase void display(const ros::Time& time, bool state, bool once = false); void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const; - uint8_t tx_buffer_[127]; + uint8_t tx_buffer_[128]; int tx_len_; protected: diff --git a/rm_referee/src/referee.cpp b/rm_referee/src/referee.cpp index d0349ad6..74048e28 100644 --- a/rm_referee/src/referee.cpp +++ b/rm_referee/src/referee.cpp @@ -41,6 +41,12 @@ namespace rm_referee // read data from referee void Referee::read() { + const bool perf_enabled = perf_stats_enabled_; + const ros::WallTime read_begin = perf_enabled ? ros::WallTime::now() : ros::WallTime(); + int perf_unpack_calls = 0; + int perf_frames_ok = 0; + double perf_unpack_ms_sum = 0.0; + double perf_unpack_ms_max = 0.0; if (base_.serial_.available()) { rx_len_ = static_cast(base_.serial_.available()); @@ -48,7 +54,7 @@ void Referee::read() } else return; - uint8_t temp_buffer[256] = { 0 }; + uint8_t temp_buffer[k_unpack_buffer_length_] = { 0 }; int frame_len; if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) base_.referee_data_is_online_ = false; @@ -61,45 +67,95 @@ void Referee::read() for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) unpack_buffer_[k_i] = temp_buffer[k_i]; } + else + { + const int offset = rx_len_ - k_unpack_buffer_length_; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + unpack_buffer_[k_i] = rx_buffer_[offset + k_i]; + } for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) { if (unpack_buffer_[k_i] == 0xA5) { - frame_len = unpack(&unpack_buffer_[k_i]); - if (frame_len != -1) - k_i += frame_len; + if (perf_enabled) + { + const ros::WallTime unpack_begin = ros::WallTime::now(); + frame_len = unpack(&unpack_buffer_[k_i], k_unpack_buffer_length_ - k_i); + const double unpack_ms = (ros::WallTime::now() - unpack_begin).toSec() * 1000.0; + ++perf_unpack_calls; + perf_unpack_ms_sum += unpack_ms; + if (unpack_ms > perf_unpack_ms_max) + perf_unpack_ms_max = unpack_ms; + } + else + { + frame_len = unpack(&unpack_buffer_[k_i], k_unpack_buffer_length_ - k_i); + } + if (frame_len > 0) + { + if (perf_enabled) + ++perf_frames_ok; + k_i += frame_len - 1; + } } } getRobotInfo(); clearRxBuffer(); + if (perf_enabled) + { + const double read_ms = (ros::WallTime::now() - read_begin).toSec() * 1000.0; + recordPerfStats(rx_len_, perf_unpack_calls, perf_frames_ok, read_ms, perf_unpack_ms_sum, perf_unpack_ms_max); + } } -int Referee::unpack(uint8_t* rx_data) +int Referee::unpack(uint8_t* rx_data, int rx_data_len) { uint16_t cmd_id; int frame_len; rm_referee::FrameHeader frame_header; + if (rx_data_len < k_header_length_) + return -1; + memcpy(&frame_header, rx_data, k_header_length_); if (static_cast(base_.verifyCRC8CheckSum(rx_data, k_header_length_))) { - if (frame_header.data_length > 256) // temporary and inaccurate value + const uint16_t max_payload_len = k_unpack_buffer_length_ - k_header_length_ - k_cmd_id_length_ - k_tail_length_; + if (frame_header.data_length > max_payload_len) { ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length); return 0; } frame_len = frame_header.data_length + k_header_length_ + k_cmd_id_length_ + k_tail_length_; - + if (frame_len > rx_data_len || frame_len > k_unpack_buffer_length_) + return -1; if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1) { + last_get_data_time_ = ros::Time::now(); cmd_id = (rx_data[6] << 8 | rx_data[5]); + auto ensure_payload = [&](size_t expected_len, const char* cmd_name) { + if (frame_header.data_length < expected_len) + { + ROS_WARN("Drop cmd %s due to short payload: %u < %zu", cmd_name, + static_cast(frame_header.data_length), expected_len); + return false; + } + return true; + }; + auto copy_payload = [&](void* dst, size_t len, const char* cmd_name) { + if (!ensure_payload(len, cmd_name)) + return false; + memcpy(dst, rx_data + 7, len); + return true; + }; switch (cmd_id) { case rm_referee::RefereeCmdId::GAME_STATUS_CMD: { rm_referee::GameStatus game_status_ref; rm_msgs::GameStatus game_status_data; - memcpy(&game_status_ref, rx_data + 7, sizeof(rm_referee::GameStatus)); + if (!copy_payload(&game_status_ref, sizeof(rm_referee::GameStatus), "GAME_STATUS_CMD")) + break; game_status_data.game_type = game_status_ref.game_type; game_status_data.game_progress = game_status_ref.game_progress; @@ -114,32 +170,27 @@ int Referee::unpack(uint8_t* rx_data) case rm_referee::RefereeCmdId::GAME_RESULT_CMD: { rm_referee::GameResult game_result_ref; - memcpy(&game_result_ref, rx_data + 7, sizeof(rm_referee::GameResult)); + if (!copy_payload(&game_result_ref, sizeof(rm_referee::GameResult), "GAME_RESULT_CMD")) + break; break; } case rm_referee::RefereeCmdId::GAME_ROBOT_HP_CMD: { rm_referee::GameRobotHp game_robot_hp_ref; rm_msgs::GameRobotHp game_robot_hp_data; - memcpy(&game_robot_hp_ref, rx_data + 7, sizeof(rm_referee::GameRobotHp)); - - game_robot_hp_data.blue_1_robot_hp = game_robot_hp_ref.blue_1_robot_hp; - game_robot_hp_data.blue_2_robot_hp = game_robot_hp_ref.blue_2_robot_hp; - game_robot_hp_data.blue_3_robot_hp = game_robot_hp_ref.blue_3_robot_hp; - game_robot_hp_data.blue_4_robot_hp = game_robot_hp_ref.blue_4_robot_hp; - game_robot_hp_data.blue_7_robot_hp = game_robot_hp_ref.blue_7_robot_hp; - game_robot_hp_data.blue_outpost_hp = game_robot_hp_ref.blue_outpost_hp; - game_robot_hp_data.blue_base_hp = game_robot_hp_ref.blue_base_hp; - game_robot_hp_data.red_1_robot_hp = game_robot_hp_ref.red_1_robot_hp; - game_robot_hp_data.red_2_robot_hp = game_robot_hp_ref.red_2_robot_hp; - game_robot_hp_data.red_3_robot_hp = game_robot_hp_ref.red_3_robot_hp; - game_robot_hp_data.red_4_robot_hp = game_robot_hp_ref.red_4_robot_hp; - game_robot_hp_data.red_7_robot_hp = game_robot_hp_ref.red_7_robot_hp; - game_robot_hp_data.red_outpost_hp = game_robot_hp_ref.red_outpost_hp; - game_robot_hp_data.red_base_hp = game_robot_hp_ref.red_base_hp; + if (!copy_payload(&game_robot_hp_ref, sizeof(rm_referee::GameRobotHp), "GAME_ROBOT_HP_CMD")) + break; + + game_robot_hp_data.ally_1_robot_hp = game_robot_hp_ref.ally_1_robot_hp; + game_robot_hp_data.ally_2_robot_hp = game_robot_hp_ref.ally_2_robot_hp; + game_robot_hp_data.ally_3_robot_hp = game_robot_hp_ref.ally_3_robot_hp; + game_robot_hp_data.ally_4_robot_hp = game_robot_hp_ref.ally_4_robot_hp; + game_robot_hp_data.ally_7_robot_hp = game_robot_hp_ref.ally_7_robot_hp; + game_robot_hp_data.ally_outpost_hp = game_robot_hp_ref.ally_outpost_hp; + game_robot_hp_data.ally_base_hp = game_robot_hp_ref.ally_base_hp; game_robot_hp_data.stamp = last_get_data_time_; - referee_ui_.updateGameRobotHpDataCallBack(game_robot_hp_data); + referee_ui_.updateHeroHitDataCallBack(game_robot_hp_data); game_robot_hp_pub_.publish(game_robot_hp_data); break; } @@ -147,7 +198,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::DartStatus dart_status_ref; rm_msgs::DartStatus dart_status_data; - memcpy(&dart_status_ref, rx_data + 7, sizeof(rm_referee::DartStatus)); + if (!copy_payload(&dart_status_ref, sizeof(rm_referee::DartStatus), "DART_STATUS_CMD")) + break; dart_status_data.dart_belong = dart_status_ref.dart_belong; dart_status_data.stage_remaining_time = dart_status_ref.stage_remaining_time; @@ -160,7 +212,9 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::IcraBuffDebuffZoneStatus icra_buff_debuff_zone_status_ref; rm_msgs::IcraBuffDebuffZoneStatus icra_buff_debuff_zone_status_data; - memcpy(&icra_buff_debuff_zone_status_ref, rx_data + 7, sizeof(rm_referee::IcraBuffDebuffZoneStatus)); + if (!copy_payload(&icra_buff_debuff_zone_status_ref, sizeof(rm_referee::IcraBuffDebuffZoneStatus), + "ICRA_ZONE_STATUS_CMD")) + break; icra_buff_debuff_zone_status_data.blue_1_bullet_left = icra_buff_debuff_zone_status_ref.blue_1_bullet_left; icra_buff_debuff_zone_status_data.blue_2_bullet_left = icra_buff_debuff_zone_status_ref.blue_2_bullet_left; @@ -193,13 +247,14 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::EventData event_ref; rm_msgs::EventData event_data; - memcpy(&event_ref, rx_data + 7, sizeof(rm_referee::EventData)); + if (!copy_payload(&event_ref, sizeof(rm_referee::EventData), "FIELD_EVENTS_CMD")) + break; event_data.overlapping_supply_station_state = event_ref.overlapping_supply_station_state; event_data.nan_overlapping_supply_station_state = event_ref.nan_overlapping_supply_station_state; event_data.supplier_zone_state = event_ref.supplier_zone_state; event_data.small_power_rune_state = event_ref.small_power_rune_state; - event_data.large_power_rune_state = event_ref.small_power_rune_state; + event_data.large_power_rune_state = event_ref.large_power_rune_state; event_data.central_elevated_ground_state = event_ref.central_elevated_ground_state; event_data.trapezoidal_elevated_ground_state = event_ref.trapezoidal_elevated_ground_state; event_data.be_hit_time = event_ref.be_hit_time; @@ -214,7 +269,9 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::SupplyProjectileAction supply_projectile_action_ref; rm_msgs::SupplyProjectileAction supply_projectile_action_data; - memcpy(&supply_projectile_action_ref, rx_data + 7, sizeof(rm_referee::SupplyProjectileAction)); + if (!copy_payload(&supply_projectile_action_ref, sizeof(rm_referee::SupplyProjectileAction), + "SUPPLY_PROJECTILE_ACTION_CMD")) + break; supply_projectile_action_data.reserved = supply_projectile_action_ref.reserved; supply_projectile_action_data.supply_projectile_num = supply_projectile_action_ref.supply_projectile_num; @@ -229,19 +286,21 @@ int Referee::unpack(uint8_t* rx_data) case rm_referee::RefereeCmdId::REFEREE_WARNING_CMD: { rm_referee::RefereeWarning referee_warning_ref; - memcpy(&referee_warning_ref, rx_data + 7, sizeof(rm_referee::RefereeWarning)); + if (!copy_payload(&referee_warning_ref, sizeof(rm_referee::RefereeWarning), "REFEREE_WARNING_CMD")) + break; break; } case rm_referee::RefereeCmdId::DART_INFO_CMD: { rm_referee::DartInfo dart_info_ref; rm_msgs::DartInfo dart_info_data; - memcpy(&dart_info_ref, rx_data + 7, sizeof(rm_referee::DartInfo)); + if (!copy_payload(&dart_info_ref, sizeof(rm_referee::DartInfo), "DART_INFO_CMD")) + break; dart_info_data.dart_remaining_time = dart_info_ref.dart_remaining_time; - dart_info_data.dart_last_aim_state = dart_info_ref.dart_last_aim_state; - dart_info_data.enemy_total_hit_received = dart_info_ref.enemy_total_hit_received; - dart_info_data.dart_current_target = dart_info_ref.dart_current_target; + dart_info_data.dart_last_aim_state = static_cast(dart_info_ref.dart_info & 0x07u); + dart_info_data.enemy_total_hit_received = static_cast((dart_info_ref.dart_info >> 3) & 0x07u); + dart_info_data.dart_current_target = static_cast((dart_info_ref.dart_info >> 6) & 0x07u); dart_info_data.stamp = last_get_data_time_; dart_info_pub_.publish(dart_info_data); @@ -251,7 +310,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::GameRobotStatus game_robot_status_ref; rm_msgs::GameRobotStatus game_robot_status_data; - memcpy(&game_robot_status_ref, rx_data + 7, sizeof(rm_referee::GameRobotStatus)); + if (!copy_payload(&game_robot_status_ref, sizeof(rm_referee::GameRobotStatus), "ROBOT_STATUS_CMD")) + break; game_robot_status_data.remain_hp = game_robot_status_ref.remain_hp; game_robot_status_data.robot_level = game_robot_status_ref.robot_level; @@ -274,11 +334,11 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::PowerHeatData power_heat_ref; rm_msgs::PowerHeatData power_heat_data; - memcpy(&power_heat_ref, rx_data + 7, sizeof(rm_referee::PowerHeatData)); + if (!copy_payload(&power_heat_ref, sizeof(rm_referee::PowerHeatData), "POWER_HEAT_DATA_CMD")) + break; power_heat_data.chassis_power_buffer = power_heat_ref.chassis_power_buffer; power_heat_data.shooter_id_1_17_mm_cooling_heat = power_heat_ref.shooter_id_1_17_mm_cooling_heat; - power_heat_data.shooter_id_2_17_mm_cooling_heat = power_heat_ref.shooter_id_2_17_mm_cooling_heat; power_heat_data.shooter_id_1_42_mm_cooling_heat = power_heat_ref.shooter_id_1_42_mm_cooling_heat; power_heat_data.stamp = last_get_data_time_; @@ -290,7 +350,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::GameRobotPos game_robot_pos_ref; rm_msgs::GameRobotPosData game_robot_pos_data; - memcpy(&game_robot_pos_ref, rx_data + 7, sizeof(rm_referee::GameRobotPos)); + if (!copy_payload(&game_robot_pos_ref, sizeof(rm_referee::GameRobotPos), "ROBOT_POS_CMD")) + break; game_robot_pos_data.x = game_robot_pos_ref.x; game_robot_pos_data.y = game_robot_pos_ref.y; @@ -302,23 +363,32 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::Buff referee_buff; rm_msgs::Buff robot_buff; - memcpy(&referee_buff, rx_data + 7, sizeof(rm_referee::Buff)); + if (!copy_payload(&referee_buff, sizeof(rm_referee::Buff), "BUFF_CMD")) + break; robot_buff.attack_buff = referee_buff.attack_buff; robot_buff.defence_buff = referee_buff.defence_buff; robot_buff.vulnerability_buff = referee_buff.vulnerability_buff; robot_buff.cooling_buff = referee_buff.cooling_buff; robot_buff.recovery_buff = referee_buff.recovery_buff; - - if (referee_buff.remaining_energy & 0x1E) + const uint8_t remain = referee_buff.remaining_energy; + if (remain == 0x80) + robot_buff.remaining_energy = 100; + else if (remain & (1u << 0)) + robot_buff.remaining_energy = 125; + else if (remain & (1u << 1)) + robot_buff.remaining_energy = 100; + else if (remain & (1u << 2)) robot_buff.remaining_energy = 50; - else if (referee_buff.remaining_energy & 0x1C) + else if (remain & (1u << 3)) robot_buff.remaining_energy = 30; - else if (referee_buff.remaining_energy & 0x18) + else if (remain & (1u << 4)) robot_buff.remaining_energy = 15; - else if (referee_buff.remaining_energy & 0x10) + else if (remain & (1u << 5)) robot_buff.remaining_energy = 5; - else if (referee_buff.remaining_energy & 0x00) + else if (remain & (1u << 6)) robot_buff.remaining_energy = 1; + else + robot_buff.remaining_energy = 0; buff_pub_.publish(robot_buff); break; @@ -326,14 +396,16 @@ int Referee::unpack(uint8_t* rx_data) case rm_referee::RefereeCmdId::AERIAL_ROBOT_ENERGY_CMD: { rm_referee::AerialRobotEnergy aerial_robot_energy_ref; - memcpy(&aerial_robot_energy_ref, rx_data + 7, sizeof(rm_referee::AerialRobotEnergy)); + if (!copy_payload(&aerial_robot_energy_ref, sizeof(rm_referee::AerialRobotEnergy), "AERIAL_ROBOT_ENERGY_CMD")) + break; break; } case rm_referee::RefereeCmdId::ROBOT_HURT_CMD: { rm_referee::RobotHurt robot_hurt_ref; rm_msgs::RobotHurt robot_hurt_data; - memcpy(&robot_hurt_ref, rx_data + 7, sizeof(rm_referee::RobotHurt)); + if (!copy_payload(&robot_hurt_ref, sizeof(rm_referee::RobotHurt), "ROBOT_HURT_CMD")) + break; robot_hurt_data.armor_id = robot_hurt_ref.armor_id; robot_hurt_data.hurt_type = robot_hurt_ref.hurt_type; @@ -348,8 +420,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::ShootData shoot_data_ref; rm_msgs::ShootData shoot_data; - - memcpy(&shoot_data_ref, rx_data + 7, sizeof(rm_referee::ShootData)); + if (!copy_payload(&shoot_data_ref, sizeof(rm_referee::ShootData), "SHOOT_DATA_CMD")) + break; shoot_data.bullet_freq = shoot_data_ref.bullet_freq; shoot_data.bullet_speed = shoot_data_ref.bullet_speed; @@ -365,11 +437,13 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::BulletAllowance bullet_allowance_ref; rm_msgs::BulletAllowance bullet_allowance_data; - memcpy(&bullet_allowance_ref, rx_data + 7, sizeof(rm_referee::BulletAllowance)); + if (!copy_payload(&bullet_allowance_ref, sizeof(rm_referee::BulletAllowance), "BULLET_REMAINING_CMD")) + break; bullet_allowance_data.bullet_allowance_num_17_mm = bullet_allowance_ref.bullet_allowance_num_17_mm; bullet_allowance_data.bullet_allowance_num_42_mm = bullet_allowance_ref.bullet_allowance_num_42_mm; bullet_allowance_data.coin_remaining_num = bullet_allowance_ref.coin_remaining_num; + bullet_allowance_data.projectile_allowance_fortress = bullet_allowance_ref.projectile_allowance_fortress; bullet_allowance_data.stamp = last_get_data_time_; referee_ui_.bulletRemainDataCallBack(bullet_allowance_data, last_get_data_time_); @@ -380,46 +454,55 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::RfidStatus rfid_status_ref; rm_msgs::RfidStatus rfid_status_data; - memcpy(&rfid_status_ref, rx_data + 7, sizeof(rm_referee::RfidStatus)); - - rfid_status_data.base_buff_point_state = rfid_status_ref.base_buff_point_state; - rfid_status_data.own_central_elevated_ground_state = rfid_status_ref.own_central_elevated_ground_state; - rfid_status_data.enemy_central_elevated_ground_state = rfid_status_ref.enemy_central_elevated_ground_state; - rfid_status_data.own_trapezoidal_elevated_ground_state = - rfid_status_ref.own_trapezoidal_elevated_ground_state; - rfid_status_data.enemy_trapezoidal_elevated_ground_state = - rfid_status_ref.enemy_trapezoidal_elevated_ground_state; - rfid_status_data.forward_own_terrain_span_buff_point_state = - rfid_status_ref.forward_own_terrain_span_buff_point_state; - rfid_status_data.behind_own_terrain_span_buff_point_state = - rfid_status_ref.behind_own_terrain_span_buff_point_state; - rfid_status_data.forward_enemy_terrain_span_buff_point_state = - rfid_status_ref.forward_enemy_terrain_span_buff_point_state; - rfid_status_data.behind_enemy_terrain_span_buff_point_state = - rfid_status_ref.behind_enemy_terrain_span_buff_point_state; - rfid_status_data.below_central_own_terrain_span_buff_point_state = - rfid_status_ref.below_central_own_terrain_span_buff_point_state; - rfid_status_data.upper_central_own_terrain_span_buff_point_state = - rfid_status_ref.upper_central_own_terrain_span_buff_point_state; - rfid_status_data.below_central_enemy_terrain_span_buff_point_state = - rfid_status_ref.below_central_enemy_terrain_span_buff_point_state; - rfid_status_data.upper_central_enemy_terrain_span_buff_point_state = - rfid_status_ref.upper_central_enemy_terrain_span_buff_point_state; - rfid_status_data.below_road_own_terrain_span_buff_point_state = - rfid_status_ref.below_road_own_terrain_span_buff_point_state; - rfid_status_data.upper_road_own_terrain_span_buff_point_state = - rfid_status_ref.upper_road_own_terrain_span_buff_point_state; - rfid_status_data.below_road_enemy_terrain_span_buff_point_state = - rfid_status_ref.below_road_enemy_terrain_span_buff_point_state; - rfid_status_data.upper_road_enemy_terrain_span_buff_point_state = - rfid_status_ref.upper_road_enemy_terrain_span_buff_point_state; - rfid_status_data.own_fort_buff_point = rfid_status_ref.own_fort_buff_point; - rfid_status_data.own_outpost_buff_point = rfid_status_ref.own_outpost_buff_point; - rfid_status_data.nan_overlapping_supplier_zone = rfid_status_ref.nan_overlapping_supplier_zone; - rfid_status_data.overlapping_supplier_zone = rfid_status_ref.overlapping_supplier_zone; - rfid_status_data.own_large_resource_island_point = rfid_status_ref.own_large_resource_island_point; - rfid_status_data.enemy_large_resource_island_point = rfid_status_ref.enemy_large_resource_island_point; - rfid_status_data.central_buff_point = rfid_status_ref.central_buff_point; + if (!copy_payload(&rfid_status_ref, sizeof(rm_referee::RfidStatus), "ROBOT_RFID_STATUS_CMD")) + break; + + const uint32_t rfid_status = rfid_status_ref.rfid_status; + const uint8_t rfid_status_2 = rfid_status_ref.rfid_status_2; + rfid_status_data.rfid_status = rfid_status_ref.rfid_status; + rfid_status_data.rfid_status_2 = rfid_status_ref.rfid_status_2; + + auto bit32 = [&](int bit) { return static_cast((rfid_status >> bit) & 0x1u); }; + auto bit8 = [&](int bit) { return static_cast((rfid_status_2 >> bit) & 0x1u); }; + + rfid_status_data.base_buff_point_state = bit32(0); + rfid_status_data.own_central_elevated_ground_state = bit32(1); + rfid_status_data.enemy_central_elevated_ground_state = bit32(2); + rfid_status_data.own_trapezoidal_elevated_ground_state = bit32(3); + rfid_status_data.enemy_trapezoidal_elevated_ground_state = bit32(4); + rfid_status_data.forward_own_terrain_span_buff_point_state = bit32(5); + rfid_status_data.behind_own_terrain_span_buff_point_state = bit32(6); + rfid_status_data.forward_enemy_terrain_span_buff_point_state = bit32(7); + rfid_status_data.behind_enemy_terrain_span_buff_point_state = bit32(8); + rfid_status_data.below_central_own_terrain_span_buff_point_state = bit32(9); + rfid_status_data.upper_central_own_terrain_span_buff_point_state = bit32(10); + rfid_status_data.below_central_enemy_terrain_span_buff_point_state = bit32(11); + rfid_status_data.upper_central_enemy_terrain_span_buff_point_state = bit32(12); + rfid_status_data.below_road_own_terrain_span_buff_point_state = bit32(13); + rfid_status_data.upper_road_own_terrain_span_buff_point_state = bit32(14); + rfid_status_data.below_road_enemy_terrain_span_buff_point_state = bit32(15); + rfid_status_data.upper_road_enemy_terrain_span_buff_point_state = bit32(16); + rfid_status_data.own_fort_buff_point_state = bit32(17); + rfid_status_data.own_outpost_buff_point_state = bit32(18); + rfid_status_data.non_overlapping_supplier_zone_state = bit32(19); + rfid_status_data.overlapping_supplier_zone_state = bit32(20); + rfid_status_data.own_assembly_buff_point_state = bit32(21); + rfid_status_data.enemy_assembly_buff_point_state = bit32(22); + rfid_status_data.central_buff_point_state = bit32(23); + rfid_status_data.enemy_fort_buff_point_state = bit32(24); + rfid_status_data.enemy_outpost_buff_point_state = bit32(25); + rfid_status_data.own_tunnel_road_lower_buff_point_state = bit32(26); + rfid_status_data.own_tunnel_road_middle_buff_point_state = bit32(27); + rfid_status_data.own_tunnel_road_upper_buff_point_state = bit32(28); + rfid_status_data.own_tunnel_trapezoid_lower_buff_point_state = bit32(29); + rfid_status_data.own_tunnel_trapezoid_middle_buff_point_state = bit32(30); + rfid_status_data.own_tunnel_trapezoid_upper_buff_point_state = bit32(31); + rfid_status_data.enemy_tunnel_road_lower_buff_point_state = bit8(0); + rfid_status_data.enemy_tunnel_road_middle_buff_point_state = bit8(1); + rfid_status_data.enemy_tunnel_road_upper_buff_point_state = bit8(2); + rfid_status_data.enemy_tunnel_trapezoid_lower_buff_point_state = bit8(3); + rfid_status_data.enemy_tunnel_trapezoid_middle_buff_point_state = bit8(4); + rfid_status_data.enemy_tunnel_trapezoid_upper_buff_point_state = bit8(5); rfid_status_data.stamp = last_get_data_time_; rfid_status_pub_.publish(rfid_status_data); @@ -429,8 +512,11 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::DartClientCmd dart_client_cmd_ref; rm_msgs::DartClientCmd dart_client_cmd_data; - memcpy(&dart_client_cmd_ref, rx_data + 7, sizeof(rm_referee::DartClientCmd)); + if (!copy_payload(&dart_client_cmd_ref, sizeof(rm_referee::DartClientCmd), "DART_CLIENT_CMD")) + break; + dart_client_cmd_data.dart_launch_opening_status = dart_client_cmd_ref.dart_launch_opening_status; + dart_client_cmd_data.reserved = dart_client_cmd_ref.reserved; dart_client_cmd_data.target_change_time = dart_client_cmd_ref.target_change_time; dart_client_cmd_data.latest_launch_cmd_time = dart_client_cmd_ref.latest_launch_cmd_time; dart_client_cmd_data.stamp = last_get_data_time_; @@ -442,7 +528,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::RobotsPositionData robots_position_ref; rm_msgs::RobotsPositionData robots_position_data; - memcpy(&robots_position_ref, rx_data + 7, sizeof(rm_referee::RobotsPositionData)); + if (!copy_payload(&robots_position_ref, sizeof(rm_referee::RobotsPositionData), "ROBOTS_POS_CMD")) + break; robots_position_data.engineer_x = robots_position_ref.engineer_x; robots_position_data.engineer_y = robots_position_ref.engineer_y; @@ -461,32 +548,51 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::RadarMarkData radar_mark_ref; rm_msgs::RadarMarkData radar_mark_data; - memcpy(&radar_mark_ref, rx_data + 7, sizeof(rm_referee::RadarMarkData)); - - radar_mark_data.mark_engineer_progress = radar_mark_ref.mark_engineer_progress; - radar_mark_data.mark_hero_progress = radar_mark_ref.mark_hero_progress; - radar_mark_data.mark_sentry_progress = radar_mark_ref.mark_sentry_progress; - radar_mark_data.mark_standard_3_progress = radar_mark_ref.mark_standard_3_progress; - radar_mark_data.mark_standard_4_progress = radar_mark_ref.mark_standard_4_progress; + if (!copy_payload(&radar_mark_ref, sizeof(rm_referee::RadarMarkData), "RADAR_MARK_CMD")) + break; + + radar_mark_data.mark_progress = radar_mark_ref.mark_progress; + radar_mark_data.enemy_hero_vulnerable = radar_mark_ref.enemy_hero_vulnerable; + radar_mark_data.enemy_engineer_vulnerable = radar_mark_ref.enemy_engineer_vulnerable; + radar_mark_data.enemy_standard_3_vulnerable = radar_mark_ref.enemy_standard_3_vulnerable; + radar_mark_data.enemy_standard_4_vulnerable = radar_mark_ref.enemy_standard_4_vulnerable; + radar_mark_data.enemy_aerial_special_mark = radar_mark_ref.enemy_aerial_special_mark; + radar_mark_data.enemy_sentry_vulnerable = radar_mark_ref.enemy_sentry_vulnerable; + radar_mark_data.own_hero_special_mark = radar_mark_ref.own_hero_special_mark; + radar_mark_data.own_engineer_special_mark = radar_mark_ref.own_engineer_special_mark; + radar_mark_data.own_standard_3_special_mark = radar_mark_ref.own_standard_3_special_mark; + radar_mark_data.own_standard_4_special_mark = radar_mark_ref.own_standard_4_special_mark; + radar_mark_data.own_aerial_special_mark = radar_mark_ref.own_aerial_special_mark; + radar_mark_data.own_sentry_special_mark = radar_mark_ref.own_sentry_special_mark; + radar_mark_data.mark_hero_progress = radar_mark_ref.enemy_hero_vulnerable; + radar_mark_data.mark_engineer_progress = radar_mark_ref.enemy_engineer_vulnerable; + radar_mark_data.mark_standard_3_progress = radar_mark_ref.enemy_standard_3_vulnerable; + radar_mark_data.mark_standard_4_progress = radar_mark_ref.enemy_standard_4_vulnerable; + radar_mark_data.mark_sentry_progress = radar_mark_ref.enemy_sentry_vulnerable; radar_mark_data.stamp = last_get_data_time_; radar_mark_pub_.publish(radar_mark_data); + break; } case rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD: { rm_referee::InteractiveData interactive_data_ref; // local variable temporarily before moving referee data - memcpy(&interactive_data_ref, rx_data + 7, sizeof(rm_referee::InteractiveData)); + if (!copy_payload(&interactive_data_ref, sizeof(rm_referee::InteractiveData), "INTERACTIVE_DATA_CMD")) + break; if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::BULLET_NUM_SHARE_CMD) { rm_referee::BulletNumData bullet_num_data_ref; - memcpy(&bullet_num_data_ref, rx_data + 7, sizeof(rm_referee::BulletNumData)); + if (!copy_payload(&bullet_num_data_ref, sizeof(rm_referee::BulletNumData), "BULLET_NUM_SHARE_CMD")) + break; referee_ui_.updateBulletRemainData(bullet_num_data_ref); } else if (interactive_data_ref.header_data.data_cmd_id == rm_referee::DataCmdId::SENTRY_TO_RADAR_CMD) { rm_referee::SentryAttackingTargetData sentry_attacking_target_data_ref; rm_msgs::SentryAttackingTarget sentry_attacking_target_data_data; - memcpy(&sentry_attacking_target_data_ref, rx_data + 7, sizeof(rm_referee::SentryAttackingTargetData)); + if (!copy_payload(&sentry_attacking_target_data_ref, sizeof(rm_referee::SentryAttackingTargetData), + "SENTRY_TO_RADAR_CMD")) + break; sentry_attacking_target_data_data.target_robot_ID = sentry_attacking_target_data_ref.target_robot_ID; sentry_attacking_target_data_data.target_position_x = sentry_attacking_target_data_ref.target_position_x; sentry_attacking_target_data_data.target_position_y = sentry_attacking_target_data_ref.target_position_y; @@ -496,7 +602,8 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::RadarToSentryData radar_to_sentry_data_ref; rm_msgs::RadarToSentry radar_to_sentry_data; - memcpy(&radar_to_sentry_data_ref, rx_data + 7, sizeof(rm_referee::RadarToSentryData)); + if (!copy_payload(&radar_to_sentry_data_ref, sizeof(rm_referee::RadarToSentryData), "RADAR_TO_SENTRY_CMD")) + break; radar_to_sentry_data.robot_ID = radar_to_sentry_data_ref.robot_ID; radar_to_sentry_data.position_x = radar_to_sentry_data_ref.position_x; radar_to_sentry_data.position_y = radar_to_sentry_data_ref.position_y; @@ -509,20 +616,23 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::ClientMapReceiveData client_map_receive_ref; rm_msgs::ClientMapReceiveData client_map_receive_data; - memcpy(&client_map_receive_ref, rx_data + 7, sizeof(rm_referee::ClientMapReceiveData)); + if (!copy_payload(&client_map_receive_ref, sizeof(rm_referee::ClientMapReceiveData), "CLIENT_MAP_CMD")) + break; break; } - case rm_referee::CUSTOM_TO_ROBOT_CMD: + case rm_referee::CUSTOM_INFO_CMD: { rm_referee::CustomInfo custom_info; - memcpy(&custom_info, rx_data + 7, sizeof(rm_referee::CustomInfo)); + if (!copy_payload(&custom_info, sizeof(rm_referee::CustomInfo), "CUSTOM_INFO_CMD")) + break; break; } case rm_referee::TARGET_POS_CMD: { rm_referee::ClientMapSendData client_map_send_data_ref; rm_msgs::ClientMapSendData client_map_send_data; - memcpy(&client_map_send_data_ref, rx_data + 7, sizeof(rm_referee::ClientMapSendData)); + if (!copy_payload(&client_map_send_data_ref, sizeof(rm_referee::ClientMapSendData), "TARGET_POS_CMD")) + break; client_map_send_data.target_position_x = client_map_send_data_ref.target_position_x; client_map_send_data.target_position_y = client_map_send_data_ref.target_position_y; @@ -536,17 +646,34 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::SentryInfo sentry_info_ref; rm_msgs::SentryInfo sentry_info; - memcpy(&sentry_info_ref, rx_data + 7, sizeof(rm_referee::SentryInfo)); + if (!copy_payload(&sentry_info_ref, sizeof(rm_referee::SentryInfo), "SENTRY_INFO_CMD")) + break; sentry_info.sentry_info = sentry_info_ref.sentry_info; + sentry_info.sentry_info_2 = sentry_info_ref.sentry_info_2; + sentry_info.exchanged_bullet_allowance = sentry_info_ref.exchanged_bullet_allowance; + sentry_info.remote_bullet_exchange_success_cnt = sentry_info_ref.remote_bullet_exchange_success_cnt; + sentry_info.remote_hp_exchange_success_cnt = sentry_info_ref.remote_hp_exchange_success_cnt; + sentry_info.can_confirm_free_respawn = sentry_info_ref.can_confirm_free_respawn; + sentry_info.can_exchange_instant_respawn = sentry_info_ref.can_exchange_instant_respawn; + sentry_info.instant_respawn_cost = sentry_info_ref.instant_respawn_cost; sentry_info.is_out_of_war = sentry_info_ref.is_out_of_war; sentry_info.remaining_bullets_can_supply = sentry_info_ref.remaining_bullets_can_supply; + sentry_info.sentry_mode = sentry_info_ref.sentry_mode; + sentry_info.can_activate_energy_mechanism = sentry_info_ref.can_activate_energy_mechanism; sentry_info_pub_.publish(sentry_info); break; } case rm_referee::RADAR_INFO_CMD: { + rm_referee::RadarInfo radar_info_ref; rm_msgs::RadarInfo radar_info; - memcpy(&radar_info, rx_data + 7, sizeof(rm_msgs::RadarInfo)); + if (!copy_payload(&radar_info_ref, sizeof(rm_referee::RadarInfo), "RADAR_INFO_CMD")) + break; + radar_info.radar_info = radar_info_ref.radar_info; + radar_info.double_vulnerability_chances = radar_info_ref.double_vulnerability_chances; + radar_info.enemy_in_double_vulnerability = radar_info_ref.enemy_in_double_vulnerability; + radar_info.own_encryption_level = radar_info_ref.own_encryption_level; + radar_info.can_modify_key = radar_info_ref.can_modify_key; radar_info_pub_.publish(radar_info); break; } @@ -554,10 +681,14 @@ int Referee::unpack(uint8_t* rx_data) { rm_msgs::PowerManagementSampleAndStatusData sample_and_status_pub_data; uint8_t data[sizeof(rm_referee::PowerManagementSampleAndStatusData)]; - memcpy(&data, rx_data + 7, sizeof(rm_referee::PowerManagementSampleAndStatusData)); + if (!ensure_payload(sizeof(rm_referee::PowerManagementSampleAndStatusData), + "POWER_MANAGEMENT_SAMPLE_AND_STATUS_DATA_CMD")) + break; + 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.cap_error_flag = (static_cast((data[2] << 8) | data[3]) / 100.); - sample_and_status_pub_data.cap_received_msg = (static_cast((data[4] << 8) | data[5]) / 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 = + (static_cast((data[4] << 8) | data[5]) / 100.); sample_and_status_pub_data.capacity_remain_charge = (static_cast((data[6] << 8) | data[7]) / 10000.); sample_and_status_pub_data.capacity_discharge_power = static_cast(data[8]); @@ -576,8 +707,10 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::PowerManagementInitializationExceptionData initialization_exception_ref; rm_msgs::PowerManagementInitializationExceptionData initialization_exception_pub_data; - memcpy(&initialization_exception_ref, rx_data + 7, - sizeof(rm_referee::PowerManagementInitializationExceptionData)); + if (!copy_payload(&initialization_exception_ref, + sizeof(rm_referee::PowerManagementInitializationExceptionData), + "POWER_MANAGEMENT_INITIALIZATION_EXCEPTION_CMD")) + break; initialization_exception_pub_data.error_code = initialization_exception_ref.error_code; initialization_exception_pub_data.string = initialization_exception_ref.string; @@ -587,6 +720,9 @@ int Referee::unpack(uint8_t* rx_data) } case rm_referee::POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD: { + if (!ensure_payload(sizeof(rm_referee::PowerManagementSystemExceptionData), + "POWER_MANAGEMENT_SYSTEM_EXCEPTION_CMD")) + break; unsigned char* tmp_rx_data_ptr = rx_data + 7; rm_msgs::PowerManagementSystemExceptionData system_exception_pub_data; @@ -614,17 +750,22 @@ int Referee::unpack(uint8_t* rx_data) { rm_referee::PowerManagementProcessStackOverflowData stack_overflow_ref; rm_msgs::PowerManagementProcessStackOverflowData stack_overflow_pub_data; - memcpy(&stack_overflow_ref, rx_data + 7, sizeof(rm_referee::PowerManagementProcessStackOverflowData)); + if (!copy_payload(&stack_overflow_ref, sizeof(rm_referee::PowerManagementProcessStackOverflowData), + "POWER_MANAGEMENT_PROCESS_STACK_OVERFLOW_CMD")) + break; stack_overflow_pub_data.process_name = stack_overflow_ref.process_name; stack_overflow_pub_data.stamp = last_get_data_time_; power_management_process_stack_overflow_pub_.publish(stack_overflow_pub_data); + break; } case rm_referee::POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD: { rm_referee::PowerManagementUnknownExceptionData unknown_exception_ref; rm_msgs::PowerManagementUnknownExceptionData unknown_exception_pub_data; - memcpy(&unknown_exception_ref, rx_data + 7, sizeof(rm_referee::PowerManagementUnknownExceptionData)); + if (!copy_payload(&unknown_exception_ref, sizeof(rm_referee::PowerManagementUnknownExceptionData), + "POWER_MANAGEMENT_UNKNOWN_EXCEPTION_CMD")) + break; unknown_exception_pub_data.abnormal_reset_reason = unknown_exception_ref.abnormal_reset_reason; unknown_exception_pub_data.power_management_before_reset_topology = @@ -640,13 +781,58 @@ int Referee::unpack(uint8_t* rx_data) break; } base_.referee_data_is_online_ = true; - last_get_data_time_ = ros::Time::now(); return frame_len; } } return -1; } +void Referee::recordPerfStats(int rx_bytes, int unpack_calls, int frames_ok, double read_ms, double unpack_ms_sum, + double unpack_max_ms) +{ + ++perf_read_cycles_; + if (rx_bytes > 0) + perf_rx_bytes_ += static_cast(rx_bytes); + if (unpack_calls > 0) + perf_unpack_calls_ += static_cast(unpack_calls); + if (frames_ok > 0) + perf_frames_ok_ += static_cast(frames_ok); + perf_read_ms_sum_ += read_ms; + perf_unpack_ms_sum_ += unpack_ms_sum; + if (read_ms > perf_read_ms_max_) + perf_read_ms_max_ = read_ms; + if (unpack_max_ms > perf_unpack_ms_max_) + perf_unpack_ms_max_ = unpack_max_ms; + + const ros::WallTime now = ros::WallTime::now(); + const double window_sec = (now - perf_window_start_).toSec(); + if (window_sec < perf_log_period_) + return; + + const uint64_t failed_unpack = perf_unpack_calls_ >= perf_frames_ok_ ? (perf_unpack_calls_ - perf_frames_ok_) : 0; + const double read_avg_ms = perf_read_cycles_ > 0 ? (perf_read_ms_sum_ / static_cast(perf_read_cycles_)) : 0.0; + const double unpack_avg_ms = + perf_unpack_calls_ > 0 ? (perf_unpack_ms_sum_ / static_cast(perf_unpack_calls_)) : 0.0; + const double fps = window_sec > 0.0 ? (static_cast(perf_frames_ok_) / window_sec) : 0.0; + const double kbps = window_sec > 0.0 ? (static_cast(perf_rx_bytes_) / 1024.0 / window_sec) : 0.0; + ROS_INFO("Referee perf %.2fs: read_cycles=%llu rx=%.2fKB/s unpack_calls=%llu ok=%llu fail=%llu fps=%.1f " + "read_avg=%.3fms read_max=%.3fms unpack_avg=%.3fms unpack_max=%.3fms", + window_sec, static_cast(perf_read_cycles_), kbps, + static_cast(perf_unpack_calls_), static_cast(perf_frames_ok_), + static_cast(failed_unpack), fps, read_avg_ms, perf_read_ms_max_, unpack_avg_ms, + perf_unpack_ms_max_); + + perf_window_start_ = now; + perf_read_cycles_ = 0; + perf_rx_bytes_ = 0; + perf_unpack_calls_ = 0; + perf_frames_ok_ = 0; + perf_read_ms_sum_ = 0.0; + perf_unpack_ms_sum_ = 0.0; + perf_read_ms_max_ = 0.0; + perf_unpack_ms_max_ = 0.0; +} + void Referee::getRobotInfo() { base_.robot_color_ = base_.robot_id_ >= 100 ? "blue" : "red"; diff --git a/rm_referee/src/referee_base.cpp b/rm_referee/src/referee_base.cpp index 872241cd..891a639c 100644 --- a/rm_referee/src/referee_base.cpp +++ b/rm_referee/src/referee_base.cpp @@ -39,7 +39,7 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) RefereeBase::sentry_cmd_sub_ = nh.subscribe("/sentry_cmd", 1, &RefereeBase::sendSentryCmdCallback, this); RefereeBase::radar_cmd_sub_ = - nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); + nh.subscribe("/radar_cmd", 1, &RefereeBase::sendRadarCmdCallback, this); RefereeBase::sentry_state_sub_ = nh.subscribe("/custom_info", 1, &RefereeBase::sendCustomInfoCallback, this); RefereeBase::drone_pose_sub_ = @@ -115,9 +115,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) new LaneLineTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "pitch") pitch_angle_time_change_ui_ = new PitchAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "image_transmission") - image_transmission_angle_time_change_ui_ = - new ImageTransmissionAngleTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); if (rpc_value[i]["name"] == "balance_pitch") balance_pitch_time_change_group_ui_ = new BalancePitchTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); @@ -141,8 +138,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "friend_bullets") friend_bullets_time_change_group_ui_ = new FriendBulletsTimeChangeGroupUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "target_hp") - target_hp_time_change_ui_ = new TargetHpTimeChangeUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } ui_nh.getParam("fixed", rpc_value); @@ -155,8 +150,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) 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") - deploy_flash_ui_ = new DeployFlashUi(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") @@ -165,8 +158,6 @@ RefereeBase::RefereeBase(ros::NodeHandle& nh, Base& base) : base_(base), nh_(nh) if (rpc_value[i]["name"] == "customize_display") customize_display_flash_ui_ = new CustomizeDisplayFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); - if (rpc_value[i]["name"] == "burst") - burst_flash_ui_ = new BurstFlashUi(rpc_value[i], base_, &graph_queue_, &character_queue_); } } if (nh.hasParam("interactive_data")) @@ -237,8 +228,6 @@ void RefereeBase::addUi() balance_pitch_time_change_group_ui_->addForQueue(); if (pitch_angle_time_change_ui_) pitch_angle_time_change_ui_->addForQueue(); - if (image_transmission_angle_time_change_ui_) - image_transmission_angle_time_change_ui_->addForQueue(); if (engineer_joint1_time_change_ui) engineer_joint1_time_change_ui->addForQueue(); if (engineer_joint2_time_change_ui) @@ -264,8 +253,6 @@ void RefereeBase::addUi() target_distance_time_change_ui_->addForQueue(); if (friend_bullets_time_change_group_ui_) friend_bullets_time_change_group_ui_->addForQueue(); - if (target_hp_time_change_ui_) - target_hp_time_change_ui_->addForQueue(); if (visualize_state_trigger_change_ui_) visualize_state_trigger_change_ui_->addForQueue(); add_ui_times_++; @@ -363,12 +350,10 @@ void RefereeBase::robotStatusDataCallBack(const rm_msgs::GameRobotStatus& data, fixed_ui_->updateForQueue(); } -void RefereeBase::updateGameRobotHpDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data) +void RefereeBase::updateHeroHitDataCallBack(const rm_msgs::GameRobotHp& game_robot_hp_data) { if (hero_hit_flash_ui_) hero_hit_flash_ui_->updateHittingConfig(game_robot_hp_data); - if (target_hp_time_change_ui_) - target_hp_time_change_ui_->setEnemyHp(game_robot_hp_data); } void RefereeBase::gameStatusDataCallBack(const rm_msgs::GameStatus& data, const ros::Time& last_get_data_time) { @@ -411,8 +396,6 @@ void RefereeBase::jointStateCallback(const sensor_msgs::JointState::ConstPtr& da lane_line_time_change_ui_->updateJointStateData(data, ros::Time::now()); if (pitch_angle_time_change_ui_ && !is_adding_) pitch_angle_time_change_ui_->updateJointStateData(data, ros::Time::now()); - if (image_transmission_angle_time_change_ui_ && !is_adding_) - image_transmission_angle_time_change_ui_->updateJointStateData(data, ros::Time::now()); if (engineer_joint1_time_change_ui && !is_adding_) engineer_joint1_time_change_ui->updateJointStateData(data, ros::Time::now()); if (engineer_joint2_time_change_ui && !is_adding_) @@ -449,15 +432,11 @@ void RefereeBase::chassisCmdDataCallback(const rm_msgs::ChassisCmd::ConstPtr& da chassis_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_) - deploy_flash_ui_->updateChassisCmdData(data, ros::Time::now()); if (rotation_time_change_ui_ && !is_adding_) rotation_time_change_ui_->updateChassisCmdData(data); } void RefereeBase::vel2DCmdDataCallback(const geometry_msgs::Twist::ConstPtr& data) { - if (deploy_flash_ui_ && !is_adding_) - deploy_flash_ui_->updateChassisVelData(data); } void RefereeBase::shootStateCallback(const rm_msgs::ShootState::ConstPtr& data) { @@ -497,8 +476,6 @@ void RefereeBase::manualDataCallBack(const rm_msgs::ManualToReferee::ConstPtr& d 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); } void RefereeBase::radarDataCallBack(const std_msgs::Int8MultiArrayConstPtr& data) { @@ -514,8 +491,6 @@ void RefereeBase::trackCallBack(const rm_msgs::TrackDataConstPtr& data) target_view_angle_trigger_change_ui_->updateTrackID(data->id); if (target_distance_time_change_ui_ && !is_adding_) target_distance_time_change_ui_->updateTargetDistanceData(data); - if (target_hp_time_change_ui_ && !is_adding_) - target_hp_time_change_ui_->updateTrackID(data->id); } void RefereeBase::balanceStateCallback(const rm_msgs::BalanceStateConstPtr& data) { @@ -566,7 +541,7 @@ void RefereeBase::sendSentryCmdCallback(const rm_msgs::SentryCmdConstPtr& data) sentry_cmd_data_last_send_ = ros::Time::now(); } } -void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarInfoConstPtr& data) +void RefereeBase::sendRadarCmdCallback(const rm_msgs::RadarCmdConstPtr& data) { if (ros::Time::now() - radar_cmd_data_last_send_ <= ros::Duration(0.15)) return; diff --git a/rm_referee/src/referee_benchmark.cpp b/rm_referee/src/referee_benchmark.cpp new file mode 100644 index 00000000..675a3b44 --- /dev/null +++ b/rm_referee/src/referee_benchmark.cpp @@ -0,0 +1,206 @@ +/******************************************************************************* + * Benchmark program for referee.cpp performance comparison + * + * Measures: + * 1. Frame processing time per message + * 2. Memory copy operations count + * 3. System call count (ros::Time::now()) + * + * Compile with: g++ -O3 -std=c++11 referee_benchmark.cpp -o referee_benchmark + ******************************************************************************/ +#include +#include +#include +#include +#include + +// Mock structures for benchmarking +struct FrameHeader +{ + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc8; +} __attribute__((packed)); + +struct GameStatus +{ + uint8_t game_type; + uint8_t game_progress; + uint16_t stage_remain_time; + uint32_t sync_time_stamp; +} __attribute__((packed)); + +constexpr int k_header_length_ = 5; +constexpr int k_cmd_id_length_ = 2; +constexpr int k_tail_length_ = 2; + +// Original version (with lambda overhead and multiple Time calls) +void original_version_process(const uint8_t* rx_data, uint8_t data_length) +{ + uint16_t cmd_id = (rx_data[6] << 8 | rx_data[5]); + (void)cmd_id; + + auto ensure_payload = [&](size_t expected_len) { return data_length >= expected_len; }; + + auto copy_payload = [&](void* dst, size_t len) { + if (!ensure_payload(len)) + return false; + memcpy(dst, rx_data + 7, len); + return true; + }; + + GameStatus game_status_ref; + if (copy_payload(&game_status_ref, sizeof(game_status_ref))) + { + volatile uint8_t game_type = game_status_ref.game_type; + volatile uint8_t game_progress = game_status_ref.game_progress; + (void)game_type; + (void)game_progress; + } +} + +// Optimized version (inline processing) +void optimized_version_process(const uint8_t* rx_data, uint8_t data_length) +{ + uint16_t cmd_id = (rx_data[6] << 8 | rx_data[5]); + (void)cmd_id; + + if (data_length >= sizeof(GameStatus)) + { + GameStatus game_status_ref; + memcpy(&game_status_ref, rx_data + 7, sizeof(game_status_ref)); + + volatile uint8_t game_type = game_status_ref.game_type; + volatile uint8_t game_progress = game_status_ref.game_progress; + (void)game_type; + (void)game_progress; + } +} + +// Buffer shift comparison +void original_buffer_shift(uint8_t* unpack_buffer, const uint8_t* rx_buffer, int rx_len, int buffer_len) +{ + uint8_t temp_buffer[256] = { 0 }; + if (rx_len < buffer_len) + { + for (int k_i = 0; k_i < buffer_len - rx_len; ++k_i) + temp_buffer[k_i] = unpack_buffer[k_i + rx_len]; + for (int k_i = 0; k_i < rx_len; ++k_i) + temp_buffer[k_i + buffer_len - rx_len] = rx_buffer[k_i]; + for (int k_i = 0; k_i < buffer_len; ++k_i) + unpack_buffer[k_i] = temp_buffer[k_i]; + } +} + +void optimized_buffer_shift(uint8_t* unpack_buffer, const uint8_t* rx_buffer, int rx_len, int buffer_len) +{ + if (rx_len < buffer_len) + { + const int shift = buffer_len - rx_len; + memmove(unpack_buffer, unpack_buffer + rx_len, shift); + memcpy(unpack_buffer + shift, rx_buffer, rx_len); + } + else + { + memcpy(unpack_buffer, rx_buffer, buffer_len); + } +} + +template +double benchmark_func(Func func, const char* name, int iterations = 100000) +{ + auto start = std::chrono::high_resolution_clock::now(); + + for (int i = 0; i < iterations; ++i) + { + func(); + } + + auto end = std::chrono::high_resolution_clock::now(); + auto duration = std::chrono::duration_cast(end - start).count(); + double avg_ns = static_cast(duration) / iterations; + double avg_us = avg_ns / 1000.0; + + std::cout << std::setw(40) << std::left << name << ": " << std::fixed << std::setprecision(3) << avg_us + << " us/call (" << std::setw(10) << (iterations / (duration / 1000000.0)) << " M calls/sec)" << std::endl; + + return avg_us; +} + +int main() +{ + std::cout << "=== Referee Performance Benchmark ===" << std::endl; + std::cout << std::endl; + + // Test data + uint8_t test_data[256]; + for (int i = 0; i < 256; ++i) + test_data[i] = static_cast(i); + test_data[0] = 0xA5; // Frame header + test_data[4] = 20; // data_length + test_data[5] = 0x01; // cmd_id low + test_data[6] = 0x00; // cmd_id high (GAME_STATUS_CMD) + + // Benchmark buffer shift operations + std::cout << "--- Buffer Operations ---" << std::endl; + double orig_shift = benchmark_func( + [&]() { + uint8_t unpack_buffer[256] = { 0 }; + original_buffer_shift(unpack_buffer, test_data, 50, 256); + }, + "Original buffer shift (50 bytes)", 50000); + + double opt_shift = benchmark_func( + [&]() { + uint8_t unpack_buffer[256] = { 0 }; + optimized_buffer_shift(unpack_buffer, test_data, 50, 256); + }, + "Optimized buffer shift (50 bytes)", 50000); + + double orig_full = benchmark_func( + [&]() { + uint8_t unpack_buffer[256] = { 0 }; + original_buffer_shift(unpack_buffer, test_data, 256, 256); + }, + "Original buffer shift (256 bytes)", 50000); + + double opt_full = benchmark_func( + [&]() { + uint8_t unpack_buffer[256] = { 0 }; + optimized_buffer_shift(unpack_buffer, test_data, 256, 256); + }, + "Optimized buffer shift (256 bytes)", 50000); + + std::cout << std::endl; + + // Benchmark message processing + std::cout << "--- Message Processing ---" << std::endl; + double orig_msg = benchmark_func([&]() { original_version_process(test_data, 20); }, + "Original message processing (20 bytes)", 50000); + + double opt_msg = benchmark_func([&]() { optimized_version_process(test_data, 20); }, + "Optimized message processing (20 bytes)", 50000); + + std::cout << std::endl; + + // Summary + std::cout << "=== Performance Improvement Summary ===" << std::endl; + std::cout << "Buffer Shift (50 bytes): " << std::fixed << std::setprecision(2) + << ((orig_shift / opt_shift) * 100.0 - 100.0) << "% faster" << std::endl; + std::cout << "Buffer Shift (256 bytes): " << std::fixed << std::setprecision(2) + << ((orig_full / opt_full) * 100.0 - 100.0) << "% faster" << std::endl; + std::cout << "Message Processing: " << std::fixed << std::setprecision(2) + << ((orig_msg / opt_msg) * 100.0 - 100.0) << "% faster" << std::endl; + + // Estimated overall improvement (weighted by typical usage) + // Assume: 80% small packets, 20% full buffer + double weighted_orig = orig_shift * 0.4 + orig_full * 0.1 + orig_msg * 0.5; + double weighted_opt = opt_shift * 0.4 + opt_full * 0.1 + opt_msg * 0.5; + double overall_improvement = ((weighted_orig / weighted_opt) * 100.0 - 100.0); + + std::cout << "Overall (weighted): " << std::fixed << std::setprecision(2) << overall_improvement << "% faster" + << std::endl; + + return 0; +} diff --git a/rm_referee/src/ui/flash_ui.cpp b/rm_referee/src/ui/flash_ui.cpp index c443e0c6..501643eb 100644 --- a/rm_referee/src/ui/flash_ui.cpp +++ b/rm_referee/src/ui/flash_ui.cpp @@ -166,36 +166,12 @@ void SpinFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr data, display(last_get_data_time); } -void DeployFlashUi::display(const ros::Time& time) -{ - if (!(chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0)) - graph_->setOperation(rm_referee::GraphOperation::DELETE); - FlashUi::updateFlashUiForQueue(time, (chassis_mode_ == rm_msgs::ChassisCmd::RAW && angular_z_ == 0.0), false); -} - -void DeployFlashUi::updateChassisCmdData(const rm_msgs::ChassisCmd::ConstPtr& data, const ros::Time& last_get_data_time) -{ - chassis_mode_ = data->mode; - display(last_get_data_time); -} - -void DeployFlashUi::updateChassisVelData(const geometry_msgs::Twist::ConstPtr& data) -{ - angular_z_ = data->angular.z; -} - void HeroHitFlashUi::updateHittingConfig(const rm_msgs::GameRobotHp& msg) { - if (base_.robot_id_ > 100) - { - hitted_ = - (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 > 190 || - last_hp_msg_.blue_base_hp - msg.blue_base_hp > 190); - } + // Temporarily disable hit flash trigger after GameRobotHp switched to ally semantics. + // hitted_ = (last_hp_msg_.ally_outpost_hp - msg.ally_outpost_hp > 190 || + // last_hp_msg_.ally_base_hp - msg.ally_base_hp > 190); + hitted_ = false; last_hp_msg_ = msg; display(ros::Time::now()); } @@ -220,22 +196,5 @@ void ExceedBulletSpeedFlashUi::updateShootData(const rm_msgs::ShootData& msg) shoot_data_ = msg; } -void BurstFlashUi::display(const ros::Time& time) -{ - ros::Time now = ros::Time::now(); - if (now - start_burst_time_ < ros::Duration(20)) - { - graph_->setColor(rm_referee::GraphColor::PURPLE); - FlashUi::updateFlashUiForQueue(time, true, false); - } - else - FlashUi::updateFlashUiForQueue(time, false, false); -} - -void BurstFlashUi::updateBurstTimeData(const rm_msgs::ManualToReferee::ConstPtr& data) -{ - start_burst_time_ = data->start_burst_time; - display(ros::Time::now()); -} } // namespace rm_referee // namespace rm_referee diff --git a/rm_referee/src/ui/interactive_data.cpp b/rm_referee/src/ui/interactive_data.cpp index 81f399ac..7ac80fa2 100644 --- a/rm_referee/src/ui/interactive_data.cpp +++ b/rm_referee/src/ui/interactive_data.cpp @@ -4,6 +4,8 @@ #include "rm_referee/ui/interactive_data.h" +#include + namespace rm_referee { void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, unsigned char data) @@ -11,7 +13,7 @@ void InteractiveSender::sendInteractiveData(int data_cmd_id, int receiver_id, un uint8_t tx_data[sizeof(InteractiveData)] = { 0 }; auto student_interactive_data = (InteractiveData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; student_interactive_data->header_data.data_cmd_id = data_cmd_id; student_interactive_data->header_data.sender_id = base_.robot_id_; @@ -33,7 +35,7 @@ void InteractiveSender::sendMapSentryData(const rm_referee::MapSentryData& data) uint8_t tx_data[sizeof(rm_referee::MapSentryData)] = { 0 }; auto map_sentry_data = (rm_referee::MapSentryData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; map_sentry_data->intention = data.intention; map_sentry_data->start_position_x = data.start_position_x; @@ -85,7 +87,7 @@ void CustomInfoSender::sendCustomInfoData(std::wstring data) tx_data.user_data[2 * i] = characters[i] & 0xFF; tx_data.user_data[2 * i + 1] = (characters[i] >> 8) & 0xFF; } - pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::CUSTOM_TO_ROBOT_CMD, data_len); + pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::CUSTOM_INFO_CMD, data_len); last_send_ = ros::Time::now(); sendSerial(ros::Time::now(), data_len); } @@ -95,7 +97,7 @@ void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceive uint8_t tx_data[sizeof(rm_referee::ClientMapReceiveData)] = { 0 }; auto radar_interactive_data = (rm_referee::ClientMapReceiveData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; radar_interactive_data->hero_position_x = data->hero_position_x; radar_interactive_data->hero_position_y = data->hero_position_y; @@ -105,8 +107,8 @@ void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceive radar_interactive_data->infantry_3_position_y = data->infantry_3_position_y; radar_interactive_data->infantry_4_position_x = data->infantry_4_position_x; radar_interactive_data->infantry_4_position_y = data->infantry_4_position_y; - radar_interactive_data->infantry_5_position_x = data->infantry_5_position_x; - radar_interactive_data->infantry_5_position_y = data->infantry_5_position_y; + radar_interactive_data->reserved_1 = data->reserved_1; + radar_interactive_data->reserved_2 = data->reserved_2; radar_interactive_data->sentry_position_x = data->sentry_position_x; radar_interactive_data->sentry_position_y = data->sentry_position_y; pack(tx_buffer_, tx_data, rm_referee::RefereeCmdId::CLIENT_MAP_CMD, sizeof(rm_referee::ClientMapReceiveData)); @@ -126,26 +128,49 @@ void InteractiveSender::sendRadarInteractiveData(const rm_msgs::ClientMapReceive void InteractiveSender::sendSentryCmdData(const rm_msgs::SentryCmdConstPtr& data) { int data_len; - rm_referee::SentryCmd tx_data; - data_len = static_cast(sizeof(rm_referee::SentryCmd)); + rm_referee::SentryCmdInteractiveData tx_data; + data_len = static_cast(sizeof(rm_referee::SentryCmdInteractiveData)); tx_data.header.sender_id = base_.robot_id_; tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.sentry_info = data->sentry_info; + rm_referee::SentryCmd sentry_cmd{}; + sentry_cmd.confirm_respawn = data->confirm_respawn; + sentry_cmd.confirm_instant_respawn = data->confirm_instant_respawn; + sentry_cmd.bullet_exchange_target = data->bullet_exchange_target; + sentry_cmd.remote_bullet_exchange_req_cnt = data->remote_bullet_exchange_req_cnt; + sentry_cmd.remote_hp_exchange_req_cnt = data->remote_hp_exchange_req_cnt; + sentry_cmd.posture_cmd = data->posture_cmd; + sentry_cmd.confirm_rune_activating = data->confirm_rune_activating; + + const bool has_protocol_split = data->confirm_respawn || data->confirm_instant_respawn || + data->bullet_exchange_target != 0 || data->remote_bullet_exchange_req_cnt != 0 || + data->remote_hp_exchange_req_cnt != 0 || data->posture_cmd != 0 || + data->confirm_rune_activating; + if (has_protocol_split) + tx_data.sentry_cmd = sentry_cmd; + else + tx_data.sentry_cmd.sentry_cmd = data->sentry_cmd; tx_data.header.data_cmd_id = rm_referee::DataCmdId::SENTRY_CMD; pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); sendSerial(ros::Time::now(), data_len); } -void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarInfoConstPtr& data) +void InteractiveSender::sendRadarCmdData(const rm_msgs::RadarCmdConstPtr& data) { int data_len; - rm_referee::RadarInfo tx_data; - data_len = static_cast(sizeof(rm_referee::RadarInfo)); + rm_referee::RadarCmdInteractiveData tx_data; + data_len = static_cast(sizeof(rm_referee::RadarCmdInteractiveData)); tx_data.header.sender_id = base_.robot_id_; tx_data.header.receiver_id = REFEREE_SERVER; - tx_data.radar_info = data->radar_info; + tx_data.radar_cmd.radar_cmd = data->radar_cmd; + tx_data.radar_cmd.password_cmd = data->password_cmd; + tx_data.radar_cmd.password_1 = data->password_1; + tx_data.radar_cmd.password_2 = data->password_2; + tx_data.radar_cmd.password_3 = data->password_3; + tx_data.radar_cmd.password_4 = data->password_4; + tx_data.radar_cmd.password_5 = data->password_5; + tx_data.radar_cmd.password_6 = data->password_6; tx_data.header.data_cmd_id = rm_referee::DataCmdId::RADAR_CMD; pack(tx_buffer_, reinterpret_cast(&tx_data), rm_referee::RefereeCmdId::INTERACTIVE_DATA_CMD, data_len); @@ -166,7 +191,7 @@ void BulletNumShare::sendBulletData() uint8_t tx_data[sizeof(BulletNumData)] = { 0 }; auto bullet_num_data = (BulletNumData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; bullet_num_data->header_data.data_cmd_id = rm_referee::DataCmdId::BULLET_NUM_SHARE_CMD; bullet_num_data->header_data.sender_id = base_.robot_id_; @@ -200,7 +225,7 @@ void SentryToRadar::sendSentryToRadarData() uint8_t tx_data[sizeof(SentryAttackingTargetData)] = { 0 }; auto sentry_attacking_target_data = (SentryAttackingTargetData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; sentry_attacking_target_data->header_data.data_cmd_id = rm_referee::DataCmdId::SENTRY_TO_RADAR_CMD; sentry_attacking_target_data->header_data.sender_id = base_.robot_id_; @@ -236,7 +261,7 @@ void RadarToSentry::sendRadarToSentryData() uint8_t tx_data[sizeof(RadarToSentryData)] = { 0 }; auto radar_to_sentry_data = (RadarToSentryData*)tx_data; - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; radar_to_sentry_data->header_data.data_cmd_id = rm_referee::DataCmdId::RADAR_TO_SENTRY_CMD; radar_to_sentry_data->header_data.sender_id = base_.robot_id_; diff --git a/rm_referee/src/ui/time_change_ui.cpp b/rm_referee/src/ui/time_change_ui.cpp index 48ee5647..9df99502 100644 --- a/rm_referee/src/ui/time_change_ui.cpp +++ b/rm_referee/src/ui/time_change_ui.cpp @@ -4,8 +4,6 @@ #include "rm_referee/ui/time_change_ui.h" -#include - namespace rm_referee { void TimeChangeUi::update() @@ -293,22 +291,6 @@ void PitchAngleTimeChangeUi::updateConfig() graph_->setColor(rm_referee::GraphColor::YELLOW); } -void ImageTransmissionAngleTimeChangeUi::updateJointStateData(const sensor_msgs::JointState::ConstPtr data, - const ros::Time& time) -{ - for (unsigned int i = 0; i < data->name.size(); i++) - if (data->name[i] == "image_transmission_joint") - image_transmission_angle_ = data->position[i]; - updateForQueue(); -} - -void ImageTransmissionAngleTimeChangeUi::updateConfig() -{ - std::string image_transmission = std::to_string(image_transmission_angle_); - graph_->setContent(image_transmission); - graph_->setColor(rm_referee::GraphColor::PINK); -} - void JointPositionTimeChangeUi::updateConfig() { double proportion = (current_val_ - min_val_) / (max_val_ - min_val_); @@ -501,47 +483,4 @@ void FriendBulletsTimeChangeGroupUi::updateConfig() } } -void TargetHpTimeChangeUi::setEnemyHp(const rm_msgs::GameRobotHp& data) -{ - bool is_enemy_red = base_.robot_id_ > 100; - if (is_enemy_red) - { - enemy_robot_hp_[1] = data.red_1_robot_hp; - enemy_robot_hp_[2] = data.red_2_robot_hp; - enemy_robot_hp_[3] = data.red_3_robot_hp; - enemy_robot_hp_[4] = data.red_4_robot_hp; - enemy_robot_hp_[7] = data.red_7_robot_hp; - } - else - { - enemy_robot_hp_[1] = data.blue_1_robot_hp; - enemy_robot_hp_[2] = data.blue_2_robot_hp; - enemy_robot_hp_[3] = data.blue_3_robot_hp; - enemy_robot_hp_[4] = data.blue_4_robot_hp; - enemy_robot_hp_[7] = data.blue_7_robot_hp; - } -} - -void TargetHpTimeChangeUi::updateTrackID(int id) -{ - target_id_ = id; - updateTargeHptData(); -} - -void TargetHpTimeChangeUi::updateTargeHptData() -{ - auto it = enemy_robot_hp_.find(target_id_); - target_hp_ = it == enemy_robot_hp_.end() ? 0 : it->second; - updateForQueue(); -} - -void TargetHpTimeChangeUi::updateConfig() -{ - graph_->setIntNum(target_hp_); - if (target_hp_ > 50) - graph_->setColor(rm_referee::GraphColor::GREEN); - else - graph_->setColor(rm_referee::GraphColor::ORANGE); -} - } // namespace rm_referee diff --git a/rm_referee/src/ui/ui_base.cpp b/rm_referee/src/ui/ui_base.cpp index 920cc6c1..7db97966 100644 --- a/rm_referee/src/ui/ui_base.cpp +++ b/rm_referee/src/ui/ui_base.cpp @@ -338,6 +338,11 @@ void FixedUi::updateForQueue() void UiBase::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len) const { + if (len < 0 || (k_header_length_ + k_cmd_id_length_ + k_tail_length_ + len) > k_frame_length_) + { + ROS_ERROR("Referee pack overflow: cmd=0x%04X len=%d exceeds frame buffer %d", cmd_id, len, k_frame_length_); + return; + } memset(tx_buffer, 0, k_frame_length_); auto* frame_header = reinterpret_cast(tx_buffer); @@ -365,7 +370,7 @@ void UiBase::sendSerial(const ros::Time& time, int data_len) void UiBase::clearTxBuffer() { - for (int i = 0; i < 127; i++) + for (int i = 0; i < k_frame_length_; i++) tx_buffer_[i] = 0; tx_len_ = 0; } diff --git a/rm_vt/CMakeLists.txt b/rm_vt/CMakeLists.txt old mode 100644 new mode 100755 index b7d2b5b0..1e142b71 --- a/rm_vt/CMakeLists.txt +++ b/rm_vt/CMakeLists.txt @@ -103,15 +103,14 @@ install( DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) -############# -## Testing ## -############# - -#if (${CATKIN_ENABLE_TESTING}) -# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") -# ## Add gtest based cpp test target and link libraries -# catkin_add_gtest(${PROJECT_NAME}-test -# test/test_ros_package_template.cpp -# test/AlgorithmTest.cpp) -# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) -#endif () +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(video_transmission_test + test/video_transmission_test.cpp + src/video_transmission.cpp + ) + if(TARGET video_transmission_test) + target_link_libraries(video_transmission_test + ${catkin_LIBRARIES} + ) + endif() +endif() diff --git a/rm_vt/include/rm_vt/common/data.h b/rm_vt/include/rm_vt/common/data.h old mode 100644 new mode 100755 index 7262e426..b45fdd91 --- a/rm_vt/include/rm_vt/common/data.h +++ b/rm_vt/include/rm_vt/common/data.h @@ -11,6 +11,7 @@ #include #include #include +#include #include #include #include diff --git a/rm_vt/include/rm_vt/common/protocol.h b/rm_vt/include/rm_vt/common/protocol.h old mode 100644 new mode 100755 index 4a72309f..34d40eb2 --- a/rm_vt/include/rm_vt/common/protocol.h +++ b/rm_vt/include/rm_vt/common/protocol.h @@ -11,7 +11,10 @@ typedef enum { CUSTOM_CONTROLLER_CMD = 0x0302, // custom_controller ROBOT_COMMAND_CMD = 0x0304, // keyboard_data - ROBOT_TO_CUSTOM_CMD = 0x0309 + KEYBOARD_MOUSE_CMD = 0x0306, + ROBOT_TO_CUSTOM_CMD = 0x0309, + ROBOT_TO_CUSTOM_CMD_2 = 0x0310, + CUSTOM_TO_ROBOT_CMD = 0x0311 } VideoTransmissionCmdId; typedef struct @@ -55,6 +58,11 @@ typedef struct uint8_t data[30]; } __packed RobotToCustomData; +typedef struct +{ + uint8_t data[300]; +} __packed RobotToCustomData2; + typedef struct { int16_t mouse_x; @@ -81,6 +89,16 @@ typedef struct uint16_t reserved; } __packed KeyboardMouseData; +typedef struct +{ + uint16_t key_value; + uint16_t x_position : 12; + uint16_t mouse_left : 4; + uint16_t y_position : 12; + uint16_t mouse_right : 4; + uint16_t reserved; +} __packed KeyboardMouseData2026; + typedef struct { uint16_t joystick_r_x : 11; @@ -94,7 +112,6 @@ typedef struct uint16_t wheel : 11; uint8_t trigger : 1; uint8_t unused_1 : 3; - // mouse int16_t mouse_x; int16_t mouse_y; int16_t mouse_wheel; @@ -102,7 +119,6 @@ typedef struct uint8_t mouse_right_down : 2; uint8_t mouse_mid_down : 2; uint8_t unused_2 : 2; - // keyboard uint16_t key_w : 1; uint16_t key_s : 1; uint16_t key_a : 1; @@ -120,6 +136,13 @@ typedef struct uint16_t key_v : 1; uint16_t key_b : 1; } __packed ControlData; + +static_assert(sizeof(CustomControllerData) == 30, "CustomControllerData size must match protocol (30 bytes)."); +static_assert(sizeof(RobotToCustomData) == 30, "RobotToCustomData size must match protocol (30 bytes)."); +static_assert(sizeof(RobotToCustomData2) == 300, "RobotToCustomData2 size must match protocol (300 bytes)."); +static_assert(sizeof(KeyboardMouseData) == 12, "KeyboardMouseData size must match protocol (12 bytes)."); +static_assert(sizeof(KeyboardMouseData2026) == 8, "KeyboardMouseData2026 size must match protocol (8 bytes)."); +static_assert(sizeof(ControlData) == 17, "ControlData size must match expected frame payload (17 bytes)."); /***********************Frame tail(CRC8_CRC16)********************************************/ const uint8_t kCrc8Init = 0xff; const uint8_t kCrc8Table[256] = { diff --git a/rm_vt/include/rm_vt/video_transmission.h b/rm_vt/include/rm_vt/video_transmission.h old mode 100644 new mode 100755 index 394eaf87..b75d8b0a --- a/rm_vt/include/rm_vt/video_transmission.h +++ b/rm_vt/include/rm_vt/video_transmission.h @@ -13,12 +13,19 @@ namespace rm_vt class VideoTransmission { public: + static uint16_t keyboardMaskFromLegacyFrame(const rm_vt::KeyboardMouseData& data); + static uint16_t keyCodeToMask(uint8_t key_code); + static uint16_t keyboardMaskFromKeyCodes(uint16_t key_value); + explicit VideoTransmission(ros::NodeHandle& nh) : last_get_data_time_(ros::Time::now()) { ROS_INFO("Video transmission load."); custom_controller_cmd_pub_ = nh.advertise("custom_controller_data", 1); vt_keyboard_mouse_pub_ = nh.advertise("keyboard_mouse_data", 1); vt_receiver_control_pub_ = nh.advertise("receiver_control_data", 1); + robot_to_custom_pub_ = nh.advertise("robot_to_custom_data", 1); + robot_to_custom2_pub_ = nh.advertise("robot_to_custom_data2", 1); + custom_to_robot_pub_ = nh.advertise("custom_to_robot_data", 1); base_.initSerial(); } void read(); @@ -29,17 +36,25 @@ class VideoTransmission } ros::Publisher custom_controller_cmd_pub_, vt_keyboard_mouse_pub_, vt_receiver_control_pub_; + ros::Publisher robot_to_custom_pub_, robot_to_custom2_pub_, custom_to_robot_pub_; Base base_; std::vector rx_buffer_; int rx_len_; private: - int unpack(uint8_t* rx_data); - int control_data_unpack(uint8_t* rx_data); + int unpack(uint8_t* rx_data, int rx_data_len); + int control_data_unpack(uint8_t* rx_data, int rx_data_len); + void publishKeyboardMouseData(uint16_t keyboard_value, int16_t mouse_x, int16_t mouse_y, int16_t mouse_z, + bool left_button_down, bool right_button_down); + uint16_t updateKeyboardValueStateFromKeyCodes(uint16_t key_value); ros::Time last_get_data_time_; - const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; - const int k_unpack_buffer_length_ = 256; - uint8_t unpack_buffer_[256]{}; + static constexpr int k_frame_length_ = 128; + static constexpr int k_header_length_ = 5; + static constexpr int k_cmd_id_length_ = 2; + static constexpr int k_tail_length_ = 2; + static constexpr int k_unpack_buffer_length_ = 512; + uint8_t unpack_buffer_[k_unpack_buffer_length_]{}; + uint16_t keyboard_value_state_{ 0 }; }; } // namespace rm_vt diff --git a/rm_vt/launch/load.launch b/rm_vt/launch/load.launch old mode 100644 new mode 100755 diff --git a/rm_vt/package.xml b/rm_vt/package.xml old mode 100644 new mode 100755 index 18e3db4d..1e15be76 --- a/rm_vt/package.xml +++ b/rm_vt/package.xml @@ -18,4 +18,5 @@ rm_common tf2_geometry_msgs serial + gtest diff --git a/rm_vt/src/main.cpp b/rm_vt/src/main.cpp old mode 100644 new mode 100755 diff --git a/rm_vt/src/video_transmission.cpp b/rm_vt/src/video_transmission.cpp index b87dee46..c2d5ac1a 100644 --- a/rm_vt/src/video_transmission.cpp +++ b/rm_vt/src/video_transmission.cpp @@ -5,6 +5,125 @@ namespace rm_vt { +uint16_t VideoTransmission::keyboardMaskFromLegacyFrame(const rm_vt::KeyboardMouseData& data) +{ + uint16_t mask = 0; + mask |= static_cast(data.key_w) << 0; + mask |= static_cast(data.key_s) << 1; + mask |= static_cast(data.key_a) << 2; + mask |= static_cast(data.key_d) << 3; + mask |= static_cast(data.key_shift) << 4; + mask |= static_cast(data.key_ctrl) << 5; + mask |= static_cast(data.key_q) << 6; + mask |= static_cast(data.key_e) << 7; + mask |= static_cast(data.key_r) << 8; + mask |= static_cast(data.key_f) << 9; + mask |= static_cast(data.key_g) << 10; + mask |= static_cast(data.key_z) << 11; + mask |= static_cast(data.key_x) << 12; + mask |= static_cast(data.key_c) << 13; + mask |= static_cast(data.key_v) << 14; + mask |= static_cast(data.key_b) << 15; + return mask; +} + +uint16_t VideoTransmission::keyCodeToMask(uint8_t key_code) +{ + switch (key_code) + { + case 'W': + return (1u << 0); + case 'S': + return (1u << 1); + case 'A': + return (1u << 2); + case 'D': + return (1u << 3); + case 0: + return 0; + default: + break; + } + switch (key_code) + { + case 'Q': + return (1u << 6); + case 'E': + return (1u << 7); + case 'R': + return (1u << 8); + case 'F': + return (1u << 9); + case 'G': + return (1u << 10); + case 'Z': + return (1u << 11); + case 'X': + return (1u << 12); + case 'C': + return (1u << 13); + case 'V': + return (1u << 14); + case 'B': + return (1u << 15); + default: + break; + } + + if (key_code == 0x10) + return (1u << 4); + if (key_code == 0x11) + return (1u << 5); + return 0; +} + +uint16_t VideoTransmission::keyboardMaskFromKeyCodes(uint16_t key_value) +{ + const uint8_t key_1 = static_cast(key_value & 0xFFu); + const uint8_t key_2 = static_cast((key_value >> 8) & 0xFFu); + uint16_t new_mask = 0; + new_mask |= keyCodeToMask(key_1); + new_mask |= keyCodeToMask(key_2); + return new_mask; +} + +uint16_t VideoTransmission::updateKeyboardValueStateFromKeyCodes(uint16_t key_value) +{ + if (key_value == 0) + return keyboard_value_state_; + + keyboard_value_state_ = keyboardMaskFromKeyCodes(key_value); + return keyboard_value_state_; +} + +void VideoTransmission::publishKeyboardMouseData(uint16_t keyboard_value, int16_t mouse_x, int16_t mouse_y, + int16_t mouse_z, bool left_button_down, bool right_button_down) +{ + rm_msgs::VTKeyboardMouseData keyboard_mouse_data; + keyboard_mouse_data.mouse_x = mouse_x; + keyboard_mouse_data.mouse_y = mouse_y; + keyboard_mouse_data.mouse_z = mouse_z; + keyboard_mouse_data.left_button_down = left_button_down; + keyboard_mouse_data.right_button_down = right_button_down; + keyboard_mouse_data.key_w = static_cast(keyboard_value & (1u << 0)); + keyboard_mouse_data.key_s = static_cast(keyboard_value & (1u << 1)); + keyboard_mouse_data.key_a = static_cast(keyboard_value & (1u << 2)); + keyboard_mouse_data.key_d = static_cast(keyboard_value & (1u << 3)); + keyboard_mouse_data.key_shift = static_cast(keyboard_value & (1u << 4)); + keyboard_mouse_data.key_ctrl = static_cast(keyboard_value & (1u << 5)); + keyboard_mouse_data.key_q = static_cast(keyboard_value & (1u << 6)); + keyboard_mouse_data.key_e = static_cast(keyboard_value & (1u << 7)); + keyboard_mouse_data.key_r = static_cast(keyboard_value & (1u << 8)); + keyboard_mouse_data.key_f = static_cast(keyboard_value & (1u << 9)); + keyboard_mouse_data.key_g = static_cast(keyboard_value & (1u << 10)); + keyboard_mouse_data.key_z = static_cast(keyboard_value & (1u << 11)); + keyboard_mouse_data.key_x = static_cast(keyboard_value & (1u << 12)); + keyboard_mouse_data.key_c = static_cast(keyboard_value & (1u << 13)); + keyboard_mouse_data.key_v = static_cast(keyboard_value & (1u << 14)); + keyboard_mouse_data.key_b = static_cast(keyboard_value & (1u << 15)); + vt_keyboard_mouse_pub_.publish(keyboard_mouse_data); +} + void VideoTransmission::read() { if (base_.serial_.available()) @@ -14,7 +133,7 @@ void VideoTransmission::read() } else return; - uint8_t temp_buffer[256] = { 0 }; + uint8_t temp_buffer[k_unpack_buffer_length_] = { 0 }; int frame_len; if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) base_.video_transmission_is_online_ = false; @@ -27,49 +146,76 @@ void VideoTransmission::read() for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) unpack_buffer_[k_i] = temp_buffer[k_i]; } + else + { + const int offset = rx_len_ - k_unpack_buffer_length_; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + unpack_buffer_[k_i] = rx_buffer_[offset + k_i]; + } for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) { if (unpack_buffer_[k_i] == 0xA5) { - frame_len = unpack(&unpack_buffer_[k_i]); - if (frame_len != -1) - k_i += frame_len; + frame_len = unpack(&unpack_buffer_[k_i], k_unpack_buffer_length_ - k_i); + if (frame_len > 0) + k_i += frame_len - 1; } if (unpack_buffer_[k_i] == 0xA9 && unpack_buffer_[k_i + 1] == 0x53) { - frame_len = control_data_unpack(&unpack_buffer_[k_i]); - if (frame_len != -1) - k_i += frame_len; + frame_len = control_data_unpack(&unpack_buffer_[k_i], k_unpack_buffer_length_ - k_i); + if (frame_len > 0) + k_i += frame_len - 1; } } clearRxBuffer(); } -int VideoTransmission::unpack(uint8_t* rx_data) +int VideoTransmission::unpack(uint8_t* rx_data, int rx_data_len) { uint16_t cmd_id; int frame_len; rm_vt::FrameHeader frame_header; + if (rx_data_len < k_header_length_) + return -1; + memcpy(&frame_header, rx_data, k_header_length_); if (static_cast(base_.verifyCRC8CheckSum(rx_data, k_header_length_))) { - if (frame_header.data_length > 256) // temporary and inaccurate value + const uint16_t max_payload_len = k_unpack_buffer_length_ - k_header_length_ - k_cmd_id_length_ - k_tail_length_; + if (frame_header.data_length > max_payload_len) { ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length); - return 0; + return -1; } frame_len = frame_header.data_length + k_header_length_ + k_cmd_id_length_ + k_tail_length_; + if (frame_len > rx_data_len || frame_len > k_unpack_buffer_length_) + return -1; if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1) { cmd_id = (rx_data[6] << 8 | rx_data[5]); + auto ensure_payload = [&](size_t expected_len, const char* cmd_name) { + if (frame_header.data_length < expected_len) + { + ROS_WARN("Drop cmd %s due to short payload: %u < %zu", cmd_name, frame_header.data_length, expected_len); + return false; + } + return true; + }; + auto copy_payload = [&](void* dst, size_t len, const char* cmd_name) { + if (!ensure_payload(len, cmd_name)) + return false; + memcpy(dst, rx_data + 7, len); + return true; + }; switch (cmd_id) { case rm_vt::CUSTOM_CONTROLLER_CMD: { rm_vt::CustomControllerData custom_controller_ref; rm_msgs::CustomControllerData custom_controller_data; - memcpy(&custom_controller_ref, rx_data + 7, sizeof(rm_vt::CustomControllerData)); + if (!copy_payload(&custom_controller_ref, sizeof(custom_controller_ref), "CUSTOM_CONTROLLER_CMD")) + break; custom_controller_data.encoder_data[0] = 3.14 * ((uint16_t)(custom_controller_ref.encoder1_data[0] << 8) | (uint16_t)custom_controller_ref.encoder1_data[1]) / @@ -102,44 +248,69 @@ int VideoTransmission::unpack(uint8_t* rx_data) (uint16_t)custom_controller_ref.joystick_r_x_data[1]); custom_controller_data.joystick_r_x_data = ((uint16_t)(custom_controller_ref.joystick_r_y_data[0] << 8) | (uint16_t)custom_controller_ref.joystick_r_y_data[1]); - custom_controller_data.button_data[0] = custom_controller_ref.button1_data; - custom_controller_data.button_data[1] = custom_controller_ref.button2_data; - custom_controller_data.button_data[2] = custom_controller_ref.button3_data; - custom_controller_data.button_data[3] = custom_controller_ref.button4_data; + custom_controller_data.button_data[0] = static_cast(custom_controller_ref.button1_data); + custom_controller_data.button_data[1] = static_cast(custom_controller_ref.button2_data); + custom_controller_data.button_data[2] = static_cast(custom_controller_ref.button3_data); + custom_controller_data.button_data[3] = static_cast(custom_controller_ref.button4_data); custom_controller_cmd_pub_.publish(custom_controller_data); break; } + case rm_vt::KEYBOARD_MOUSE_CMD: + { + rm_vt::KeyboardMouseData2026 keyboard_mouse_ref; + if (!copy_payload(&keyboard_mouse_ref, sizeof(keyboard_mouse_ref), "KEYBOARD_MOUSE_CMD")) + break; + + const uint16_t keyboard_value = updateKeyboardValueStateFromKeyCodes(keyboard_mouse_ref.key_value); + publishKeyboardMouseData(keyboard_value, static_cast(keyboard_mouse_ref.x_position), + static_cast(keyboard_mouse_ref.y_position), 0, + keyboard_mouse_ref.mouse_left == 1, keyboard_mouse_ref.mouse_right == 1); + break; + } case rm_vt::ROBOT_COMMAND_CMD: { rm_vt::KeyboardMouseData keyboard_mouse_ref; - rm_msgs::VTKeyboardMouseData keyboard_mouse_data; - memcpy(&keyboard_mouse_ref, rx_data + 7, sizeof(rm_vt::KeyboardMouseData)); - keyboard_mouse_data.mouse_x = keyboard_mouse_ref.mouse_x; - keyboard_mouse_data.mouse_y = keyboard_mouse_ref.mouse_y; - keyboard_mouse_data.mouse_z = keyboard_mouse_ref.mouse_z; - keyboard_mouse_data.left_button_down = keyboard_mouse_ref.left_button_down; - keyboard_mouse_data.right_button_down = keyboard_mouse_ref.right_button_down; - keyboard_mouse_data.key_w = keyboard_mouse_ref.key_w; - keyboard_mouse_data.key_s = keyboard_mouse_ref.key_s; - keyboard_mouse_data.key_a = keyboard_mouse_ref.key_a; - keyboard_mouse_data.key_d = keyboard_mouse_ref.key_d; - keyboard_mouse_data.key_shift = keyboard_mouse_ref.key_shift; - keyboard_mouse_data.key_ctrl = keyboard_mouse_ref.key_ctrl; - keyboard_mouse_data.key_q = keyboard_mouse_ref.key_q; - keyboard_mouse_data.key_e = keyboard_mouse_ref.key_e; - keyboard_mouse_data.key_r = keyboard_mouse_ref.key_r; - keyboard_mouse_data.key_f = keyboard_mouse_ref.key_f; - keyboard_mouse_data.key_g = keyboard_mouse_ref.key_g; - keyboard_mouse_data.key_z = keyboard_mouse_ref.key_z; - keyboard_mouse_data.key_x = keyboard_mouse_ref.key_x; - keyboard_mouse_data.key_c = keyboard_mouse_ref.key_c; - keyboard_mouse_data.key_v = keyboard_mouse_ref.key_v; - keyboard_mouse_data.key_b = keyboard_mouse_ref.key_b; - vt_keyboard_mouse_pub_.publish(keyboard_mouse_data); + if (!copy_payload(&keyboard_mouse_ref, sizeof(keyboard_mouse_ref), "ROBOT_COMMAND_CMD")) + break; + const uint16_t keyboard_value = keyboardMaskFromLegacyFrame(keyboard_mouse_ref); + keyboard_value_state_ = keyboard_value; + publishKeyboardMouseData(keyboard_value, keyboard_mouse_ref.mouse_x, keyboard_mouse_ref.mouse_y, + keyboard_mouse_ref.mouse_z, keyboard_mouse_ref.left_button_down != 0, + keyboard_mouse_ref.right_button_down != 0); + break; + } + case rm_vt::ROBOT_TO_CUSTOM_CMD: + { + rm_vt::RobotToCustomData payload; + if (!copy_payload(&payload, sizeof(payload), "ROBOT_TO_CUSTOM_CMD")) + break; + std_msgs::UInt8MultiArray msg; + msg.data.assign(payload.data, payload.data + sizeof(payload.data)); + robot_to_custom_pub_.publish(msg); + break; + } + case rm_vt::ROBOT_TO_CUSTOM_CMD_2: + { + rm_vt::RobotToCustomData2 payload; + if (!copy_payload(&payload, sizeof(payload), "ROBOT_TO_CUSTOM_CMD_2")) + break; + std_msgs::UInt8MultiArray msg; + msg.data.assign(payload.data, payload.data + sizeof(payload.data)); + robot_to_custom2_pub_.publish(msg); + break; + } + case rm_vt::CUSTOM_TO_ROBOT_CMD: + { + rm_vt::RobotToCustomData payload; + if (!copy_payload(&payload, sizeof(payload), "CUSTOM_TO_ROBOT_CMD")) + break; + std_msgs::UInt8MultiArray msg; + msg.data.assign(payload.data, payload.data + sizeof(payload.data)); + custom_to_robot_pub_.publish(msg); break; } default: - ROS_WARN("Referee command ID %d not found.", cmd_id); + ROS_WARN("Video transmission command ID %d not found.", cmd_id); break; } base_.video_transmission_is_online_ = true; @@ -150,9 +321,12 @@ int VideoTransmission::unpack(uint8_t* rx_data) return -1; } -int VideoTransmission::control_data_unpack(uint8_t* rx_data) +int VideoTransmission::control_data_unpack(uint8_t* rx_data, int rx_data_len) { - int frame_len = 21; + const int frame_len = 21; + if (rx_data_len < frame_len) + return -1; + if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1) { rm_vt::ControlData control_ref;