mag is noisy but I think the filter works

umrover · Feb 11, 2025 · 7a408ef · 7a408ef
1 parent 4a22444
commit 7a408ef
Show file tree

Hide file tree

Showing 10 changed files with 110 additions and 25 deletions.
diff --git a/config/localization.yaml b/config/localization.yaml
@@ -30,6 +30,6 @@ rover_gps_driver:
     frame_id: "base_link"
 
 heading_filter:
-  ros_parameters:
+  ros__parameters:
     world_frame: "map"
     rover_frame: "base_link"
diff --git a/launch/localization.launch.py b/launch/localization.launch.py
@@ -12,20 +12,20 @@
 
 def generate_launch_description():
 
-    # rover_gps_driver_node = Node(
-    #     package="mrover",
-    #     executable="rover_gps_driver",
-    #     name="rover_gps_driver",
-    #     parameters=[os.path.join(get_package_share_directory("mrover"), "config", "localization.yaml")],
-    # )
-
     rover_gps_driver_node = Node(
         package="mrover",
-        executable="rover_gps_driver.py",
+        executable="rover_gps_driver",
         name="rover_gps_driver",
         parameters=[os.path.join(get_package_share_directory("mrover"), "config", "localization.yaml")],
     )
 
+    # rover_gps_driver_node = Node(
+    #     package="mrover",
+    #     executable="rover_gps_driver.py",
+    #     name="rover_gps_driver",
+    #     parameters=[os.path.join(get_package_share_directory("mrover"), "config", "localization.yaml")],
+    # )
+
     gps_linearization_node = Node(
         package="mrover",
         executable="gps_linearization.py",
@@ -40,4 +40,13 @@ def generate_launch_description():
         parameters=[os.path.join(get_package_share_directory("mrover"), "config", "zed.yaml")],
     )
 
-    return LaunchDescription([rover_gps_driver_node, gps_linearization_node, zed_node])
+    heading_filter_node = Node(
+        package="mrover",
+        executable="heading_filter",
+        name="heading_filter",
+        parameters=[os.path.join(get_package_share_directory("mrover"), "config", "localization.yaml")],
+    )
+
+    return LaunchDescription([rover_gps_driver_node, gps_linearization_node, zed_node, heading_filter_node])
+
+    # return LaunchDescription([zed_node])
diff --git a/localization/gps_linearization.py b/localization/gps_linearization.py
@@ -36,6 +36,7 @@ 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

diff --git a/localization/heading_filter/heading_filter.cpp b/localization/heading_filter/heading_filter.cpp
@@ -1,4 +1,6 @@
 #include "heading_filter.hpp"
