vive_filter.cpp

#define _CRT_SECURE_NO_WARNINGS

//#define NDEBUG

#include <deque>

#include <vector>

#include <cmath>

#include <memory>

#include <string>

#include <iostream>

#include <chrono>

#include <functional>

#include <assert.h>

#include "rclcpp/rclcpp.hpp"

#include "tf2_ros/transform_listener.h"

#include "tf2_ros/buffer.h"

#include "tf2_ros/transform_broadcaster.h"

#include "geometry_msgs/msg/transform_stamped.hpp"

using namespace std::chrono_literals;

std::vector<double> butterworth_filter(const std::vector<double>& input, double cutoff_frequency, double sampling_rate, int order)

{

    int N = input.size();

    std::vector<double> output(N, 0.0);

    // cf, init

    double nyquist = 0.5 * sampling_rate;

    double normalized_cutoff = cutoff_frequency / nyquist;

    std::vector<double> a(order + 1, 0.0);

    std::vector<double> b(order + 1, 0.0);

    double theta = M_PI * normalized_cutoff;

    double d = 1.0 / std::tan(theta);

    double d_squared = d * d;

    a[0] = 1.0 / (1.0 + std::sqrt(2.0) * d + d_squared);

    a[1] = 2.0 * a[0];

    a[2] = a[0];

    b[1] = 2.0 * (1.0 - d_squared) * a[0];

    b[2] = (1.0 - std::sqrt(2.0) * d + d_squared) * a[0];

    for (int i = 2; i < N; ++i)

    {

#ifdef NDEBUG

#else

        printf("\nFiltering index: %i\n",i);

#endif

        output[i] = a[0] * input[i] + a[1] * input[i - 1] + a[2] * input[i - 2] - b[1] * output[i - 1] - b[2] * output[i - 2];

    }

    return output;

}

class Filter

{

public:

    Filter() {}

    void set_history_size(size_t new_size)

    {

        history_size_ = new_size;

        if (x_history_.size() > history_size_) x_history_.resize(history_size_);

        if (y_history_.size() > history_size_) y_history_.resize(history_size_);

        if (z_history_.size() > history_size_) z_history_.resize(history_size_);

    }

    geometry_msgs::msg::TransformStamped apply_filter(const geometry_msgs::msg::TransformStamped& transform)

    {

        x_history_.push_back(transform.transform.translation.x);

        y_history_.push_back(transform.transform.translation.y);

        z_history_.push_back(transform.transform.translation.z);

        if (x_history_.size() > history_size_) x_history_.pop_front();

        if (y_history_.size() > history_size_) y_history_.pop_front();

        if (z_history_.size() > history_size_) z_history_.pop_front();

#ifdef NDEBUG

#else

        printf("\nHistory Size: %zu", history_size_);

        printf("\nDeque x_history_ size: %zu", x_history_.size());

        printf("\nDeque y_history_ size: %zu", y_history_.size());

        printf("\nDeque z_history_ size: %zu", z_history_.size());

#endif

        geometry_msgs::msg::TransformStamped filtered_transform = transform;

        if (x_history_.size() >= history_size_)

        {

            std::vector<double> x_filtered = butterworth_filter(

                std::vector<double>(x_history_.begin(), x_history_.end()), cutoff_frequency_, sampling_rate_, order_);

            std::vector<double> y_filtered = butterworth_filter(

                std::vector<double>(y_history_.begin(), y_history_.end()), cutoff_frequency_, sampling_rate_, order_);

            std::vector<double> z_filtered = butterworth_filter(

                std::vector<double>(z_history_.begin(), z_history_.end()), cutoff_frequency_, sampling_rate_, order_);

            filtered_transform.transform.translation.x = x_filtered.back();

            filtered_transform.transform.translation.y = y_filtered.back();

            filtered_transform.transform.translation.z = z_filtered.back();

        }

        return filtered_transform;

    }

private:

    std::deque<double> x_history_;

    std::deque<double> y_history_;

    std::deque<double> z_history_;

    size_t history_size_ = 20;

    double cutoff_frequency_ = 10.0;

    double sampling_rate_ = 100.0;

    int order_ = 2;

};

class FrameListener : public rclcpp::Node

{

public:

