Can play rotating skirmish at different heights.

cfg/Forecast.cfg

@@ -10,22 +10,27 @@ gen.add("max_match_distance", double_t, 0, "", 0.2, 0, 2)
 gen.add("tracking_threshold", int_t, 0, "", 5, 0, 15)
 gen.add("lost_threshold", int_t, 0, "", 5, 0, 15)
 
-gen.add("max_jump_angle", double_t, 0, "", 0.2, 0, 2)
+gen.add("max_jump_angle", double_t, 0, "", 0.02, 0, 2)
 gen.add("max_jump_period", double_t, 0, "", 0.8, 0, 2)
-gen.add("allow_following_range", double_t, 0, "", 0.3, 0, 2)
+gen.add("allow_following_range", double_t, 0, "", 0.5, 0, 2)
 
 gen.add("forecast_readied", bool_t, 0, "", True)
 gen.add("reset", bool_t, 0, "", False)
-gen.add("min_target_quantity", int_t, 0, "", 5, 0, 1500)
-gen.add("line_speed", double_t, 0, "", 0.8, 0, 2)
-gen.add("z_c", double_t, 0, "", 0.6, 0, 2)
+gen.add("min_target_quantity", int_t, 0, "", 3, 0, 1500)
+gen.add("const_distance", double_t, 0, "", 7.3, 0, 10)
 gen.add("outpost_radius", double_t, 0, "", 0.35, 0, 2)
 gen.add("rotate_speed", double_t, 0, "", 0.5, 0, 2)
-gen.add("y_thred", double_t, 0, "", 0.03, 0, 2)
+gen.add("y_thred", double_t, 0, "", 0.1, 0, 2)
 gen.add("time_thred", double_t, 0, "", 0.01, 0, 2)
-gen.add("time_offset", double_t, 0, "", 0.53, 0, 2)
+gen.add("time_offset", double_t, 0, "", 0.98, 0, 2)
+gen.add("ramp_time_offset",double_t,0,"", 0.98, 0,2)
+gen.add("ramp_threshold",double_t,0,"", 0.05, 0,1.57)
+
+
+gen.add("ring_highland_distance_offset",double_t,0,"",0,-10,10)
+gen.add("source_island_distance_offset",double_t,0,"",0,-10,10)
 
 # gen.add("is_filter_readied", bool_t, 0, "", False)
 # gen.add("min_lw_ratio", double_t, 0, "", 2.4, 0, 4)
 
-exit(gen.generate(PACKAGE, "rm_forecast", "Forecast"))
+exit(gen.generate(PACKAGE, "rm_forecast", "Forecast"))

config/forecast.yaml

@@ -1,13 +1,38 @@
 max_match_distance: 0.2
 tracking_threshold: 5
 lost_threshold: 5
-max_jump_angle: 0.05
+max_jump_angle: 0.02
 max_jump_period: 0.8
-allow_following_range: 0.3
-y_thred: 0.03
+allow_following_range: 0.5
+y_thred: 0.1
 time_thred: 0.01
+time_offset: 0.9
+ramp_time_offset: 1.05
+ramp_threshold: 0.05
+min_target_quantity: 6
+const_distance: 7.3
+outpost_radius: 0.3
+rotate_speed: 0.1
+ring_highland_distance_offset: -0.18
+source_island_distance_offset: -0.35
+forecast_readied: true
+reset: false
 interpolation_fly_time:
   - [ 3.5,0.40 ]
   - [ 4.0,0.366666 ]
   - [ 4.5,0.337 ]
   - [ 5.0,0.30 ]
+interpolation_base_distance_on_ring_highland:
+  - [ 7.128,6.2 ]
+  - [ 7.43,6.5 ]
+  - [ 7.67,6.65 ]
+  - [ 7.72,6.75 ]
+interpolation_base_distance_on_resource_island:
+  #  for vedio
+  - [ 8.985, 6.73 ]
+  - [ 9.225,6.95 ]
+  - [ 9.88,7.2 ]
+
+#  for real
+#  - [ 9.22,7.0 ]
+#  - [ 9.9,7.3 ]

include/forecast_node.h

@@ -6,7 +6,10 @@
 
 #include "kalman_filter.h"
 #include "rm_common/linear_interpolation.h"
+#include <rm_common/filters/filters.h>
 #include "rm_common/ori_tool.h"
