diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h index 74a60619..ee37bf7f 100644 --- a/rm_common/include/rm_common/decision/command_sender.h +++ b/rm_common/include/rm_common/decision/command_sender.h @@ -51,6 +51,7 @@ #include #include #include +#include #include #include #include @@ -564,6 +565,32 @@ class BalanceCommandSender : public CommandSenderBase void setZero() override{}; }; +class LegCommandSender : public CommandSenderBase +{ +public: + explicit LegCommandSender(ros::NodeHandle& nh) : CommandSenderBase(nh) + { + } + + void setJump(bool jump) + { + msg_.jump = jump; + } + void setLgeLength(double length) + { + msg_.leg_length = length; + } + bool getJump() + { + return msg_.jump; + } + double getLgeLength() + { + return msg_.leg_length; + } + void setZero() override{}; +}; + class Vel3DCommandSender : public HeaderStampCommandSenderBase { public: diff --git a/rm_common/include/rm_common/decision/power_limit.h b/rm_common/include/rm_common/decision/power_limit.h index cf83a753..b4f2c963 100644 --- a/rm_common/include/rm_common/decision/power_limit.h +++ b/rm_common/include/rm_common/decision/power_limit.h @@ -66,6 +66,8 @@ class PowerLimit 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()); } typedef enum { @@ -124,7 +126,7 @@ class PowerLimit chassis_cmd.power_limit = burst_power_; else { - switch (cap_state_) + switch (is_new_capacitor_ ? expect_state_ : cap_state_) { case NORMAL: normal(chassis_cmd); @@ -186,6 +188,7 @@ class PowerLimit double charge_power_{}, extra_power_{}, burst_power_{}; double buffer_threshold_{}; double power_gain_{}; + bool is_new_capacitor_{}; uint8_t expect_state_{}, cap_state_{}; bool referee_is_online_{ false }; diff --git a/rm_msgs/CMakeLists.txt b/rm_msgs/CMakeLists.txt index 0038f82c..1399bfdd 100644 --- a/rm_msgs/CMakeLists.txt +++ b/rm_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ add_message_files( GimbalCmd.msg GimbalDesError.msg GimbalPosState.msg + LegCmd.msg LpData.msg LocalHeatState.msg KalmanData.msg diff --git a/rm_msgs/msg/LegCmd.msg b/rm_msgs/msg/LegCmd.msg new file mode 100644 index 00000000..57f90085 --- /dev/null +++ b/rm_msgs/msg/LegCmd.msg @@ -0,0 +1,2 @@ +bool jump +float64 leg_length