Merge branch 'master' into traj_1

rm-controls · Jul 19, 2024 · b296a79 · b296a79
2 parents 04509d2 + 4f9a8a4
commit b296a79
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 4 deletions.
diff --git a/rm_common/include/rm_common/decision/command_sender.h b/rm_common/include/rm_common/decision/command_sender.h
@@ -263,6 +263,10 @@ class ChassisCommandSender : public TimeStampCommandSenderBase<rm_msgs::ChassisC
   {
     power_limit_->setRefereeStatus(status);
   }
+  void setFollowVelDes(double follow_vel_des)
+  {
+    msg_.follow_vel_des = follow_vel_des;
+  }
   void sendChassisCommand(const ros::Time& time, bool is_gyro)
   {
     power_limit_->setLimitPower(msg_, is_gyro);
@@ -357,13 +361,14 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
     nh.getParam("wheel_speed_18", wheel_speed_18_);
     nh.getParam("wheel_speed_30", wheel_speed_30_);
     nh.param("extra_wheel_speed_once", extra_wheel_speed_once_, 0.);
-    if (!nh.getParam("gimbal_error_tolerance", gimbal_error_tolerance_))
-      ROS_ERROR("gimbal error tolerance no defined (namespace: %s)", nh.getNamespace().c_str());
     if (!nh.getParam("target_acceleration_tolerance", target_acceleration_tolerance_))
     {
       target_acceleration_tolerance_ = 0.;
       ROS_INFO("target_acceleration_tolerance no defined(namespace: %s), set to zero.", nh.getNamespace().c_str());
     }
+    if (!nh.getParam("track_armor_error_tolerance", track_armor_error_tolerance_))
+      ROS_ERROR("track armor error tolerance no defined (namespace: %s)", nh.getNamespace().c_str());
+    nh.param("track_buff_error_tolerance", track_buff_error_tolerance_, track_armor_error_tolerance_);
   }
   ~ShooterCommandSender()
   {
@@ -410,7 +415,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
         return;
       }
     }
-    if (((gimbal_des_error_.error > gimbal_error_tolerance_ && time - gimbal_des_error_.stamp < ros::Duration(0.1)) ||
+    double gimbal_error_tolerance = track_data_.id == 12 ? track_buff_error_tolerance_ : track_armor_error_tolerance_;
+    if (((gimbal_des_error_.error > gimbal_error_tolerance && time - gimbal_des_error_.stamp < ros::Duration(0.1)) ||
          (track_data_.accel > target_acceleration_tolerance_)) ||
         (!suggest_fire_.data && armor_type_ == rm_msgs::StatusChangeRequest::ARMOR_OUTPOST_BASE))
       if (msg_.mode == rm_msgs::ShootCmd::PUSH)
@@ -495,7 +501,8 @@ class ShooterCommandSender : public TimeStampCommandSenderBase<rm_msgs::ShootCmd
   double speed_10_{}, speed_15_{}, speed_16_{}, speed_18_{}, speed_30_{}, speed_des_{};
   double wheel_speed_10_{}, wheel_speed_15_{}, wheel_speed_16_{}, wheel_speed_18_{}, wheel_speed_30_{},
       wheel_speed_des_{};
-  double gimbal_error_tolerance_{};
+  double track_armor_error_tolerance_{};
+  double track_buff_error_tolerance_{};
   double target_acceleration_tolerance_{};
   double extra_wheel_speed_once_{};
   double total_extra_wheel_speed_{};
@@ -834,6 +841,11 @@ class DoubleBarrelCommandSender
     shooter_ID1_cmd_sender_->updateSuggestFireData(data);
     shooter_ID2_cmd_sender_->updateSuggestFireData(data);
   }
+  void updateShootBeforehandCmd(const rm_msgs::ShootBeforehandCmd& data)
+  {
+    shooter_ID1_cmd_sender_->updateShootBeforehandCmd(data);
+    shooter_ID2_cmd_sender_->updateShootBeforehandCmd(data);
+  }
 
   void setMode(int mode)
   {

diff --git a/rm_msgs/msg/ChassisCmd.msg b/rm_msgs/msg/ChassisCmd.msg
@@ -7,6 +7,7 @@ uint8 FALLEN = 4
 uint8 mode
 geometry_msgs/Accel accel
 float64 power_limit
+float64 follow_vel_des
 string follow_source_frame
 string command_source_frame
 time stamp