diff --git a/eagleye_core/navigation/CMakeLists.txt b/eagleye_core/navigation/CMakeLists.txt index 5a45e60c..b8cb8693 100755 --- a/eagleye_core/navigation/CMakeLists.txt +++ b/eagleye_core/navigation/CMakeLists.txt @@ -44,7 +44,9 @@ ament_auto_add_library(eagleye_navigation SHARED src/rtk_dead_reckoning.cpp src/rtk_heading.cpp src/velocity_estimator.cpp + src/rtkfix_plane_validation.cpp include/eagleye_navigation/eagleye_navigation.hpp + include/eagleye_navigation/rtkfix_plane_validation.hpp ) ament_auto_find_build_dependencies() diff --git a/eagleye_core/navigation/include/eagleye_navigation/rtkfix_plane_validation.hpp b/eagleye_core/navigation/include/eagleye_navigation/rtkfix_plane_validation.hpp new file mode 100644 index 00000000..1a81996c --- /dev/null +++ b/eagleye_core/navigation/include/eagleye_navigation/rtkfix_plane_validation.hpp @@ -0,0 +1,93 @@ +// Copyright (c) 2023, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +#ifndef RTKFIX_PLANE_VALIDATION_HPP +#define RTKFIX_PLANE_VALIDATION_HPP + +#include + +#include +#include +#include +#include + +#include + +// RtkfixPlaneValidation +struct RtkfixPlaneValidationParameter +{ + double validation_minimum_interval; + double validation_maximum_interval; + double outlier_threshold; + + void load(const std::string& yaml_file) + { + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); + + this->validation_minimum_interval = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["validation_minimum_interval"].as(); + this->validation_maximum_interval = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["validation_maximum_interval"].as(); + this->outlier_threshold = conf["/**"]["ros__parameters"]["rtkfix_plane_validation"]["outlier_threshold"].as(); + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mRtkfixPlaneValidationParameter YAML Error: " << e.msg << "\033[m" << std::endl; + exit(3); + } + } +}; + +struct RtkfixPlaneValidationStatus +{ + bool is_estimated_now; + bool is_estimation_started; + bool is_estimation_reliable; + sensor_msgs::msg::NavSatFix fix_msg; +}; + +class RtkfixPlaneValidationEstimator +{ +public: + RtkfixPlaneValidationEstimator(); + void setParameter(const RtkfixPlaneValidationParameter& param); + RtkfixPlaneValidationStatus estimate(const nmea_msgs::msg::Gpgga gga, const eagleye_msgs::msg::Distance distance, const geometry_msgs::msg::Vector3Stamped enu_vel); + +private: +// // Param + RtkfixPlaneValidationParameter param_; + + double gga_time_last_; + bool is_first_epoch_; + + std::vector gga_buffer_; + std::vector gga_update_buffer_; + std::vector reliable_status_buffer_; + std::vector unreliable_status_buffer_; + std::vector distance_buffer_; + std::vector enu_vel_buffer_; +}; + +#endif /* RTKFIX_PLANE_VALIDATION_HPP */ \ No newline at end of file diff --git a/eagleye_core/navigation/src/rtkfix_plane_validation.cpp b/eagleye_core/navigation/src/rtkfix_plane_validation.cpp new file mode 100644 index 00000000..7b38ccac --- /dev/null +++ b/eagleye_core/navigation/src/rtkfix_plane_validation.cpp @@ -0,0 +1,185 @@ +// Copyright (c) 2023, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * rtkfix_plane_validation.cpp + * Author MapIV Takanose + */ + +#include "eagleye_navigation/rtkfix_plane_validation.hpp" +#include "eagleye_coordinate/eagleye_coordinate.hpp" + +#include + +RtkfixPlaneValidationEstimator::RtkfixPlaneValidationEstimator() +{ + // Initialization + is_first_epoch_ = true; + gga_time_last_ = 0; +} + +void RtkfixPlaneValidationEstimator::setParameter(const RtkfixPlaneValidationParameter& param) +{ + // Param + param_ = param; + // std::cout << "***debug-code: param_.validation_minimum_interval: " << param_.validation_minimum_interval << std::endl; + // std::cout << "***debug-code: param_.validation_maximum_interval: " << param_.validation_maximum_interval << std::endl; + // std::cout << "***debug-code: param_.outlier_threshold: " << param_.outlier_threshold << std::endl; +} + +RtkfixPlaneValidationStatus RtkfixPlaneValidationEstimator::estimate(const nmea_msgs::msg::Gpgga gga, const eagleye_msgs::msg::Distance distance, const geometry_msgs::msg::Vector3Stamped enu_vel) +{ + RtkfixPlaneValidationStatus status; + status.is_estimation_started = false; + status.is_estimated_now = false; + status.is_estimation_reliable = false; + + // buffering + rclcpp::Time gga_timestamp_now(gga.header.stamp); + double gga_time_now = gga_timestamp_now.seconds(); + bool gga_update_flag = false; + if(gga_time_last_ != gga_time_now && gga.gps_qual == 4) + { + gga_update_flag = true; + gga_time_last_ = gga_time_now; + } + + bool reliable_status = false; + bool unreliable_status = false; + gga_buffer_.push_back(gga); + gga_update_buffer_.push_back(gga_update_flag); + distance_buffer_.push_back(distance.distance); + enu_vel_buffer_.push_back(enu_vel); + reliable_status_buffer_.push_back(reliable_status); + unreliable_status_buffer_.push_back(unreliable_status); + + if(is_first_epoch_) + { + is_first_epoch_ = false; + return status; + } + + double distance_start = distance_buffer_.front(); + double distance_end = distance_buffer_.back(); + + if(distance_end - distance_start < param_.validation_minimum_interval) return status; + + while(1) + { + distance_start = distance_buffer_.front(); + distance_end = distance_buffer_.back(); + if(distance_end - distance_start < param_.validation_maximum_interval) break; + + gga_buffer_.erase(gga_buffer_.begin()); + gga_update_buffer_.erase(gga_update_buffer_.begin()); + distance_buffer_.erase(distance_buffer_.begin()); + enu_vel_buffer_.erase(enu_vel_buffer_.begin()); + reliable_status_buffer_.erase(reliable_status_buffer_.begin()); + unreliable_status_buffer_.erase(unreliable_status_buffer_.begin()); + } + + // Search for past Fix solutions + if(!gga_update_flag) return status; + + int buffer_length = distance_buffer_.size(); + int index_start; + for(int iter = 0; iter < buffer_length; iter++) + { + index_start = buffer_length-1 -iter; + if(distance_end - distance_buffer_[index_start] < param_.validation_minimum_interval) continue; + if(gga_update_buffer_[index_start] && !unreliable_status_buffer_[index_start]) break; + } + if(!gga_update_buffer_[index_start]) return status; + + // Fix solution verification by dead reckoning + double init_llh_pos[3]; + double init_enu_pos[3]; + double init_ecef_base_pos[3]; + init_llh_pos[0] = gga_buffer_[index_start].lat *M_PI/180; + init_llh_pos[1] = gga_buffer_[index_start].lon *M_PI/180; + init_llh_pos[2] = gga_buffer_[index_start].alt + gga_buffer_[index_start].undulation; + llh2xyz(init_llh_pos,init_ecef_base_pos); + xyz2enu(init_ecef_base_pos, init_ecef_base_pos, init_enu_pos); + + double relative_position_x = init_enu_pos[0]; + double relative_position_y = init_enu_pos[1]; + for(int iter = index_start+1; iter < buffer_length; iter++) + { + rclcpp::Time t1(enu_vel_buffer_[iter].header.stamp); + rclcpp::Time t0(enu_vel_buffer_[iter-1].header.stamp); + double dt = t1.seconds() - t0.seconds(); + relative_position_x = relative_position_x + enu_vel_buffer_[iter].vector.x * dt; + relative_position_y = relative_position_y + enu_vel_buffer_[iter].vector.y * dt; + } + + double target_llh_pos[3]; + double target_enu_pos[3]; + double target_ecef_pos[3]; + target_llh_pos[0] = gga_buffer_[buffer_length-1].lat *M_PI/180; + target_llh_pos[1] = gga_buffer_[buffer_length-1].lon *M_PI/180; + target_llh_pos[2] = gga_buffer_[buffer_length-1].alt + gga_buffer_[buffer_length-1].undulation; + llh2xyz(target_llh_pos,target_ecef_pos); + xyz2enu(target_ecef_pos, init_ecef_base_pos, target_enu_pos); + + double residual_x = target_enu_pos[0] - relative_position_x; + double residual_y = target_enu_pos[1] - relative_position_y; + double residual_2d = std::sqrt(residual_x*residual_x + residual_y*residual_y); + + if(residual_2d > param_.outlier_threshold) + { + unreliable_status_buffer_[buffer_length-1] = true; + return status; + } + + if(reliable_status_buffer_[index_start]) + { + status.fix_msg.header = gga_buffer_[buffer_length-1].header; + status.fix_msg.latitude = gga_buffer_[buffer_length-1].lat; + status.fix_msg.longitude = gga_buffer_[buffer_length-1].lon; + status.fix_msg.altitude = gga_buffer_[buffer_length-1].alt + gga_buffer_[buffer_length-1].undulation; + status.is_estimation_reliable = true; + } + reliable_status_buffer_[buffer_length-1] = true; + + // Debug + // rclcpp::Time time_now(enu_vel.header.stamp); + // rclcpp::Time target_time(gga_buffer_[buffer_length-1].header.stamp); + // rclcpp::Time initial_time(gga_buffer_[index_start].header.stamp); + // std::cout << std::setprecision(std::numeric_limits::max_digits10); + // std::cout << "***debug-code: RtkfixPlaneValidationEstimator::estimate: " << time_now.seconds() << std::endl; + // std::cout << "***debug-code: Target gga time: " << target_time.seconds() << std::endl; + // std::cout << "***debug-code: initial gga time: " << initial_time.seconds() << std::endl; + // std::cout << "***debug-code: index_start: " << index_start << std::endl; + // std::cout << "***debug-code: gga_update_buffer_ init: " << (gga_update_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl; + // std::cout << "***debug-code: distance interval: " << distance_buffer_[buffer_length-1] - distance_buffer_[index_start] << std::endl; + // std::cout << "***debug-code: residual: " << residual_2d << std::endl; + // std::cout << "***debug-code: unreliable_status_buffer init: " << (unreliable_status_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl; + // std::cout << "***debug-code: reliable_status_buffer init: " << (reliable_status_buffer_[index_start] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl; + // std::cout << "***debug-code: reliable_status_buffer last: " << (reliable_status_buffer_[buffer_length-1] ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl; + // std::cout << "***debug-code: is_estimation_reliable: " << (status.is_estimation_reliable ? "\033[1;32mTrue\033[m" : "\033[1;31mFalse\033[m") << std::endl; + // std::cout << std::endl; + + return status; +} \ No newline at end of file diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index 37a85a04..6f44e0f9 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -110,7 +110,11 @@ ament_auto_add_executable(velocity_estimator src/velocity_estimator_node.cpp ) -install(TARGETS +ament_auto_add_executable(rtkfix_plane_validation + src/rtkfix_plane_validation_node.cpp +) + +install(TARGETS tf_converted_imu correction_imu distance @@ -129,6 +133,7 @@ install(TARGETS angular_velocity_offset_stop enable_additional_rolling rolling + rtkfix_plane_validation DESTINATION lib/$(PROJECT_NAME) ) diff --git a/eagleye_rt/config/README.md b/eagleye_rt/config/README.md index f1de4f5e..363fea8a 100644 --- a/eagleye_rt/config/README.md +++ b/eagleye_rt/config/README.md @@ -280,3 +280,11 @@ Figure shows the relationship between these parameters. | gnss_receiving_threshold | double | Threshold of minimum GNSS reception rate (Value from 0~1) | 0.2 | | outlier_threshold | double | Outlier threshold due to GNSS multipath [m/s] | 0.1 | | outlier_ratio_threshold | double | Ratio of allowable outliers in the interval (Value from 0~1) | 0.5 | + +### rtkfix_plane_validation + +| Name | Type | Description | Default value | +| :---------------------------------- | :----- | :------------------------------------------------------------------------------ | :------------------- | +| validation_minimum_interval | double | Minimum length of DR used for the validation [m] | 5 | +| validation_maximum_interval | double | Maximum length of DR used for the validation [m] | 10 | +| outlier_threshold | double | Threshold to recognize the Fix solution as an outlier. [m] | 0.3 | \ No newline at end of file diff --git a/eagleye_rt/config/eagleye_config.yaml b/eagleye_rt/config/eagleye_config.yaml index 92e9f6c4..6b52f0f0 100644 --- a/eagleye_rt/config/eagleye_config.yaml +++ b/eagleye_rt/config/eagleye_config.yaml @@ -172,3 +172,8 @@ gnss_receiving_threshold: 0.2 outlier_ratio_threshold: 0.5 outlier_threshold: 0.1 + + rtkfix_plane_validation: + validation_minimum_interval: 5 + validation_maximum_interval: 10 + outlier_threshold: 0.3 diff --git a/eagleye_rt/launch/eagleye_rt.launch.xml b/eagleye_rt/launch/eagleye_rt.launch.xml index 3736d779..5ca31258 100644 --- a/eagleye_rt/launch/eagleye_rt.launch.xml +++ b/eagleye_rt/launch/eagleye_rt.launch.xml @@ -135,8 +135,12 @@ - + + + + + diff --git a/eagleye_rt/src/rtkfix_plane_validation_node.cpp b/eagleye_rt/src/rtkfix_plane_validation_node.cpp new file mode 100644 index 00000000..6aa896f9 --- /dev/null +++ b/eagleye_rt/src/rtkfix_plane_validation_node.cpp @@ -0,0 +1,182 @@ +// Copyright (c) 2023, Map IV, Inc. +// All rights reserved. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// * Redistributions of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// * Redistributions in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// * Neither the name of the Map IV, Inc. nor the names of its contributors +// may be used to endorse or promote products derived from this software +// without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND +// ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED +// WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +// DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDER BE LIABLE FOR ANY +// DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +// (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +// LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND +// ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +/* + * rtkfix_plane_validation_node.cpp + * Author MapIV Takanose + */ + +#include "rclcpp/rclcpp.hpp" +#include "eagleye_navigation/rtkfix_plane_validation.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" + +class RtkfixPlaneValidationNode +{ +public: + RtkfixPlaneValidationNode(rclcpp::Node::SharedPtr node) : node_(node) + { + std::string yaml_file; + node_->declare_parameter("yaml_file",yaml_file); + node_->get_parameter("yaml_file",yaml_file); + // std::cout << "yaml_file: " << yaml_file << std::endl; + + std::string subscribe_gga_topic_name = "gnss/gga"; + std::string subscribe_fix_topic_name = "gnss/fix"; + + RtkfixPlaneValidationParameter param; + param.load(yaml_file); + estimator_.setParameter(param); + + reliable_rtkfix_pub_ = node->create_publisher("gnss/reliable_rtkfix", 1000); + // gga_sub_ = node->create_subscription(subscribe_gga_topic_name, 1000, std::bind(&RtkfixPlaneValidationNode::ggaCallback, this, std::placeholders::_1)); + fix_sub_ = node->create_subscription(subscribe_fix_topic_name, 1000, std::bind(&RtkfixPlaneValidationNode::fixCallback, this, std::placeholders::_1)); + distance_sub_ = node->create_subscription("distance", 1000, std::bind(&RtkfixPlaneValidationNode::distanceCallback, this, std::placeholders::_1)); + enu_vel_sub_ = node->create_subscription("enu_vel", 1000, std::bind(&RtkfixPlaneValidationNode::enuvelCallback, this, std::placeholders::_1)); + } + void run() + { + rclcpp::spin(node_); + } + +private: + // ROS + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr reliable_rtkfix_pub_; + rclcpp::Subscription::SharedPtr gga_sub_; + rclcpp::Subscription::SharedPtr fix_sub_; + rclcpp::Subscription::SharedPtr distance_sub_; + rclcpp::Subscription::SharedPtr enu_vel_sub_; + + nmea_msgs::msg::Gpgga gga_; + sensor_msgs::msg::NavSatFix::ConstSharedPtr fix_ptr_; + eagleye_msgs::msg::Distance distance_; + geometry_msgs::msg::Vector3Stamped enu_vel_; + + bool is_gga_ready_ = false; + bool is_distance_ready_ = false; + + // Estimator + RtkfixPlaneValidationEstimator estimator_; + + void fixCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) + { + // fix2gga + nmea_msgs::msg::Gpgga gga; + gga.header = msg->header; + // gga.utc_seconds = msg->header.stamp.seconds(); + gga.lat = msg->latitude; + gga.lon = msg->longitude; + gga.lat_dir = msg->latitude > 0 ? "N" : "S"; + gga.lon_dir = msg->longitude > 0 ? "E" : "W"; + + gga.num_sats = msg->status.service; + gga.hdop = msg->position_covariance[0]; + gga.alt = msg->altitude; + gga.altitude_units = "M"; + gga.undulation = 0; + gga.undulation_units = "M"; + gga.diff_age = 0; + gga.station_id = ""; + + double std_pos = 0.1; // [m] + if (msg->position_covariance[0] < std_pos * std_pos) + { + gga.gps_qual = 4; + } + else + { + gga.gps_qual = 0; + } + + gga_ = gga; + fix_ptr_ = msg; + is_gga_ready_ = true; + } + + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) + { + gga_ = *msg; + is_gga_ready_ = true; + + // rclcpp::Time ros_clock(gga_.header.stamp); + // double time = ros_clock.seconds(); + // std::cout << std::setprecision(std::numeric_limits::max_digits10); + // std::cout << "***debug-code: gga header: " << time << std::endl; + } + + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) + { + distance_ = *msg; + is_distance_ready_ = true; + + // rclcpp::Time ros_clock(distance_.header.stamp); + // double time = ros_clock.seconds(); + // std::cout << std::setprecision(std::numeric_limits::max_digits10); + // std::cout << "***debug-code: distance header: " << time << std::endl; + } + + void enuvelCallback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) + { + // Skip until msgs are ready + if (!is_gga_ready_) return; + if (!is_distance_ready_) return; + + enu_vel_ = *msg; + + // rclcpp::Time ros_clock(enu_vel_.header.stamp); + // double time = ros_clock.seconds(); + // std::cout << std::setprecision(std::numeric_limits::max_digits10); + // std::cout << "***debug-code: enu_vel header: " << time << std::endl; + + // // Trigger estimation + RtkfixPlaneValidationStatus reliable_rtkfix_status = estimator_.estimate(gga_, distance_, enu_vel_); + + if(reliable_rtkfix_status.is_estimation_reliable) + { + // Convert to ROS message + sensor_msgs::msg::NavSatFix reliable_rtkfix_msg; + reliable_rtkfix_msg = reliable_rtkfix_status.fix_msg; + + if(fix_ptr_ != nullptr) + { + reliable_rtkfix_msg.position_covariance = fix_ptr_->position_covariance; + } + // Publish + reliable_rtkfix_pub_->publish(reliable_rtkfix_msg); + } + } + +}; + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + + RtkfixPlaneValidationNode node(rclcpp::Node::make_shared("rtkfix_plane_validation")); + node.run(); + + return 0; + +}