From 56ee768af4d05caab91894fde4a9fdb195bb54c6 Mon Sep 17 00:00:00 2001 From: Pearl Lin Date: Fri, 7 Feb 2025 14:54:18 -0500 Subject: [PATCH] started heading filter --- localization/gps_linearization.py | 40 +++-------- .../heading_filter/heading_filter.cpp | 40 +++++++++++ .../heading_filter/heading_filter.hpp | 69 +++++++++++++++++++ localization/heading_filter/pch.hpp | 21 ++++++ 4 files changed, 139 insertions(+), 31 deletions(-) create mode 100644 localization/heading_filter/heading_filter.cpp create mode 100644 localization/heading_filter/heading_filter.hpp create mode 100644 localization/heading_filter/pch.hpp diff --git a/localization/gps_linearization.py b/localization/gps_linearization.py index b1d274b9..f69dd361 100755 --- a/localization/gps_linearization.py +++ b/localization/gps_linearization.py @@ -16,19 +16,14 @@ from pymap3d.enu import geodetic2enu import rclpy -from geometry_msgs.msg import Vector3Stamped +from geometry_msgs.msg import Vector3Stamped, Vector3 from rclpy import Parameter from rclpy.executors import ExternalShutdownException from rclpy.node import Node -from sensor_msgs.msg import NavSatFix, Imu -import tf2_ros - -import message_filters -from lie.conversions import to_tf_tree, SE3 +from sensor_msgs.msg import NavSatFix class GPSLinearization(Node): - def __init__(self) -> None: super().__init__("gps_linearization") @@ -41,39 +36,22 @@ def __init__(self) -> None: ("world_frame", Parameter.Type.STRING), ], ) - self.ref_lat = self.get_parameter("ref_lat").value self.ref_lon = self.get_parameter("ref_lon").value self.ref_alt = self.get_parameter("ref_alt").value self.world_frame = self.get_parameter("world_frame").value - self.gps_sub = message_filters.Subscriber(self, NavSatFix, "gps/fix") - self.orientation_sub = message_filters.Subscriber(self, Imu, "zed_imu/data_raw") - - self.tf_broadcaster = tf2_ros.TransformBroadcaster(self) - - self.sync = message_filters.ApproximateTimeSynchronizer([self.gps_sub, self.orientation_sub], 10, 1) - self.sync.registerCallback(self.synced_gps_imu_callback) + self.pos_pub = self.create_publisher(Vector3Stamped, "linearized_position", 10) - def synced_gps_imu_callback(self, gps_msg: NavSatFix, imu_msg: Imu): + self.gps_sub = self.create_subscription(NavSatFix, "gps/fix", self.gps_callback, 10) - if np.isnan([gps_msg.latitude, gps_msg.longitude, gps_msg.altitude]).any(): + def gps_callback(self, msg: NavSatFix): + if np.isnan([msg.latitude, msg.longitude, msg.altitude]).any(): self.get_logger().warn("Received NaN GPS data, ignoring") return - quaternion = np.array( - [imu_msg.orientation.x, imu_msg.orientation.y, imu_msg.orientation.z, imu_msg.orientation.w] - ) - quaternion = quaternion / np.linalg.norm(quaternion) - - x, y, z = geodetic2enu( - gps_msg.latitude, gps_msg.longitude, gps_msg.altitude, self.ref_lat, self.ref_lon, self.ref_alt, deg=True - ) - pose = SE3(position=np.array([x, y, 0]), quaternion=quaternion) - - to_tf_tree(tf_broadcaster=self.tf_broadcaster, se3=pose, child_frame="base_link", parent_frame=self.world_frame) - - self.get_logger().info(f"Published to TF Tree: Position({x}, {y}, {z}), Orientation({imu_msg.orientation})") + x, y, _ = geodetic2enu(msg.latitude, msg.longitude, 0.0, self.ref_lat, self.ref_lon, self.ref_alt, deg=True) + self.pos_pub.publish(Vector3Stamped(header=msg.header, vector=Vector3(x=x, y=y))) def main() -> None: @@ -88,4 +66,4 @@ def main() -> None: if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/localization/heading_filter/heading_filter.cpp b/localization/heading_filter/heading_filter.cpp new file mode 100644 index 00000000..688639de --- /dev/null +++ b/localization/heading_filter/heading_filter.cpp @@ -0,0 +1,40 @@ +#include "heading_filter.hpp" + +namespace mrover { + + HeadingFilter::HeadingFilter() : Node("heading_filter") { + + // publishers + linearized_position_sub.subscribe(this, "/linearized_position", 1); + imu_sub.subscribe(this, "/zed_imu/data_raw", 10); + mag_sub.subscribe(this, "/zed_imu/mag", 10); + rtk_heading_sub.subscribe(this, "/heading/fix", 1); + rtk_heading_status_sub.subscribe(this, "/heading_fix_status", 1); + + // synchronizer + uint32_t queue_size = 10; + sync = std::make_shared>>( + message_filters::sync_policies::ApproximateTime(queue_size), + linearized_position_sub, + imu_sub, + mag_sub, + rtk_heading_sub, + rtk_heading_status_sub + ); + + sync->setAgePenalty(0.50); + sync->registerCallback(std::bind(&HeadingFilter::synced_position_orientation_callback, this, _1, _2)); + + } + + void HeadingFilter::synced_position_orientation_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position, + const sensor_msgs::msg::Imu::ConstSharedPtr &imu, + const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag, + const mrover::msg::Heading::ConstSharedPtr &rtk_heading, + const mrover::msg::FixStatus::ConstSharedPtr &rtk_heading_status) { + + // to be implemented... + + } +} // namespace mrover \ No newline at end of file diff --git a/localization/heading_filter/heading_filter.hpp b/localization/heading_filter/heading_filter.hpp new file mode 100644 index 00000000..8e8f7768 --- /dev/null +++ b/localization/heading_filter/heading_filter.hpp @@ -0,0 +1,69 @@ +#pragma once + +#include "pch.hpp" +#include +#include + +namespace mrover { + + using std::placeholders::_1; + using std::placeholders::_2; + + class HeadingFilter : public rclcpp::Node { + + private: + + void correct_and_publish(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position); + + // subscribers + rclcpp::Subscription::SharedPtr linearized_position_sub; + rclcpp::Subscription::SharedPtr imu_sub; + rclcpp::Subscription::SharedPtr mag_sub; + + message_filters::Subscriber rtk_heading_sub; + message_filters::Subscriber rtk_heading_status_sub; + + // rclcpp::Subscription::SharedPtr rtk_heading_sub; + rclcpp::Subscription::SharedPtr rtk_heading_status_sub; + + // rtk heading data watchdog + rclcpp::TimerBase::SharedPtr rtk_heading_watchdog; + + // data store + std::optional last_rtk_heading; + std::optional last_rtk_heading_time; + std::optional rtk_heading_correction_rotation; + + // thresholding for data + constexpr static float HEADING_THRESHOLD = 10; + constexpr static bool HEADING_CORRECTED = false; + + + + // void synced_position_orientation_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position, + // const sensor_msgs::msg::Imu::ConstSharedPtr &imu, + // const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag, + // const mrover::msg::Heading::ConstSharedPtr &rtk_heading, + // const mrover::msg::FixStatus::ConstSharedPtr &rtk_heading_status); + + // // subscribers + // message_filters::Subscriber linearized_position_sub; + // message_filters::Subscriber imu_sub; + // message_filters::Subscriber mag_sub; + // message_filters::Subscriber rtk_heading_sub; + // message_filters::Subscriber rtk_heading_status_sub; + + // // synchronizer + // std::shared_ptr>> sync; + + + public: + + HeadingFilter(); + void spin(); + + + } // class HeadingFilter + +} // namespace mrover \ No newline at end of file diff --git a/localization/heading_filter/pch.hpp b/localization/heading_filter/pch.hpp new file mode 100644 index 00000000..32f0d4fb --- /dev/null +++ b/localization/heading_filter/pch.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include + +#include +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include \ No newline at end of file