+#include <rm_common/tf_rt_broadcaster.h>
+#include "rm_common/ros_utilities.h"
 #include "spin_observer.h"
 #include "tracker.h"
 #include <Eigen/Core>
@@ -33,6 +36,7 @@
 #include <tf2_ros/transform_listener.h>
 #include <thread>
 #include <vector>
+#include <realtime_tools/realtime_buffer.h>
 
 using namespace std;
 namespace rm_forecast
@@ -44,10 +48,18 @@ static double max_jump_angle_{};
 static double max_jump_period_{};
 static double allow_following_range_{};
 
+struct Config
+{
+  double const_distance, outpost_radius, rotate_speed, y_thred, time_thred, time_offset, ramp_time_offset,
+      ramp_threshold, ring_highland_distance_offset, source_island_distance_offset;
+  int min_target_quantity;
+  bool forecast_readied, reset;
+};
+
 class Forecast_Node : public nodelet::Nodelet
 {
 public:
-  Forecast_Node() = default;
+  Forecast_Node() : detection_filter_(100), track_filter_(20){};
   ~Forecast_Node() override
   {
     if (this->my_thread_.joinable())
@@ -74,6 +86,7 @@ class Forecast_Node : public nodelet::Nodelet
   // Last time received msg
   ros::Time last_time_;
   ros::Time last_min_time_;
+  ros::Time temp_min_time_;
 
   // Initial KF matrices
   KalmanFilterMatrices kf_matrices_;
@@ -97,23 +110,33 @@ class Forecast_Node : public nodelet::Nodelet
                                           const rm_msgs::TargetDetection point_3,
                                           const rm_msgs::TargetDetection point_4);
 
-  bool forecast_readied_ = true;
-  int armor_type_ = 1, min_target_quantity_ = 5, target_quantity_ = 0;
-  double time_offset_ = 0.53;
-  double time_thred_ = 0.01;
-  double y_thred_ = 0.05;
-  double min_distance_x_, min_distance_y_, min_distance_z_;
-  double fly_time_{}, bullet_solver_fly_time_{};
+  int armor_type_ = 0, target_quantity_ = 0;
+  double min_distance_x_{}, min_distance_y_{}, min_distance_z_{}, temp_min_distance_x_{}, temp_min_distance_y_{},
+      temp_min_distance_z_{};
+  double fly_time_{}, bullet_solver_fly_time_{}, pitch_enter_time_{}, last_pitch_time_{};
 
   rm_msgs::TargetDetectionArray max_x_target_;
   rm_msgs::TargetDetectionArray min_x_target_;
   rm_msgs::TargetDetectionArray min_distance_target_;
 
+  double min_camera_distance_;
   rm_common::LinearInterp interpolation_fly_time_;
+  rm_common::LinearInterp interpolation_base_distance_on_ring_highland_;
+  rm_common::LinearInterp interpolation_base_distance_on_resource_island_;
+  rm_common::TfRtBroadcaster tf_broadcaster_;
+
+  Vector3WithFilter<double> track_filter_;
+  MovingAverageFilter<double> detection_filter_;
 
   ros::ServiceServer status_change_srv_;
 
   std::thread my_thread_;
   image_transport::Publisher image_pub_;
+
+  Config config_{};
+  realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;
+  bool dynamic_reconfig_initialized_ = false;
+
+  geometry_msgs::Point target_;
 };
 }  // namespace rm_forecast

include/spin_observer.h

@@ -18,6 +18,7 @@
 #include <rm_msgs/TargetDetection.h>
 #include <rm_msgs/TargetDetectionArray.h>
 #include <rm_msgs/TrackData.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #include <ros/ros.h>
 #include <thread>
 #include <vector>
@@ -30,7 +31,7 @@ class SpinObserver
 public:
     SpinObserver();
 
-    void update(rm_msgs::TrackData & target_msg, ros::Time &current_time, double &max_jump_angle, double &max_jump_period, double &allow_following_range);
+    void update(rm_msgs::TrackData & target_msg, geometry_msgs::TransformStamped & odom2pitch, ros::Time &current_time, double &max_jump_angle, double &max_jump_period, double &allow_following_range);
 
 //    auto_aim_interfaces::msg::SpinInfo spin_info_msg;
     rm_msgs::TrackData spin_track_data;