Sentry mode when hit by dart.

include/rm_manual/chassis_gimbal_shooter_cover_manual.h

@@ -33,6 +33,7 @@ class ChassisGimbalShooterCoverManual : public ChassisGimbalShooterManual
   void zRelease();
   void wPress() override;
   void wPressing() override;
+  void mouseRightPress() override;
 
   virtual void ctrlZPress();
   virtual void ctrlZRelease()

include/rm_manual/chassis_gimbal_shooter_manual.h

@@ -60,7 +60,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
     shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
     prepare_shoot_ = true;
   }
-  void mouseRightPress();
+  virtual void mouseRightPress();
   void mouseRightRelease()
   {
     gimbal_cmd_sender_->setMode(rm_msgs::GimbalCmd::RATE);
@@ -108,6 +108,7 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   void ctrlRPress();
   void ctrlRRelease();
   virtual void ctrlQPress();
+  void eventDataCallback(const rm_msgs::EventData ::ConstPtr& data) override;
 
   InputEvent self_inspection_event_, game_start_event_, e_event_, c_event_, g_event_, q_event_, b_event_, x_event_,
       r_event_, v_event_, ctrl_f_event_, ctrl_v_event_, ctrl_b_event_, ctrl_q_event_, ctrl_r_event_, shift_event_,
@@ -124,7 +125,18 @@ class ChassisGimbalShooterManual : public ChassisGimbalManual
   geometry_msgs::PointStamped point_out_;
   uint8_t last_shoot_freq_{};
 
-  bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, adjust_image_transmission_ = false;
+  enum TargetHitByDart
+  {
+    OUTPOST = 1,
+    BASE = 2,
+    MOVE_BASE = 3
+  };
+
+  ros::Time start_timer_time_{};
+
+  bool prepare_shoot_ = false, is_balance_ = false, use_scope_ = false, is_auto_ = false,
+       adjust_image_transmission_ = false;
   double yaw_current_{};
+  int count_{}, last_time_hit_by_dart_{};
 };
 }  // namespace rm_manual

include/rm_manual/manual_base.h

@@ -27,6 +27,7 @@
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/GameStatus.h>
 #include <rm_msgs/GameRobotHp.h>
+#include <rm_msgs/EventData.h>
 #include <rm_msgs/BalanceState.h>
 #include <rm_msgs/PowerHeatData.h>
 #include <rm_msgs/ActuatorState.h>
@@ -76,6 +77,9 @@ class ManualBase
   virtual void shootBeforehandCmdCallback(const rm_msgs::ShootBeforehandCmd ::ConstPtr& data)
   {
   }
+  virtual void eventDataCallback(const rm_msgs::EventData ::ConstPtr& data)
+  {
+  }
   virtual void odomCallback(const nav_msgs::Odometry::ConstPtr& data)
   {
   }
@@ -145,7 +149,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_, shoot_data_sub_;
+      suggest_fire_sub_, shoot_beforehand_cmd_sub_, shoot_data_sub_, event_data_sub_;
 
   sensor_msgs::JointState joint_state_;
   rm_msgs::TrackData track_data_;

src/chassis_gimbal_shooter_cover_manual.cpp

@@ -211,4 +211,32 @@ void ChassisGimbalShooterCoverManual::ctrlZPress()
   }
 }
 
+void ChassisGimbalShooterCoverManual::mouseRightPress()
+{
+  ChassisGimbalShooterManual::mouseRightPress();
+  if (is_auto_)
+  {
+    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
+    {
+      shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::PUSH);
+      shooter_cmd_sender_->checkError(ros::Time::now());
+    }
+  }
+  else if (!mouse_left_event_.getState() && shooter_cmd_sender_->getMsg()->mode == rm_msgs::ShootCmd::PUSH)
+    shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::READY);
+}
 }  // namespace rm_manual

src/chassis_gimbal_shooter_manual.cpp

@@ -198,6 +198,7 @@ void ChassisGimbalShooterManual::remoteControlTurnOff()
   shooter_calibration_->stop();
   gimbal_calibration_->stop();
   use_scope_ = false;
+  is_auto_ = false;
   adjust_image_transmission_ = false;
 }
 
@@ -215,6 +216,7 @@ void ChassisGimbalShooterManual::robotDie()
   ManualBase::robotDie();
   shooter_cmd_sender_->setMode(rm_msgs::ShootCmd::STOP);
   use_scope_ = false;
+  is_auto_ = false;
   adjust_image_transmission_ = false;
 }
 
@@ -628,4 +630,20 @@ void ChassisGimbalShooterManual::ctrlQPress()
   gimbal_calibration_->reset();
 }
 
+void ChassisGimbalShooterManual::eventDataCallback(const rm_msgs::EventData::ConstPtr& data)
+{
+  ChassisGimbalManual::eventDataCallback(data);
+  if (data->be_hit_time != last_time_hit_by_dart_)
+  {
+    last_time_hit_by_dart_ = data->be_hit_time;
+    start_timer_time_ = ros::Time::now();
+    count_ = 0;
+  }
+  if ((data->be_hit_target == OUTPOST && (ros::Time::now() - start_timer_time_).toSec() <= 5) ||
+      (data->be_hit_target == BASE && (ros::Time::now() - start_timer_time_).toSec() <= 10) ||
+      (data->be_hit_target == MOVE_BASE && (ros::Time::now() - start_timer_time_).toSec() <= 15))
+    is_auto_ = true;
+  else
+    is_auto_ = false;
+}
 }  // namespace rm_manual

src/manual_base.cpp

@@ -33,6 +33,7 @@ ManualBase::ManualBase(ros::NodeHandle& nh, ros::NodeHandle& nh_referee)
       "power_management/sample_and_status", 10, &ManualBase::capacityDataCallback, this);
   power_heat_data_sub_ =
       nh_referee.subscribe<rm_msgs::PowerHeatData>("power_heat_data", 10, &ManualBase::powerHeatDataCallback, this);
+  event_data_sub_ = nh_referee.subscribe<rm_msgs::EventData>("event_data", 10, &ManualBase::eventDataCallback, this);
   shoot_data_sub_ = nh_referee.subscribe<rm_msgs::ShootData>("shoot_data", 10, &ManualBase::shootDataCallback, this);
 
   // pub