diff --git a/config/public/aloam/alt_aloam.yaml b/config/public/aloam/alt_aloam.yaml index 5550903..799abe0 100644 --- a/config/public/aloam/alt_aloam.yaml +++ b/config/public/aloam/alt_aloam.yaml @@ -25,7 +25,7 @@ mrs_uav_managers: pos_aloam: state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.001 # measurement noise covariance (R) + noise: 0.1 # measurement noise covariance (R) noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) message: type: "nav_msgs/Odometry" diff --git a/config/public/aloam/hdg_aloam.yaml b/config/public/aloam/hdg_aloam.yaml index 9b7cb81..95f611d 100644 --- a/config/public/aloam/hdg_aloam.yaml +++ b/config/public/aloam/hdg_aloam.yaml @@ -16,12 +16,13 @@ mrs_uav_managers: vel: 0.1 # velocity state corrections: [ - "hdg_aloam" + "hdg_aloam", + "hdg_rate_gyro" ] hdg_aloam: state_id: 0 # 0 - position, 1 - velocity, 2 - acceleration - noise: 0.0001 # measurement noise covariance (R) + noise: 0.01 # measurement noise covariance (R) noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) message: type: "nav_msgs/Odometry" @@ -31,3 +32,17 @@ mrs_uav_managers: time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy processors: [] # types of processors attached to this measurement + + hdg_rate_gyro: + state_id: 1 # 0 - position, 1 - velocity, 2 - acceleration + noise: 0.1 # measurement noise covariance (R) + noise_unhealthy_coeff: 100.0 # covariance gets multiplied by this coefficient when correction is unhealthy (R) + message: + type: "geometry_msgs/Vector3Stamped" + topic: "hw_api/angular_velocity" # without uav namespace + orientation_topic: "hw_api/orientation" # without uav namespace + limit: + delay: 2.0 # [s] messages with higher delay will flag correction as unhealthy + time_since_last: 1.0 # [s] larger time step between messages will flag correction as unhealthy + + processors: [] # types of processors attached to this measurement diff --git a/include/mrs_uav_state_estimators/estimators/correction.h b/include/mrs_uav_state_estimators/estimators/correction.h index d797ee1..4e4a437 100644 --- a/include/mrs_uav_state_estimators/estimators/correction.h +++ b/include/mrs_uav_state_estimators/estimators/correction.h @@ -146,6 +146,9 @@ class Correction { std::optional getCorrectionFromVector(const geometry_msgs::Vector3StampedConstPtr msg); void callbackVector(const geometry_msgs::Vector3Stamped::ConstPtr msg); + mrs_lib::SubscribeHandler sh_orientation_; // for obtaining heading rate + std::string orientation_topic_; + mrs_lib::SubscribeHandler sh_quat_; measurement_t prev_hdg_measurement_; bool got_first_hdg_measurement_ = false; @@ -257,7 +260,8 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e ROS_ERROR("[%s]: wrong state id: %d of correction %s", getPrintName().c_str(), state_id_tmp, getName().c_str()); ros::shutdown(); } - if (state_id_ == VELOCITY) { + + if (state_id_ == StateId_t::VELOCITY) { ph->param_loader->loadParam("body_frame", is_in_body_frame_, true); } @@ -272,11 +276,6 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e processors_[proc_name] = createProcessorFromName(proc_name, nh); } - if (!ph->param_loader->loadedSuccessfully()) { - ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str()); - ros::shutdown(); - } - // | ------------- initialize dynamic reconfigure ------------- | drmgr_ = std::make_unique(ros::NodeHandle("~/" + getNamespacedName()), getPrintName()); drmgr_->config.noise = R_; @@ -339,6 +338,14 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e } } + // | ------ subscribe orientation for obtaingin hdg rate ------ | + if (est_type_ == EstimatorType_t::HEADING && state_id_ == StateId_t::VELOCITY) { + ph->param_loader->loadParam("message/orientation_topic", orientation_topic_); + orientation_topic_ = "/" + ch_->uav_name + "/" + orientation_topic_; + sh_orientation_ = mrs_lib::SubscribeHandler(shopts, orientation_topic_); + } + + // | --------------- initialize publish handlers -------------- | if (ch_->debug_topics.correction) { ph_correction_raw_ = mrs_lib::PublisherHandler(nh, est_name_ + "/correction/" + getName() + "/raw", 10); @@ -348,6 +355,12 @@ Correction::Correction(ros::NodeHandle& nh, const std::string& e ph_delay_ = mrs_lib::PublisherHandler(nh, est_name_ + "/correction/" + getName() + "/delay", 10); } + // | --- check whether all parameters were loaded correctly --- | + if (!ph->param_loader->loadedSuccessfully()) { + ROS_ERROR("[%s]: Could not load all non-optional parameters. Shutting down.", getPrintName().c_str()); + ros::shutdown(); + } + healthy_time_ = ros::Time(0); is_initialized_ = true; @@ -1204,7 +1217,7 @@ std::optional::measurement_t> Correction::measurement_t> Correction::measurement_t> Correctionpose.pose.orientation).getHeadingRate(msg->twist.twist.angular); */ - /* return measurement; */ - /* } */ - /* catch (...) { */ - /* ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromOdometry())", getPrintName().c_str()); */ - /* return {}; */ - /* } */ - /* break; */ - /* } */ + case StateId_t::VELOCITY: { + try { + if (!sh_orientation_.hasMsg()) { + ROS_INFO_THROTTLE(1.0, "[%s]: %s orientation on topic: %s", getPrintName().c_str(), Support::waiting_for_string.c_str(), orientation_topic_.c_str()); + return {}; + } + measurement_t measurement; + measurement(0) = mrs_lib::AttitudeConverter(sh_orientation_.getMsg()->quaternion).getHeadingRate(msg->vector); + return measurement; + } + catch (...) { + ROS_ERROR_THROTTLE(1.0, "[%s]: Exception caught during getting heading rate (getCorrectionFromVector())", getPrintName().c_str()); + return {}; + } + break; + } default: { - ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromOdometry() switch", getPrintName().c_str()); + ROS_ERROR_THROTTLE(1.0, "[%s]: unhandled case in getCorrectionFromVector() switch", getPrintName().c_str()); return {}; } }