Add legged manual for legged_balance.

include/rm_manual/balance_manual.h

@@ -1,5 +1,5 @@
 //
-// Created by yuchen on 2023/4/3.
+// Created by kook on 9/28/24.
 //
 
 #pragma once
@@ -15,6 +15,8 @@ class BalanceManual : public ChassisGimbalShooterCoverManual
 
 protected:
   void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
+  void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
+
   void wPress() override;
   void sPress() override;
   void aPress() override;
@@ -26,28 +28,13 @@ class BalanceManual : public ChassisGimbalShooterCoverManual
   void aPressing() override;
   void sPressing() override;
   void dPressing() override;
-  void bPress() override;
-  void vPress() override;
   void ctrlZPress() override;
   void rightSwitchDownRise() override;
   void rightSwitchMidRise() override;
-
   void sendCommand(const ros::Time& time) override;
-  void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
-  void ctrlXPress();
-  void modeFallen(ros::Duration duration);
-  void modeNormalize();
-  rm_common::BalanceCommandSender* balance_cmd_sender_{};
 
 private:
-  void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg);
-
-  ros::Subscriber state_sub_;
-  double balance_dangerous_angle_;
-
   bool flank_ = false, reverse_ = false;
   std::string flank_frame_, reverse_frame_;
-
-  InputEvent v_event_, ctrl_x_event_, auto_fallen_event_;
 };
 }  // namespace rm_manual

include/rm_manual/legged_wheel_balance_manual.h

@@ -0,0 +1,39 @@
+//
+// Created by kook on 9/28/24.
+//
+
+#pragma once
+
+#include "rm_manual/balance_manual.h"
+
+namespace rm_manual
+{
+class LeggedWheelBalanceManual : public BalanceManual
+{
+public:
+  LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee);
+
+protected:
+  void updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
+  void shiftPress() override;
+  void shiftRelease() override;
+  void bPress() override;
+  void bRelease() override;
+  void ctrlZPress() override;
+  void rightSwitchDownRise() override;
+  void rightSwitchMidRise() override;
+  void ctrlShiftPress();
+  void ctrlShiftRelease();
+
+  void sendCommand(const ros::Time& time) override;
+  void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
+  void ctrlGPress();
+  rm_common::LegCommandSender* legCommandSender_{};
+
+private:
+  bool stretch_ = false, stretching_ = false, is_increasing_length_ = false;
+  InputEvent ctrl_shift_event_, ctrl_g_event_;
+  ros::Subscriber unstick_sub_;
+  void unstickCallback(const std_msgs::BoolConstPtr& msg);
+};
+}  // namespace rm_manual

include/rm_manual/wheeled_balance_manual.h

@@ -0,0 +1,37 @@
+//
+// Created by yuchen on 2023/4/3.
+//
+
+#pragma once
+
+#include "rm_manual/balance_manual.h"
+
+namespace rm_manual
+{
+class WheeledBalanceManual : public BalanceManual
+{
+public:
+  WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee);
+
+protected:
+  void sendCommand(const ros::Time& time) override;
+  void checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data) override;
+  void rightSwitchDownRise() override;
+  void rightSwitchMidRise() override;
+  void ctrlZPress() override;
+  void bPress() override;
+  void vPress() override;
+  void ctrlXPress();
+  void modeFallen(ros::Duration duration);
+  void modeNormalize();
+  rm_common::BalanceCommandSender* balance_chassis_cmd_sender_{};
+
+private:
+  void balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg);
+
+  ros::Subscriber state_sub_;
+  double balance_dangerous_angle_;
+
+  InputEvent ctrl_x_event_, auto_fallen_event_;
+};
+}  // namespace rm_manual

src/balance_manual.cpp

@@ -1,5 +1,5 @@
 //
-// Created by yuchen on 2023/4/3.
+// Created by kook on 9/28/24.
 //
 
 #include "rm_manual/balance_manual.h"
@@ -10,21 +10,9 @@ BalanceManual::BalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
   : ChassisGimbalShooterCoverManual(nh, nh_referee)
 {
   ros::NodeHandle balance_nh(nh, "balance");
-  balance_cmd_sender_ = new rm_common::BalanceCommandSender(balance_nh);
-  balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
-
-  nh.param("flank_frame", flank_frame_, std::string("flank_frame"));
-  nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame"));
-  nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3);
-
+  balance_nh.param("flank_frame", flank_frame_, std::string("flank_frame"));
+  balance_nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame"));
   is_balance_ = true;
