Skip to content

Commit

Permalink
Merge branch 'master' of https://github.com/ctu-mrs/mrs_uav_state_est…
Browse files Browse the repository at this point in the history 
…imators
  • Loading branch information
@klaxalk
klaxalk committed Nov 26, 2024
2 parents 8af229d + a4217b0 commit fd502ba
Showing 1 changed file with 44 additions and 19 deletions.
63 changes: 44 additions & 19 deletions include/mrs_uav_state_estimators/estimators/correction.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -199,6 +199,7 @@ class Correction {
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_raw_;
mrs_lib::PublisherHandler<mrs_msgs::EstimatorCorrection> ph_correction_proc_;
mrs_lib::PublisherHandler<mrs_msgs::Float64Stamped> ph_delay_;
mrs_lib::PublisherHandler<geometry_msgs::PointStamped> ph_mag_field_untilted_;

const std::string est_name_;
const std::string name_;
Expand Down Expand Up @@ -387,6 +388,8 @@ Correction<n_measurements>::Correction(ros::NodeHandle& nh, const std::string& e
}
case MessageType_t::MAG_FIELD: {
sh_mag_field_ = mrs_lib::SubscribeHandler<sensor_msgs::MagneticField>(shopts, msg_topic_, &Correction::callbackMagField, this);
ph_mag_field_untilted_ = mrs_lib::PublisherHandler<geometry_msgs::PointStamped>(nh, est_name_ + "/correction/" + getName() + "/fcu_untilted", 10);

break;
}
case MessageType_t::POINT: {
Expand Down Expand Up @@ -1228,7 +1231,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}
break;
}

// handle latalt estimators
case EstimatorType_t::LATALT: {

Expand All @@ -1250,7 +1253,6 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}
break;
}

}

ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
Expand Down Expand Up @@ -1383,7 +1385,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
auto res = getZVelUntilted(msg->linear_acceleration, msg->header);
if (res) {
measurement_t measurement;
measurement = res.value();
measurement = res.value();
measurement(0) -= gravity_norm_;
return measurement;
} else {
Expand Down Expand Up @@ -1469,7 +1471,6 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}
break;
}

}
ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
return {};
Expand Down Expand Up @@ -1739,7 +1740,6 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}
break;
}

}

ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
Expand Down Expand Up @@ -1861,23 +1861,49 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
// handle heading estimators
case EstimatorType_t::HEADING: {

Eigen::Matrix3d rot;
/* Eigen::Matrix3d rot; */

auto res = ch_->transformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp);
if (res) {
rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation));
} else {
ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(),
ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str());
return {};
}
/* auto res = ch_->transformer->getTransform(ch_->frames.ns_fcu, ch_->frames.ns_fcu_untilted, msg->header.stamp); */
/* if (res) { */
/* rot = Eigen::Matrix3d(mrs_lib::AttitudeConverter(res.value().transform.rotation)); */
/* } else { */
/* ROS_ERROR_THROTTLE(1.0, "[%s]: Could not obtain transform from %s to %s. Not using this correction.", getPrintName().c_str(), */
/* ch_->frames.ns_fcu_untilted.c_str(), ch_->frames.ns_fcu.c_str()); */
/* return {}; */
/* } */

const Eigen::Vector3d mag_vec(msg->magnetic_field.x, msg->magnetic_field.y, msg->magnetic_field.z);
const Eigen::Vector3d proj_mag_field = rot * mag_vec;
geometry_msgs::Vector3 mag_vec;
mag_vec.x = msg->magnetic_field.x;
mag_vec.y = msg->magnetic_field.y;
mag_vec.z = msg->magnetic_field.z;

if (transform_to_frame_enabled_) {
std_msgs::Header header = msg->header;
header.frame_id = transform_from_frame_;
auto res = transformVecToFrame(mag_vec, header, transform_to_frame_);
if (res) {
mag_vec.x = res.value().x;
mag_vec.y = res.value().y;
mag_vec.z = res.value().z;
} else {
ROS_WARN_THROTTLE(1.0, "[%s]: could not transform mag field vector", getPrintName().c_str());
}
}

geometry_msgs::PointStamped mag_vec_msg;
mag_vec_msg.header.stamp = msg->header.stamp;
mag_vec_msg.header.frame_id = transform_to_frame_;
mag_vec_msg.point.x = mag_vec.x;
mag_vec_msg.point.y = mag_vec.y;
mag_vec_msg.point.z = mag_vec.z;
ph_mag_field_untilted_.publish(mag_vec_msg);
const double mag_hdg = atan2(mag_vec.y, mag_vec.x);
/* const Eigen::Vector3d mag_vec(mag_vec_pt.x, mag_vec_pt.y, mag_vec_pt.z); */
/* const Eigen::Vector3d proj_mag_field = rot * mag_vec; */
/* mrs_msgs::Float64Stamped hdg_stamped; */
/* hdg_stamped.header = msg->header; */
/* hdg_stamped.value = atan2(proj_mag_field.y(), proj_mag_field.x()); */
const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x());
/* const double mag_hdg = atan2(proj_mag_field.y(), proj_mag_field.x()); */

measurement_t measurement;

Expand All @@ -1888,7 +1914,7 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
got_first_mag_hdg_ = true;
}

measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_) + M_PI / 2; // may be weirdness of px4 heading (NED vs ENU or something)
measurement(0) = -mrs_lib::geometry::radians::unwrap(mag_hdg, mag_hdg_previous_); // may be weirdness of px4 heading (NED vs ENU or something)
mag_hdg_previous_ = mag_hdg;
return measurement;
}
Expand Down Expand Up @@ -2142,7 +2168,6 @@ std::optional<typename Correction<n_measurements>::measurement_t> Correction<n_m
}
break;
}

}

ROS_ERROR("[%s]: FIXME: should not be possible to get into this part of code", getPrintName().c_str());
Expand Down

0 comments on commit fd502ba

Please sign in to comment.