+#include <Eigen/src/Geometry/AngleAxis.h>
+#include <rclcpp/logging.hpp>
 
 namespace mrover {
 
@@ -20,7 +22,8 @@ namespace mrover {
         rtk_heading_sub.subscribe(this, "/heading/fix");
         rtk_heading_status_sub.subscribe(this, "/heading_fix_status");
         imu_sub.subscribe(this, "/zed_imu/data_raw");
-        mag_sub.subscribe(this, "/zed_imu/mag");
+        // mag_sub.subscribe(this, "/zed_imu/mag");
+        mag_heading_sub.subscribe(this, "/zed_imu/mag_heading");
 
         // data watchdogs
         // rtk_heading_watchdog = this->create_wall_timer(RTK_HEADING_WATCHDOG_TIMEOUT.to_chrono<std::chrono::milliseconds>(), [&]() {
@@ -36,7 +39,7 @@ namespace mrover {
         imu_and_mag_watchdog = this->create_wall_timer(IMU_AND_MAG_WATCHDOG_TIMEOUT.to_chrono<std::chrono::milliseconds>(), [&]() {
             RCLCPP_WARN(get_logger(), "ZED IMU data watchdog expired");
             last_imu.reset();
-            last_mag.reset();
+            last_mag_heading.reset();
         });
 
         // synchronizers
@@ -51,10 +54,10 @@ namespace mrover {
         rtk_heading_sync->setAgePenalty(0.5);
         rtk_heading_sync->registerCallback(&HeadingFilter::sync_rtk_heading_callback, this);
 
-        imu_and_mag_sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField>>>(
-            message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField>(queue_size),
+        imu_and_mag_sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, mrover::msg::Heading>>>(
+            message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Imu, mrover::msg::Heading>(queue_size),
             imu_sub,
-            mag_sub
+            mag_heading_sub
         );
 
         imu_and_mag_sync->setAgePenalty(0.5);
@@ -95,32 +98,85 @@ namespace mrover {
         }
         // magnetometer when correction already exists
         else if (curr_heading_correction) {
-            double measured_heading = (M_PI / 2) - std::atan2(last_mag->magnetic_field.y, last_mag->magnetic_field.x);
+            // double measured_heading = (M_PI / 2) - std::atan2(last_mag->magnetic_field.y, last_mag->magnetic_field.x);
+            double measured_heading = last_mag_heading->heading;
+
+            if (measured_heading > 270) {
+                measured_heading = measured_heading - 360;
+            }
+
+            measured_heading = (90 - measured_heading) * (M_PI / 180);
+
             double heading_correction_delta = measured_heading - uncorrected_heading;
 
-            R2d prev_heading_correction_forward = curr_heading_correction.value().coeffs().col(0).head(2);
+            R2d prev_heading_correction_forward = curr_heading_correction->rotation().col(0).head(2);
+
+            // R2d prev_heading_correction_forward = curr_heading_correction.value().coeffs().col(0).head(2);
+
+            // Eigen::MatrixXd mat = curr_heading_correction.value().coeffs();
+
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(0,0), mat(0, 1), mat(0, 2)));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(1,0), mat(1, 1), mat(1, 2)));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(2,0), mat(2, 1), mat(2, 2)));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction (0,0)", mat(0,0)));
             double prev_heading_correction_delta = std::atan2(prev_heading_correction_forward.y(), prev_heading_correction_forward.x());
 
             // compare curr_heading_correction with correction made from mag
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("heading difference: {}", std::abs(prev_heading_correction_delta - heading_correction_delta)));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("prev heading correction delta: {}", prev_heading_correction_delta));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("heading correction delta: {}", heading_correction_delta));
+            // double measured_heading = (M_PI / 2) - std::atan2(last_mag->magnetic_field.y, last_mag->magnetic_field.x);/ RCLCPP_INFO_STREAM(get_logger(), std::format("mag heading: {}", measured_heading));
+            RCLCPP_INFO_STREAM(get_logger(), std::format("mag heading: {}", measured_heading));
+            RCLCPP_INFO_STREAM(get_logger(), std::format("uncorrected heading: {}", uncorrected_heading));
+
+
             if (std::abs(prev_heading_correction_delta - heading_correction_delta) < HEADING_THRESHOLD) {
                 curr_heading_correction = Eigen::AngleAxisd(heading_correction_delta, R3d::UnitZ());
             }
 
         }
         // magnetometer when correction does not exist
         else {
-            double measured_heading = (M_PI / 2) - std::atan2(last_mag->magnetic_field.y, last_mag->magnetic_field.x);
+            double measured_heading = last_mag_heading->heading;
+
+            if (measured_heading > 270) {
+                measured_heading = measured_heading - 360;
+            }
+
+            measured_heading = (90 - measured_heading) * (M_PI / 180);
+
             double heading_correction_delta = measured_heading - uncorrected_heading;
+
             curr_heading_correction = Eigen::AngleAxisd(heading_correction_delta, R3d::UnitZ());
+
+            Eigen::MatrixXd mat = curr_heading_correction->rotation();
+            // curr_heading_correction = correction.toRotationMatrix();
+
+
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("measured heading: {}", measured_heading));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("uncorrected heading: {}", uncorrected_heading));
+            // RCLCPP_INFO_STREAM(get_logger(), std::format("heading correction delta: {}", heading_correction_delta));
+            // curr_heading_correction = Eigen::AngleAxisd(heading_correction_delta, R3d::UnitZ());
+
+            // Eigen::AngleAxisd rotation_temp = Eigen::AngleAxisd(heading_correction_delta, Eigen::Vector3d::UnitZ());
+            // Eigen::Matrix3d mat = rotation_temp.toRotationMatrix();
+
+            // Eigen::Matrix3d mat = curr_heading_correction.value().coeffs();
+
+            // Eigen::Matrix3d mat = curr_heading_correction.value().coeffs();
+
+            RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(0,0), mat(0, 1), mat(0, 2)));
+            RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(1,0), mat(1, 1), mat(1, 2)));
+            RCLCPP_INFO_STREAM(get_logger(), std::format("curr_heading_correction: {} {} {}", mat(2,0), mat(2, 1), mat(2, 2)));
         }
 
 
     }
 
