Merge branch 'master' into traj

rm-controls · Jul 19, 2024 · a9bf537 · a9bf537
2 parents 506e1e1 + de20f92
commit a9bf537
Show file tree

Hide file tree

Showing 5 changed files with 32 additions and 7 deletions.
diff --git a/include/rm_manual/chassis_gimbal_shooter_cover_manual.h b/include/rm_manual/chassis_gimbal_shooter_cover_manual.h
@@ -29,7 +29,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void rightSwitchUpRise() override;
   void rPress() override;
   void ePress() override;
-  void zPressing();
+  void zPress();
   void zRelease();
   void wPress() override;
   void wPressing() override;

diff --git a/include/rm_manual/chassis_gimbal_shooter_manual.h b/include/rm_manual/chassis_gimbal_shooter_manual.h
@@ -52,6 +52,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
   void leftSwitchUpOn(ros::Duration duration);
+  void leftSwitchUpFall();
   void mouseLeftPress();
   void mouseLeftRelease()
   {
@@ -122,6 +123,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   rm_common::CalibrationQueue* gimbal_calibration_;
 
   geometry_msgs::PointStamped point_out_;
+  uint8_t last_shoot_freq_{};
 
   bool prepare_shoot_ = false, turn_flag_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false,
        adjust_image_transmission_ = false;

diff --git a/src/chassis_gimbal_manual.cpp b/src/chassis_gimbal_manual.cpp
@@ -47,11 +47,19 @@ void ChassisGimbalManual::updateRc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ManualBase::updateRc(dbus_data);
   gimbal_cmd_sender_->setRate(-dbus_data->ch_l_x, -dbus_data->ch_l_y);
+  if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE)
+    chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
+  else
+    chassis_cmd_sender_->setFollowVelDes(0.);
 }
 void ChassisGimbalManual::updatePc(const rm_msgs::DbusData::ConstPtr& dbus_data)
 {
   ManualBase::updatePc(dbus_data);
   gimbal_cmd_sender_->setRate(-dbus_data->m_x * gimbal_scale_, dbus_data->m_y * gimbal_scale_);
+  if (gimbal_cmd_sender_->getMsg()->mode == rm_msgs::GimbalCmd::RATE)
+    chassis_cmd_sender_->setFollowVelDes(gimbal_cmd_sender_->getMsg()->rate_yaw);
+  else
+    chassis_cmd_sender_->setFollowVelDes(0.);
 }
 
 void ChassisGimbalManual::checkReferee()

diff --git a/src/chassis_gimbal_shooter_cover_manual.cpp b/src/chassis_gimbal_shooter_cover_manual.cpp
@@ -25,8 +25,8 @@ ChassisGimbalShooterCoverManual::ChassisGimbalShooterCoverManual(ros::NodeHandle
 
   ctrl_z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::ctrlZPress, this),
                         boost::bind(&ChassisGimbalShooterCoverManual::ctrlZRelease, this));
-  z_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterCoverManual::zPressing, this));
-  z_event_.setFalling(boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this));
+  z_event_.setEdge(boost::bind(&ChassisGimbalShooterCoverManual::zPress, this),
+                   boost::bind(&ChassisGimbalShooterCoverManual::zRelease, this));
 }
 
 void ChassisGimbalShooterCoverManual::changeSpeedMode(SpeedMode speed_mode)
@@ -159,14 +159,15 @@ void ChassisGimbalShooterCoverManual::ePress()
   switch_exposure_srv_->callService();
 }
 
-void ChassisGimbalShooterCoverManual::zPressing()
+void ChassisGimbalShooterCoverManual::zPress()
 {
+  last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency();
   shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL);
 }
 
 void ChassisGimbalShooterCoverManual::zRelease()
 {
-  shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::LOW);
+  shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
 }
 
 void ChassisGimbalShooterCoverManual::wPress()

diff --git a/src/chassis_gimbal_shooter_manual.cpp b/src/chassis_gimbal_shooter_manual.cpp
@@ -40,6 +40,7 @@ ChassisGimbalShooterManual::ChassisGimbalShooterManual(ros::NodeHandle& nh, ros:
   self_inspection_event_.setRising(boost::bind(&ChassisGimbalShooterManual::selfInspectionStart, this));
   game_start_event_.setRising(boost::bind(&ChassisGimbalShooterManual::gameStart, this));
   left_switch_up_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::leftSwitchUpOn, this, _1));
+  left_switch_up_event_.setFalling(boost::bind(&ChassisGimbalShooterManual::leftSwitchUpFall, this));
   left_switch_mid_event_.setActiveHigh(boost::bind(&ChassisGimbalShooterManual::leftSwitchMidOn, this, _1));
   e_event_.setEdge(boost::bind(&ChassisGimbalShooterManual::ePress, this),
                    boost::bind(&ChassisGimbalShooterManual::eRelease, this));
@@ -315,6 +316,12 @@ void ChassisGimbalShooterManual::leftSwitchUpRise()
 {
   ChassisGimbalManual::leftSwitchUpRise();
   gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
+  last_shoot_freq_ = shooter_cmd_sender_->getShootFrequency();
+}
+
+void ChassisGimbalShooterManual::leftSwitchUpFall()
+{
+  shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
 }
 
 void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
@@ -325,11 +332,13 @@ void ChassisGimbalShooterManual::leftSwitchUpOn(ros::Duration duration)
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::TRACK);
   if (duration > ros::Duration(1.))
   {
+    shooter_cmd_sender_->setShootFrequency(last_shoot_freq_);
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
     shooter_cmd_sender_->checkError(ros::Time::now());
   }
   else if (duration < ros::Duration(0.02))
   {
+    shooter_cmd_sender_->setShootFrequency(rm_common::HeatLimit::MINIMAL);
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
     shooter_cmd_sender_->checkError(ros::Time::now());
   }
@@ -346,8 +355,13 @@ void ChassisGimbalShooterManual::mouseLeftPress()
   }
   if (prepare_shoot_)
   {
-    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
-    shooter_cmd_sender_->checkError(ros::Time::now());
+    if (!mouse_right_event_.getState() || (mouse_right_event_.getState() && track_data_.id != 0))
+    {
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+      shooter_cmd_sender_->checkError(ros::Time::now());
+    }
+    else
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
   }
 }