Merge branch 'rm-controls:master' into master

Author: GuraMumei

rm_common/include/rm_common/decision/command_sender.h

@@ -51,6 +51,7 @@
 #include <rm_msgs/GameRobotHp.h>
 #include <rm_msgs/StatusChangeRequest.h>
 #include <rm_msgs/ShootData.h>
+#include <rm_msgs/LegCmd.h>
 #include <geometry_msgs/TwistStamped.h>
 #include <sensor_msgs/JointState.h>
 #include <nav_msgs/Odometry.h>
@@ -564,6 +565,32 @@ class BalanceCommandSender : public CommandSenderBase<std_msgs::UInt8>
   void setZero() override{};
 };
 
+class LegCommandSender : public CommandSenderBase<rm_msgs::LegCmd>
+{
+public:
+  explicit LegCommandSender(ros::NodeHandle& nh) : CommandSenderBase<rm_msgs::LegCmd>(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<geometry_msgs::TwistStamped>
 {
 public:

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 };

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

rm_msgs/msg/LegCmd.msg

@@ -0,0 +1,2 @@
+bool jump
+float64 leg_length