Add rls and new ui

rm_common/include/rm_common/decision/invincible_detect.h

@@ -0,0 +1,54 @@
+//
+// Created by ray on 25-1-11.
+//
+
+#pragma once
+
+#include <ros/ros.h>
+#include <rm_msgs/GameStatus.h>
+#include <rm_msgs/GameRobotStatus.h>
+
+namespace rm_common
+{
+class InvincibleDetect
+{
+public:
+  InvincibleDetect(ros::NodeHandle& nh)
+  {
+    if (!nh.getParam("invincible_check", invincible_check_))
+      ROS_ERROR("Invincible_check no defined (namespace: %s)", nh.getNamespace().c_str());
+  }
+  typedef enum
+  {
+    INJURABLE = 0,
+    IN_SUPPLY_BASE = 1,
+    NORMAL_FRESHLY_RESURRECTED = 2,
+    MONEY_FRESHLY_RESURRECTED = 3,
+  } InvincibleState;
+
+  //  void updateEnemyStatus(int robot_id)
+  // 调用上面的update函数，需要输入敌方id，然后函数中直接更新对应id机器人的无敌状态
+  //  InvincibleState getInvincibleState(int robot_id)
+  // 用这个函数来get对应机器人的无敌状态，然后用这个状态做判断，如果不是INJURABLE，就把shooter设为READY
+
+private:
+  //  updateState()
+  void updateRobotInfo(int track_id, rm_msgs::GameRobotStatus& data)
+  {
+    robot_id = data.robot_id;
+    if (robot_id < 100)
+      track_id = track_id + 100;
+  }
+
+  void updateInvincibleState(int track_id, bool is_red)
+  {
+  }
+
+  int last_hp;
+  float last_revive_time;
+  float last_heal_time;
+  int robot_id;
+  InvincibleState invincible_state = INJURABLE;
+  XmlRpc::XmlRpcValue invincible_check_;
+};
+}  // namespace rm_common

rm_common/include/rm_common/decision/power_limit.h

@@ -39,6 +39,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <string>
 
 #include <ros/ros.h>
 #include <rm_msgs/ChassisCmd.h>
