Add wheeled_balance_manual and legged_wheel_balance_manual.

include/rm_manual/legged_wheel_balance_manual.h

@@ -0,0 +1,37 @@
+//
+// 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;
+  InputEvent ctrl_shift_event_, ctrl_g_event_;
+};
+}  // namespace rm_manual

include/rm_manual/wheeled_balance_manual.h

@@ -32,6 +32,6 @@ class WheeledBalanceManual : public BalanceManual
   ros::Subscriber state_sub_;
   double balance_dangerous_angle_;
 
-  InputEvent v_event_, ctrl_x_event_, auto_fallen_event_;
+  InputEvent ctrl_x_event_, auto_fallen_event_;
 };
 }  // namespace rm_manual

src/balance_manual.cpp

@@ -9,8 +9,9 @@ namespace rm_manual
 BalanceManual::BalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
   : ChassisGimbalShooterCoverManual(nh, nh_referee)
 {
-  nh.param("flank_frame", flank_frame_, std::string("flank_frame"));
-  nh.param("reverse_frame", reverse_frame_, std::string("yaw_reverse_frame"));
+  ros::NodeHandle balance_nh(nh, "balance");
+  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;
 }
 
@@ -91,7 +92,7 @@ void BalanceManual::shiftPress()
 {
   ChassisGimbalShooterCoverManual::shiftPress();
   chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::UP_SLOPE);
-  chassis_cmd_sender_->updateSafetyPower(60);
+  chassis_cmd_sender_->updateSafetyPower(220);
 }
 
 void BalanceManual::wPress()

src/legged_wheel_balance_manual.cpp

@@ -0,0 +1,120 @@
+//
+// Created by kook on 9/28/24.
+//
+
+#include "rm_manual/legged_wheel_balance_manual.h"
+
+namespace rm_manual
+{
+LeggedWheelBalanceManual::LeggedWheelBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
+  : BalanceManual(nh, nh_referee)
+{
+  ros::NodeHandle leg_nh(nh, "balance/legged_wheel_chassis");
+  legCommandSender_ = new rm_common::LegCommandSender(leg_nh);
+  legCommandSender_->setLgeLength(0.18);
+  legCommandSender_->setJump(false);
+
+  b_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::bPress, this),
+                   boost::bind(&LeggedWheelBalanceManual::bRelease, this));
+  ctrl_shift_event_.setEdge(boost::bind(&LeggedWheelBalanceManual::ctrlShiftPress, this),
+                            boost::bind(&LeggedWheelBalanceManual::ctrlShiftRelease, this));
+  ctrl_g_event_.setRising(boost::bind(&LeggedWheelBalanceManual::ctrlGPress, this));
+}
+
+void LeggedWheelBalanceManual::sendCommand(const ros::Time& time)
+{
+  BalanceManual::sendCommand(time);
+  legCommandSender_->sendCommand(time);
+}
+
+void LeggedWheelBalanceManual::checkKeyboard(const rm_msgs::DbusData::ConstPtr& dbus_data)
+{
+  ChassisGimbalShooterCoverManual::checkKeyboard(dbus_data);
+  ctrl_g_event_.update(dbus_data->key_ctrl && dbus_data->key_g);
+  ctrl_shift_event_.update(dbus_data->key_ctrl && dbus_data->key_shift);
+}
+
+void LeggedWheelBalanceManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data)
+{
+  BalanceManual::updateRc(dbus_data);
+  if (!is_gyro_)
+  {  // Capacitor enter fast charge when chassis stop.
+    if (!dbus_data->wheel && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW &&
+        std::sqrt(std::pow(vel_cmd_sender_->getMsg()->linear.x, 2) + std::pow(vel_cmd_sender_->getMsg()->linear.y, 2)) >
+            0.0)
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+    else if (chassis_power_ < 6.0 && chassis_cmd_sender_->getMsg()->mode == rm_msgs::ChassisCmd::FOLLOW)
+      chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+  }
+  else
+  {
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+  }
+}
+
+void LeggedWheelBalanceManual::rightSwitchDownRise()
+{
+  BalanceManual::rightSwitchDownRise();
+}
+
+void LeggedWheelBalanceManual::rightSwitchMidRise()
+{
+  BalanceManual::rightSwitchMidRise();
+}
+
+void LeggedWheelBalanceManual::ctrlZPress()
+{
+  BalanceManual::ctrlZPress();
+  if (!supply_)
+  {
+    is_gyro_ = false;
+    legCommandSender_->setLgeLength(0.18);
+  }
+}
+
+void LeggedWheelBalanceManual::shiftRelease()
+{
+  BalanceManual::shiftRelease();
+}
+
+void LeggedWheelBalanceManual::shiftPress()
+{
+  BalanceManual::shiftPress();
+}
+
+void LeggedWheelBalanceManual::ctrlShiftPress()
+{
+  legCommandSender_->setJump(true);
+}
+
+void LeggedWheelBalanceManual::ctrlShiftRelease()
+{
+  legCommandSender_->setJump(false);
+}
+
+void LeggedWheelBalanceManual::bPress()
+{
+  ChassisGimbalShooterCoverManual::bPress();
+  chassis_cmd_sender_->updateSafetyPower(60);
+}
+
+void LeggedWheelBalanceManual::bRelease()
+{
+  chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::BURST);
+  chassis_cmd_sender_->updateSafetyPower(60);
+}
+
+void LeggedWheelBalanceManual::ctrlGPress()
+{
+  if (!stretch_)
+  {
+    legCommandSender_->setLgeLength(0.25);
+    stretch_ = true;
+  }
+  else
+  {
+    legCommandSender_->setLgeLength(0.18);
+    stretch_ = false;
+  }
+}
+}  // namespace rm_manual

src/main.cpp

@@ -7,6 +7,7 @@
 #include "rm_manual/engineer_manual.h"
 #include "rm_manual/dart_manual.h"
 #include "rm_manual/wheeled_balance_manual.h"
+#include "rm_manual/legged_wheel_balance_manual.h"
 
 int main(int argc, char** argv)
 {
@@ -24,8 +25,10 @@ int main(int argc, char** argv)
     manual_control = new rm_manual::EngineerManual(nh, nh_referee);
   else if (robot == "dart")
     manual_control = new rm_manual::DartManual(nh, nh_referee);
-  else if (robot == "balance")
-    manual_control = new rm_manual::BalanceManual(nh, nh_referee);
+  else if (robot == "wheeled_balance")
+    manual_control = new rm_manual::WheeledBalanceManual(nh, nh_referee);
+  else if (robot == "legged_wheel_balance")
+    manual_control = new rm_manual::LeggedWheelBalanceManual(nh, nh_referee);
   else
   {
     ROS_ERROR("no robot type ");

src/wheeled_balance_manual.cpp

@@ -9,11 +9,11 @@ namespace rm_manual
 WheeledBalanceManual::WheeledBalanceManual(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
   : BalanceManual(nh, nh_referee)
 {
-  ros::NodeHandle balance_chassis_nh(nh, "balance_chassis");
+  ros::NodeHandle balance_chassis_nh(nh, "balance/wheeled_chassis");
   balance_chassis_cmd_sender_ = new rm_common::BalanceCommandSender(balance_chassis_nh);
   balance_chassis_cmd_sender_->setBalanceMode(rm_msgs::BalanceState::NORMAL);
 
-  nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3);
+  balance_chassis_nh.param("balance_dangerous_angle", balance_dangerous_angle_, 0.3);
 
   state_sub_ = balance_chassis_nh.subscribe<rm_msgs::BalanceState>("/state", 1,
                                                                    &WheeledBalanceManual::balanceStateCallback, this);