Skip to content

Commit

Permalink
heading filter updates
Browse files Browse the repository at this point in the history
  • Loading branch information
@pearlastrid
pearlastrid committed Feb 9, 2025
1 parent 55411c8 commit 4a20d95
Show file tree
Hide file tree
Showing 2 changed files with 52 additions and 17 deletions.
45 changes: 34 additions & 11 deletions localization/heading_filter/heading_filter.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,12 @@
#include "heading_filter.hpp"
#include "mrover/msg/detail/fix_status__struct.hpp"
#include <chrono>
#include <cstdint>
#include <geometry_msgs/msg/detail/vector3_stamped__struct.hpp>
#include <memory>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <sensor_msgs/msg/detail/magnetic_field__struct.hpp>

namespace mrover {

Expand All @@ -14,18 +17,20 @@ namespace mrover {
correct_and_publish(position);
});

imu_sub = this->create_subscription<sensor_msgs::msg::Imu>("/zed_imu/data_raw", 10, [&](const sensor_msgs::msg::Imu::ConstSharedPtr &imu) {
last_imu = *imu;
});
// imu_sub = this->create_subscription<sensor_msgs::msg::Imu>("/zed_imu/data_raw", 10, [&](const sensor_msgs::msg::Imu::ConstSharedPtr &imu) {
// last_imu = *imu;
// });

mag_sub = this->create_subscription<sensor_msgs::msg::MagneticField>("/zed_imu/mag", 10, [&](const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag) {
last_mag = *mag;
});
// mag_sub = this->create_subscription<sensor_msgs::msg::MagneticField>("/zed_imu/mag", 10, [&](const sensor_msgs::msg::MagneticField::ConstSharedPtr &mag) {
// last_mag = *mag;
// });

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");

// rtk heading data watchdog
// data watchdogs
rtk_heading_watchdog = this->create_wall_timer(RTK_HEADING_WATCHDOG_TIMEOUT.to_chrono<std::chrono::milliseconds>(), [&]() {
RCLCPP_WARN(get_logger(), "RTK heading data watchdog expired");
last_rtk_heading.reset();
Expand All @@ -36,16 +41,32 @@ namespace mrover {
// last_rtk_heading_correction_rotation.reset();
});

// synchronizer
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();
});

// synchronizers
uint32_t queue_size = 10;
sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<mrover::msg::Heading, mrover::msg::FixStatus>>>(

rtk_heading_sync = std::make_shared<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<mrover::msg::Heading, mrover::msg::FixStatus>>>(
message_filters::sync_policies::ApproximateTime<mrover::msg::Heading, mrover::msg::FixStatus>(queue_size),
rtk_heading_sub,
rtk_heading_status_sub
);

sync->setAgePenalty(0.5);
sync->registerCallback(&HeadingFilter::sync_rtk_heading_callback, this);
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_sub,
mag_sub
);

imu_and_mag_sync->setAgePenalty(0.5);
imu_and_mag_sync->registerCallback(&HeadingFilter::sync_imu_and_mag_callback, this);


}
Expand All @@ -57,6 +78,8 @@ namespace mrover {
// last_rtk_heading_time = heading->header.stamp;
}

// void HeadingFilter::sync_imu_and_mag_callback(const sensor_msgs::msg::)

void HeadingFilter::correct_and_publish(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr &position) {

if (!last_imu) {
Expand Down
24 changes: 18 additions & 6 deletions localization/heading_filter/heading_filter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,11 @@

#include "mrover/msg/detail/fix_status__struct.hpp"
#include "pch.hpp"
#include <memory>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/synchronizer.h>
#include <sensor_msgs/msg/detail/magnetic_field__struct.hpp>

namespace mrover {

Expand All @@ -14,21 +19,27 @@ 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);

// 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;
// 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<mrover::msg::Heading> rtk_heading_sub;
message_filters::Subscriber<mrover::msg::FixStatus> rtk_heading_status_sub;

// synchronizer
// synchronizers
std::shared_ptr<message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime
<mrover::msg::Heading, mrover::msg::FixStatus>>> sync;
<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;

// rtk heading data watchdog
// data watchdogs
rclcpp::TimerBase::SharedPtr rtk_heading_watchdog;
rclcpp::TimerBase::SharedPtr imu_and_mag_watchdog;

// data store
std::optional<double> last_rtk_heading;
Expand All @@ -40,7 +51,8 @@ namespace mrover {
std::optional<sensor_msgs::msg::MagneticField> last_mag;

// thresholding for data
const rclcpp::Duration RTK_HEADING_WATCHDOG_TIMEOUT = rclcpp::Duration::from_seconds(2.0);
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 = 10;
// constexpr static bool HEADING_CORRECTED = false;

Expand Down

0 comments on commit 4a20d95

Please sign in to comment.