-    void HeadingFilter::sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag) {
+    void HeadingFilter::sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const mrover::msg::Heading::ConstSharedPtr &mag_heading) {
         imu_and_mag_watchdog.reset();
         last_imu = *imu;
-        last_mag = *mag;
+        last_mag_heading = *mag_heading;
     }
 
     void HeadingFilter::correct_and_publish(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position) {

diff --git a/localization/heading_filter/heading_filter.hpp b/localization/heading_filter/heading_filter.hpp
@@ -13,15 +13,16 @@ namespace mrover {
 
         void correct_and_publish(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position);
         void sync_rtk_heading_callback(const mrover::msg::Heading::ConstSharedPtr &heading, const mrover::msg::FixStatus::ConstSharedPtr &heading_status);
-        void sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag);
+        void sync_imu_and_mag_callback(const sensor_msgs::msg::Imu::ConstSharedPtr &imu, const mrover::msg::Heading::ConstSharedPtr &mag_heading);
 
         // subscribers and publishers
         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<sensor_msgs::msg::Imu> imu_sub;
-        message_filters::Subscriber<sensor_msgs::msg::MagneticField> mag_sub;
+        // message_filters::Subscriber<sensor_msgs::msg::MagneticField> mag_sub;
+        message_filters::Subscriber<mrover::msg::Heading> mag_heading_sub;
         message_filters::Subscriber<mrover::msg::Heading> rtk_heading_sub;
         message_filters::Subscriber<mrover::msg::FixStatus> rtk_heading_status_sub;
 
@@ -34,7 +35,7 @@ namespace mrover {
         std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime
             <mrover::msg::Heading, mrover::msg::FixStatus>>> rtk_heading_sync;
         std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime
-            <sensor_msgs::msg::Imu, sensor_msgs::msg::MagneticField>>> imu_and_mag_sync;
+            <sensor_msgs::msg::Imu, mrover::msg::Heading>>> imu_and_mag_sync;
 
         // data watchdogs
         // rclcpp::TimerBase::SharedPtr rtk_heading_watchdog;
@@ -47,12 +48,12 @@ namespace mrover {
         // std::optional<SO3d> last_rtk_heading_correction_rotation;
         std::optional<SO3d> curr_heading_correction;
         std::optional<sensor_msgs::msg::Imu> last_imu;
-        std::optional<sensor_msgs::msg::MagneticField> last_mag;
+        std::optional<mrover::msg::Heading> last_mag_heading;
 
         // thresholding for data
         // const rclcpp::Duration RTK_HEADING_WATCHDOG_TIMEOUT = rclcpp::Duration::from_seconds(1.5);
         const rclcpp::Duration IMU_AND_MAG_WATCHDOG_TIMEOUT = rclcpp::Duration::from_seconds(1.0);
-        constexpr static float HEADING_THRESHOLD = M_PI / 8;
+        constexpr static float HEADING_THRESHOLD = M_PI / 16;
         // constexpr static bool HEADING_CORRECTED = false;
 
         std::string rover_frame;

diff --git a/localization/rover_gps_driver/rover_gps_driver.cpp b/localization/rover_gps_driver/rover_gps_driver.cpp
@@ -54,6 +54,10 @@ namespace mrover {
 
             if (stoi(tokens[GNGGA_QUAL_POS]) == 0) {
                 RCLCPP_WARN(get_logger(), "Invalid fix. Are we inside?");
+                nav_sat_fix.latitude = 2;
+                nav_sat_fix.longitude = 2;
+                nav_sat_fix.altitude = 0;
+                gps_pub->publish(nav_sat_fix);
                 return;
             }
             else {

diff --git a/perception/zed_wrapper/pch.hpp b/perception/zed_wrapper/pch.hpp
@@ -12,6 +12,7 @@
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
 #include <sensor_msgs/point_cloud2_iterator.hpp>
+#include <mrover/msg/heading.hpp>
 
 // ZED
 #include <sl/Camera.hpp>

diff --git a/perception/zed_wrapper/zed_wrapper.bridge.cpp b/perception/zed_wrapper/zed_wrapper.bridge.cpp
@@ -44,6 +44,10 @@ namespace mrover {
         msg.magnetic_field_covariance[8] = 0.047e-6;
     }
 
+    auto fillMagHeadingMessage(sl::SensorsData::MagnetometerData const& magData, mrover::msg::Heading &msg) -> void {
+        msg.heading = magData.magnetic_heading;
+    }
+
     auto fillImageMessage(sl::Mat const& bgra, sensor_msgs::msg::Image::UniquePtr const& msg) -> void {
         assert(bgra.getChannels() == 4);
         assert(msg);

diff --git a/perception/zed_wrapper/zed_wrapper.cpp b/perception/zed_wrapper/zed_wrapper.cpp
@@ -25,6 +25,7 @@ namespace mrover {
             mPcPub = create_publisher<sensor_msgs::msg::PointCloud2>("zed/left/points", 1);
             mRightCamInfoPub = create_publisher<sensor_msgs::msg::CameraInfo>("zed/right/camera_info", 1);
             mLeftCamInfoPub = create_publisher<sensor_msgs::msg::CameraInfo>("zed/left/camera_info", 1);
+            mMagHeadingPub = create_publisher<mrover::msg::Heading>("zed_imu/mag_heading", 1);
 
             // Declare and set Params
             int imageWidth{};
@@ -206,10 +207,15 @@ namespace mrover {
 
                     if (mZedInfo.camera_model != sl::MODEL::ZED_M) {
                         sensor_msgs::msg::MagneticField magMsg;
+                        mrover::msg::Heading headingMsg;
                         fillMagMessage(sensorData.magnetometer, magMsg);
+                        fillMagHeadingMessage(sensorData.magnetometer, headingMsg);
                         magMsg.header.frame_id = "zed_mag_frame";
                         magMsg.header.stamp = now();
                         mMagPub->publish(magMsg);
+                        headingMsg.header.frame_id = "zed_mag_frame";
+                        headingMsg.header.stamp = now();
+                        mMagHeadingPub->publish(headingMsg);
                     }
                 }
 

diff --git a/perception/zed_wrapper/zed_wrapper.hpp b/perception/zed_wrapper/zed_wrapper.hpp
@@ -68,6 +68,7 @@ namespace mrover {
         rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr mPcPub;
         rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr mLeftCamInfoPub;
         rclcpp::Publisher<sensor_msgs::msg::CameraInfo>::SharedPtr mRightCamInfoPub;
+        rclcpp::Publisher<mrover::msg::Heading>::SharedPtr mMagHeadingPub;
 
         // Thread
 
@@ -92,6 +93,8 @@ namespace mrover {
 
     auto fillMagMessage(sl::SensorsData::MagnetometerData const& magData, sensor_msgs::msg::MagneticField& msg) -> void;
 
+    auto fillMagHeadingMessage(sl::SensorsData::MagnetometerData const& magData, mrover::msg::Heading &msg) -> void;
+
     auto checkCudaError(cudaError_t error) -> void;
 
     void fillPointCloudMessageFromGpu(sl::Mat& xyzGpu, sl::Mat& bgraGpu, sl::Mat& normalsGpu, PointCloudGpu& pcGpu, sensor_msgs::msg::PointCloud2::UniquePtr const& msg);