Publish fly time.

rm-controls · Apr 12, 2024 · 393241c · 393241c
1 parent 96cb624
commit 393241c
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 1 deletion.
diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h
@@ -47,7 +47,7 @@
 #include <rm_common/eigen_types.h>
 #include <rm_common/ros_utilities.h>
 #include <std_msgs/Bool.h>
-#include <rm_msgs/GimbalDesError.h>
+#include <std_msgs/Float64.h>
 #include <rm_msgs/TrackData.h>
 #include <rm_msgs/ShootBeforehandCmd.h>
 
@@ -90,6 +90,7 @@ class BulletSolver
   std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_desire_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<visualization_msgs::Marker>> path_real_pub_;
   std::shared_ptr<realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>> shoot_beforehand_cmd_pub_;
+  std::shared_ptr<realtime_tools::RealtimePublisher<std_msgs::Float64>> fly_time_pub_;
   ros::Subscriber identified_target_change_sub_;
   ros::Time switch_armor_time_{};
   realtime_tools::RealtimeBuffer<Config> config_rt_buffer_;

diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp
@@ -86,6 +86,7 @@ BulletSolver::BulletSolver(ros::NodeHandle& controller_nh)
       new realtime_tools::RealtimePublisher<visualization_msgs::Marker>(controller_nh, "model_real", 10));
   shoot_beforehand_cmd_pub_.reset(
       new realtime_tools::RealtimePublisher<rm_msgs::ShootBeforehandCmd>(controller_nh, "shoot_beforehand_cmd", 10));
+  fly_time_pub_.reset(new realtime_tools::RealtimePublisher<std_msgs::Float64>(controller_nh, "fly_time", 10));
 
   identified_target_change_sub_ = controller_nh.subscribe<std_msgs::Bool>(
       "/armor_processor/change", 10, &BulletSolver::identifiedTargetChangeCB, this);
@@ -202,6 +203,11 @@ bool BulletSolver::solve(geometry_msgs::Point pos, geometry_msgs::Vector3 vel, d
     if (count >= 20 || std::isnan(error))
       return false;
   }
+  if (fly_time_pub_->trylock())
+  {
+    fly_time_pub_->msg_.data = fly_time_;
+    fly_time_pub_->unlockAndPublish();
+  }
   return true;
 }