From 3c338e2dcdd19b9b0a6dfb68b1a806116b60f52c Mon Sep 17 00:00:00 2001
From: liyixin135 <421159734@qq.com>
Date: Wed, 31 Jul 2024 16:20:40 +0800
Subject: [PATCH] Add sentry mode when pressing ctrlR.

---
 .../chassis_gimbal_shooter_cover_manual.h     |  3 ++
 .../rm_manual/chassis_gimbal_shooter_manual.h |  2 +-
 src/chassis_gimbal_shooter_cover_manual.cpp   | 32 +++++++++++++++++++
 3 files changed, 36 insertions(+), 1 deletion(-)

diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
index 6773fde3..9d4f8ab4 100644
--- a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -33,6 +33,8 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void zRelease();
   void wPress() override;
   void wPressing() override;
+  void ctrlRPressing();
+  void ctrlRRelease() override;
 
   virtual void ctrlZPress();
   virtual void ctrlZRelease()
@@ -50,5 +52,6 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   ros::Time last_switch_time_;
   bool supply_ = false;
   bool cover_close_ = true;
+  int count_{};
 };
 }  // namespace rm_manual
diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
index 09350aa1..9a0de1f1 100644
--- a/include/rm_manual/chassis_gimbal_shooter_manual.h
+++ b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -106,7 +106,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void ctrlVPress();
   void ctrlBPress();
   void ctrlRPress();
-  void ctrlRRelease();
+  virtual void ctrlRRelease();
   virtual void ctrlQPress();
 
   InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_,
diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
index 0c685f5f..0f984a03 100644
--- a/src/chassis_gimbal_shooter_cover_manual.cpp
+++ b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -25,6 +25,7 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
+  ctrl_r_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterCoverManual::ctrlRPressing, this));
   z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this),
                    boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this));
 }
@@ -211,4 +212,35 @@ void ChassisGimbalShooterCoverManual::ctrlZPress()
   }
 }
 
+void ChassisGimbalShooterCoverManual::ctrlRPressing()
+{
+  if (!is_gyro_)
+  {
+    chassis_cmd_sender_->power_limit_->updateState(rm_common::PowerLimit::NORMAL);
+    setChassisMode(rm_msgs::ChassisCmd::RAW);
+  }
+  if (track_data_.id == 0)
+  {
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRAJ);
+    double traj_yaw = M_PI * count_ / 1000;
+    double traj_pitch = 0.15 * sin(2 * M_PI * (count_ % 1100) / 1100) + 0.15;
+    count_++;
+    gimbal_cmd_sender_->setGimbalTraj(traj_yaw, traj_pitch);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+  }
+  else
+  {
+    gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+    shooter_cmd_sender_->checkError(ros::Time::now());
+  }
+}
+
+void ChassisGimbalShooterCoverManual::ctrlRRelease()
+{
+  count_ = 0;
+  gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
+  shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+}
+
 }  // namespace rm_manual