Merge branch 'rm-controls:master' into master

include/rm_manual/chassis_gimbal_manual.h

@@ -30,6 +30,7 @@ class ChassisGimbalManual : public ManualBase
   void dbusDataCallback(const rm_msgs::DbusData::ConstPtr& data) override;
   void capacityDataCallback(const rm_msgs::PowerManagementSampleAndStatusData ::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
+  void setChassisMode(int mode);
   virtual void wPress()
   {
     x_scale_ = x_scale_ >= 1.0 ? 1.0 : x_scale_ + 1.0;

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;

include/rm_manual/chassis_gimbal_shooter_manual.h

@@ -51,7 +51,9 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data) override;
   void suggestFireCallback(const std_msgs::Bool::ConstPtr& data) override;
   void trackCallback(const rm_msgs::TrackData::ConstPtr& data) override;
+  void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data) override;
   void leftSwitchUpOn(ros::Duration duration);
+  void leftSwitchUpFall();
   void mouseLeftPress();
   void mouseLeftRelease()
   {
@@ -86,7 +88,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   virtual void bPress();
   virtual void bRelease();
   virtual void rPress();
-  virtual void xReleasing();
+  virtual void xRelease();
   virtual void shiftPress();
   virtual void shiftRelease();
   void qPress()
@@ -104,6 +106,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void ctrlVPress();
   void ctrlBPress();
   void ctrlRPress();
+  void ctrlRRelease();
   virtual void ctrlQPress();
 
   InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_,
@@ -119,9 +122,9 @@ 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,
-       adjust_image_transmission_ = false;
+  bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false;
   double yaw_current_{};
 };
 }  // namespace rm_manual

include/rm_manual/manual_base.h

@@ -34,6 +34,7 @@
 #include <rm_msgs/GimbalDesError.h>
 #include <rm_msgs/GameRobotStatus.h>
 #include <rm_msgs/ManualToReferee.h>
+#include <rm_msgs/ShootData.h>
 #include <rm_msgs/PowerManagementSampleAndStatusData.h>
 #include "rm_manual/input_event.h"
 
@@ -88,6 +89,9 @@ class ManualBase
   virtual void suggestFireCallback(const std_msgs::Bool::ConstPtr& data)
   {
   }
+  virtual void shootDataCallback(const rm_msgs::ShootData::ConstPtr& data)
+  {
+  }
 
   // Referee
   virtual void chassisOutputOn()
@@ -141,7 +145,7 @@ class ManualBase
 
   ros::Subscriber odom_sub_, dbus_sub_, track_sub_, referee_sub_, capacity_sub_, game_status_sub_, joint_state_sub_,
       game_robot_hp_sub_, actuator_state_sub_, power_heat_data_sub_, gimbal_des_error_sub_, game_robot_status_sub_,
-      suggest_fire_sub_, shoot_beforehand_cmd_sub_;
+      suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_;
 
   sensor_msgs::JointState joint_state_;
   rm_msgs::TrackData track_data_;

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()
@@ -214,4 +222,24 @@ void ChassisGimbalManual::mouseMidRise(double m_z)
   }
 }
 
+void ChassisGimbalManual::setChassisMode(int mode)
+{
+  switch (mode)
+  {
+    case rm_msgs::ChassisCmd::RAW:
+      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);
+      break;
+    case rm_msgs::ChassisCmd::FOLLOW:
+      chassis_cmd_sender_->setMode(rm_msgs::ChassisCmd::FOLLOW);
+      is_gyro_ = false;
+      vel_cmd_sender_->setAngularZVel(0.0);
+      break;
+  }
+}
+
 }  // namespace rm_manual

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()