    FrameListener() : Node("frame_listener_node"), buffer_(get_clock()), listener_(buffer_), filter_(std::make_shared<Filter>())

    {

        timer_ = this->create_wall_timer(50ms, std::bind(&FrameListener::timer_callback, this));

        publisher_ = this->create_publisher<geometry_msgs::msg::TransformStamped>("LHR_BFB7B3C7_filtered", 10);

    }

private:

    void timer_callback()

    {

        geometry_msgs::msg::TransformStamped transformStamped;

        try

        {

            transformStamped = buffer_.lookupTransform("libsurvive_world", "LHR-BFB7B3C7", tf2::TimePointZero);

            RCLCPP_INFO(this->get_logger(), "Received transform (unfiltered): "

                "Translation: [%f, %f, %f], "

                "Rotation: [x: %f, y: %f, z: %f, w: %f]",

                transformStamped.transform.translation.x,

                transformStamped.transform.translation.y,

                transformStamped.transform.translation.z,

                transformStamped.transform.rotation.x,

                transformStamped.transform.rotation.y,

                transformStamped.transform.rotation.z,

                transformStamped.transform.rotation.w);

            geometry_msgs::msg::TransformStamped filtered_transform = filter_->apply_filter(transformStamped);

            publisher_->publish(filtered_transform);

#ifdef NDEBUG

#else

            RCLCPP_INFO(this->get_logger(), "Published unfiltered transform.");

#endif

        }

        catch (tf2::TransformException& ex)

        {

            RCLCPP_WARN(this->get_logger(), "Could not transform 'LHR-BFB7B3C7' to 'libsurvive_world': %s", ex.what());

        }

    }

    tf2_ros::Buffer buffer_;

    tf2_ros::TransformListener listener_;

    rclcpp::Publisher<geometry_msgs::msg::TransformStamped>::SharedPtr publisher_;

    rclcpp::TimerBase::SharedPtr timer_;

    std::shared_ptr<Filter> filter_;

};

class FramePublisher : public rclcpp::Node

{

public:

    FramePublisher() : Node("frame_publisher_node"), buffer_(get_clock()), listener_(buffer_)

    {

        timer_ = this->create_wall_timer(50ms, std::bind(&FramePublisher::timer_callback, this));

        broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

    }

private:

    void timer_callback()

    {

        geometry_msgs::msg::TransformStamped transformStamped;

        try

        {

            transformStamped = buffer_.lookupTransform("libsurvive_world", "LHR-BFB7B3C7", tf2::TimePointZero);

            RCLCPP_INFO(this->get_logger(), "Publishing transform (filtered): "

                "Translation: [%f, %f, %f], "

                "Rotation: [x: %f, y: %f, z: %f, w: %f]",

                transformStamped.transform.translation.x,

                transformStamped.transform.translation.y,

                transformStamped.transform.translation.z,

                transformStamped.transform.rotation.x,

                transformStamped.transform.rotation.y,

                transformStamped.transform.rotation.z,

                transformStamped.transform.rotation.w);

            transformStamped.child_frame_id = "LHR-BFB7B3C7_filtered";

            broadcaster_->sendTransform(transformStamped);

#ifdef NDEBUG

#else

            RCLCPP_INFO(this->get_logger(), "Published filtered transform.");

#endif

        }

        catch (tf2::TransformException& ex)

        {

            RCLCPP_WARN(this->get_logger(), "Could not transform 'LHR-BFB7B3C7' to 'libsurvive_world': %s", ex.what());

        }

    }

    tf2_ros::Buffer buffer_;

    tf2_ros::TransformListener listener_;

    std::shared_ptr<tf2_ros::TransformBroadcaster> broadcaster_;

    rclcpp::TimerBase::SharedPtr timer_;

};

int main(int argc, char* argv[])

{

    rclcpp::init(argc, argv);

#ifdef NDEBUG

#else

    printf("\nROS run"); 

#endif

    auto frame_listener_node = std::make_shared<FrameListener>();

    auto frame_publisher_node = std::make_shared<FramePublisher>();

    rclcpp::executors::SingleThreadedExecutor executor;

    executor.add_node(frame_listener_node);

    executor.add_node(frame_publisher_node);

    executor.spin();

    rclcpp::shutdown();

    return 0;

}