passthrough - kickoff compatibility fix

include/mrs_uav_state_estimators/estimators/state/passthrough.h

@@ -37,7 +37,6 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
   using CommonHandlers_t = mrs_uav_managers::estimation_manager::CommonHandlers_t;
 
 private:
-
   const std::string package_name_ = "mrs_uav_state_estimators";
 
   const std::string est_lat_name_ = "lat_passthrough";
@@ -58,6 +57,7 @@ class Passthrough : public mrs_uav_managers::StateEstimator {
   std::atomic<int> counter_odom_msgs_ = 0;
   ros::WallTime    t_check_hz_last_;
   double           prev_avg_hz_ = 0;
+  bool             kickoff_     = false;
 
   ros::Timer                 timer_update_;
   void                       timerUpdate(const ros::TimerEvent &event);

src/estimators/state/passthrough.cpp

@@ -33,6 +33,7 @@ void Passthrough::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr<C
   // | --------------------- load parameters -------------------- |
   ph_->param_loader->loadParam("max_flight_z", max_flight_z_);
   ph_->param_loader->loadParam("message/topic", msg_topic_);
+  ph_->param_loader->loadParam("kickoff", kickoff_, false);
   msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_;
 
   // | --------------- subscribers initialization --------------- |
@@ -171,9 +172,9 @@ void Passthrough::timerCheckPassthroughOdomHz([[maybe_unused]] const ros::TimerE
       }
 
       // the message rate must be higher than required by the control manager
-      if (avg_hz < ch_->desired_uav_state_rate) {
+      if (!kickoff_ && avg_hz < ch_->desired_uav_state_rate) {
         ROS_ERROR(
-            "[%s]: rate of passthrough odom: %.2f Hz lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
+            "[%s]: rate of passthrough odom: %.2f Hz is lower than desired uav_state rate: %.2f Hz. Flight not allowed. Provide higher passthrough odometry rate "
             "or use a higher-level controller.",
             getPrintName().c_str(), avg_hz, ch_->desired_uav_state_rate);
         // note: might run the publishing asynchronously on the desired rate in this case to still be able to fly