-  state_sub_ = balance_nh.subscribe<rm_msgs::BalanceState>("/state", 1, &BalanceManual::balanceStateCallback, this);
-  x_event_.setRising(boost::bind(&BalanceManual::xPress, this));
-  g_event_.setRising(boost::bind(&BalanceManual::gPress, this));
-  v_event_.setRising(boost::bind(&BalanceManual::vPress, this));
-  auto_fallen_event_.setActiveHigh(boost::bind(&BalanceManual::modeFallen, this, _1));
-  auto_fallen_event_.setDelayTriggered(boost::bind(&BalanceManual::modeNormalize, this), 1.5, true);
-  ctrl_x_event_.setRising(boost::bind(&BalanceManual::ctrlXPress, this));
 }
 
 void BalanceManual::sendCommand(const ros::Time& time)
@@ -49,14 +37,11 @@ void BalanceManual::sendCommand(const ros::Time& time)
 
   ChassisGimbalShooterManual::sendCommand(time);
   cover_command_sender_->sendCommand(time);
-  balance_cmd_sender_->sendCommand(time);
 }
 
 void BalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data);
-  v_event_.update(dbus_data->key_v && !dbus_data->key_ctrl);
-  ctrl_x_event_.update(dbus_data->key_ctrl && dbus_data->key_x);
 }
 
 void BalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data)
@@ -72,15 +57,13 @@ void BalanceManual::rightSwitchDownRise()
 {
   ChassisGimbalShooterCoverManual::rightSwitchDownRise();
   state_ = RC;
-  balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
   chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN);
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
 }
 
 void BalanceManual::rightSwitchMidRise()
 {
   ChassisGimbalShooterCoverManual::rightSwitchMidRise();
-  balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
   chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
   chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
 }
@@ -90,13 +73,11 @@ void BalanceManual::ctrlZPress()
   ChassisGimbalShooterCoverManual::ctrlZPress();
   if (supply_)
   {
-    balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
     chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FALLEN);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::CHARGE);
   }
   else
   {
-    balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
     chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
     chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
   }
@@ -111,18 +92,7 @@ void BalanceManual::shiftPress()
 {
   ChassisGimbalShooterCoverManual::shiftPress();
   chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::UP_SLOPE);
-  chassis_cmd_sender_->updateSafetyPower(60);
-}
-
-void BalanceManual::vPress()
-{
-  chassis_cmd_sender_->updateSafetyPower(80);
-}
-
-void BalanceManual::bPress()
-{
-  ChassisGimbalShooterCoverManual::bPress();
-  chassis_cmd_sender_->updateSafetyPower(100);
+  chassis_cmd_sender_->updateSafetyPower(220);
 }
 
 void BalanceManual::wPress()
@@ -197,54 +167,13 @@ void BalanceManual::cPress()
 {
   if (is_gyro_)
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
-    vel_cmd_sender_->setAngularZVel(0.0);
-    is_gyro_ = false;
+    setChassisMode(rm_msgs::ChassisCmd::FOLLOW);
   }
   else
   {
-    chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::RAW);
-    is_gyro_ = true;
-    if (x_scale_ != 0.0 || y_scale_ != 0.0)
-      vel_cmd_sender_->setAngularZVel(gyro_rotate_reduction_);
-    else
-      vel_cmd_sender_->setAngularZVel(1.0);
+    setChassisMode(rm_msgs::ChassisCmd::RAW);
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
   }
 }
 
-void BalanceManual::ctrlXPress()
-{
-  if (balance_cmd_sender_->getMsg()->data == rm_msgs::BalanceState::NORMAL)
-    balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
-  else
-    balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
-}
-
-void BalanceManual::balanceStateCallback(const rm_msgs::BalanceState::ConstPtr& msg)
-{
-  if ((ros::Time::now() - msg->header.stamp).toSec() < 0.2)
-  {
-    if (std::abs(msg->theta) > balance_dangerous_angle_)
-      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
-    if (msg->mode == rm_msgs::BalanceState::NORMAL)
-      auto_fallen_event_.update(std::abs(msg->theta) > 0.42 && std::abs(msg->x_dot) > 1.5 &&
-                                vel_cmd_sender_->getMsg()->linear.x == 0 && vel_cmd_sender_->getMsg()->linear.y == 0 &&
-                                vel_cmd_sender_->getMsg()->angular.z == 0);
-  }
-}
-
-void BalanceManual::modeNormalize()
-{
-  balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
-  ROS_INFO("mode normalize");
-}
-
-void BalanceManual::modeFallen(ros::Duration duration)
-{
-  if (duration.toSec() > 0.3)
-  {
-    balance_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::FALLEN);
-    ROS_INFO("mode fallen");
-  }
-}
 }  // namespace rm_manual