diff --git a/include/mrs_uav_state_estimators/estimators/state/passthrough.h b/include/mrs_uav_state_estimators/estimators/state/passthrough.h index 08b9a1a..9e8811e 100644 --- a/include/mrs_uav_state_estimators/estimators/state/passthrough.h +++ b/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 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); diff --git a/src/estimators/state/passthrough.cpp b/src/estimators/state/passthrough.cpp index 64fc5ef..8f81784 100644 --- a/src/estimators/state/passthrough.cpp +++ b/src/estimators/state/passthrough.cpp @@ -33,6 +33,7 @@ void Passthrough::initialize(ros::NodeHandle &parent_nh, const std::shared_ptrparam_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