started heading filter

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()

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::Synchronizer<message_filters::sync_policies::ApproximateTime
+            <geometry_msgs::msg::Vector3Stamped, sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField, mrover::msg::Heading, mrover::msg::FixStatus>>>(
+                message_filters::sync_policies::ApproximateTime<geometry_msgs::msg::Vector3Stamped, sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField, mrover::msg::Heading, mrover::msg::FixStatus>(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

localization/heading_filter/heading_filter.hpp

@@ -0,0 +1,69 @@
+#pragma once
+
+#include "pch.hpp"
+#include <geometry_msgs/msg/detail/vector3_stamped__struct.hpp>
+#include <sensor_msgs/msg/detail/magnetic_field__struct.hpp>
+
+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<geometry_msgs::msg::Vector3Stamped>::SharedPtr linearized_position_sub;
+        rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub;
+        rclcpp::Subscription<sensor_msgs::msg::MagneticField>::SharedPtr mag_sub;
+
+        message_filters::Subscriber<mrover::msg::Heading> rtk_heading_sub;
+        message_filters::Subscriber<mrover::msg::FixStatus> rtk_heading_status_sub;
+
+        // rclcpp::Subscription<mrover::msg::Heading>::SharedPtr rtk_heading_sub;
+        rclcpp::Subscription<mrover::msg::FixStatus>::SharedPtr rtk_heading_status_sub;
+
+        // rtk heading data watchdog
+        rclcpp::TimerBase::SharedPtr rtk_heading_watchdog;
+
+        // data store
+        std::optional<double> last_rtk_heading;
+        std::optional<builtin_interfaces::msg::Time> last_rtk_heading_time;
+        std::optional<SO3d> 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<geometry_msgs::msg::Vector3Stamped> linearized_position_sub;
+        // message_filters::Subscriber<sensor_msgs::msg::Imu> imu_sub;
+        // message_filters::Subscriber<sensor_msgs::msg::MagneticField> mag_sub;
+        // message_filters::Subscriber<mrover::msg::Heading> rtk_heading_sub;
+        // message_filters::Subscriber<mrover::msg::FixStatus> rtk_heading_status_sub;
+
+        // // synchronizer
+        // std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime
+        //     <geometry_msgs::msg::Vector3Stamped, sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField, mrover::msg::Heading, mrover::msg::FixStatus>>> sync;
+
+
+    public:
+
+        HeadingFilter();
+        void spin();
+
+
+    } // class HeadingFilter
+
+} // namespace mrover

localization/heading_filter/pch.hpp

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <memory>
+
+#include <rclcpp/node.hpp>
+#include <rclcpp/executors.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <message_filters/subscriber.hpp>
+#include <message_filters/synchronizer.hpp>
+#include <mssage_filter/sync_policies/approximate_time.hpp>
+
+#include <geometry_msgs/msg/vector3_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+#include <sensor_msgs/msg/magnetic_field.hpp>
+#include <mrover/msg/heading.hpp>
+#include <mrover/msg/fix_type.hpp>
+#include <mrover/msg/fix_status.hpp>
+#include <builtin_interfaces/msg/time.hpp>
+
+#include <lie.hpp>