diff --git a/include/mrs_uav_state_estimators/estimators/state/passthrough.h b/include/mrs_uav_state_estimators/estimators/state/passthrough.h index ae293da..08b9a1a 100644 --- a/include/mrs_uav_state_estimators/estimators/state/passthrough.h +++ b/include/mrs_uav_state_estimators/estimators/state/passthrough.h @@ -37,6 +37,7 @@ 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"; @@ -48,15 +49,20 @@ class Passthrough : public mrs_uav_managers::StateEstimator { const bool is_core_plugin_; mrs_lib::SubscribeHandler sh_passthrough_odom_; + void callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg); double _critical_timeout_passthrough_odom_; std::string msg_topic_; + ros::Timer timer_check_passthrough_odom_hz_; + void timerCheckPassthroughOdomHz(const ros::TimerEvent &event); + std::atomic counter_odom_msgs_ = 0; + ros::WallTime t_check_hz_last_; + double prev_avg_hz_ = 0; + ros::Timer timer_update_; void timerUpdate(const ros::TimerEvent &event); nav_msgs::OdometryConstPtr prev_msg_; bool first_iter_ = true; - ros::Timer timer_check_health_; - void timerCheckHealth(const ros::TimerEvent &event); bool isConverged(); @@ -69,7 +75,7 @@ class Passthrough : public mrs_uav_managers::StateEstimator { ~Passthrough(void) { } - void initialize(ros::NodeHandle &nh, const std::shared_ptr &ch, const std::shared_ptr &ph) override; + void initialize(ros::NodeHandle &parent_nh, const std::shared_ptr &ch, const std::shared_ptr &ph) override; bool start(void) override; bool pause(void) override; bool reset(void) override; diff --git a/src/estimators/state/passthrough.cpp b/src/estimators/state/passthrough.cpp index 35a02f1..64fc5ef 100644 --- a/src/estimators/state/passthrough.cpp +++ b/src/estimators/state/passthrough.cpp @@ -11,32 +11,30 @@ namespace passthrough { /* initialize() //{*/ -void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr &ch, const std::shared_ptr &ph) { +void Passthrough::initialize(ros::NodeHandle &parent_nh, const std::shared_ptr &ch, const std::shared_ptr &ph) { ch_ = ch; ph_ = ph; + ros::NodeHandle nh(parent_nh); + ns_frame_id_ = ch_->uav_name + "/" + frame_id_; // | --------------- param loader initialization -------------- | if (is_core_plugin_) { - ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); - ph->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); + ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/private/" + getName() + "/" + getName() + ".yaml"); + ph_->param_loader->addYamlFile(ros::package::getPath(package_name_) + "/config/public/" + getName() + "/" + getName() + ".yaml"); } - ph->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/"); + ph_->param_loader->setPrefix(ch_->package_name + "/" + Support::toSnakeCase(ch_->nodelet_name) + "/" + getName() + "/"); // | --------------------- load parameters -------------------- | - ph->param_loader->loadParam("max_flight_z", max_flight_z_); - ph->param_loader->loadParam("message/topic", msg_topic_); + ph_->param_loader->loadParam("max_flight_z", max_flight_z_); + ph_->param_loader->loadParam("message/topic", msg_topic_); msg_topic_ = "/" + ch_->uav_name + "/" + msg_topic_; - // | ------------------ timers initialization ----------------- | - timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this, false, false); // not running after init - timer_check_health_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerCheckHealth, this); - // | --------------- subscribers initialization --------------- | mrs_lib::SubscribeHandlerOptions shopts; shopts.nh = nh; @@ -47,7 +45,12 @@ void Passthrough::initialize(ros::NodeHandle &nh, const std::shared_ptr(shopts, msg_topic_); + sh_passthrough_odom_ = mrs_lib::SubscribeHandler(shopts, msg_topic_, &Passthrough::callbackPassthroughOdom, this); + + // | ------------------ timers initialization ----------------- | + t_check_hz_last_ = ros::WallTime::now(); + prev_avg_hz_ = 0; + timer_check_passthrough_odom_hz_ = nh.createTimer(ros::Rate(1.0), &Passthrough::timerCheckPassthroughOdomHz, this); // | ---------------- publishers initialization --------------- | ph_odom_ = mrs_lib::PublisherHandler(nh, Support::toSnakeCase(getName()) + "/odom", 10); // needed for tf @@ -93,7 +96,7 @@ bool Passthrough::start(void) { if (isInState(READY_STATE)) { - timer_update_.start(); + /* timer_update_.start(); */ changeState(STARTED_STATE); return true; @@ -135,12 +138,65 @@ bool Passthrough::reset(void) { } /*//}*/ +/* timerCheckPassthroughOdomHz() //{*/ +void Passthrough::timerCheckPassthroughOdomHz([[maybe_unused]] const ros::TimerEvent &event) { + + if (!isInitialized()) { + return; + } + + if (isInState(INITIALIZED_STATE)) { + + // first wait for a message + if (!sh_passthrough_odom_.hasMsg()) { + ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_passthrough_odom_.topicName().c_str()); + return; + } + + // calculate average rate of messages + if (counter_odom_msgs_ > 0) { + ros::WallTime t_now = ros::WallTime::now(); + double dt = (t_now - t_check_hz_last_).toSec(); + double avg_hz = counter_odom_msgs_ / dt; + t_check_hz_last_ = t_now; + counter_odom_msgs_ = 0; + ROS_INFO("[%s]: rate of passthrough odom: %.2f Hz", getPrintName().c_str(), avg_hz); + + // check message rate stability + if (abs(avg_hz - prev_avg_hz_) >= 5) { + ROS_INFO("[%s]: %s stable passthrough odometry rate. now: %.2f Hz prev: %.2f Hz", getPrintName().c_str(), Support::waiting_for_string.c_str(), avg_hz, + prev_avg_hz_); + prev_avg_hz_ = avg_hz; + return; + } + + // the message rate must be higher than required by the control manager + if (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 " + "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 + // the states would then be interpolated by ZOH (bleeh) + /* timer_update_ = nh.createTimer(ros::Rate(ch_->desired_uav_state_rate), &Passthrough::timerUpdate, this); */ + return; + } + } + + changeState(READY_STATE); + ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str()); + timer_check_passthrough_odom_hz_.stop(); + } +} +/*//}*/ + /* timerUpdate() //{*/ void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) { - if (!isInitialized()) { - return; + if (isInState(STARTED_STATE)) { + + changeState(RUNNING_STATE); } const ros::Time time_now = ros::Time::now(); @@ -202,29 +258,81 @@ void Passthrough::timerUpdate([[maybe_unused]] const ros::TimerEvent &event) { } /*//}*/ -/*//{ timerCheckHealth() */ -void Passthrough::timerCheckHealth([[maybe_unused]] const ros::TimerEvent &event) { +/*//{ callbackPassthroughOdom() */ +void Passthrough::callbackPassthroughOdom(const nav_msgs::Odometry::ConstPtr msg) { + + counter_odom_msgs_++; if (!isInitialized()) { return; } - if (isInState(INITIALIZED_STATE)) { + if (isInState(STARTED_STATE)) { - if (sh_passthrough_odom_.hasMsg()) { - changeState(READY_STATE); - ROS_INFO_THROTTLE(1.0, "[%s]: Estimator is ready to start", getPrintName().c_str()); - } else { - ROS_INFO_THROTTLE(1.0, "[%s]: %s msg on topic %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), sh_passthrough_odom_.topicName().c_str()); - return; - } + changeState(RUNNING_STATE); } + if (first_iter_) { + prev_msg_ = msg; + first_iter_ = false; + } - if (isInState(STARTED_STATE)) { + nav_msgs::Odometry odom = *msg; - changeState(RUNNING_STATE); - } + // change the frame_ids + odom.header.frame_id = ns_frame_id_; + odom.child_frame_id = ch_->frames.ns_fcu; + + mrs_msgs::UavState uav_state = uav_state_init_; + + // here we are assuming the passthrough odometry has correct timestamp + uav_state.header.stamp = odom.header.stamp; + /* const ros::Time time_now = ros::Time::now(); */ + /* uav_state.header.stamp = time_now; */ + + uav_state.pose.position = odom.pose.pose.position; + uav_state.pose.orientation = odom.pose.pose.orientation; + uav_state.velocity.angular = odom.twist.twist.angular; + + // uav_state has velocities in the parent frame + uav_state.velocity.linear = Support::rotateVector(odom.twist.twist.linear, odom.pose.pose.orientation); + + + nav_msgs::Odometry innovation = innovation_init_; + innovation.header.stamp = odom.header.stamp; + + innovation.pose.pose.position.x = prev_msg_->pose.pose.position.x - msg->pose.pose.position.x; + innovation.pose.pose.position.y = prev_msg_->pose.pose.position.y - msg->pose.pose.position.y; + innovation.pose.pose.position.z = prev_msg_->pose.pose.position.z - msg->pose.pose.position.z; + + mrs_msgs::Float64ArrayStamped pose_covariance, twist_covariance; + pose_covariance.header.stamp = odom.header.stamp; + twist_covariance.header.stamp = odom.header.stamp; + + const int n_states = 6; // TODO this should be defined somewhere else + pose_covariance.values.resize(n_states * n_states); + pose_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10; + pose_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10; + pose_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10; + + twist_covariance.values.resize(n_states * n_states); + twist_covariance.values.at(n_states * AXIS_X + AXIS_X) = 1e-10; + twist_covariance.values.at(n_states * AXIS_Y + AXIS_Y) = 1e-10; + twist_covariance.values.at(n_states * AXIS_Z + AXIS_Z) = 1e-10; + + mrs_lib::set_mutexed(mtx_uav_state_, uav_state, uav_state_); + mrs_lib::set_mutexed(mtx_odom_, odom, odom_); + mrs_lib::set_mutexed(mtx_innovation_, innovation, innovation_); + mrs_lib::set_mutexed(mtx_covariance_, pose_covariance, pose_covariance_); + mrs_lib::set_mutexed(mtx_covariance_, twist_covariance, twist_covariance_); + + publishUavState(); + publishOdom(); + publishCovariance(); + publishInnovation(); + publishDiagnostics(); + + prev_msg_ = msg; } /*//}*/