diff --git a/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h b/rm_gimbal_controllers/include/rm_gimbal_controllers/bullet_solver.h index 261151b9..33641784 100644 --- 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 #include #include -#include +#include #include #include @@ -90,6 +90,7 @@ class BulletSolver std::shared_ptr> path_desire_pub_; std::shared_ptr> path_real_pub_; std::shared_ptr> shoot_beforehand_cmd_pub_; + std::shared_ptr> fly_time_pub_; ros::Subscriber identified_target_change_sub_; ros::Time switch_armor_time_{}; realtime_tools::RealtimeBuffer config_rt_buffer_; diff --git a/rm_gimbal_controllers/src/bullet_solver.cpp b/rm_gimbal_controllers/src/bullet_solver.cpp index e4057828..b9226e5c 100644 --- 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(controller_nh, "model_real", 10)); shoot_beforehand_cmd_pub_.reset( new realtime_tools::RealtimePublisher(controller_nh, "shoot_beforehand_cmd", 10)); + fly_time_pub_.reset(new realtime_tools::RealtimePublisher(controller_nh, "fly_time", 10)); identified_target_change_sub_ = controller_nh.subscribe( "/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; }