Merge pull request #114 from liyixin135/sentry_mode

Add sentry mode when pressing ctrlR
rm-controls · Jul 31, 2024 · ee7b016 · ee7b016
2 parents b97f0b1 + 3c338e2
commit ee7b016
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 1 deletion.
diff --git 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
@@ -107,7 +107,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
@@ -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