@@ -75,14 +76,12 @@ class PowerLimit
       ROS_ERROR("Burst power no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("gyro_power", gyro_power_))
       ROS_ERROR("Gyro power no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("upstairs_power", upstairs_power_))
-      ROS_ERROR("Upstairs power no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("max_power_limit", max_power_limit_))
       ROS_ERROR("max power limit no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("power_gain", power_gain_))
-      ROS_ERROR("power gain no defined (namespace: %s)", nh.getNamespace().c_str());
-    if (!nh.getParam("total_burst_time", total_burst_time_))
-      ROS_ERROR("total burst time no defined (namespace: %s)", nh.getNamespace().c_str());
+    if (!nh.getParam("robot_type", robot_type_))
+      ROS_WARN("Only standard and hero robot types are supported (namespace: %s)", nh.getNamespace().c_str());
+    if (!nh.getParam("upstairs_power", upstairs_power_))
+      ROS_ERROR("Upstairs power no defined (namespace: %s)", nh.getNamespace().c_str());
     default_max_power_limit_ = max_power_limit_;
     default_burst_power_ = burst_power_;
   }
@@ -97,37 +96,55 @@ class PowerLimit
 
   void updateSafetyPower(int safety_power)
   {
-    if (safety_power > 0)
-      safety_power_ = safety_power;
-    ROS_INFO("update safety power: %d", safety_power);
+    if (robot_type_ == "standard")
+      safety_power_ = 40 + robot_id_ * 5;
+    else if (robot_type_ == "hero")
+      safety_power_ = 45 + robot_id_ * 5;
+    else
+      safety_power_ = safety_power > 0 ? safety_power : safety_power_;
+    ROS_WARN("update safety power: %.0f", safety_power_);
   }
+
+  void updateBurstPower(int burst_power)
+  {
+    burst_power_ = burst_power > 0 ? burst_power : burst_power_;
+    ROS_INFO("update burst power: %.0f", burst_power_);
+  }
+
   void updateState(uint8_t state)
   {
     expect_state_ = state;
   }
+
   void setGameRobotData(const rm_msgs::GameRobotStatus data)
   {
+    robot_level_ = data.robot_level;
     robot_id_ = data.robot_id;
     chassis_power_limit_ = data.chassis_power_limit;
   }
+
   void setChassisPowerBuffer(const rm_msgs::PowerHeatData data)
   {
     chassis_power_buffer_ = data.chassis_power_buffer;
   }
+
   void setCapacityData(const rm_msgs::PowerManagementSampleAndStatusData data)
   {
-    capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(0.3);
+    capacity_is_online_ = ros::Time::now() - data.stamp < ros::Duration(5);
     cap_energy_ = data.capacity_remain_charge;
     cap_state_ = data.state_machine_running_state;
   }
+
   void setRefereeStatus(bool status)
   {
     referee_is_online_ = status;
   }
+
   void setStartBurstTime(const ros::Time start_burst_time)
   {
     start_burst_time_ = start_burst_time;
   }
+
   ros::Time getStartBurstTime() const
   {
     return start_burst_time_;
@@ -188,7 +205,7 @@ class PowerLimit
     if (robot_id_ == rm_msgs::GameRobotStatus::BLUE_ENGINEER || robot_id_ == rm_msgs::GameRobotStatus::RED_ENGINEER)
       chassis_cmd.power_limit = 400;
     else
-    {  // standard and hero
+    {
       if (referee_is_online_)
       {
         if (capacity_is_online_ && expect_state_ != ALLOFF)
@@ -218,7 +235,10 @@ class PowerLimit
           normal(chassis_cmd);
       }
       else
+      {
+        updateSafetyPower(safety_power_);
         chassis_cmd.power_limit = safety_power_;
+      }
     }
     applyPosturePowerScale(chassis_cmd);
   }
@@ -279,8 +299,10 @@ class PowerLimit
 
   uint8_t expect_state_{}, cap_state_{};
 
+  std::string robot_type_{};
   int chassis_power_buffer_{};
-  int robot_id_{}, chassis_power_limit_{};
+  int robot_id_{}, robot_level_{};
+  int chassis_power_limit_{};
   double max_power_limit_{ 70.0 };
   float cap_energy_{};
   double safety_power_{};
@@ -291,13 +313,11 @@ class PowerLimit
   double disable_normal_cap_threshold_{};
   double extra_power_{}, burst_power_{}, gyro_power_{}, upstairs_power_{};
   double default_max_power_limit_{}, default_burst_power_{};
-  double power_gain_{};
 
   bool allow_gyro_cap_{ false }, allow_use_cap_{ false };
   double posture_power_scale_{ 1.0 };
 
   ros::Time start_burst_time_{};
-  int total_burst_time_{};
 
   bool referee_is_online_{ false };
   bool capacity_is_online_{ false };

rm_common/include/rm_common/math_utilities.h

@@ -76,3 +76,13 @@ T alpha(T cutoff, double freq)
   T te = 1.0 / freq;
   return 1.0 / (1.0 + tau / te);
 }
+
+template <typename T>
+T limit(T val, T min, T max)
+{
+  if (val > max)
+    return max;
+  if (val < min)
+    return min;
+  return val;
+}

rm_common/include/rm_common/rls.h

@@ -0,0 +1,165 @@
+//
+// Created by xiezd on 3/25/26.
+//
+
+#pragma once
+
+#include <cmath>
+#include <limits>
+#include <stdexcept>
+
+#include <Eigen/Dense>
+#include "eigen_types.h"
+
+#define RLS_ASSERT(condition, message)                                                                                 \
+  if (!(condition))                                                                                                    \
+  throw std::invalid_argument(message)
+
+template <typename T>
+class Rls
+{
+public:
+  Rls(int xSize, int ySize, T lambda, T pInit) : xSize_(xSize), ySize_(ySize), lambda_(lambda)
+  {
+    RLS_ASSERT(xSize_ > 0, "rls: xSize should be positive");
+    RLS_ASSERT(ySize_ > 0, "rls: ySize should be positive");
+    RLS_ASSERT(std::abs(lambda_) > std::numeric_limits<T>::epsilon(), "rls: lambda should be non-zero");
+
+    x_.resize(xSize_, 1);
+    x_.setZero();
+    xt_.resize(1, xSize_);
+    xt_.setZero();
+
+    w_.resize(xSize_, ySize_);
+    w_.setZero();
+    output_.resize(xSize_, ySize_);
+    output_.setZero();
+    k_.resize(xSize_, 1);
+    k_.setZero();
+    kNumerator_.resize(xSize_, 1);
+    kNumerator_.setZero();
+
+    p_.resize(xSize_, xSize_);
+    p_.setIdentity();
+    p_ *= pInit;
+
+    y_.resize(ySize_, 1);
+    y_.setZero();
+    u_.resize(ySize_, 1);
+    u_.setZero();
+    e_.resize(ySize_, 1);
+    e_.setZero();
+  }
+
+  template <typename TX>
+  bool setX(const Eigen::MatrixBase<TX>& x)
+  {
+    static_assert(TX::ColsAtCompileTime == 1 || TX::ColsAtCompileTime == Eigen::Dynamic,
+                  "rls: X should be a column vector");
+    RLS_ASSERT(x.rows() == xSize_ && x.cols() == 1, "rls: X size mismatch");
+    x_ = x;
+    return true;
+  }
+
+  template <typename TY>
+  bool setY(const Eigen::MatrixBase<TY>& y)
+  {
+    static_assert(TY::ColsAtCompileTime == 1 || TY::ColsAtCompileTime == Eigen::Dynamic,
+                  "rls: Y should be a column vector");
+    RLS_ASSERT(y.rows() == ySize_ && y.cols() == 1, "rls: Y size mismatch");
+    y_ = y;
+    return true;
+  }
+
+  bool setY(T y)
+  {
+    RLS_ASSERT(ySize_ == 1, "rls: setY(scalar) requires ySize==1");
+    y_(0, 0) = y;
+    return true;
+  }
+
+  template <typename TW>
+  bool setW(const Eigen::MatrixBase<TW>& w)
+  {
+    RLS_ASSERT(w.rows() == xSize_ && w.cols() == ySize_, "rls: W size mismatch");
+    w_ = w;
+    return true;
+  }
+
+  template <typename TU>
+  bool setU(const Eigen::MatrixBase<TU>& u)
+  {
+    static_assert(TU::ColsAtCompileTime == 1 || TU::ColsAtCompileTime == Eigen::Dynamic,
+                  "rls: U should be a column vector");
+    RLS_ASSERT(u.rows() == ySize_ && u.cols() == 1, "rls: U size mismatch");
+    u_ = u;
+    return true;
+  }
+
+  bool setU(T u)
+  {
+    RLS_ASSERT(ySize_ == 1, "rls: setU(scalar) requires ySize==1");
+    u_(0, 0) = u;
+    return true;
+  }
+
+  bool update()
+  {
+    xt_ = x_.transpose();
+    // Compute predicted output using current weights: u_pred = x^T * w
+    // Compute prediction error: e = y - u_pred
+    e_ = y_ - u_;
+
+    kNumerator_ = p_ * x_;
+    T denominator = lambda_ + (xt_ * p_ * x_)(0, 0);
+    RLS_ASSERT(std::abs(denominator) > std::numeric_limits<T>::epsilon(),
+               "rls: denominator too small, numerical instability");
+
+    // Compute gain vector: k = P*x / (lambda + x^T*P*x)
+    k_ = kNumerator_ / denominator;
+    // Update parameters: w = w + k * e^T
+    output_ = w_ + k_ * e_.transpose();
+    w_ = output_;
+
+    // Update covariance matrix: P = (P - k*x^T*P) / lambda
+    p_ = (p_ - k_ * xt_ * p_) / lambda_;
+    return true;
+  }
+
+  DMat<T> getW() const
+  {
+    return w_;
+  }
+
+  DMat<T> getP() const
+  {
+    return p_;
+  }
+
+  DVec<T> getError() const
+  {
+    return e_;
+  }
+
+  DVec<T> getEstimate() const
+  {
+    return u_;
+  }
+
+private:
+  int xSize_;
+  int ySize_;
+  T lambda_;
+
+  DVec<T> x_;
+  DMat<T> xt_;
+  DMat<T> w_;
+  DMat<T> output_;
+  DVec<T> k_;
+  DVec<T> kNumerator_;
+  DMat<T> p_;
+
+  DVec<T> y_;
+  DVec<T> u_;
+  DVec<T> e_;
+};