diff --git a/eagleye_rt/CMakeLists.txt b/eagleye_rt/CMakeLists.txt index 0a46bb09..772326b0 100644 --- a/eagleye_rt/CMakeLists.txt +++ b/eagleye_rt/CMakeLists.txt @@ -10,7 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -if($ENV{ROS_DISTRO} STREQUAL "galactic") +if("$ENV{ROS_DISTRO}" STREQUAL "galactic") add_definitions(-DROS_DISTRO_GALACTIC) endif() diff --git a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp index becffdcc..f3d90a26 100644 --- a/eagleye_rt/src/angular_velocity_offset_stop_node.cpp +++ b/eagleye_rt/src/angular_velocity_offset_stop_node.cpp @@ -32,71 +32,83 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -static geometry_msgs::msg::TwistStamped velocity; -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop; -static sensor_msgs::msg::Imu imu; +class AngularVelocityOffsetStopNode : public rclcpp::Node +{ +public: + AngularVelocityOffsetStopNode() : Node("eagleye_angular_velocity_offset_stop") + { + std::string subscribe_twist_topic_name = "vehicle/twist"; -struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter; -struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status; + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} + try + { + YAML::Node conf = YAML::LoadFile(yaml_file); -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - imu.header = msg->header; - imu.orientation = msg->orientation; - imu.orientation_covariance = msg->orientation_covariance; - imu.angular_velocity = msg->angular_velocity; - imu.angular_velocity_covariance = msg->angular_velocity_covariance; - imu.linear_acceleration = msg->linear_acceleration; - imu.linear_acceleration_covariance = msg->linear_acceleration_covariance; - angular_velocity_offset_stop.header = msg->header; - angular_velocity_offset_stop_estimate(velocity, imu, angular_velocity_offset_stop_parameter, &angular_velocity_offset_stop_status, &angular_velocity_offset_stop); - pub->publish(angular_velocity_offset_stop); -} + angular_velocity_offset_stop_parameter_.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + angular_velocity_offset_stop_parameter_.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + angular_velocity_offset_stop_parameter_.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as(); + angular_velocity_offset_stop_parameter_.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as(); -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_angular_velocity_offset_stop"); + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "imu_rate " << angular_velocity_offset_stop_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter_.stop_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter_.estimated_interval << std::endl; + std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter_.outlier_threshold << std::endl; + } + catch (YAML::Exception& e) + { + std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } - std::string subscribe_twist_topic_name = "vehicle/twist"; + sub1_ = this->create_subscription( + subscribe_twist_topic_name, 1000, + std::bind(&AngularVelocityOffsetStopNode::velocityCallback, this, std::placeholders::_1)); + sub2_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&AngularVelocityOffsetStopNode::imuCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("angular_velocity_offset_stop", 1000); + } - 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; +private: + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_; + sensor_msgs::msg::Imu imu_; - try - { - YAML::Node conf = YAML::LoadFile(yaml_file); + struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter_; + struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status_ = {}; - angular_velocity_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - angular_velocity_offset_stop_parameter.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as(); - angular_velocity_offset_stop_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as(); + rclcpp::Subscription::SharedPtr sub1_; + rclcpp::Subscription::SharedPtr sub2_; + rclcpp::Publisher::SharedPtr pub_; - std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; - std::cout << "imu_rate " << angular_velocity_offset_stop_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter.stop_judgment_threshold << std::endl; - std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter.estimated_interval << std::endl; - std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter.outlier_threshold << std::endl; - } - catch (YAML::Exception& e) + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + velocity_ = *msg; } - auto sub1 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); - auto sub2 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - pub = node->create_publisher("angular_velocity_offset_stop", 1000); - - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + imu_.header = msg->header; + imu_.orientation = msg->orientation; + imu_.orientation_covariance = msg->orientation_covariance; + imu_.angular_velocity = msg->angular_velocity; + imu_.angular_velocity_covariance = msg->angular_velocity_covariance; + imu_.linear_acceleration = msg->linear_acceleration; + imu_.linear_acceleration_covariance = msg->linear_acceleration_covariance; + angular_velocity_offset_stop_.header = msg->header; + angular_velocity_offset_stop_estimate(velocity_, imu_, angular_velocity_offset_stop_parameter_, &angular_velocity_offset_stop_status_, &angular_velocity_offset_stop_); + pub_->publish(angular_velocity_offset_stop_); + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/correction_imu.cpp b/eagleye_rt/src/correction_imu.cpp index 89c5287e..9f18b1d8 100644 --- a/eagleye_rt/src/correction_imu.cpp +++ b/eagleye_rt/src/correction_imu.cpp @@ -32,79 +32,98 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset; -static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop; -static eagleye_msgs::msg::AccXOffset acc_x_offset; -static eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor; -static sensor_msgs::msg::Imu imu; - -static sensor_msgs::msg::Imu correction_imu; - - -void yaw_rate_offset_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset = *msg; -} - -void angular_velocity_offset_stop_callback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg) -{ - angular_velocity_offset_stop = *msg; -} - -void acc_x_offset_callback(const eagleye_msgs::msg::AccXOffset::ConstSharedPtr msg) -{ - acc_x_offset = *msg; -} - -void acc_x_scale_factor_callback(const eagleye_msgs::msg::AccXScaleFactor::ConstSharedPtr msg) -{ - acc_x_scale_factor = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class CorrectionImuNode : public rclcpp::Node { - imu = *msg; +public: + CorrectionImuNode() : Node("eagleye_correction_imu") + { + sub1_ = this->create_subscription( + "yaw_rate_offset_2nd", rclcpp::QoS(10), + std::bind(&CorrectionImuNode::yawRateOffsetCallback, this, std::placeholders::_1)); + sub2_ = this->create_subscription( + "angular_velocity_offset_stop", rclcpp::QoS(10), + std::bind(&CorrectionImuNode::angularVelocityOffsetStopCallback, this, std::placeholders::_1)); + sub3_ = this->create_subscription( + "acc_x_offset", rclcpp::QoS(10), + std::bind(&CorrectionImuNode::accXOffsetCallback, this, std::placeholders::_1)); + sub4_ = this->create_subscription( + "acc_x_scale_factor", rclcpp::QoS(10), + std::bind(&CorrectionImuNode::accXScaleFactorCallback, this, std::placeholders::_1)); + sub5_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&CorrectionImuNode::imuCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("imu/data_corrected", rclcpp::QoS(10)); + } - correction_imu.header = imu.header; - correction_imu.orientation = imu.orientation; - correction_imu.orientation_covariance = imu.orientation_covariance; - correction_imu.angular_velocity_covariance = imu.angular_velocity_covariance; - correction_imu.linear_acceleration_covariance = imu.linear_acceleration_covariance; +private: + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_; + eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_; + eagleye_msgs::msg::AccXOffset acc_x_offset_; + eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor_; + sensor_msgs::msg::Imu imu_; + sensor_msgs::msg::Imu correction_imu_; + + rclcpp::Subscription::SharedPtr sub1_; + rclcpp::Subscription::SharedPtr sub2_; + rclcpp::Subscription::SharedPtr sub3_; + rclcpp::Subscription::SharedPtr sub4_; + rclcpp::Subscription::SharedPtr sub5_; + rclcpp::Publisher::SharedPtr pub_; + + void yawRateOffsetCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_ = *msg; + } - if (acc_x_offset.status.enabled_status == true && acc_x_scale_factor.status.enabled_status) + void angularVelocityOffsetStopCallback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg) { - correction_imu.linear_acceleration.x = imu.linear_acceleration.x * acc_x_scale_factor.acc_x_scale_factor + acc_x_offset.acc_x_offset; - correction_imu.linear_acceleration.y = imu.linear_acceleration.y; - correction_imu.linear_acceleration.z = imu.linear_acceleration.z; + angular_velocity_offset_stop_ = *msg; } - else + + void accXOffsetCallback(const eagleye_msgs::msg::AccXOffset::ConstSharedPtr msg) { - correction_imu.linear_acceleration.x = imu.linear_acceleration.x; - correction_imu.linear_acceleration.y = imu.linear_acceleration.y; - correction_imu.linear_acceleration.z = imu.linear_acceleration.z; + acc_x_offset_ = *msg; } - correction_imu.angular_velocity.x = imu.angular_velocity.x + angular_velocity_offset_stop.angular_velocity_offset.x; - correction_imu.angular_velocity.y = imu.angular_velocity.y + angular_velocity_offset_stop.angular_velocity_offset.y; - correction_imu.angular_velocity.z = -1 * (imu.angular_velocity.z + angular_velocity_offset_stop.angular_velocity_offset.z); + void accXScaleFactorCallback(const eagleye_msgs::msg::AccXScaleFactor::ConstSharedPtr msg) + { + acc_x_scale_factor_ = *msg; + } - pub->publish(correction_imu); -} + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + imu_ = *msg; + + correction_imu_.header = imu_.header; + correction_imu_.orientation = imu_.orientation; + correction_imu_.orientation_covariance = imu_.orientation_covariance; + correction_imu_.angular_velocity_covariance = imu_.angular_velocity_covariance; + correction_imu_.linear_acceleration_covariance = imu_.linear_acceleration_covariance; + + if (acc_x_offset_.status.enabled_status == true && acc_x_scale_factor_.status.enabled_status) + { + correction_imu_.linear_acceleration.x = imu_.linear_acceleration.x * acc_x_scale_factor_.acc_x_scale_factor + acc_x_offset_.acc_x_offset; + correction_imu_.linear_acceleration.y = imu_.linear_acceleration.y; + correction_imu_.linear_acceleration.z = imu_.linear_acceleration.z; + } + else + { + correction_imu_.linear_acceleration.x = imu_.linear_acceleration.x; + correction_imu_.linear_acceleration.y = imu_.linear_acceleration.y; + correction_imu_.linear_acceleration.z = imu_.linear_acceleration.z; + } + + correction_imu_.angular_velocity.x = imu_.angular_velocity.x + angular_velocity_offset_stop_.angular_velocity_offset.x; + correction_imu_.angular_velocity.y = imu_.angular_velocity.y + angular_velocity_offset_stop_.angular_velocity_offset.y; + correction_imu_.angular_velocity.z = -1 * (imu_.angular_velocity.z + angular_velocity_offset_stop_.angular_velocity_offset.z); + + pub_->publish(correction_imu_); + } +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_correction_imu"); - - auto sub1 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("angular_velocity_offset_stop", rclcpp::QoS(10), angular_velocity_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("acc_x_offset", rclcpp::QoS(10), acc_x_offset_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("acc_x_scale_factor", rclcpp::QoS(10), acc_x_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - pub = node->create_publisher("imu/data_corrected", rclcpp::QoS(10)); - - rclcpp::spin(node); - + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/distance_node.cpp b/eagleye_rt/src/distance_node.cpp index 07bb9b8c..f7de6345 100644 --- a/eagleye_rt/src/distance_node.cpp +++ b/eagleye_rt/src/distance_node.cpp @@ -28,52 +28,61 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -rclcpp::Publisher::SharedPtr _pub; -static geometry_msgs::msg::TwistStamped _velocity; -static eagleye_msgs::msg::StatusStamped _velocity_status; -static eagleye_msgs::msg::Distance _distance; - -struct DistanceStatus _distance_status; +class DistanceNode : public rclcpp::Node +{ +public: + DistanceNode() : Node("eagleye_distance") + { + this->declare_parameter("use_can_less_mode", use_can_less_mode_); + this->get_parameter("use_can_less_mode", use_can_less_mode_); -static bool _use_can_less_mode; + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&DistanceNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&DistanceNode::velocityStatusCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("distance", rclcpp::QoS(10)); + } -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - _velocity_status = *msg; -} +private: + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::Distance distance_; + DistanceStatus distance_status_ = {}; + bool use_can_less_mode_ = false; -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - if(_use_can_less_mode && !_velocity_status.status.enabled_status) return; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; - _velocity = *msg; - _distance.header = msg->header; - _distance.header.frame_id = "base_link"; - distance_estimate(_velocity, &_distance_status, &_distance); + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) + { + velocity_status_ = *msg; + } - if (_distance_status.time_last != 0) + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - _pub->publish(_distance); + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + velocity_ = *msg; + distance_.header = msg->header; + distance_.header.frame_id = "base_link"; + distance_estimate(velocity_, &distance_status_, &distance_); + + if (distance_status_.time_last != 0) { + pub_->publish(distance_); + } } -} +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_distance"); - - node->declare_parameter("use_can_less_mode",_use_can_less_mode); - node->get_parameter("use_can_less_mode",_use_can_less_mode); - - auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub2 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - _pub = node->create_publisher("distance", rclcpp::QoS(10)); - - rclcpp::spin(node); - + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/enable_additional_rolling_node.cpp b/eagleye_rt/src/enable_additional_rolling_node.cpp index f363779a..2d148aec 100644 --- a/eagleye_rt/src/enable_additional_rolling_node.cpp +++ b/eagleye_rt/src/enable_additional_rolling_node.cpp @@ -30,148 +30,188 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -rclcpp::Publisher::SharedPtr _pub1; -rclcpp::Publisher::SharedPtr _pub2; - -static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor; -static geometry_msgs::msg::TwistStamped _velocity; -static eagleye_msgs::msg::StatusStamped _velocity_status; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_2nd; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_stop; -static eagleye_msgs::msg::Distance _distance; -static geometry_msgs::msg::PoseStamped _localization_pose; -static eagleye_msgs::msg::AngularVelocityOffset _angular_velocity_offset_stop; -static sensor_msgs::msg::Imu _imu; - -static eagleye_msgs::msg::Rolling _rolling_angle; -static eagleye_msgs::msg::AccYOffset _acc_y_offset; - -struct EnableAdditionalRollingParameter _rolling_parameter; -struct EnableAdditionalRollingStatus _rolling_status; - -static bool _use_can_less_mode; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - _velocity_status = *msg; -} - -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) -{ - _velocity_scale_factor = *msg; -} - -void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) -{ - _distance = *msg; -} - -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_2nd = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_stop = *msg; -} - -void localization_pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - _localization_pose = *msg; -} - -void angular_velocity_offset_stop_callback(const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg) -{ - _angular_velocity_offset_stop = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class EnableAdditionalRollingNode : public rclcpp::Node { - if(_use_can_less_mode && !_velocity_status.status.enabled_status) return; - - eagleye_msgs::msg::StatusStamped velocity_enable_status; - if(_use_can_less_mode) +public: + EnableAdditionalRollingNode() : Node("eagleye_enable_additional_rolling") { - velocity_enable_status = _velocity_status; + std::string subscribe_localization_pose_topic_name; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + subscribe_localization_pose_topic_name = + conf["/**"]["ros__parameters"]["localization_pose_topic"].as(); + + rolling_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + rolling_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + rolling_parameter_.update_distance = + conf["/**"]["ros__parameters"]["enable_additional_rolling"]["update_distance"] + .as(); + rolling_parameter_.moving_average_time = + conf["/**"]["ros__parameters"]["enable_additional_rolling"]["moving_average_time"] + .as(); + rolling_parameter_.sync_judgment_threshold = + conf["/**"]["ros__parameters"]["enable_additional_rolling"]["sync_judgment_threshold"] + .as(); + rolling_parameter_.sync_search_period = + conf["/**"]["ros__parameters"]["enable_additional_rolling"]["sync_search_period"] + .as(); + + std::cout << "subscribe_localization_pose_topic_name " + << subscribe_localization_pose_topic_name << std::endl; + std::cout << "imu_rate " << rolling_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " << rolling_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "update_distance " << rolling_parameter_.update_distance << std::endl; + std::cout << "moving_average_time " << rolling_parameter_.moving_average_time << std::endl; + std::cout << "sync_judgment_threshold " << rolling_parameter_.sync_judgment_threshold + << std::endl; + std::cout << "sync_search_period " << rolling_parameter_.sync_search_period << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31menable_additional_rolling Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + + sub_velocity_scale_factor_ = + this->create_subscription( + "velocity_scale_factor", 1000, + std::bind( + &EnableAdditionalRollingNode::velocityScaleFactorCallback, this, + std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", 1000, + std::bind( + &EnableAdditionalRollingNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", 1000, + std::bind( + &EnableAdditionalRollingNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_distance_ = this->create_subscription( + "distance", 1000, + std::bind(&EnableAdditionalRollingNode::distanceCallback, this, std::placeholders::_1)); + sub_localization_pose_ = this->create_subscription( + subscribe_localization_pose_topic_name, 1000, + std::bind( + &EnableAdditionalRollingNode::localizationPoseCallback, this, std::placeholders::_1)); + sub_angular_velocity_offset_stop_ = + this->create_subscription( + "angular_velocity_offset_stop", 1000, + std::bind( + &EnableAdditionalRollingNode::angularVelocityOffsetStopCallback, this, + std::placeholders::_1)); + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&EnableAdditionalRollingNode::imuCallback, this, std::placeholders::_1)); + + pub_acc_y_offset_ = this->create_publisher( + "acc_y_offset_additional_rolling", 1000); + pub_rolling_ = + this->create_publisher("enable_additional_rolling", 1000); } - else + +private: + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::Distance distance_; + geometry_msgs::msg::PoseStamped localization_pose_; + eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_; + sensor_msgs::msg::Imu imu_; + + eagleye_msgs::msg::Rolling rolling_angle_; + eagleye_msgs::msg::AccYOffset acc_y_offset_; + + EnableAdditionalRollingParameter rolling_parameter_; + EnableAdditionalRollingStatus rolling_status_ = {}; + + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_acc_y_offset_; + rclcpp::Publisher::SharedPtr pub_rolling_; + rclcpp::Subscription::SharedPtr + sub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_distance_; + rclcpp::Subscription::SharedPtr sub_localization_pose_; + rclcpp::Subscription::SharedPtr + sub_angular_velocity_offset_stop_; + rclcpp::Subscription::SharedPtr sub_imu_; + + void velocityScaleFactorCallback( + const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { - velocity_enable_status.header = _velocity_scale_factor.header; - velocity_enable_status.status = _velocity_scale_factor.status; + velocity_scale_factor_ = *msg; } - _imu = *msg; - _acc_y_offset.header = msg->header; - _acc_y_offset.header.frame_id = "imu"; - _rolling_angle.header = msg->header; - _rolling_angle.header.frame_id = "base_link"; - enable_additional_rolling_estimate(_velocity, velocity_enable_status, _yaw_rate_offset_2nd, _yaw_rate_offset_stop, _distance, _imu, - _localization_pose, _angular_velocity_offset_stop, _rolling_parameter, &_rolling_status, &_rolling_angle, &_acc_y_offset); - _pub1->publish(_acc_y_offset); - _pub2->publish(_rolling_angle); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_enable_additional_rolling"); - - std::string subscribe_localization_pose_topic_name; - - 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; - - try + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - subscribe_localization_pose_topic_name = conf["/**"]["ros__parameters"]["localization_pose_topic"].as(); - - _rolling_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - _rolling_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - - _rolling_parameter.update_distance = conf["/**"]["ros__parameters"]["enable_additional_rolling"]["update_distance"].as(); - _rolling_parameter.moving_average_time = conf["/**"]["ros__parameters"]["enable_additional_rolling"]["moving_average_time"].as(); - _rolling_parameter.sync_judgment_threshold = conf["/**"]["ros__parameters"]["enable_additional_rolling"]["sync_judgment_threshold"].as(); - _rolling_parameter.sync_search_period = conf["/**"]["ros__parameters"]["enable_additional_rolling"]["sync_search_period"].as(); - - std::cout<< "subscribe_localization_pose_topic_name " << subscribe_localization_pose_topic_name << std::endl; - - std::cout << "imu_rate " << _rolling_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << _rolling_parameter.stop_judgment_threshold << std::endl; + yaw_rate_offset_2nd_ = *msg; + } - std::cout << "update_distance " << _rolling_parameter.update_distance << std::endl; - std::cout << "moving_average_time " << _rolling_parameter.moving_average_time << std::endl; - std::cout << "sync_judgment_threshold " << _rolling_parameter.sync_judgment_threshold << std::endl; - std::cout << "sync_search_period " << _rolling_parameter.sync_search_period << std::endl; + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; } - catch (YAML::Exception& e) + + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { - std::cerr << "\033[1;31menable_additional_rolling Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + distance_ = *msg; } - auto sub1 = node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); - auto sub2 = node->create_subscription("yaw_rate_offset_2nd", 1000, yaw_rate_offset_2nd_callback); - auto sub3 = node->create_subscription("yaw_rate_offset_stop", 1000, yaw_rate_offset_stop_callback); - auto sub4 = node->create_subscription("distance", 1000, distance_callback); - auto sub5 = node->create_subscription(subscribe_localization_pose_topic_name, 1000, localization_pose_callback); - auto sub6 = node->create_subscription("angular_velocity_offset_stop", 1000, angular_velocity_offset_stop_callback); - auto sub7 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + void localizationPoseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) + { + localization_pose_ = *msg; + } - _pub1 = node->create_publisher("acc_y_offset_additional_rolling", 1000); - _pub2 = node->create_publisher("enable_additional_rolling", 1000); + void angularVelocityOffsetStopCallback( + const eagleye_msgs::msg::AngularVelocityOffset::ConstSharedPtr msg) + { + angular_velocity_offset_stop_ = *msg; + } - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if (use_can_less_mode_) { + velocity_enable_status = velocity_status_; + } else { + velocity_enable_status.header = velocity_scale_factor_.header; + velocity_enable_status.status = velocity_scale_factor_.status; + } + + imu_ = *msg; + acc_y_offset_.header = msg->header; + acc_y_offset_.header.frame_id = "imu"; + rolling_angle_.header = msg->header; + rolling_angle_.header.frame_id = "base_link"; + enable_additional_rolling_estimate( + velocity_, velocity_enable_status, yaw_rate_offset_2nd_, yaw_rate_offset_stop_, distance_, + imu_, localization_pose_, angular_velocity_offset_stop_, rolling_parameter_, + &rolling_status_, &rolling_angle_, &acc_y_offset_); + pub_acc_y_offset_->publish(acc_y_offset_); + pub_rolling_->publish(rolling_angle_); + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; -} \ No newline at end of file +} diff --git a/eagleye_rt/src/heading_interpolate_node.cpp b/eagleye_rt/src/heading_interpolate_node.cpp index 64299f62..c14da259 100644 --- a/eagleye_rt/src/heading_interpolate_node.cpp +++ b/eagleye_rt/src/heading_interpolate_node.cpp @@ -28,144 +28,165 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset; -static eagleye_msgs::msg::Heading heading; -static eagleye_msgs::msg::SlipAngle slip_angle; - -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::Heading heading_interpolate; - -struct HeadingInterpolateParameter heading_interpolate_parameter; -struct HeadingInterpolateStatus heading_interpolate_status; - -static bool _use_can_less_mode; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset = *msg; -} - -void heading_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading = *msg; -} - -void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) -{ - slip_angle = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class HeadingInterpolateNode : public rclcpp::Node { - if(_use_can_less_mode && !velocity_status.status.enabled_status) return; - - imu = *msg; - heading_interpolate.header = msg->header; - heading_interpolate.header.frame_id = "base_link"; - heading_interpolate_estimate(imu,velocity,yaw_rate_offset_stop,yaw_rate_offset,heading,slip_angle,heading_interpolate_parameter, - &heading_interpolate_status,&heading_interpolate); - pub->publish(heading_interpolate); -} +public: + HeadingInterpolateNode(int argc, char** argv) : Node("eagleye_heading_interpolate") + { + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + heading_interpolate_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + heading_interpolate_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + heading_interpolate_parameter_.sync_search_period = + conf["/**"]["ros__parameters"]["heading_interpolate"]["sync_search_period"].as(); + heading_interpolate_parameter_.proc_noise = + conf["/**"]["ros__parameters"]["heading_interpolate"]["proc_noise"].as(); + + std::cout << "imu_rate " << heading_interpolate_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " + << heading_interpolate_parameter_.stop_judgment_threshold << std::endl; + std::cout << "sync_search_period " << heading_interpolate_parameter_.sync_search_period + << std::endl; + std::cout << "proc_noise " << heading_interpolate_parameter_.proc_noise << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_heading_interpolate"); + std::string publish_topic_name = "/publish_topic_name/invalid"; + std::string subscribe_topic_name_1 = "/subscribe_topic_name/invalid_1"; + std::string subscribe_topic_name_2 = "/subscribe_topic_name/invalid_2"; + + if (argc > 2) { + if (strcmp(argv[1], "1st") == 0) { + publish_topic_name = "heading_interpolate_1st"; + subscribe_topic_name_1 = "yaw_rate_offset_stop"; + subscribe_topic_name_2 = "heading_1st"; + } else if (strcmp(argv[1], "2nd") == 0) { + publish_topic_name = "heading_interpolate_2nd"; + subscribe_topic_name_1 = "yaw_rate_offset_1st"; + subscribe_topic_name_2 = "heading_2nd"; + } else if (strcmp(argv[1], "3rd") == 0) { + publish_topic_name = "heading_interpolate_3rd"; + subscribe_topic_name_1 = "yaw_rate_offset_2nd"; + subscribe_topic_name_2 = "heading_3rd"; + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid argument"); + rclcpp::shutdown(); + } + } else { + RCLCPP_ERROR(this->get_logger(), "No arguments"); + rclcpp::shutdown(); + } - 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; + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&HeadingInterpolateNode::imuCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&HeadingInterpolateNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&HeadingInterpolateNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind( + &HeadingInterpolateNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_ = this->create_subscription( + subscribe_topic_name_1, 1000, + std::bind(&HeadingInterpolateNode::yawRateOffsetCallback, this, std::placeholders::_1)); + sub_heading_ = this->create_subscription( + subscribe_topic_name_2, 1000, + std::bind(&HeadingInterpolateNode::headingCallback, this, std::placeholders::_1)); + sub_slip_angle_ = this->create_subscription( + "slip_angle", rclcpp::QoS(10), + std::bind(&HeadingInterpolateNode::slipAngleCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher(publish_topic_name, rclcpp::QoS(10)); + } - try +private: + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_; + eagleye_msgs::msg::Heading heading_; + eagleye_msgs::msg::SlipAngle slip_angle_; + eagleye_msgs::msg::Heading heading_interpolate_; + HeadingInterpolateParameter heading_interpolate_parameter_; + HeadingInterpolateStatus heading_interpolate_status_ = {}; + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_; + rclcpp::Subscription::SharedPtr sub_heading_; + rclcpp::Subscription::SharedPtr sub_slip_angle_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - heading_interpolate_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - heading_interpolate_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - heading_interpolate_parameter.sync_search_period = conf["/**"]["ros__parameters"]["heading_interpolate"]["sync_search_period"].as(); - heading_interpolate_parameter.proc_noise = conf["/**"]["ros__parameters"]["heading_interpolate"]["proc_noise"].as(); - - std::cout << "imu_rate " << heading_interpolate_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << heading_interpolate_parameter.stop_judgment_threshold << std::endl; - std::cout << "sync_search_period " << heading_interpolate_parameter.sync_search_period << std::endl; - std::cout << "proc_noise " << heading_interpolate_parameter.proc_noise << std::endl; + velocity_ = *msg; } - catch (YAML::Exception& e) + + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + velocity_status_ = *msg; } - std::string publish_topic_name = "/publish_topic_name/invalid"; - std::string subscribe_topic_name_1 = "/subscribe_topic_name/invalid_1"; - std::string subscribe_topic_name_2 = "/subscribe_topic_name/invalid_2"; + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; + } - if (argc > 2) + void yawRateOffsetCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - if (strcmp(argv[1], "1st") == 0) - { - publish_topic_name = "heading_interpolate_1st"; - subscribe_topic_name_1 = "yaw_rate_offset_stop"; - subscribe_topic_name_2 = "heading_1st"; - } - else if (strcmp(argv[1], "2nd") == 0) - { - publish_topic_name = "heading_interpolate_2nd"; - subscribe_topic_name_1 = "yaw_rate_offset_1st"; - subscribe_topic_name_2 = "heading_2nd"; - } - else if (strcmp(argv[1], "3rd") == 0) - { - publish_topic_name = "heading_interpolate_3rd"; - subscribe_topic_name_1 = "yaw_rate_offset_2nd"; - subscribe_topic_name_2 = "heading_3rd"; - } - else - { - RCLCPP_ERROR(node->get_logger(),"Invalid argument"); - rclcpp::shutdown(); - } + yaw_rate_offset_ = *msg; } - else + + void headingCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - RCLCPP_ERROR(node->get_logger(),"No arguments"); - rclcpp::shutdown(); + heading_ = *msg; } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub3 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub4 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); - auto sub5 = node->create_subscription(subscribe_topic_name_1, 1000, yaw_rate_offset_callback); - auto sub6 = node->create_subscription(subscribe_topic_name_2, 1000, heading_callback); - auto sub7 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); - pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); + void slipAngleCallback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) + { + slip_angle_ = *msg; + } - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + imu_ = *msg; + heading_interpolate_.header = msg->header; + heading_interpolate_.header.frame_id = "base_link"; + heading_interpolate_estimate( + imu_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_, heading_, slip_angle_, + heading_interpolate_parameter_, &heading_interpolate_status_, &heading_interpolate_); + pub_->publish(heading_interpolate_); + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(argc, argv)); return 0; } diff --git a/eagleye_rt/src/heading_node.cpp b/eagleye_rt/src/heading_node.cpp index 3543c877..5e4b3554 100644 --- a/eagleye_rt/src/heading_node.cpp +++ b/eagleye_rt/src/heading_node.cpp @@ -28,259 +28,287 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static nmea_msgs::msg::Gprmc nmea_rmc; -static eagleye_msgs::msg::Heading multi_antenna_heading; -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset; -static eagleye_msgs::msg::SlipAngle slip_angle; -static eagleye_msgs::msg::Heading heading_interpolate; - -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::Heading heading; - -struct HeadingParameter heading_parameter; -struct HeadingStatus heading_status; - -std::string use_gnss_mode; -static bool use_can_less_mode; -static bool use_multi_antenna_mode; - -bool is_first_correction_velocity = false; - -bool skip_static_initialization = false; -double yaw_rate_offset_stop_in_skip_mode = 0.0; - -std::string node_name = "eagleye_heading"; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - rtklib_nav = *msg; -} - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +class HeadingNode : public rclcpp::Node { - velocity = *msg; - // To avoid unnecessary buffering when it's just sitting there right after start-up, we're making it so it doesn't buffer. - // Multi-antenna mode is an exception. - if (is_first_correction_velocity == false && msg->twist.linear.x > heading_parameter.moving_judgment_threshold) +public: + HeadingNode(int argc, char** argv) : Node("eagleye_heading") { - is_first_correction_velocity = true; - } -} - -void pose_callback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - tf2::Quaternion orientation; - tf2::fromMsg(msg->pose.orientation, orientation); - double roll, pitch, yaw; - tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); - double heading = - yaw + (90* M_PI / 180); - - multi_antenna_heading.header = msg->header; - multi_antenna_heading.heading_angle = heading; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset = *msg; -} - -void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) -{ - slip_angle = *msg; -} + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_rmc_topic_name = "gnss/rmc"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + this->declare_parameter("use_multi_antenna_mode", use_multi_antenna_mode_); + this->get_parameter("use_multi_antenna_mode", use_multi_antenna_mode_); + std::cout << "yaml_file: " << yaml_file << std::endl; + std::cout << "use_multi_antenna_mode: " << use_multi_antenna_mode_ << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_gnss_mode_ = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); + heading_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + heading_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + heading_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + heading_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + heading_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["heading"]["estimated_minimum_interval"].as(); + heading_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["heading"]["estimated_maximum_interval"].as(); + heading_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["heading"]["gnss_receiving_threshold"].as(); + heading_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["heading"]["outlier_threshold"].as(); + heading_parameter_.outlier_ratio_threshold = + conf["/**"]["ros__parameters"]["heading"]["outlier_ratio_threshold"].as(); + heading_parameter_.curve_judgment_threshold = + conf["/**"]["ros__parameters"]["heading"]["curve_judgment_threshold"].as(); + heading_parameter_.init_STD = + conf["/**"]["ros__parameters"]["heading"]["init_STD"].as(); + skip_static_initialization_ = + conf["/**"]["ros__parameters"]["heading"]["skip_static_initialization"].as(); + yaw_rate_offset_stop_in_skip_mode_ = + conf["/**"]["ros__parameters"]["heading"]["yaw_rate_offset_stop_in_skip_mode"].as(); + + std::cout << "use_gnss_mode " << use_gnss_mode_ << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name + << std::endl; + std::cout << "subscribe_rmc_topic_name " << subscribe_rmc_topic_name << std::endl; + std::cout << "imu_rate " << heading_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << heading_parameter_.gnss_rate << std::endl; + std::cout << "stop_judgment_threshold " << heading_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "moving_judgment_threshold " << heading_parameter_.moving_judgment_threshold + << std::endl; + std::cout << "estimated_minimum_interval " << heading_parameter_.estimated_minimum_interval + << std::endl; + std::cout << "estimated_maximum_interval " << heading_parameter_.estimated_maximum_interval + << std::endl; + std::cout << "gnss_receiving_threshold " << heading_parameter_.gnss_receiving_threshold + << std::endl; + std::cout << "outlier_threshold " << heading_parameter_.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << heading_parameter_.outlier_ratio_threshold + << std::endl; + std::cout << "curve_judgment_threshold " << heading_parameter_.curve_judgment_threshold + << std::endl; + std::cout << "init_STD " << heading_parameter_.init_STD << std::endl; + std::cout << "skip_static_initialization " << skip_static_initialization_ << std::endl; + std::cout << "yaw_rate_offset_stop_in_skip_mode " << yaw_rate_offset_stop_in_skip_mode_ + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mheading Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } -void heading_interpolate_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate = *msg; -} + std::string publish_topic_name = "/publish_topic_name/invalid"; + std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; + std::string subscribe_topic_name2 = "/subscribe_topic_name2/invalid"; + + if (argc > 2) { + if (strcmp(argv[1], "1st") == 0) { + publish_topic_name = "heading_1st"; + subscribe_topic_name = "yaw_rate_offset_stop"; + subscribe_topic_name2 = "heading_interpolate_1st"; + } else if (strcmp(argv[1], "2nd") == 0) { + publish_topic_name = "heading_2nd"; + subscribe_topic_name = "yaw_rate_offset_1st"; + subscribe_topic_name2 = "heading_interpolate_2nd"; + } else if (strcmp(argv[1], "3rd") == 0) { + publish_topic_name = "heading_3rd"; + subscribe_topic_name = "yaw_rate_offset_2nd"; + subscribe_topic_name2 = "heading_interpolate_3rd"; + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid argument"); + rclcpp::shutdown(); + } + } else { + RCLCPP_ERROR(this->get_logger(), "No arguments"); + rclcpp::shutdown(); + } -void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) -{ - nmea_rmc = *msg; -} + if (use_multi_antenna_mode_) { + is_first_correction_velocity_ = true; + } -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if (!is_first_correction_velocity) - { - RCLCPP_WARN(rclcpp::get_logger(node_name), "is_first_correction_velocity is false."); - return; + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&HeadingNode::imuCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&HeadingNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_rmc_ = this->create_subscription( + subscribe_rmc_topic_name, 1000, + std::bind(&HeadingNode::rmcCallback, this, std::placeholders::_1)); + sub_gnss_compass_pose_ = this->create_subscription( + "gnss_compass_pose", 1000, + std::bind(&HeadingNode::poseCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&HeadingNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&HeadingNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&HeadingNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_ = this->create_subscription( + subscribe_topic_name, 1000, + std::bind(&HeadingNode::yawRateOffsetCallback, this, std::placeholders::_1)); + sub_slip_angle_ = this->create_subscription( + "slip_angle", rclcpp::QoS(10), + std::bind(&HeadingNode::slipAngleCallback, this, std::placeholders::_1)); + sub_heading_interpolate_ = this->create_subscription( + subscribe_topic_name2, 1000, + std::bind(&HeadingNode::headingInterpolateCallback, this, std::placeholders::_1)); + pub_ = + this->create_publisher(publish_topic_name, rclcpp::QoS(10)); } - if(use_can_less_mode && !velocity_status.status.enabled_status) + +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + nmea_msgs::msg::Gprmc nmea_rmc_; + eagleye_msgs::msg::Heading multi_antenna_heading_; + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_; + eagleye_msgs::msg::SlipAngle slip_angle_; + eagleye_msgs::msg::Heading heading_interpolate_; + eagleye_msgs::msg::Heading heading_; + HeadingParameter heading_parameter_; + HeadingStatus heading_status_ = {}; + std::string use_gnss_mode_; + bool use_can_less_mode_ = false; + bool use_multi_antenna_mode_ = false; + bool is_first_correction_velocity_ = false; + bool skip_static_initialization_ = false; + double yaw_rate_offset_stop_in_skip_mode_ = 0.0; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_rmc_; + rclcpp::Subscription::SharedPtr sub_gnss_compass_pose_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_; + rclcpp::Subscription::SharedPtr sub_slip_angle_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - RCLCPP_WARN(rclcpp::get_logger(node_name), "velocity_status is not enabled."); - return; + rtklib_nav_ = *msg; } - if(!yaw_rate_offset_stop.status.enabled_status) + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - if(skip_static_initialization) - { - yaw_rate_offset_stop.yaw_rate_offset = yaw_rate_offset_stop_in_skip_mode; - } - else - { - RCLCPP_WARN(rclcpp::get_logger(node_name), "Heading estimation is not started because the stop calibration is not yet completed."); - return; + velocity_ = *msg; + if ( + is_first_correction_velocity_ == false && + msg->twist.linear.x > heading_parameter_.moving_judgment_threshold) { + is_first_correction_velocity_ = true; } } - imu = *msg; - heading.header = msg->header; - heading.header.frame_id = "base_link"; - bool use_rtklib_mode = use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB"; - bool use_nmea_mode = use_gnss_mode == "nmea" || use_gnss_mode == "NMEA"; - if (use_rtklib_mode && !use_multi_antenna_mode) // use RTKLIB mode - heading_estimate(rtklib_nav,imu,velocity,yaw_rate_offset_stop,yaw_rate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); - else if (use_nmea_mode && !use_multi_antenna_mode) // use NMEA mode - heading_estimate(nmea_rmc,imu,velocity,yaw_rate_offset_stop,yaw_rate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); - else if (use_multi_antenna_mode) - heading_estimate(multi_antenna_heading, imu, velocity, yaw_rate_offset_stop, yaw_rate_offset, slip_angle, - heading_interpolate, heading_parameter, &heading_status, &heading); - - if (heading.status.estimate_status == true || use_multi_antenna_mode) + void poseCallback(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) { - pub->publish(heading); + tf2::Quaternion orientation; + tf2::fromMsg(msg->pose.orientation, orientation); + double roll, pitch, yaw; + tf2::Matrix3x3(orientation).getRPY(roll, pitch, yaw); + double heading = -yaw + (90 * M_PI / 180); + + multi_antenna_heading_.header = msg->header; + multi_antenna_heading_.heading_angle = heading; } - heading.status.estimate_status = false; -} -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(node_name); - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - std::string subscribe_rmc_topic_name = "gnss/rmc"; - - std::string yaml_file; - node->declare_parameter("yaml_file",yaml_file); - node->get_parameter("yaml_file",yaml_file); - node->declare_parameter("use_multi_antenna_mode",use_multi_antenna_mode); - node->get_parameter("use_multi_antenna_mode",use_multi_antenna_mode); - std::cout << "yaml_file: " << yaml_file << std::endl; - std::cout << "use_multi_antenna_mode: " << use_multi_antenna_mode << std::endl; - - try + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - - heading_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - heading_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - heading_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - heading_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - heading_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["heading"]["estimated_minimum_interval"].as(); - heading_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["heading"]["estimated_maximum_interval"].as(); - heading_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["heading"]["gnss_receiving_threshold"].as(); - heading_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_threshold"].as(); - heading_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["heading"]["outlier_ratio_threshold"].as(); - heading_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["heading"]["curve_judgment_threshold"].as(); - heading_parameter.init_STD = conf["/**"]["ros__parameters"]["heading"]["init_STD"].as(); - skip_static_initialization = conf["/**"]["ros__parameters"]["heading"]["skip_static_initialization"].as(); - yaw_rate_offset_stop_in_skip_mode = conf["/**"]["ros__parameters"]["heading"]["yaw_rate_offset_stop_in_skip_mode"].as(); - - std::cout<< "use_gnss_mode " << use_gnss_mode << std::endl; - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - std::cout<< "subscribe_rmc_topic_name " << subscribe_rmc_topic_name << std::endl; - - std::cout << "imu_rate " << heading_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << heading_parameter.gnss_rate << std::endl; - std::cout << "stop_judgment_threshold " << heading_parameter.stop_judgment_threshold << std::endl; - std::cout << "moving_judgment_threshold " << heading_parameter.moving_judgment_threshold << std::endl; - - std::cout << "estimated_minimum_interval " << heading_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << heading_parameter.estimated_maximum_interval << std::endl; - std::cout << "gnss_receiving_threshold " << heading_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_threshold " << heading_parameter.outlier_threshold << std::endl; - std::cout << "outlier_ratio_threshold " << heading_parameter.outlier_ratio_threshold << std::endl; - std::cout << "curve_judgment_threshold " << heading_parameter.curve_judgment_threshold << std::endl; - std::cout << "init_STD " << heading_parameter.init_STD << std::endl; - std::cout << "skip_static_initialization " << skip_static_initialization << std::endl; - std::cout << "yaw_rate_offset_stop_in_skip_mode " << yaw_rate_offset_stop_in_skip_mode << std::endl; + velocity_status_ = *msg; } - catch (YAML::Exception& e) + + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - std::cerr << "\033[1;31mheading Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + yaw_rate_offset_stop_ = *msg; } - std::string publish_topic_name = "/publish_topic_name/invalid"; - std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; - std::string subscribe_topic_name2 = "/subscribe_topic_name2/invalid"; - - if (argc > 2) + void yawRateOffsetCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - if (strcmp(argv[1], "1st") == 0) - { - publish_topic_name = "heading_1st"; - subscribe_topic_name = "yaw_rate_offset_stop"; - subscribe_topic_name2 = "heading_interpolate_1st"; - } - else if (strcmp(argv[1], "2nd") == 0) - { - publish_topic_name = "heading_2nd"; - subscribe_topic_name = "yaw_rate_offset_1st"; - subscribe_topic_name2 = "heading_interpolate_2nd"; - } - else if (strcmp(argv[1], "3rd") == 0) - { - publish_topic_name = "heading_3rd"; - subscribe_topic_name = "yaw_rate_offset_2nd"; - subscribe_topic_name2 = "heading_interpolate_3rd"; - } - else - { - RCLCPP_ERROR(node->get_logger(),"Invalid argument"); - rclcpp::shutdown(); - } + yaw_rate_offset_ = *msg; } - else + + void slipAngleCallback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) { - RCLCPP_ERROR(node->get_logger(),"No arguments"); - rclcpp::shutdown(); + slip_angle_ = *msg; } - if(use_multi_antenna_mode) + void headingInterpolateCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - // When using multi-antenna mode, set is_first_correction_velocity to true even when stationary for the first time. - is_first_correction_velocity = true; + heading_interpolate_ = *msg; } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub3 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback); - auto sub4 = node->create_subscription("gnss_compass_pose", 1000, pose_callback); - auto sub5 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub6 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub7 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); - auto sub8 = node->create_subscription(subscribe_topic_name, 1000, yaw_rate_offset_callback); - auto sub9 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); - auto sub10 = node->create_subscription(subscribe_topic_name2 , 1000, heading_interpolate_callback); - pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); + void rmcCallback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) { nmea_rmc_ = *msg; } - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (!is_first_correction_velocity_) { + RCLCPP_WARN(this->get_logger(), "is_first_correction_velocity is false."); + return; + } + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) { + RCLCPP_WARN(this->get_logger(), "velocity_status is not enabled."); + return; + } + if (!yaw_rate_offset_stop_.status.enabled_status) { + if (skip_static_initialization_) { + yaw_rate_offset_stop_.yaw_rate_offset = yaw_rate_offset_stop_in_skip_mode_; + } else { + RCLCPP_WARN( + this->get_logger(), + "Heading estimation is not started because the stop calibration is not yet completed."); + return; + } + } + imu_ = *msg; + heading_.header = msg->header; + heading_.header.frame_id = "base_link"; + bool use_rtklib_mode = use_gnss_mode_ == "rtklib" || use_gnss_mode_ == "RTKLIB"; + bool use_nmea_mode = use_gnss_mode_ == "nmea" || use_gnss_mode_ == "NMEA"; + if (use_rtklib_mode && !use_multi_antenna_mode_) + heading_estimate( + rtklib_nav_, imu_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_, slip_angle_, + heading_interpolate_, heading_parameter_, &heading_status_, &heading_); + else if (use_nmea_mode && !use_multi_antenna_mode_) + heading_estimate( + nmea_rmc_, imu_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_, slip_angle_, + heading_interpolate_, heading_parameter_, &heading_status_, &heading_); + else if (use_multi_antenna_mode_) + heading_estimate( + multi_antenna_heading_, imu_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_, + slip_angle_, heading_interpolate_, heading_parameter_, &heading_status_, &heading_); + + if (heading_.status.estimate_status == true || use_multi_antenna_mode_) { + pub_->publish(heading_); + } + heading_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(argc, argv)); return 0; } diff --git a/eagleye_rt/src/height_node.cpp b/eagleye_rt/src/height_node.cpp index ed17c374..c55c433f 100644 --- a/eagleye_rt/src/height_node.cpp +++ b/eagleye_rt/src/height_node.cpp @@ -28,146 +28,169 @@ * Author MapIV Takanose */ - #include "rclcpp/rclcpp.hpp" - #include "eagleye_coordinate/eagleye_coordinate.hpp" - #include "eagleye_navigation/eagleye_navigation.hpp" - - static sensor_msgs::msg::Imu imu; - static nmea_msgs::msg::Gpgga gga; - static geometry_msgs::msg::TwistStamped velocity; - static eagleye_msgs::msg::StatusStamped velocity_status; - static eagleye_msgs::msg::Distance distance; - - rclcpp::Publisher::SharedPtr pub1; - rclcpp::Publisher::SharedPtr pub2; - rclcpp::Publisher::SharedPtr pub3; - rclcpp::Publisher::SharedPtr pub4; - rclcpp::Publisher::SharedPtr pub5; - static eagleye_msgs::msg::Height height; - static eagleye_msgs::msg::Pitching pitching; - static eagleye_msgs::msg::AccXOffset acc_x_offset; - static eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor; - - struct HeightParameter height_parameter; - struct HeightStatus height_status; - - static bool use_can_less_mode; - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga = *msg; -} - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} +#include "eagleye_coordinate/eagleye_coordinate.hpp" +#include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) +class HeightNode : public rclcpp::Node { - distance = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - - imu = *msg; - height.header = msg->header; - height.header.frame_id = "base_link"; - pitching.header = msg->header; - pitching.header.frame_id = "base_link"; - acc_x_offset.header = msg->header; - acc_x_scale_factor.header = msg->header; - pitching_estimate(imu,gga,velocity,distance,height_parameter,&height_status,&height,&pitching,&acc_x_offset,&acc_x_scale_factor); - pub1->publish(height); - pub2->publish(pitching); - pub3->publish(acc_x_offset); - pub4->publish(acc_x_scale_factor); - - if (height_status.flag_reliability == true) +public: + HeightNode() : Node("eagleye_height") { - pub5->publish(gga); + std::string subscribe_gga_topic_name = "gnss/gga"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + height_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + height_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + height_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + height_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["height"]["estimated_minimum_interval"].as(); + height_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["height"]["estimated_maximum_interval"].as(); + height_parameter_.update_distance = + conf["/**"]["ros__parameters"]["height"]["update_distance"].as(); + height_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["height"]["gnss_receiving_threshold"].as(); + height_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["height"]["outlier_threshold"].as(); + height_parameter_.outlier_ratio_threshold = + conf["/**"]["ros__parameters"]["height"]["outlier_ratio_threshold"].as(); + height_parameter_.moving_average_time = + conf["/**"]["ros__parameters"]["height"]["moving_average_time"].as(); + + std::cout << "imu_rate " << height_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << height_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << height_parameter_.moving_judgment_threshold + << std::endl; + std::cout << "estimated_minimum_interval " << height_parameter_.estimated_minimum_interval + << std::endl; + std::cout << "estimated_maximum_interval " << height_parameter_.estimated_maximum_interval + << std::endl; + std::cout << "update_distance " << height_parameter_.update_distance << std::endl; + std::cout << "gnss_receiving_threshold " << height_parameter_.gnss_receiving_threshold + << std::endl; + std::cout << "outlier_threshold " << height_parameter_.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << height_parameter_.outlier_ratio_threshold + << std::endl; + std::cout << "moving_average_time " << height_parameter_.moving_average_time << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mheight Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&HeightNode::imuCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, 1000, + std::bind(&HeightNode::ggaCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&HeightNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&HeightNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_distance_ = this->create_subscription( + "distance", rclcpp::QoS(10), + std::bind(&HeightNode::distanceCallback, this, std::placeholders::_1)); + + pub_height_ = this->create_publisher("height", 1000); + pub_pitching_ = this->create_publisher("pitching", 1000); + pub_acc_x_offset_ = + this->create_publisher("acc_x_offset", 1000); + pub_acc_x_scale_factor_ = + this->create_publisher("acc_x_scale_factor", 1000); + pub_gga_ = + this->create_publisher("navsat/reliability_gga", 1000); } - height_status.flag_reliability = false; - height.status.estimate_status = false; - pitching.status.estimate_status = false; - acc_x_offset.status.estimate_status = false; - acc_x_scale_factor.status.estimate_status = false; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_height"); - - std::string subscribe_gga_topic_name = "gnss/gga"; - - 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; - - try +private: + sensor_msgs::msg::Imu imu_; + nmea_msgs::msg::Gpgga gga_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::Distance distance_; + eagleye_msgs::msg::Height height_; + eagleye_msgs::msg::Pitching pitching_; + eagleye_msgs::msg::AccXOffset acc_x_offset_; + eagleye_msgs::msg::AccXScaleFactor acc_x_scale_factor_; + HeightParameter height_parameter_; + HeightStatus height_status_ = {}; + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_height_; + rclcpp::Publisher::SharedPtr pub_pitching_; + rclcpp::Publisher::SharedPtr pub_acc_x_offset_; + rclcpp::Publisher::SharedPtr pub_acc_x_scale_factor_; + rclcpp::Publisher::SharedPtr pub_gga_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_distance_; + + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_ = *msg; } + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - height_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - height_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - height_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - - height_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["height"]["estimated_minimum_interval"].as(); - height_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["height"]["estimated_maximum_interval"].as(); - height_parameter.update_distance = conf["/**"]["ros__parameters"]["height"]["update_distance"].as(); - height_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["height"]["gnss_receiving_threshold"].as(); - height_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["height"]["outlier_threshold"].as(); - height_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["height"]["outlier_ratio_threshold"].as(); - height_parameter.moving_average_time = conf["/**"]["ros__parameters"]["height"]["moving_average_time"].as(); - - std::cout << "imu_rate " << height_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << height_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << height_parameter.moving_judgment_threshold << std::endl; - - std::cout << "estimated_minimum_interval " << height_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << height_parameter.estimated_maximum_interval << std::endl; - std::cout << "update_distance " << height_parameter.update_distance << std::endl; - std::cout << "gnss_receiving_threshold " << height_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_threshold " << height_parameter.outlier_threshold << std::endl; - std::cout << "outlier_ratio_threshold " << height_parameter.outlier_ratio_threshold << std::endl; - std::cout << "moving_average_time " << height_parameter.moving_average_time << std::endl; + velocity_ = *msg; } - catch (YAML::Exception& e) + + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - std::cerr << "\033[1;31mheight Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + velocity_status_ = *msg; } + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) + { + distance_ = *msg; + } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub5 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); - - std::string publish_height_topic_name = "height"; - std::string publish_pitching_topic_name = "pitching"; - std::string publish_acc_x_offset_topic_name = "acc_x_offset"; - std::string publish_acc_x_scale_factor_topic_name = "acc_x_scale_factor"; - std::string publish_nav_sat_gga_topic_name = "navsat/reliability_gga"; - - pub1 = node->create_publisher(publish_height_topic_name, 1000); - pub2 = node->create_publisher(publish_pitching_topic_name, 1000); - pub3 = node->create_publisher(publish_acc_x_offset_topic_name, 1000); - pub4 = node->create_publisher(publish_acc_x_scale_factor_topic_name, 1000); - pub5 = node->create_publisher(publish_nav_sat_gga_topic_name, 1000); - - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + imu_ = *msg; + height_.header = msg->header; + height_.header.frame_id = "base_link"; + pitching_.header = msg->header; + pitching_.header.frame_id = "base_link"; + acc_x_offset_.header = msg->header; + acc_x_scale_factor_.header = msg->header; + pitching_estimate( + imu_, gga_, velocity_, distance_, height_parameter_, &height_status_, &height_, &pitching_, + &acc_x_offset_, &acc_x_scale_factor_); + pub_height_->publish(height_); + pub_pitching_->publish(pitching_); + pub_acc_x_offset_->publish(acc_x_offset_); + pub_acc_x_scale_factor_->publish(acc_x_scale_factor_); + + if (height_status_.flag_reliability == true) { + pub_gga_->publish(gga_); + } + + height_status_.flag_reliability = false; + height_.status.estimate_status = false; + pitching_.status.estimate_status = false; + acc_x_offset_.status.estimate_status = false; + acc_x_scale_factor_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/monitor_node.cpp b/eagleye_rt/src/monitor_node.cpp index 457550ff..72bd6566 100755 --- a/eagleye_rt/src/monitor_node.cpp +++ b/eagleye_rt/src/monitor_node.cpp @@ -28,1175 +28,1474 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -#include #include +#include -static sensor_msgs::msg::Imu _imu; -static rtklib_msgs::msg::RtklibNav _rtklib_nav; -static sensor_msgs::msg::NavSatFix _rtklib_fix; -static nmea_msgs::msg::Gpgga _gga; -static geometry_msgs::msg::TwistStamped _velocity; -static geometry_msgs::msg::TwistStamped _correction_velocity; -static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor; -static eagleye_msgs::msg::Distance _distance; -static eagleye_msgs::msg::Heading _heading_1st; -static eagleye_msgs::msg::Heading _heading_interpolate_1st; -static eagleye_msgs::msg::Heading _heading_2nd; -static eagleye_msgs::msg::Heading _heading_interpolate_2nd; -static eagleye_msgs::msg::Heading _heading_3rd; -static eagleye_msgs::msg::Heading _heading_interpolate_3rd; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_1st; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_2nd; -static eagleye_msgs::msg::SlipAngle _slip_angle; -static eagleye_msgs::msg::Height _height; -static eagleye_msgs::msg::Pitching _pitching; -static eagleye_msgs::msg::Rolling _rolling; -static eagleye_msgs::msg::Position _enu_relative_pos; -static geometry_msgs::msg::Vector3Stamped _enu_vel; -static eagleye_msgs::msg::Position _enu_absolute_pos; -static eagleye_msgs::msg::Position _enu_absolute_pos_interpolate; -static sensor_msgs::msg::NavSatFix _eagleye_fix; -static geometry_msgs::msg::TwistStamped _eagleye_twist; - -static geometry_msgs::msg::TwistStamped::ConstSharedPtr _comparison_velocity_ptr; -static sensor_msgs::msg::Imu _corrected_imu; - -static bool _gga_sub_status; -static bool _print_status, _log_output_status, _log_header_make = false; -static std::string _output_log_dir; - -static double _imu_time_last; -static double _rtklib_nav_time_last; -static double _navsat_gga_time_last; -static double _velocity_time_last; -static double _velocity_scale_factor_time_last; -static double _distance_time_last; -static double _heading_1st_time_last; -static double _heading_interpolate_1st_time_last; -static double _heading_2nd_time_last; -static double _heading_interpolate_2nd_time_last; -static double _heading_3rd_time_last; -static double _heading_interpolate_3rd_time_last; -static double _yaw_rate_offset_stop_time_last; -static double _yaw_rate_offset_1st_time_last; -static double _yaw_rate_offset_2nd_time_last; -static double _slip_angle_time_last; -static double _height_time_last; -static double _pitching_time_last; -static double _enu_vel_time_last; -static double _enu_absolute_pos_time_last; -static double _enu_absolute_pos_interpolate_time_last; -static double _eagleye_twist_time_last; - -bool _use_compare_yaw_rate = false; -double _update_rate = 10.0; -double _th_gnss_deadrock_time = 10; -double _th_diff_rad_per_sec = 0.17453; -int _num_continuous_abnormal_yaw_rate = 0; -int _th_num_continuous_abnormal_yaw_rate = 10; - -std::shared_ptr updater_; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - _rtklib_nav = *msg; -} - -void rtklib_fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) -{ - _rtklib_fix = *msg; -} - -void navsatfix_gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) +class MonitorNode : public rclcpp::Node { - _gga = *msg; - _gga_sub_status = true; -} +public: + MonitorNode() : Node("eagleye_monitor") + { + std::string subscribe_twist_topic_name = "vehicle/twist"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; + std::string comparison_twist_topic_name = "/calculated_twist"; + + this->declare_parameter("rtklib_nav_topic", subscribe_rtklib_nav_topic_name); + this->declare_parameter("gga_topic", subscribe_gga_topic_name); + this->declare_parameter("monitor.comparison_twist_topic", comparison_twist_topic_name); + this->declare_parameter("monitor.print_status", print_status_); + this->declare_parameter("monitor.log_output_status", log_output_status_); + this->declare_parameter("monitor.use_compare_yaw_rate", use_compare_yaw_rate_); + this->declare_parameter("monitor.th_diff_rad_per_sec", th_diff_rad_per_sec_); + this->declare_parameter( + "monitor.th_num_continuous_abnormal_yaw_rate", th_num_continuous_abnormal_yaw_rate_); + + this->get_parameter("rtklib_nav_topic", subscribe_rtklib_nav_topic_name); + this->get_parameter("gga_topic", subscribe_gga_topic_name); + this->get_parameter("monitor.comparison_twist_topic", comparison_twist_topic_name); + this->get_parameter("monitor.print_status", print_status_); + this->get_parameter("monitor.log_output_status", log_output_status_); + this->get_parameter("monitor.use_compare_yaw_rate", use_compare_yaw_rate_); + this->get_parameter("monitor.th_diff_rad_per_sec", th_diff_rad_per_sec_); + this->get_parameter( + "monitor.th_num_continuous_abnormal_yaw_rate", th_num_continuous_abnormal_yaw_rate_); + + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name + << std::endl; + std::cout << "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; + std::cout << "print_status " << print_status_ << std::endl; + std::cout << "log_output_status " << log_output_status_ << std::endl; + std::cout << "use_compare_yaw_rate " << use_compare_yaw_rate_ << std::endl; + if (use_compare_yaw_rate_) { + std::cout << "comparison_twist_topic_name " << comparison_twist_topic_name << std::endl; + std::cout << "th_diff_rad_per_sec " << th_diff_rad_per_sec_ << std::endl; + std::cout << "th_num_continuous_abnormal_yaw_rate " + << th_num_continuous_abnormal_yaw_rate_ << std::endl; + } -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _velocity = *msg; -} + double update_time = 1.0 / update_rate_; + updater_ = std::make_shared(this, update_time); + + updater_->setHardwareID("eagleye_topic_checker"); + updater_->add( + "eagleye_input_imu", + std::bind(&MonitorNode::imuTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_input_rtklib_nav", + std::bind(&MonitorNode::rtklibNavTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_input_navsat_gga", + std::bind(&MonitorNode::navsatGgaTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_input_velocity", + std::bind(&MonitorNode::velocityTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_velocity_scale_factor", + std::bind(&MonitorNode::velocityScaleFactorTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_distance", + std::bind(&MonitorNode::distanceTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_1st", + std::bind(&MonitorNode::heading1stTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_interpolate_1st", + std::bind( + &MonitorNode::headingInterpolate1stTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_2nd", + std::bind(&MonitorNode::heading2ndTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_interpolate_2nd", + std::bind( + &MonitorNode::headingInterpolate2ndTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_3rd", + std::bind(&MonitorNode::heading3rdTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_heading_interpolate_3rd", + std::bind( + &MonitorNode::headingInterpolate3rdTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_yaw_rate_offset_stop", + std::bind(&MonitorNode::yawRateOffsetStopTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_yaw_rate_offset_1st", + std::bind(&MonitorNode::yawRateOffset1stTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_yaw_rate_offset_2nd", + std::bind(&MonitorNode::yawRateOffset2ndTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_slip_angle", + std::bind(&MonitorNode::slipAngleTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_enu_vel", + std::bind(&MonitorNode::enuVelTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_height", + std::bind(&MonitorNode::heightTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_pitching", + std::bind(&MonitorNode::pitchingTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_enu_absolute_pos", + std::bind(&MonitorNode::enuAbsolutePosTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_enu_absolute_pos_interpolate", + std::bind( + &MonitorNode::enuAbsolutePosInterpolateTopicChecker, this, std::placeholders::_1)); + updater_->add( + "eagleye_twist", + std::bind(&MonitorNode::twistTopicChecker, this, std::placeholders::_1)); + if (use_compare_yaw_rate_) { + updater_->add( + "eagleye_imu_comparison", + std::bind(&MonitorNode::imuComparisonChecker, this, std::placeholders::_1)); + } -void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _correction_velocity = *msg; -} + time_t time_; + time_ = time(NULL); + std::stringstream time_ss; + time_ss << time_; + std::string time_str = time_ss.str(); + output_log_dir_ = ament_index_cpp::get_package_share_directory("eagleye_rt") + + "/log/eagleye_log_" + time_str + ".csv"; + if (log_output_status_) std::cout << output_log_dir_ << std::endl; + + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&MonitorNode::imuCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&MonitorNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_rtklib_fix_ = this->create_subscription( + "rtklib/fix", rclcpp::QoS(10), + std::bind(&MonitorNode::rtklibFixCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, 1000, + std::bind(&MonitorNode::navsatfixGgaCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + subscribe_twist_topic_name, 1000, + std::bind(&MonitorNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_scale_factor_ = + this->create_subscription( + "velocity_scale_factor", rclcpp::QoS(10), + std::bind(&MonitorNode::velocityScaleFactorCallback, this, std::placeholders::_1)); + sub_distance_ = this->create_subscription( + "distance", rclcpp::QoS(10), + std::bind(&MonitorNode::distanceCallback, this, std::placeholders::_1)); + sub_heading_1st_ = this->create_subscription( + "heading_1st", rclcpp::QoS(10), + std::bind(&MonitorNode::heading1stCallback, this, std::placeholders::_1)); + sub_heading_interpolate_1st_ = this->create_subscription( + "heading_interpolate_1st", rclcpp::QoS(10), + std::bind(&MonitorNode::headingInterpolate1stCallback, this, std::placeholders::_1)); + sub_heading_2nd_ = this->create_subscription( + "heading_2nd", rclcpp::QoS(10), + std::bind(&MonitorNode::heading2ndCallback, this, std::placeholders::_1)); + sub_heading_interpolate_2nd_ = this->create_subscription( + "heading_interpolate_2nd", rclcpp::QoS(10), + std::bind(&MonitorNode::headingInterpolate2ndCallback, this, std::placeholders::_1)); + sub_heading_3rd_ = this->create_subscription( + "heading_3rd", rclcpp::QoS(10), + std::bind(&MonitorNode::heading3rdCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", rclcpp::QoS(10), + std::bind(&MonitorNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&MonitorNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_1st_ = this->create_subscription( + "yaw_rate_offset_1st", rclcpp::QoS(10), + std::bind(&MonitorNode::yawRateOffset1stCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", rclcpp::QoS(10), + std::bind(&MonitorNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_slip_angle_ = this->create_subscription( + "slip_angle", rclcpp::QoS(10), + std::bind(&MonitorNode::slipAngleCallback, this, std::placeholders::_1)); + sub_enu_relative_pos_ = this->create_subscription( + "enu_relative_pos", rclcpp::QoS(10), + std::bind(&MonitorNode::enuRelativePosCallback, this, std::placeholders::_1)); + sub_enu_vel_ = this->create_subscription( + "enu_vel", rclcpp::QoS(10), + std::bind(&MonitorNode::enuVelCallback, this, std::placeholders::_1)); + sub_height_ = this->create_subscription( + "height", rclcpp::QoS(10), + std::bind(&MonitorNode::heightCallback, this, std::placeholders::_1)); + sub_pitching_ = this->create_subscription( + "pitching", rclcpp::QoS(10), + std::bind(&MonitorNode::pitchingCallback, this, std::placeholders::_1)); + sub_enu_absolute_pos_ = this->create_subscription( + "enu_absolute_pos", rclcpp::QoS(10), + std::bind(&MonitorNode::enuAbsolutePosCallback, this, std::placeholders::_1)); + sub_enu_absolute_pos_interpolate_ = this->create_subscription( + "enu_absolute_pos_interpolate", rclcpp::QoS(10), + std::bind( + &MonitorNode::enuAbsolutePosInterpolateCallback, this, std::placeholders::_1)); + sub_eagleye_fix_ = this->create_subscription( + "fix", rclcpp::QoS(10), + std::bind(&MonitorNode::eagleyeFixCallback, this, std::placeholders::_1)); + sub_eagleye_twist_ = this->create_subscription( + "twist", rclcpp::QoS(10), + std::bind(&MonitorNode::eagleyeTwistCallback, this, std::placeholders::_1)); + sub_rolling_ = this->create_subscription( + "rolling", rclcpp::QoS(10), + std::bind(&MonitorNode::rollingCallback, this, std::placeholders::_1)); + sub_comparison_velocity_ = this->create_subscription( + comparison_twist_topic_name, 1000, + std::bind(&MonitorNode::comparisonVelocityCallback, this, std::placeholders::_1)); + sub_correction_velocity_ = this->create_subscription( + "velocity", 1000, + std::bind(&MonitorNode::correctionVelocityCallback, this, std::placeholders::_1)); + } + +private: + sensor_msgs::msg::Imu imu_; + rtklib_msgs::msg::RtklibNav rtklib_nav_; + sensor_msgs::msg::NavSatFix rtklib_fix_; + nmea_msgs::msg::Gpgga gga_; + geometry_msgs::msg::TwistStamped velocity_; + geometry_msgs::msg::TwistStamped correction_velocity_; + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + eagleye_msgs::msg::Distance distance_; + eagleye_msgs::msg::Heading heading_1st_; + eagleye_msgs::msg::Heading heading_interpolate_1st_; + eagleye_msgs::msg::Heading heading_2nd_; + eagleye_msgs::msg::Heading heading_interpolate_2nd_; + eagleye_msgs::msg::Heading heading_3rd_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_1st_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::SlipAngle slip_angle_; + eagleye_msgs::msg::Height height_; + eagleye_msgs::msg::Pitching pitching_; + eagleye_msgs::msg::Rolling rolling_; + eagleye_msgs::msg::Position enu_relative_pos_; + geometry_msgs::msg::Vector3Stamped enu_vel_; + eagleye_msgs::msg::Position enu_absolute_pos_; + eagleye_msgs::msg::Position enu_absolute_pos_interpolate_; + sensor_msgs::msg::NavSatFix eagleye_fix_; + geometry_msgs::msg::TwistStamped eagleye_twist_; + + geometry_msgs::msg::TwistStamped::ConstSharedPtr comparison_velocity_ptr_; + sensor_msgs::msg::Imu corrected_imu_; + + bool gga_sub_status_ = false; + bool print_status_ = false; + bool log_output_status_ = false; + bool log_header_make_ = false; + std::string output_log_dir_; + + double imu_time_last_ = 0; + double rtklib_nav_time_last_ = 0; + double navsat_gga_time_last_ = 0; + double velocity_time_last_ = 0; + double velocity_scale_factor_time_last_ = 0; + double distance_time_last_ = 0; + double heading_1st_time_last_ = 0; + double heading_interpolate_1st_time_last_ = 0; + double heading_2nd_time_last_ = 0; + double heading_interpolate_2nd_time_last_ = 0; + double heading_3rd_time_last_ = 0; + double heading_interpolate_3rd_time_last_ = 0; + double yaw_rate_offset_stop_time_last_ = 0; + double yaw_rate_offset_1st_time_last_ = 0; + double yaw_rate_offset_2nd_time_last_ = 0; + double slip_angle_time_last_ = 0; + double height_time_last_ = 0; + double pitching_time_last_ = 0; + double enu_vel_time_last_ = 0; + double enu_absolute_pos_time_last_ = 0; + double enu_absolute_pos_interpolate_time_last_ = 0; + double eagleye_twist_time_last_ = 0; + + bool use_compare_yaw_rate_ = false; + double update_rate_ = 10.0; + double th_gnss_deadrock_time_ = 10; + double th_diff_rad_per_sec_ = 0.17453; + int num_continuous_abnormal_yaw_rate_ = 0; + int th_num_continuous_abnormal_yaw_rate_ = 10; + + std::shared_ptr updater_; + + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_rtklib_fix_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr + sub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_distance_; + rclcpp::Subscription::SharedPtr sub_heading_1st_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_1st_; + rclcpp::Subscription::SharedPtr sub_heading_2nd_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_2nd_; + rclcpp::Subscription::SharedPtr sub_heading_3rd_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_1st_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + rclcpp::Subscription::SharedPtr sub_slip_angle_; + rclcpp::Subscription::SharedPtr sub_enu_relative_pos_; + rclcpp::Subscription::SharedPtr sub_enu_vel_; + rclcpp::Subscription::SharedPtr sub_height_; + rclcpp::Subscription::SharedPtr sub_pitching_; + rclcpp::Subscription::SharedPtr sub_enu_absolute_pos_; + rclcpp::Subscription::SharedPtr + sub_enu_absolute_pos_interpolate_; + rclcpp::Subscription::SharedPtr sub_eagleye_fix_; + rclcpp::Subscription::SharedPtr sub_eagleye_twist_; + rclcpp::Subscription::SharedPtr sub_rolling_; + rclcpp::Subscription::SharedPtr sub_comparison_velocity_; + rclcpp::Subscription::SharedPtr sub_correction_velocity_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) + { + rtklib_nav_ = *msg; + } -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) -{ - _velocity_scale_factor = *msg; -} + void rtklibFixCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) + { + rtklib_fix_ = *msg; + } -void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) -{ - _distance = *msg; -} + void navsatfixGgaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) + { + gga_ = *msg; + gga_sub_status_ = true; + } -void heading_1st_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_1st = *msg; -} + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + velocity_ = *msg; + } -void heading_interpolate_1st_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_interpolate_1st = *msg; -} + void correctionVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + correction_velocity_ = *msg; + } -void heading_2nd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_2nd = *msg; -} + void velocityScaleFactorCallback( + const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) + { + velocity_scale_factor_ = *msg; + } -void heading_interpolate_2nd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_interpolate_2nd = *msg; -} + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) + { + distance_ = *msg; + } -void heading_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_3rd = *msg; -} + void heading1stCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_1st_ = *msg; + } -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_interpolate_3rd = *msg; -} + void headingInterpolate1stCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_interpolate_1st_ = *msg; + } -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_stop = *msg; -} + void heading2ndCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_2nd_ = *msg; + } -void yaw_rate_offset_1st_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_1st = *msg; -} + void headingInterpolate2ndCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_interpolate_2nd_ = *msg; + } -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_2nd = *msg; -} + void heading3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_3rd_ = *msg; + } -void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) -{ - _slip_angle = *msg; -} + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) + { + heading_interpolate_3rd_ = *msg; + } -void enu_relative_pos_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - _enu_relative_pos = *msg; -} + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; + } -void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) -{ - _enu_vel = *msg; -} + void yawRateOffset1stCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_1st_ = *msg; + } -void enu_absolute_pos_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - _enu_absolute_pos = *msg; -} + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_2nd_ = *msg; + } -void height_callback(const eagleye_msgs::msg::Height::ConstSharedPtr msg) -{ - _height = *msg; -} + void slipAngleCallback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) + { + slip_angle_ = *msg; + } -void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) -{ - _pitching = *msg; -} + void enuRelativePosCallback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) + { + enu_relative_pos_ = *msg; + } -void rolling_callback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) -{ - _rolling = *msg; -} + void enuVelCallback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) + { + enu_vel_ = *msg; + } -void enu_absolute_pos_interpolate_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - _enu_absolute_pos_interpolate = *msg; -} + void enuAbsolutePosCallback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) + { + enu_absolute_pos_ = *msg; + } -void eagleye_fix_callback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) -{ - _eagleye_fix = *msg; -} + void heightCallback(const eagleye_msgs::msg::Height::ConstSharedPtr msg) + { + height_ = *msg; + } -void eagleye_twist_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _eagleye_twist = *msg; -} + void pitchingCallback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) + { + pitching_ = *msg; + } -void comparison_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _comparison_velocity_ptr = msg; -} + void rollingCallback(const eagleye_msgs::msg::Rolling::ConstSharedPtr msg) + { + rolling_ = *msg; + } + void enuAbsolutePosInterpolateCallback( + const eagleye_msgs::msg::Position::ConstSharedPtr msg) + { + enu_absolute_pos_interpolate_ = *msg; + } -void imu_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_imu.header.stamp); - auto imu_time = ros_clock.seconds(); + void eagleyeFixCallback(const sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) + { + eagleye_fix_ = *msg; + } - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void eagleyeTwistCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + eagleye_twist_ = *msg; + } - if (_imu_time_last == imu_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::STALE; - msg = "not subscribed to topic"; + void comparisonVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + comparison_velocity_ptr_ = msg; } - _imu_time_last = imu_time; - stat.summary(level, msg); -} -void rtklib_nav_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_rtklib_nav.header.stamp); - auto rtklib_nav_time = ros_clock.seconds(); + void imuTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(imu_.header.stamp); + auto imu_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; - if (_rtklib_nav_time_last - rtklib_nav_time > _th_gnss_deadrock_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; + if (imu_time_last_ == imu_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + msg = "not subscribed to topic"; + } + + imu_time_last_ = imu_time; + stat.summary(level, msg); } - _rtklib_nav_time_last = rtklib_nav_time; - stat.summary(level, msg); -} -void navsat_gga_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_gga.header.stamp); - auto navsat_gga_time = ros_clock.seconds(); + void rtklibNavTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(rtklib_nav_.header.stamp); + auto rtklib_nav_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; - if (_navsat_gga_time_last - navsat_gga_time > _th_gnss_deadrock_time || !_gga_sub_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + if (rtklib_nav_time_last_ - rtklib_nav_time > th_gnss_deadrock_time_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } + + rtklib_nav_time_last_ = rtklib_nav_time; + stat.summary(level, msg); } - _navsat_gga_time_last = navsat_gga_time; - stat.summary(level, msg); -} -void velocity_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_velocity.header.stamp); - auto velocity_time = ros_clock.seconds(); + void navsatGgaTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(gga_.header.stamp); + auto navsat_gga_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; - if (_velocity_time_last == velocity_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::STALE; - msg = "not subscribed to topic"; + if ( + navsat_gga_time_last_ - navsat_gga_time > th_gnss_deadrock_time_ || !gga_sub_status_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } + + navsat_gga_time_last_ = navsat_gga_time; + stat.summary(level, msg); } - _velocity_time_last = velocity_time; - stat.summary(level, msg); -} -void velocity_scale_factor_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_velocity_scale_factor.header.stamp); - auto velocity_scale_factor_time = ros_clock.seconds(); + void velocityTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(velocity_.header.stamp); + auto velocity_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; - if (_velocity_scale_factor_time_last == velocity_scale_factor_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!_velocity_scale_factor.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - else if (_velocity_scale_factor.status.is_abnormal) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - if (_velocity_scale_factor.status.error_code == eagleye_msgs::msg::Status::NAN_OR_INFINITE) - { - msg = "Estimated velocity scale factor is NaN or infinete"; + if (velocity_time_last_ == velocity_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::STALE; + msg = "not subscribed to topic"; } - else if (_velocity_scale_factor.status.error_code == eagleye_msgs::msg::Status::TOO_LARGE_OR_SMALL) - { - msg = "Estimated velocity scale factor is too large or too small"; - } - else - { - msg = "abnormal error of velocity_scale_factor"; - } - } - _velocity_scale_factor_time_last = velocity_scale_factor_time; - stat.summary(level, msg); -} -void distance_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_distance.header.stamp); - auto distance_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; - - if (_distance_time_last == distance_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!std::isfinite(_distance.distance)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (!_distance.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + velocity_time_last_ = velocity_time; + stat.summary(level, msg); } - _distance_time_last = distance_time; - stat.summary(level, msg); -} -void heading_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_1st.header.stamp); - auto heading_1st_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; - - if (!std::isfinite(_heading_1st.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_heading_1st_time_last - heading_1st_time > _th_gnss_deadrock_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; - } - else if (!_heading_1st.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } + void velocityScaleFactorTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(velocity_scale_factor_.header.stamp); + auto velocity_scale_factor_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (velocity_scale_factor_time_last_ == velocity_scale_factor_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!velocity_scale_factor_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } else if (velocity_scale_factor_.status.is_abnormal) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + if ( + velocity_scale_factor_.status.error_code == + eagleye_msgs::msg::Status::NAN_OR_INFINITE) { + msg = "Estimated velocity scale factor is NaN or infinete"; + } else if ( + velocity_scale_factor_.status.error_code == + eagleye_msgs::msg::Status::TOO_LARGE_OR_SMALL) { + msg = "Estimated velocity scale factor is too large or too small"; + } else { + msg = "abnormal error of velocity_scale_factor"; + } + } - _heading_1st_time_last = heading_1st_time; - stat.summary(level, msg); -} -void heading_interpolate_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_interpolate_1st.header.stamp); - auto heading_interpolate_1st_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; - - if (_heading_interpolate_1st_time_last == heading_interpolate_1st_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!std::isfinite(_heading_interpolate_1st.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; + velocity_scale_factor_time_last_ = velocity_scale_factor_time; + stat.summary(level, msg); } - else if (!_heading_interpolate_1st.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - - _heading_interpolate_1st_time_last = heading_interpolate_1st_time; - stat.summary(level, msg); -} -void heading_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_2nd.header.stamp); - auto heading_2nd_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void distanceTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(distance_.header.stamp); + auto distance_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (distance_time_last_ == distance_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(distance_.distance)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!distance_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (!std::isfinite(_heading_2nd.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_heading_2nd_time_last - heading_2nd_time > _th_gnss_deadrock_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; - } - else if (!_heading_2nd.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + distance_time_last_ = distance_time; + stat.summary(level, msg); } - _heading_2nd_time_last = heading_2nd_time; - stat.summary(level, msg); -} -void heading_interpolate_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_interpolate_2nd.header.stamp); - auto heading_interpolate_2nd_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void heading1stTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_1st_.header.stamp); + auto heading_1st_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (!std::isfinite(heading_1st_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (heading_1st_time_last_ - heading_1st_time > th_gnss_deadrock_time_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if (!heading_1st_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_heading_interpolate_2nd_time_last == heading_interpolate_2nd_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!std::isfinite(_heading_interpolate_2nd.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (!_heading_interpolate_2nd.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + heading_1st_time_last_ = heading_1st_time; + stat.summary(level, msg); } - _heading_interpolate_2nd_time_last = heading_interpolate_2nd_time; - stat.summary(level, msg); -} -void heading_3rd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_3rd.header.stamp); - auto heading_3rd_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void headingInterpolate1stTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_interpolate_1st_.header.stamp); + auto heading_interpolate_1st_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (heading_interpolate_1st_time_last_ == heading_interpolate_1st_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(heading_interpolate_1st_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!heading_interpolate_1st_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (!std::isfinite(_heading_3rd.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_heading_3rd_time_last - heading_3rd_time > _th_gnss_deadrock_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; - } - else if (!_heading_3rd.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + heading_interpolate_1st_time_last_ = heading_interpolate_1st_time; + stat.summary(level, msg); } - _heading_3rd_time_last = heading_3rd_time; - stat.summary(level, msg); -} -void heading_interpolate_3rd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_heading_interpolate_3rd.header.stamp); - auto heading_interpolate_3rd_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void heading2ndTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_2nd_.header.stamp); + auto heading_2nd_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (!std::isfinite(heading_2nd_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (heading_2nd_time_last_ - heading_2nd_time > th_gnss_deadrock_time_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if (!heading_2nd_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_heading_interpolate_3rd_time_last == heading_interpolate_3rd_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + heading_2nd_time_last_ = heading_2nd_time; + stat.summary(level, msg); } - else if (!std::isfinite(_heading_interpolate_3rd.heading_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (!_heading_interpolate_3rd.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - - _heading_interpolate_3rd_time_last = heading_interpolate_3rd_time; - stat.summary(level, msg); -} -void yaw_rate_offset_stop_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_yaw_rate_offset_stop.header.stamp); - auto yaw_rate_offset_stop_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void headingInterpolate2ndTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_interpolate_2nd_.header.stamp); + auto heading_interpolate_2nd_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (heading_interpolate_2nd_time_last_ == heading_interpolate_2nd_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(heading_interpolate_2nd_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!heading_interpolate_2nd_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_yaw_rate_offset_stop_time_last == yaw_rate_offset_stop_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!_yaw_rate_offset_stop.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + heading_interpolate_2nd_time_last_ = heading_interpolate_2nd_time; + stat.summary(level, msg); } - else if (_yaw_rate_offset_stop.status.is_abnormal) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - if(_yaw_rate_offset_stop.status.error_code == eagleye_msgs::msg::Status::NAN_OR_INFINITE) - { - msg = "estimate value is NaN or infinete"; - } - else - { - msg = "abnormal error of yaw_rate_offset_stop"; + + void heading3rdTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_3rd_.header.stamp); + auto heading_3rd_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (!std::isfinite(heading_3rd_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (heading_3rd_time_last_ - heading_3rd_time > th_gnss_deadrock_time_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if (!heading_3rd_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; } - } - _yaw_rate_offset_stop_time_last = yaw_rate_offset_stop_time; - stat.summary(level, msg); -} -void yaw_rate_offset_1st_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_yaw_rate_offset_1st.header.stamp); - auto yaw_rate_offset_1st_time = ros_clock.seconds(); + heading_3rd_time_last_ = heading_3rd_time; + stat.summary(level, msg); + } - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void headingInterpolate3rdTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(heading_interpolate_3rd_.header.stamp); + auto heading_interpolate_3rd_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (heading_interpolate_3rd_time_last_ == heading_interpolate_3rd_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(heading_interpolate_3rd_.heading_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!heading_interpolate_3rd_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_yaw_rate_offset_1st_time_last == yaw_rate_offset_1st_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + heading_interpolate_3rd_time_last_ = heading_interpolate_3rd_time; + stat.summary(level, msg); } - else if (!_yaw_rate_offset_1st.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - else if (_yaw_rate_offset_1st.status.is_abnormal) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - if(_yaw_rate_offset_1st.status.error_code == eagleye_msgs::msg::Status::NAN_OR_INFINITE) - { - msg = "estimate value is NaN or infinete"; - } - else - { - msg = "abnormal error of yaw_rate_offset_1st"; + + void yawRateOffsetStopTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(yaw_rate_offset_stop_.header.stamp); + auto yaw_rate_offset_stop_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (yaw_rate_offset_stop_time_last_ == yaw_rate_offset_stop_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!yaw_rate_offset_stop_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } else if (yaw_rate_offset_stop_.status.is_abnormal) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + if (yaw_rate_offset_stop_.status.error_code == + eagleye_msgs::msg::Status::NAN_OR_INFINITE) { + msg = "estimate value is NaN or infinete"; + } else { + msg = "abnormal error of yaw_rate_offset_stop"; + } } - } - _yaw_rate_offset_1st_time_last = yaw_rate_offset_1st_time; - stat.summary(level, msg); -} -void yaw_rate_offset_2nd_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_yaw_rate_offset_2nd.header.stamp); - auto yaw_rate_offset_2nd_time = ros_clock.seconds(); + yaw_rate_offset_stop_time_last_ = yaw_rate_offset_stop_time; + stat.summary(level, msg); + } - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void yawRateOffset1stTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(yaw_rate_offset_1st_.header.stamp); + auto yaw_rate_offset_1st_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (yaw_rate_offset_1st_time_last_ == yaw_rate_offset_1st_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!yaw_rate_offset_1st_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } else if (yaw_rate_offset_1st_.status.is_abnormal) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + if (yaw_rate_offset_1st_.status.error_code == + eagleye_msgs::msg::Status::NAN_OR_INFINITE) { + msg = "estimate value is NaN or infinete"; + } else { + msg = "abnormal error of yaw_rate_offset_1st"; + } + } - if (_yaw_rate_offset_2nd_time_last == yaw_rate_offset_2nd_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + yaw_rate_offset_1st_time_last_ = yaw_rate_offset_1st_time; + stat.summary(level, msg); } - else if (!_yaw_rate_offset_2nd.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - else if (_yaw_rate_offset_2nd.status.is_abnormal) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - if(_yaw_rate_offset_2nd.status.error_code == eagleye_msgs::msg::Status::NAN_OR_INFINITE) - { - msg = "estimate value is NaN or infinete"; - } - else - { - msg = "abnormal error of yaw_rate_offset_2nd"; + + void yawRateOffset2ndTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(yaw_rate_offset_2nd_.header.stamp); + auto yaw_rate_offset_2nd_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (yaw_rate_offset_2nd_time_last_ == yaw_rate_offset_2nd_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!yaw_rate_offset_2nd_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } else if (yaw_rate_offset_2nd_.status.is_abnormal) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + if (yaw_rate_offset_2nd_.status.error_code == + eagleye_msgs::msg::Status::NAN_OR_INFINITE) { + msg = "estimate value is NaN or infinete"; + } else { + msg = "abnormal error of yaw_rate_offset_2nd"; + } } - } - _yaw_rate_offset_2nd_time_last = yaw_rate_offset_2nd_time; - stat.summary(level, msg); -} -void slip_angle_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_slip_angle.header.stamp); - auto slip_angle_time = ros_clock.seconds(); + yaw_rate_offset_2nd_time_last_ = yaw_rate_offset_2nd_time; + stat.summary(level, msg); + } - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void slipAngleTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(slip_angle_.header.stamp); + auto slip_angle_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (slip_angle_time_last_ == slip_angle_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(slip_angle_.slip_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (slip_angle_.coefficient == 0) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "/slip_angle/manual_coefficient is not set"; + } else if (!slip_angle_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_slip_angle_time_last == slip_angle_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!std::isfinite(_slip_angle.slip_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_slip_angle.coefficient == 0) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "/slip_angle/manual_coefficient is not set"; - } - else if (!_slip_angle.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + slip_angle_time_last_ = slip_angle_time; + stat.summary(level, msg); } - _slip_angle_time_last = slip_angle_time; - stat.summary(level, msg); -} -void enu_vel_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_enu_vel.header.stamp); - auto enu_vel_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void enuVelTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(enu_vel_.header.stamp); + auto enu_vel_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if ( + !std::isfinite(enu_vel_.vector.x) || !std::isfinite(enu_vel_.vector.y) || + !std::isfinite(enu_vel_.vector.z)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (enu_vel_time_last_ == enu_vel_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } - if (!std::isfinite(_enu_vel.vector.x)||!std::isfinite(_enu_vel.vector.y)||!std::isfinite(_enu_vel.vector.z)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_enu_vel_time_last == enu_vel_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + enu_vel_time_last_ = enu_vel_time; + stat.summary(level, msg); } - _enu_vel_time_last = enu_vel_time; - stat.summary(level, msg); -} -void height_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_height.header.stamp); - auto height_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void heightTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(height_.header.stamp); + auto height_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (height_time_last_ == height_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(height_.height)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!height_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_height_time_last == height_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; - } - else if (!std::isfinite(_height.height)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (!_height.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + height_time_last_ = height_time; + stat.summary(level, msg); } - _height_time_last = height_time; - stat.summary(level, msg); -} -void pitching_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_pitching.header.stamp); - auto pitching_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void pitchingTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(pitching_.header.stamp); + auto pitching_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (pitching_time_last_ == pitching_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed to topic"; + } else if (!std::isfinite(pitching_.pitching_angle)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (!pitching_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (_pitching_time_last == pitching_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed to topic"; + pitching_time_last_ = pitching_time; + stat.summary(level, msg); } - else if (!std::isfinite(_pitching.pitching_angle)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (!_pitching.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - - _pitching_time_last = pitching_time; - stat.summary(level, msg); -} -void enu_absolute_pos_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_enu_absolute_pos.header.stamp); - auto enu_absolute_pos_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void enuAbsolutePosTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(enu_absolute_pos_.header.stamp); + auto enu_absolute_pos_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if ( + !std::isfinite(enu_absolute_pos_.enu_pos.x) || + !std::isfinite(enu_absolute_pos_.enu_pos.y) || + !std::isfinite(enu_absolute_pos_.enu_pos.z)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if (enu_absolute_pos_time_last_ - enu_absolute_pos_time > th_gnss_deadrock_time_) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if (!enu_absolute_pos_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (!std::isfinite(_enu_absolute_pos.enu_pos.x)||!std::isfinite(_enu_absolute_pos.enu_pos.y)||!std::isfinite(_enu_absolute_pos.enu_pos.z)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_enu_absolute_pos_time_last - enu_absolute_pos_time > _th_gnss_deadrock_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; + enu_absolute_pos_time_last_ = enu_absolute_pos_time; + stat.summary(level, msg); } - else if (!_enu_absolute_pos.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; - } - - _enu_absolute_pos_time_last = enu_absolute_pos_time; - stat.summary(level, msg); -} -void enu_absolute_pos_interpolate_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_enu_absolute_pos_interpolate.header.stamp); - auto enu_absolute_pos_interpolate_time = ros_clock.seconds(); - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void enuAbsolutePosInterpolateTopicChecker( + diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(enu_absolute_pos_interpolate_.header.stamp); + auto enu_absolute_pos_interpolate_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if ( + !std::isfinite(enu_absolute_pos_interpolate_.enu_pos.x) || + !std::isfinite(enu_absolute_pos_interpolate_.enu_pos.y) || + !std::isfinite(enu_absolute_pos_interpolate_.enu_pos.z)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } else if ( + enu_absolute_pos_interpolate_time_last_ == enu_absolute_pos_interpolate_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if (!enu_absolute_pos_interpolate_.status.enabled_status) { + level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + msg = "estimates have not started yet"; + } - if (!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.x)||!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.y)||!std::isfinite(_enu_absolute_pos_interpolate.enu_pos.z)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; - } - else if (_enu_absolute_pos_interpolate_time_last == enu_absolute_pos_interpolate_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "not subscribed or deadlock of more than 10 seconds"; - } - else if (!_enu_absolute_pos_interpolate.status.enabled_status) { - level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - msg = "estimates have not started yet"; + enu_absolute_pos_interpolate_time_last_ = enu_absolute_pos_interpolate_time; + stat.summary(level, msg); } - _enu_absolute_pos_interpolate_time_last = enu_absolute_pos_interpolate_time; - stat.summary(level, msg); -} -void twist_topic_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - rclcpp::Time ros_clock(_eagleye_twist.header.stamp); - auto eagleye_twist_time = ros_clock.seconds(); - - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + void twistTopicChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) + { + rclcpp::Time ros_clock(eagleye_twist_.header.stamp); + auto eagleye_twist_time = ros_clock.seconds(); + + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if (eagleye_twist_time_last_ == eagleye_twist_time) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "not subscribed or deadlock of more than 10 seconds"; + } else if ( + !std::isfinite(eagleye_twist_.twist.linear.x) || + !std::isfinite(eagleye_twist_.twist.linear.y) || + !std::isfinite(eagleye_twist_.twist.linear.z) || + !std::isfinite(eagleye_twist_.twist.angular.x) || + !std::isfinite(eagleye_twist_.twist.angular.y) || + !std::isfinite(eagleye_twist_.twist.angular.z)) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "invalid number"; + } - if (_eagleye_twist_time_last == eagleye_twist_time) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "not subscribed or deadlock of more than 10 seconds"; - } - else if (!std::isfinite(_eagleye_twist.twist.linear.x)||!std::isfinite(_eagleye_twist.twist.linear.y)||!std::isfinite(_eagleye_twist.twist.linear.z) - ||!std::isfinite(_eagleye_twist.twist.angular.x)||!std::isfinite(_eagleye_twist.twist.angular.y)||!std::isfinite(_eagleye_twist.twist.angular.z)) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "invalid number"; + eagleye_twist_time_last_ = eagleye_twist_time; + stat.summary(level, msg); } - _eagleye_twist_time_last = eagleye_twist_time; - stat.summary(level, msg); -} - -void imu_comparison_checker(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - if(_comparison_velocity_ptr == nullptr) + void imuComparisonChecker(diagnostic_updater::DiagnosticStatusWrapper& stat) { - return; - } + if (comparison_velocity_ptr_ == nullptr) { + return; + } - int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; - std::string msg = "OK"; + int8_t level = diagnostic_msgs::msg::DiagnosticStatus::OK; + std::string msg = "OK"; + + if ( + use_compare_yaw_rate_ && + th_diff_rad_per_sec_ < + std::abs( + corrected_imu_.angular_velocity.z - + comparison_velocity_ptr_->twist.angular.z)) { + num_continuous_abnormal_yaw_rate_++; + } else { + num_continuous_abnormal_yaw_rate_ = 0; + } - if(_use_compare_yaw_rate && _th_diff_rad_per_sec < - std::abs(_corrected_imu.angular_velocity.z - _comparison_velocity_ptr->twist.angular.z)) - { - _num_continuous_abnormal_yaw_rate++; + if (num_continuous_abnormal_yaw_rate_ > th_num_continuous_abnormal_yaw_rate_) { + level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + msg = "IMU Yaw Rate too large or too small compared to reference twist"; + } + stat.summary(level, msg); } - else + + void printStatus() { - _num_continuous_abnormal_yaw_rate = 0; - } + std::cout << std::endl; + std::cout << "\033[1;33m Eagleye status \033[m" << std::endl; + std::cout << std::endl; + std::cout << std::fixed; + + std::cout << "--- \033[1;34m imu(input)\033[m ------------------------------" << std::endl; + std::cout << "\033[1m linear_acceleration \033[mx " << std::setprecision(6) + << imu_.linear_acceleration.x << " [m/s^2]" << std::endl; + std::cout << "\033[1m linear acceleration \033[my " << std::setprecision(6) + << imu_.linear_acceleration.y << " [m/s^2]" << std::endl; + std::cout << "\033[1m linear acceleration \033[mz " << std::setprecision(6) + << imu_.linear_acceleration.z << " [m/s^2]" << std::endl; + std::cout << "\033[1m angular velocity \033[mx " << std::setprecision(6) + << imu_.angular_velocity.x << " [rad/s]" << std::endl; + std::cout << "\033[1m angular velocity \033[my " << std::setprecision(6) + << imu_.angular_velocity.y << " [rad/s]" << std::endl; + std::cout << "\033[1m angular velocity \033[mz " << std::setprecision(6) + << imu_.angular_velocity.z << " [rad/s]" << std::endl; + std::cout << std::endl; - if (_num_continuous_abnormal_yaw_rate > _th_num_continuous_abnormal_yaw_rate) { - level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - msg = "IMU Yaw Rate too large or too small compared to reference twist"; - } - stat.summary(level, msg); -} + std::cout << "--- \033[1;34m velocity(input)\033[m -------------------------" << std::endl; + std::cout << "\033[1m velocity \033[m" << std::setprecision(4) + << velocity_.twist.linear.x * 3.6 << " [km/h]" << std::endl; + std::cout << std::endl; -void printStatus(void) -{ - std::cout << std::endl; - std::cout<<"\033[1;33m Eagleye status \033[m"<::max_digits10) << _imu.angular_velocity.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.angular_velocity.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.angular_velocity.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _imu.linear_acceleration.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.tow << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_pos.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.ecef_vel.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_rtklib_nav.status.status.status) << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.status.service << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.latitude << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.longitude << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rtklib_nav.status.altitude << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.linear.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity.twist.angular.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _velocity_scale_factor.scale_factor << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.linear.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _correction_velocity.twist.angular.z << ","; - output_log_file << (_velocity_scale_factor.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_velocity_scale_factor.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _distance.distance << ","; - output_log_file << (_distance.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_distance.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_1st.heading_angle << ","; - output_log_file << (_heading_1st.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_1st.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_1st.heading_angle << ","; - output_log_file << (_heading_interpolate_1st.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_interpolate_1st.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_2nd.heading_angle << ","; - output_log_file << (_heading_2nd.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_2nd.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_2nd.heading_angle << ","; - output_log_file << (_heading_interpolate_2nd.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_interpolate_2nd.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_3rd.heading_angle << ","; - output_log_file << (_heading_3rd.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_3rd.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _heading_interpolate_3rd.heading_angle << ","; - output_log_file << (_heading_interpolate_3rd.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_heading_interpolate_3rd.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yaw_rate_offset_stop.yaw_rate_offset << ","; - output_log_file << (_yaw_rate_offset_stop.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_yaw_rate_offset_stop.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yaw_rate_offset_1st.yaw_rate_offset << ","; - output_log_file << (_yaw_rate_offset_1st.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_yaw_rate_offset_1st.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _yaw_rate_offset_2nd.yaw_rate_offset << ","; - output_log_file << (_yaw_rate_offset_2nd.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_yaw_rate_offset_2nd.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _slip_angle.coefficient << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _slip_angle.slip_angle << ","; - output_log_file << (_slip_angle.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_slip_angle.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_vel.vector.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.enu_pos.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos.ecef_base_pos.z << ","; - output_log_file << (_enu_absolute_pos.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_enu_absolute_pos.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.enu_pos.z << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_absolute_pos_interpolate.ecef_base_pos.z << ","; - output_log_file << (_enu_absolute_pos_interpolate.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_enu_absolute_pos_interpolate.status.estimate_status ? "1" : "0") << ","; - // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.rollrate_offset << ","; - // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.pitch_rate_offset << ","; - // output_log_file << std::setprecision(std::numeric_limits::max_digits10) << angular_velocity_offset_stop.yaw_rate_offset << ","; - // output_log_file << (angular_velocity_offset_stop.status.enabled_status ? "1" : "0") << ","; - // output_log_file << (angular_velocity_offset_stop.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _height.height << ","; - output_log_file << (_height.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_height.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _pitching.pitching_angle << ","; - output_log_file << (_pitching.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_pitching.status.estimate_status ? "1" : "0") << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // acc_x_offset - output_log_file << 0 << ","; // acc_x_offset.status.enabled_status - output_log_file << 0 << ","; // acc_x_offset.status.estimate_status - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; // acc_x_scale_factor.acc_x_scale_factor - output_log_file << 0 << ","; // acc_x_scale_factor.status.enabled_status - output_log_file << 0 << ","; // acc_x_scale_factor.status.estimate_status - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _rolling.rolling_angle << ","; - output_log_file << (_rolling.status.enabled_status ? "1" : "0") << ","; - output_log_file << (_rolling.status.estimate_status ? "1" : "0") << ","; - rclcpp::Time gga_clock(_gga.header.stamp); - double gga_time = gga_clock.seconds(); - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << gga_time << ","; //timestamp - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lat << ","; //gga_llh.latitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.lon << ","; //gga_llh.longitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _gga.alt + _gga.undulation<< ","; //gga_llh.altitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << int(_gga.gps_qual) << ","; //gga_llh.gps_qual - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.latitude << ","; //eagleye_pp_llh.latitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.longitude << ","; //eagleye_pp_llh.longitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _eagleye_fix.altitude << ","; //eagleye_pp_llh.altitude - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[0] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[1] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[2] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[3] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[4] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[5] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[6] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[7] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.orientation_covariance[8] - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.status - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; //eagleye_pp_llh.status - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.x << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.y << ","; - output_log_file << std::setprecision(std::numeric_limits::max_digits10) << _enu_relative_pos.enu_pos.z << ","; - output_log_file << (_enu_relative_pos.status.enabled_status ? "1" : "0"); - output_log_file << "\n"; + std::cout << "--- \033[1;34m heading\033[m ---------------------------------" << std::endl; + std::cout << "\033[1m heading \033[m " << std::setprecision(6) + << heading_interpolate_3rd_.heading_angle << " [rad/s]" << std::endl; + std::cout << "\033[1m status enable \033[m " + << (heading_interpolate_3rd_.status.enabled_status ? "\033[1;32mTrue\033[m" + : "\033[1;31mFalse\033[m") + << std::endl; + std::cout << std::endl; + + std::cout << "--- \033[1;34m pitching\033[m --------------------------------" << std::endl; + std::cout << "\033[1m pitching \033[m " << std::setprecision(6) + << pitching_.pitching_angle << " [rad]" << std::endl; + std::cout << "\033[1m status enable \033[m " + << (pitching_.status.enabled_status ? "\033[1;32mTrue\033[m" + : "\033[1;31mFalse\033[m") + << std::endl; + std::cout << std::endl; + + std::cout << "--- \033[1;34m height\033[m ----------------------------------" << std::endl; + std::cout << "\033[1m height \033[m " << std::setprecision(4) << height_.height << " [m]" + << std::endl; + std::cout << "\033[1m status enable \033[m " + << (height_.status.enabled_status ? "\033[1;32mTrue\033[m" + : "\033[1;31mFalse\033[m") + << std::endl; + std::cout << std::endl; + + std::cout << "--- \033[1;34m position\033[m --------------------------------" << std::endl; + std::cout << "\033[1m latitude \033[m" << std::setprecision(8) << eagleye_fix_.latitude + << " [deg]" << std::endl; + std::cout << "\033[1m longitude \033[m" << std::setprecision(8) << eagleye_fix_.longitude + << " [deg]" << std::endl; + std::cout << "\033[1m altitude \033[m" << std::setprecision(4) << eagleye_fix_.altitude + << " [m]" << std::endl; + std::cout << "\033[1m status enable \033[m " + << (enu_absolute_pos_interpolate_.status.enabled_status ? "\033[1;32mTrue\033[m" + : "\033[1;31mFalse\033[m") + << std::endl; + std::cout << std::endl; } - return; -} -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - _imu.header = msg->header; - _imu.orientation = msg->orientation; - _imu.orientation_covariance = msg->orientation_covariance; - _imu.angular_velocity = msg->angular_velocity; - _imu.angular_velocity_covariance = msg->angular_velocity_covariance; - _imu.linear_acceleration = msg->linear_acceleration; - _imu.linear_acceleration_covariance = msg->linear_acceleration_covariance; - - if (_print_status) + void outputLog() { - printStatus(); + if (!log_header_make_) { + std::ofstream output_log_file( + output_log_dir_, std::ios_base::trunc | std::ios_base::out); + std::cout << "Output file = eagleye_log.csv" << std::endl; + output_log_file + << "timestamp,imu.angular_velocity.x,imu.angular_velocity.y,imu.angular_velocity.z," + "imu.linear_acceleration.x,imu.linear_acceleration.y,imu.linear_acceleration.z" + << ",rtklib_nav.tow,rtklib_nav.ecef_pos.x,rtklib_nav.ecef_pos.y,rtklib_nav.ecef_pos.z," + "rtklib_nav.ecef_vel.x,rtklib_nav.ecef_vel.y,rtklib_nav.ecef_vel.z,rtklib_nav.status." + "status.status,rtklib_nav.status.status.service,rtklib_nav.status.latitude,rtklib_nav." + "status.longitude,rtklib_nav.status.altitude" + << ",velocity.twist.linear.x,velocity.twist.linear.y,velocity.twist.linear.z,velocity." + "twist.angular.x,velocity.twist.angular.y,velocity.twist.angular.z" + << ",velocity_scale_factor.scale_factor,correction_velocity.twist.linear.x,correction_" + "velocity.twist.linear.y,correction_velocity.twist.linear.z,correction_velocity.twist." + "angular.x,correction_velocity.twist.angular.y,correction_velocity.twist.angular.z," + "velocity_scale_factor.status.enabled_status,velocity_scale_factor.status.estimate_" + "status" + << ",distance.distance,distance.status.enabled_status,distance.status.estimate_status" + << ",heading_1st.heading_angle,heading_1st.status.enabled_status,heading_1st.status." + "estimate_status" + << ",heading_interpolate_1st.heading_angle,heading_interpolate_1st.status.enabled_status," + "heading_interpolate_1st.status.estimate_status" + << ",heading_2nd.heading_angle,heading_2nd.status.enabled_status,heading_2nd.status." + "estimate_status" + << ",heading_interpolate_2nd.heading_angle,heading_interpolate_2nd.status.enabled_status," + "heading_interpolate_2nd.status.estimate_status" + << ",heading_3rd.heading_angle,heading_3rd.status.enabled_status,heading_3rd.status." + "estimate_status" + << ",heading_interpolate_3rd.heading_angle,heading_interpolate_3rd.status.enabled_status," + "heading_interpolate_3rd.status.estimate_status" + << ",yaw_rate_offset_stop.yaw_rate_offset,yaw_rate_offset_stop.status.enabled_status," + "yaw_rate_offset_stop.status.estimate_status" + << ",yaw_rate_offset_1st.yaw_rate_offset,yaw_rate_offset_1st.status.enabled_status," + "yaw_rate_offset_1st.status.estimate_status" + << ",yaw_rate_offset_2nd.yaw_rate_offset,yaw_rate_offset_2nd.status.enabled_status," + "yaw_rate_offset_2nd.status.estimate_status" + << ",slip_angle.coefficient,slip_angle.slip_angle,slip_angle.status.enabled_status," + "slip_angle.status.estimate_status" + << ",enu_vel.vector.x,enu_vel.vector.y,enu_vel.vector.z" + << ",enu_absolute_pos.enu_pos.x,enu_absolute_pos.enu_pos.y,enu_absolute_pos.enu_pos.z," + "enu_absolute_pos.ecef_base_pos.x,enu_absolute_pos.ecef_base_pos.y,enu_absolute_pos." + "ecef_base_pos.z,enu_absolute_pos.status.enabled_status,enu_absolute_pos.status." + "estimate_status" + << ",enu_absolute_pos_interpolate.enu_pos.x,enu_absolute_pos_interpolate.enu_pos.y," + "enu_absolute_pos_interpolate.enu_pos.z,enu_absolute_pos_interpolate.ecef_base_pos.x," + "enu_absolute_pos_interpolate.ecef_base_pos.y,enu_absolute_pos_interpolate.ecef_base_" + "pos.z,enu_absolute_pos_interpolate.status.enabled_status,enu_absolute_pos_interpolate." + "status.estimate_status" + << ",height.height,height.status.enabled_status,height.status.estimate_status" + << ",pitching.pitching_angle,pitching.status.enabled_status,pitching.status.estimate_status" + << ",acc_x_offset.acc_x_offset,acc_x_offset.status.enabled_status,acc_x_offset.status." + "estimate_status" + << ",acc_x_scale_factor.acc_x_scale_factor,acc_x_scale_factor.status.enabled_status," + "acc_x_scale_factor.status.estimate_status" + << ",rolling.rolling_angle,rolling.status.enabled_status,rolling.status.estimate_status" + << ",gga_timestamp" + << ",gga_llh.latitude,gga_llh.longitude,gga_llh.altitude" + << ",gga_llh.gps_qual" + << ",eagleye_pp_llh.latitude,eagleye_pp_llh.longitude,eagleye_pp_llh.altitude" + << ",eagleye_pp_llh.orientation_covariance[0],eagleye_pp_llh.orientation_covariance[1]," + "eagleye_pp_llh.orientation_covariance[2],eagleye_pp_llh.orientation_covariance[3]," + "eagleye_pp_llh.orientation_covariance[4],eagleye_pp_llh.orientation_covariance[5]," + "eagleye_pp_llh.orientation_covariance[6],eagleye_pp_llh.orientation_covariance[7]," + "eagleye_pp_llh.orientation_covariance[8]" + << ",eagleye_pp_llh.status" + << ",eagleye_pp_llh.height_status" + << ",enu_relative_pos.enu_pos.x,enu_relative_pos.enu_pos.y,enu_relative_pos.enu_pos.z" + << ",enu_relative_pos.status.enabled_status" + << std::endl; + log_header_make_ = true; + } else { + std::ofstream output_log_file(output_log_dir_, std::ios_base::app); + rclcpp::Time imu_clock(imu_.header.stamp); + long double nano_sec = imu_clock.nanoseconds(); + long double sec_digits = std::pow(10, 9); + long double imu_time = nano_sec / sec_digits; + output_log_file << std::fixed << std::setprecision(9) << imu_time << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.angular_velocity.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.angular_velocity.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.angular_velocity.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.linear_acceleration.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.linear_acceleration.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << imu_.linear_acceleration.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.tow << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_vel.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_vel.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.ecef_vel.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << int(rtklib_nav_.status.status.status) << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.status.status.service << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.status.latitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.status.longitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rtklib_nav_.status.altitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.linear.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.linear.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.linear.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.angular.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.angular.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_.twist.angular.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << velocity_scale_factor_.scale_factor << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.linear.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.linear.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.linear.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.angular.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.angular.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << correction_velocity_.twist.angular.z << ","; + output_log_file << (velocity_scale_factor_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (velocity_scale_factor_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << distance_.distance << ","; + output_log_file << (distance_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (distance_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_1st_.heading_angle << ","; + output_log_file << (heading_1st_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_1st_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_interpolate_1st_.heading_angle << ","; + output_log_file << (heading_interpolate_1st_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_interpolate_1st_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_2nd_.heading_angle << ","; + output_log_file << (heading_2nd_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_2nd_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_interpolate_2nd_.heading_angle << ","; + output_log_file << (heading_interpolate_2nd_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_interpolate_2nd_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_3rd_.heading_angle << ","; + output_log_file << (heading_3rd_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_3rd_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << heading_interpolate_3rd_.heading_angle << ","; + output_log_file << (heading_interpolate_3rd_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (heading_interpolate_3rd_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << yaw_rate_offset_stop_.yaw_rate_offset << ","; + output_log_file << (yaw_rate_offset_stop_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (yaw_rate_offset_stop_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << yaw_rate_offset_1st_.yaw_rate_offset << ","; + output_log_file << (yaw_rate_offset_1st_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (yaw_rate_offset_1st_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << yaw_rate_offset_2nd_.yaw_rate_offset << ","; + output_log_file << (yaw_rate_offset_2nd_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (yaw_rate_offset_2nd_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << slip_angle_.coefficient << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << slip_angle_.slip_angle << ","; + output_log_file << (slip_angle_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (slip_angle_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_vel_.vector.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_vel_.vector.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_vel_.vector.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.enu_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.ecef_base_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.ecef_base_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_.ecef_base_pos.z << ","; + output_log_file << (enu_absolute_pos_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (enu_absolute_pos_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.enu_pos.z << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.ecef_base_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.ecef_base_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_absolute_pos_interpolate_.ecef_base_pos.z << ","; + output_log_file << (enu_absolute_pos_interpolate_.status.enabled_status ? "1" : "0") + << ","; + output_log_file << (enu_absolute_pos_interpolate_.status.estimate_status ? "1" : "0") + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << height_.height << ","; + output_log_file << (height_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (height_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << pitching_.pitching_angle << ","; + output_log_file << (pitching_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (pitching_.status.estimate_status ? "1" : "0") << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; // acc_x_offset + output_log_file << 0 << ","; // acc_x_offset.status.enabled_status + output_log_file << 0 << ","; // acc_x_offset.status.estimate_status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; // acc_x_scale_factor.acc_x_scale_factor + output_log_file << 0 << ","; // acc_x_scale_factor.status.enabled_status + output_log_file << 0 << ","; // acc_x_scale_factor.status.estimate_status + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << rolling_.rolling_angle << ","; + output_log_file << (rolling_.status.enabled_status ? "1" : "0") << ","; + output_log_file << (rolling_.status.estimate_status ? "1" : "0") << ","; + rclcpp::Time gga_clock(gga_.header.stamp); + double gga_time = gga_clock.seconds(); + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << gga_time + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << gga_.lat + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << gga_.lon + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << gga_.alt + gga_.undulation << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << int(gga_.gps_qual) << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << eagleye_fix_.latitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << eagleye_fix_.longitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << eagleye_fix_.altitude << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 + << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) << 0 << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_relative_pos_.enu_pos.x << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_relative_pos_.enu_pos.y << ","; + output_log_file << std::setprecision(std::numeric_limits::max_digits10) + << enu_relative_pos_.enu_pos.z << ","; + output_log_file << (enu_relative_pos_.status.enabled_status ? "1" : "0"); + output_log_file << "\n"; + } } - if(_log_output_status) + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - outputLog(); - } + imu_.header = msg->header; + imu_.orientation = msg->orientation; + imu_.orientation_covariance = msg->orientation_covariance; + imu_.angular_velocity = msg->angular_velocity; + imu_.angular_velocity_covariance = msg->angular_velocity_covariance; + imu_.linear_acceleration = msg->linear_acceleration; + imu_.linear_acceleration_covariance = msg->linear_acceleration_covariance; + + if (print_status_) { + printStatus(); + } -} + if (log_output_status_) { + outputLog(); + } + } +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_monitor"); - - std::string subscribe_twist_topic_name = "vehicle/twist"; - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - std::string subscribe_gga_topic_name = "gnss/gga"; - std::string comparison_twist_topic_name = "/calculated_twist"; - - node->declare_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->declare_parameter("gga_topic",subscribe_gga_topic_name); - node->declare_parameter("monitor.comparison_twist_topic",comparison_twist_topic_name); - node->declare_parameter("monitor.print_status",_print_status); - node->declare_parameter("monitor.log_output_status",_log_output_status); - node->declare_parameter("monitor.use_compare_yaw_rate",_use_compare_yaw_rate); - node->declare_parameter("monitor.th_diff_rad_per_sec",_th_diff_rad_per_sec); - node->declare_parameter("monitor.th_num_continuous_abnormal_yaw_rate",_th_num_continuous_abnormal_yaw_rate); - - node->get_parameter("rtklib_nav_topic",subscribe_rtklib_nav_topic_name); - node->get_parameter("gga_topic",subscribe_gga_topic_name); - node->get_parameter("monitor.comparison_twist_topic",comparison_twist_topic_name); - node->get_parameter("monitor.print_status",_print_status); - node->get_parameter("monitor.log_output_status",_log_output_status); - node->get_parameter("monitor.use_compare_yaw_rate",_use_compare_yaw_rate); - node->get_parameter("monitor.th_diff_rad_per_sec",_th_diff_rad_per_sec); - node->get_parameter("monitor.th_num_continuous_abnormal_yaw_rate",_th_num_continuous_abnormal_yaw_rate); - - std::cout<< "subscribe_rtklib_nav_topic_name "<(node, update_time); - - updater_->setHardwareID("eagleye_topic_checker"); - updater_->add("eagleye_input_imu", imu_topic_checker); - updater_->add("eagleye_input_rtklib_nav", rtklib_nav_topic_checker); - updater_->add("eagleye_input_navsat_gga", navsat_gga_topic_checker); - updater_->add("eagleye_input_velocity", velocity_topic_checker); - updater_->add("eagleye_velocity_scale_factor", velocity_scale_factor_topic_checker); - updater_->add("eagleye_distance", distance_topic_checker); - updater_->add("eagleye_heading_1st", heading_1st_topic_checker); - updater_->add("eagleye_heading_interpolate_1st", heading_interpolate_1st_topic_checker); - updater_->add("eagleye_heading_2nd", heading_2nd_topic_checker); - updater_->add("eagleye_heading_interpolate_2nd", heading_interpolate_2nd_topic_checker); - updater_->add("eagleye_heading_3rd", heading_3rd_topic_checker); - updater_->add("eagleye_heading_interpolate_3rd", heading_interpolate_3rd_topic_checker); - updater_->add("eagleye_yaw_rate_offset_stop", yaw_rate_offset_stop_topic_checker); - updater_->add("eagleye_yaw_rate_offset_1st", yaw_rate_offset_1st_topic_checker); - updater_->add("eagleye_yaw_rate_offset_2nd", yaw_rate_offset_2nd_topic_checker); - updater_->add("eagleye_slip_angle", slip_angle_topic_checker); - updater_->add("eagleye_enu_vel", enu_vel_topic_checker); - updater_->add("eagleye_height", height_topic_checker); - updater_->add("eagleye_pitching", pitching_topic_checker); - updater_->add("eagleye_enu_absolute_pos", enu_absolute_pos_topic_checker); - updater_->add("eagleye_enu_absolute_pos_interpolate", enu_absolute_pos_interpolate_topic_checker); - updater_->add("eagleye_twist", twist_topic_checker); - if(_use_compare_yaw_rate) updater_->add("eagleye_imu_comparison", imu_comparison_checker); - - time_t time_; - time_ = time(NULL); - std::stringstream time_ss; - time_ss << time_; - std::string time_str = time_ss.str(); - _output_log_dir = ament_index_cpp::get_package_share_directory("eagleye_rt") + "/log/eagleye_log_" + time_str + ".csv"; - if(_log_output_status) std::cout << _output_log_dir << std::endl; - - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub3 = node->create_subscription("rtklib/fix", rclcpp::QoS(10), rtklib_fix_callback); - auto sub4 = node->create_subscription(subscribe_gga_topic_name, 1000, navsatfix_gga_callback); - auto sub5 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); - auto sub6 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); - auto sub7 = node->create_subscription("distance", rclcpp::QoS(10), distance_callback); - auto sub8 = node->create_subscription("heading_1st", rclcpp::QoS(10), heading_1st_callback); - auto sub9 = node->create_subscription("heading_interpolate_1st", rclcpp::QoS(10), heading_interpolate_1st_callback); - auto sub10 = node->create_subscription("heading_2nd", rclcpp::QoS(10), heading_2nd_callback); - auto sub11 = node->create_subscription("heading_interpolate_2nd", rclcpp::QoS(10), heading_interpolate_2nd_callback); - auto sub12 = node->create_subscription("heading_3rd", rclcpp::QoS(10), heading_3rd_callback); - auto sub13 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); - auto sub14 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); - auto sub15 = node->create_subscription("yaw_rate_offset_1st", rclcpp::QoS(10), yaw_rate_offset_1st_callback); - auto sub16 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback); - auto sub17 = node->create_subscription("slip_angle", rclcpp::QoS(10), slip_angle_callback); - auto sub18 = node->create_subscription("enu_relative_pos", rclcpp::QoS(10), enu_relative_pos_callback); - auto sub19 = node->create_subscription("enu_vel", rclcpp::QoS(10), enu_vel_callback); - auto sub20 = node->create_subscription("height", rclcpp::QoS(10), height_callback); - auto sub21 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); - auto sub22 = node->create_subscription("enu_absolute_pos", rclcpp::QoS(10), enu_absolute_pos_callback); - auto sub23 = node->create_subscription("enu_absolute_pos_interpolate", rclcpp::QoS(10), enu_absolute_pos_interpolate_callback); - auto sub24 = node->create_subscription("fix", rclcpp::QoS(10), eagleye_fix_callback); - auto sub25 = node->create_subscription("twist", rclcpp::QoS(10), eagleye_twist_callback); - auto sub26 = node->create_subscription("rolling", rclcpp::QoS(10), rolling_callback); - auto sub27 = node->create_subscription(comparison_twist_topic_name, 1000, comparison_velocity_callback); - auto sub28 = node->create_subscription("velocity", 1000, correction_velocity_callback); - - rclcpp::spin(node); - + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/position_interpolate_node.cpp b/eagleye_rt/src/position_interpolate_node.cpp index 097dd85e..8a579d06 100644 --- a/eagleye_rt/src/position_interpolate_node.cpp +++ b/eagleye_rt/src/position_interpolate_node.cpp @@ -28,128 +28,147 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static eagleye_msgs::msg::Position enu_absolute_pos; -static geometry_msgs::msg::Vector3Stamped enu_vel; -static eagleye_msgs::msg::Height height; -static eagleye_msgs::msg::Position gnss_smooth_pos; -static nmea_msgs::msg::Gpgga gga; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; - -static eagleye_msgs::msg::Position enu_absolute_pos_interpolate; -static sensor_msgs::msg::NavSatFix eagleye_fix; -rclcpp::Publisher::SharedPtr pub1; -rclcpp::Publisher::SharedPtr pub2; - -struct PositionInterpolateParameter position_interpolate_parameter; -struct PositionInterpolateStatus position_interpolate_status; - -std::string node_name = "eagleye_position_interpolate"; - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga = *msg; -} - -void enu_absolute_pos_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - enu_absolute_pos = *msg; -} - -void gnss_smooth_pos_enu_callback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) -{ - gnss_smooth_pos = *msg; -} - -void height_callback(const eagleye_msgs::msg::Height::ConstSharedPtr msg) -{ - height = *msg; -} - -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate_3rd = *msg; -} - -void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) +class PositionInterpolateNode : public rclcpp::Node { - rclcpp::Time ros_clock(gga.header.stamp); - auto gga_time = ros_clock.seconds(); - - enu_vel = *msg; - enu_absolute_pos_interpolate.header = msg->header; - enu_absolute_pos_interpolate.header.frame_id = "base_link"; - eagleye_fix.header = msg->header; - eagleye_fix.header.frame_id = "gnss"; - position_interpolate_estimate(enu_absolute_pos,enu_vel,gnss_smooth_pos,height,heading_interpolate_3rd,position_interpolate_parameter,&position_interpolate_status,&enu_absolute_pos_interpolate,&eagleye_fix); - if (enu_absolute_pos.status.enabled_status == true) +public: + PositionInterpolateNode() : Node("eagleye_position_interpolate") { - if(eagleye_fix.latitude == 0 && eagleye_fix.longitude == 0) - { - RCLCPP_WARN(rclcpp::get_logger(node_name), "eagleye_fix is not published because latitude and longitude are 0."); - } - else - { - pub1->publish(enu_absolute_pos_interpolate); - pub2->publish(eagleye_fix); + std::string subscribe_gga_topic_name = "gnss/gga"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + position_interpolate_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + position_interpolate_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + position_interpolate_parameter_.sync_search_period = + conf["/**"]["ros__parameters"]["position_interpolate"]["sync_search_period"].as(); + position_interpolate_parameter_.proc_noise = + conf["/**"]["ros__parameters"]["position_interpolate"]["proc_noise"].as(); + + std::cout << "imu_rate " << position_interpolate_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " + << position_interpolate_parameter_.stop_judgment_threshold << std::endl; + std::cout << "sync_search_period " << position_interpolate_parameter_.sync_search_period + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); } + + sub_enu_vel_ = this->create_subscription( + "enu_vel", rclcpp::QoS(10), + std::bind(&PositionInterpolateNode::enuVelCallback, this, std::placeholders::_1)); + sub_enu_absolute_pos_ = this->create_subscription( + "enu_absolute_pos", rclcpp::QoS(10), + std::bind(&PositionInterpolateNode::enuAbsolutePosCallback, this, std::placeholders::_1)); + sub_gnss_smooth_pos_ = this->create_subscription( + "gnss_smooth_pos_enu", rclcpp::QoS(10), + std::bind( + &PositionInterpolateNode::gnssSmootPosEnuCallback, this, std::placeholders::_1)); + sub_height_ = this->create_subscription( + "height", rclcpp::QoS(10), + std::bind(&PositionInterpolateNode::heightCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, rclcpp::QoS(10), + std::bind(&PositionInterpolateNode::ggaCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", rclcpp::QoS(10), + std::bind( + &PositionInterpolateNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); + pub_pos_ = this->create_publisher( + "enu_absolute_pos_interpolate", rclcpp::QoS(10)); + pub_fix_ = this->create_publisher("fix", rclcpp::QoS(10)); } - else if (gga_time != 0) + +private: + eagleye_msgs::msg::Position enu_absolute_pos_; + geometry_msgs::msg::Vector3Stamped enu_vel_; + eagleye_msgs::msg::Height height_; + eagleye_msgs::msg::Position gnss_smooth_pos_; + nmea_msgs::msg::Gpgga gga_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + eagleye_msgs::msg::Position enu_absolute_pos_interpolate_; + sensor_msgs::msg::NavSatFix eagleye_fix_; + PositionInterpolateParameter position_interpolate_parameter_; + PositionInterpolateStatus position_interpolate_status_ = {}; + + rclcpp::Publisher::SharedPtr pub_pos_; + rclcpp::Publisher::SharedPtr pub_fix_; + rclcpp::Subscription::SharedPtr sub_enu_vel_; + rclcpp::Subscription::SharedPtr sub_enu_absolute_pos_; + rclcpp::Subscription::SharedPtr sub_gnss_smooth_pos_; + rclcpp::Subscription::SharedPtr sub_height_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_ = *msg; } + + void enuAbsolutePosCallback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) { - sensor_msgs::msg::NavSatFix fix; - fix.header = gga.header; - fix.latitude = gga.lat; - fix.longitude = gga.lon; - fix.altitude = gga.alt + gga.undulation; - pub2->publish(fix); + enu_absolute_pos_ = *msg; } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(node_name); - - std::string subscribe_gga_topic_name = "gnss/gga"; - - 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; - try + void gnssSmootPosEnuCallback(const eagleye_msgs::msg::Position::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); + gnss_smooth_pos_ = *msg; + } - position_interpolate_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - position_interpolate_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - position_interpolate_parameter.sync_search_period = conf["/**"]["ros__parameters"]["position_interpolate"]["sync_search_period"].as(); - position_interpolate_parameter.proc_noise = conf["/**"]["ros__parameters"]["position_interpolate"]["proc_noise"].as(); + void heightCallback(const eagleye_msgs::msg::Height::ConstSharedPtr msg) { height_ = *msg; } - std::cout << "imu_rate " << position_interpolate_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << position_interpolate_parameter.stop_judgment_threshold << std::endl; - std::cout << "sync_search_period " << position_interpolate_parameter.sync_search_period << std::endl; - } - catch (YAML::Exception& e) + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - std::cerr << "\033[1;31mheading_interpolate Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + heading_interpolate_3rd_ = *msg; } + void enuVelCallback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) + { + rclcpp::Time ros_clock(gga_.header.stamp); + auto gga_time = ros_clock.seconds(); + + enu_vel_ = *msg; + enu_absolute_pos_interpolate_.header = msg->header; + enu_absolute_pos_interpolate_.header.frame_id = "base_link"; + eagleye_fix_.header = msg->header; + eagleye_fix_.header.frame_id = "gnss"; + position_interpolate_estimate( + enu_absolute_pos_, enu_vel_, gnss_smooth_pos_, height_, heading_interpolate_3rd_, + position_interpolate_parameter_, &position_interpolate_status_, + &enu_absolute_pos_interpolate_, &eagleye_fix_); + if (enu_absolute_pos_.status.enabled_status == true) { + if (eagleye_fix_.latitude == 0 && eagleye_fix_.longitude == 0) { + RCLCPP_WARN( + this->get_logger(), + "eagleye_fix is not published because latitude and longitude are 0."); + } else { + pub_pos_->publish(enu_absolute_pos_interpolate_); + pub_fix_->publish(eagleye_fix_); + } + } else if (gga_time != 0) { + sensor_msgs::msg::NavSatFix fix; + fix.header = gga_.header; + fix.latitude = gga_.lat; + fix.longitude = gga_.lon; + fix.altitude = gga_.alt + gga_.undulation; + pub_fix_->publish(fix); + } + } +}; - auto sub1 = node->create_subscription("enu_vel", rclcpp::QoS(10), enu_vel_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("enu_absolute_pos", rclcpp::QoS(10), enu_absolute_pos_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("gnss_smooth_pos_enu", rclcpp::QoS(10), gnss_smooth_pos_enu_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("height", rclcpp::QoS(10), height_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription(subscribe_gga_topic_name, rclcpp::QoS(10), gga_callback); //ros::TransportHints().tcpNoDelay() - auto sub6 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); //ros::TransportHints().tcpNoDelay() - pub1 = node->create_publisher("enu_absolute_pos_interpolate", rclcpp::QoS(10)); - pub2 = node->create_publisher("fix", rclcpp::QoS(10)); - - rclcpp::spin(node); - +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/position_node.cpp b/eagleye_rt/src/position_node.cpp index c681c1bb..cab001e1 100644 --- a/eagleye_rt/src/position_node.cpp +++ b/eagleye_rt/src/position_node.cpp @@ -28,9 +28,9 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" #include #include @@ -40,209 +40,229 @@ #include #endif -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; -static eagleye_msgs::msg::Distance distance; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; -static eagleye_msgs::msg::Position enu_absolute_pos; -static geometry_msgs::msg::Vector3Stamped enu_vel; -static nmea_msgs::msg::Gpgga gga; -rclcpp::Publisher::SharedPtr pub; - -struct PositionParameter position_parameter; -struct PositionStatus position_status; - -std::string use_gnss_mode; -static bool use_can_less_mode; - -rclcpp::Clock clock_(RCL_ROS_TIME); -tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); - -std::string node_name = "eagleye_position"; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) +class PositionNode : public rclcpp::Node { - rtklib_nav = *msg; -} - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) -{ - velocity_scale_factor = *msg; -} - -void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) -{ - distance = *msg; -} - -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate_3rd = *msg; -} - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga = *msg; -} - - -void on_timer() -{ - geometry_msgs::msg::TransformStamped transformStamped; - try +public: + PositionNode() + : Node("eagleye_position"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) { - transformStamped = tfBuffer_.lookupTransform(position_parameter.tf_gnss_parent_frame, position_parameter.tf_gnss_child_frame, tf2::TimePointZero); - - position_parameter.tf_gnss_translation_x = transformStamped.transform.translation.x; - position_parameter.tf_gnss_translation_y = transformStamped.transform.translation.y; - position_parameter.tf_gnss_translation_z = transformStamped.transform.translation.z; - position_parameter.tf_gnss_rotation_x = transformStamped.transform.rotation.x; - position_parameter.tf_gnss_rotation_y = transformStamped.transform.rotation.y; - position_parameter.tf_gnss_rotation_z = transformStamped.transform.rotation.z; - position_parameter.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_gnss_mode_ = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + + position_parameter_.ecef_base_pos_x = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); + position_parameter_.ecef_base_pos_y = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); + position_parameter_.ecef_base_pos_z = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); + position_parameter_.tf_gnss_parent_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); + position_parameter_.tf_gnss_child_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); + position_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + position_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + position_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + position_parameter_.estimated_interval = + conf["/**"]["ros__parameters"]["position"]["estimated_interval"].as(); + position_parameter_.update_distance = + conf["/**"]["ros__parameters"]["position"]["update_distance"].as(); + position_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["position"]["outlier_threshold"].as(); + position_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["position"]["gnss_receiving_threshold"].as(); + position_parameter_.outlier_ratio_threshold = + conf["/**"]["ros__parameters"]["position"]["outlier_ratio_threshold"].as(); + position_parameter_.gnss_error_covariance = + conf["/**"]["ros__parameters"]["position"]["gnss_error_covariance"].as(); + + std::cout << "use_gnss_mode " << use_gnss_mode_ << std::endl; + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name + << std::endl; + std::cout << "ecef_base_pos_x " << position_parameter_.ecef_base_pos_x << std::endl; + std::cout << "ecef_base_pos_y " << position_parameter_.ecef_base_pos_y << std::endl; + std::cout << "ecef_base_pos_z " << position_parameter_.ecef_base_pos_z << std::endl; + std::cout << "tf_gnss_frame/parent " << position_parameter_.tf_gnss_parent_frame + << std::endl; + std::cout << "tf_gnss_frame/child " << position_parameter_.tf_gnss_child_frame << std::endl; + std::cout << "imu_rate " << position_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << position_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << position_parameter_.moving_judgment_threshold + << std::endl; + std::cout << "estimated_interval " << position_parameter_.estimated_interval << std::endl; + std::cout << "update_distance " << position_parameter_.update_distance << std::endl; + std::cout << "outlier_threshold " << position_parameter_.outlier_threshold << std::endl; + std::cout << "gnss_receiving_threshold " << position_parameter_.gnss_receiving_threshold + << std::endl; + std::cout << "outlier_ratio_threshold " << position_parameter_.outlier_ratio_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mposition Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + sub_enu_vel_ = this->create_subscription( + "enu_vel", 1000, + std::bind(&PositionNode::enuVelCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&PositionNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&PositionNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&PositionNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_velocity_scale_factor_ = this->create_subscription( + "velocity_scale_factor", 1000, + std::bind(&PositionNode::velocityScaleFactorCallback, this, std::placeholders::_1)); + sub_distance_ = this->create_subscription( + "distance", 1000, + std::bind(&PositionNode::distanceCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", 1000, + std::bind(&PositionNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, 1000, + std::bind(&PositionNode::ggaCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("enu_absolute_pos", 1000); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), std::bind(&PositionNode::onTimer, this)); } - catch (tf2::TransformException& ex) + +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + eagleye_msgs::msg::Distance distance_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + eagleye_msgs::msg::Position enu_absolute_pos_; + geometry_msgs::msg::Vector3Stamped enu_vel_; + nmea_msgs::msg::Gpgga gga_; + PositionParameter position_parameter_; + PositionStatus position_status_ = {}; + std::string use_gnss_mode_; + bool use_can_less_mode_ = false; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_enu_vel_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr + sub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_distance_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + rclcpp::Subscription::SharedPtr sub_gga_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - RCLCPP_WARN(rclcpp::get_logger(node_name), "%s", ex.what()); - return; + rtklib_nav_ = *msg; } -} - -void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) -{ - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - eagleye_msgs::msg::StatusStamped velocity_enable_status; - if(use_can_less_mode) + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_enable_status = velocity_status; + velocity_ = *msg; } - else + + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - velocity_enable_status.header = velocity_scale_factor.header; - velocity_enable_status.status = velocity_scale_factor.status; + velocity_status_ = *msg; } - enu_vel = *msg; - enu_absolute_pos.header = msg->header; - enu_absolute_pos.header.frame_id = "base_link"; - if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode - position_estimate(rtklib_nav, velocity, velocity_enable_status, distance, heading_interpolate_3rd, enu_vel, - position_parameter, &position_status, &enu_absolute_pos); - else if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode - position_estimate(gga, velocity, velocity_enable_status, distance, heading_interpolate_3rd, enu_vel, - position_parameter, &position_status, &enu_absolute_pos); - if (enu_absolute_pos.status.estimate_status == true) + void velocityScaleFactorCallback( + const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { - pub->publish(enu_absolute_pos); + velocity_scale_factor_ = *msg; } - enu_absolute_pos.status.estimate_status = false; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(node_name); - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - std::string subscribe_gga_topic_name = "gnss/gga"; - - 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; - try + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - - position_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); - position_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); - position_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); - - position_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); - position_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); - - position_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - position_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - position_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - - position_parameter.estimated_interval = conf["/**"]["ros__parameters"]["position"]["estimated_interval"].as(); - position_parameter.update_distance = conf["/**"]["ros__parameters"]["position"]["update_distance"].as(); - position_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["position"]["outlier_threshold"].as(); - - position_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["position"]["gnss_receiving_threshold"].as(); - position_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["position"]["outlier_ratio_threshold"].as(); - - position_parameter.gnss_error_covariance = conf["/**"]["ros__parameters"]["position"]["gnss_error_covariance"].as(); - - std::cout<< "use_gnss_mode " << use_gnss_mode << std::endl; - std::cout<< "use_can_less_mode " << use_can_less_mode << std::endl; - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - - std::cout<< "ecef_base_pos_x " << position_parameter.ecef_base_pos_x << std::endl; - std::cout<< "ecef_base_pos_y " << position_parameter.ecef_base_pos_y << std::endl; - std::cout<< "ecef_base_pos_z " << position_parameter.ecef_base_pos_z << std::endl; - - std::cout<< "tf_gnss_frame/parent " << position_parameter.tf_gnss_parent_frame << std::endl; - std::cout<< "tf_gnss_frame/child " << position_parameter.tf_gnss_child_frame << std::endl; - - std::cout << "imu_rate " << position_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << position_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << position_parameter.moving_judgment_threshold << std::endl; - - std::cout << "estimated_interval " << position_parameter.estimated_interval << std::endl; - std::cout << "update_distance " << position_parameter.update_distance << std::endl; - std::cout << "outlier_threshold " << position_parameter.outlier_threshold << std::endl; - std::cout << "gnss_receiving_threshold " << position_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_ratio_threshold " << position_parameter.outlier_ratio_threshold << std::endl; + distance_ = *msg; } - catch (YAML::Exception& e) + + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - std::cerr << "\033[1;31mposition Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + heading_interpolate_3rd_ = *msg; } + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_ = *msg; } - auto sub1 = node->create_subscription("enu_vel", 1000, enu_vel_callback); - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub5 = node->create_subscription("velocity_scale_factor", 1000, velocity_scale_factor_callback); - auto sub6 = node->create_subscription("distance", 1000, distance_callback); - auto sub7 = node->create_subscription("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback); - auto sub8 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - - pub = node->create_publisher("enu_absolute_pos", 1000); - - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(0.5)); - // auto timer_callback = std::bind(&on_timer, node); - auto timer_callback = std::bind(on_timer); - auto timer = std::make_shared>( - node->get_clock(), period_ns, std::move(timer_callback), - node->get_node_base_interface()->get_context()); - node->get_node_timers_interface()->add_timer(timer, nullptr); - - tf2_ros::TransformListener listener(tfBuffer_); + void onTimer() + { + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tf_buffer_.lookupTransform( + position_parameter_.tf_gnss_parent_frame, position_parameter_.tf_gnss_child_frame, + tf2::TimePointZero); + + position_parameter_.tf_gnss_translation_x = transformStamped.transform.translation.x; + position_parameter_.tf_gnss_translation_y = transformStamped.transform.translation.y; + position_parameter_.tf_gnss_translation_z = transformStamped.transform.translation.z; + position_parameter_.tf_gnss_rotation_x = transformStamped.transform.rotation.x; + position_parameter_.tf_gnss_rotation_y = transformStamped.transform.rotation.y; + position_parameter_.tf_gnss_rotation_z = transformStamped.transform.rotation.z; + position_parameter_.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return; + } + } - rclcpp::spin(node); + void enuVelCallback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if (use_can_less_mode_) { + velocity_enable_status = velocity_status_; + } else { + velocity_enable_status.header = velocity_scale_factor_.header; + velocity_enable_status.status = velocity_scale_factor_.status; + } + + enu_vel_ = *msg; + enu_absolute_pos_.header = msg->header; + enu_absolute_pos_.header.frame_id = "base_link"; + if (use_gnss_mode_ == "rtklib" || use_gnss_mode_ == "RTKLIB") + position_estimate( + rtklib_nav_, velocity_, velocity_enable_status, distance_, heading_interpolate_3rd_, + enu_vel_, position_parameter_, &position_status_, &enu_absolute_pos_); + else if (use_gnss_mode_ == "nmea" || use_gnss_mode_ == "NMEA") + position_estimate( + gga_, velocity_, velocity_enable_status, distance_, heading_interpolate_3rd_, enu_vel_, + position_parameter_, &position_status_, &enu_absolute_pos_); + if (enu_absolute_pos_.status.estimate_status == true) { + pub_->publish(enu_absolute_pos_); + } + enu_absolute_pos_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/rolling_node.cpp b/eagleye_rt/src/rolling_node.cpp index e029b26d..2f807d58 100644 --- a/eagleye_rt/src/rolling_node.cpp +++ b/eagleye_rt/src/rolling_node.cpp @@ -31,104 +31,108 @@ #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" -rclcpp::Publisher::SharedPtr _rolling_pub; - -static geometry_msgs::msg::TwistStamped _velocity_msg; -static eagleye_msgs::msg::StatusStamped _velocity_status_msg; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_2nd_msg; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_stop_msg; -static sensor_msgs::msg::Imu _imu_msg; - -static eagleye_msgs::msg::Rolling _rolling_msg; - -struct RollingParameter _rolling_parameter; -struct RollingStatus _rolling_status; - -static bool _use_can_less_mode; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _velocity_msg = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - _velocity_status_msg = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_stop_msg = *msg; -} - -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_2nd_msg = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if(_use_can_less_mode && !_velocity_status_msg.status.enabled_status) return; - _imu_msg = *msg; - rolling_estimate(_imu_msg, _velocity_msg, _yaw_rate_offset_stop_msg, _yaw_rate_offset_2nd_msg, - _rolling_parameter, &_rolling_status, &_rolling_msg); - _rolling_pub->publish(_rolling_msg); -} - -void setParam(rclcpp::Node::SharedPtr node) +class RollingNode : public rclcpp::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; - - try +public: + RollingNode() : Node("eagleye_rolling") { - YAML::Node conf = YAML::LoadFile(yaml_file); - - _use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - _rolling_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - _rolling_parameter.filter_process_noise = conf["/**"]["ros__parameters"]["rolling"]["filter_process_noise"].as(); - _rolling_parameter.filter_observation_noise = conf["/**"]["ros__parameters"]["rolling"]["filter_observation_noise"].as(); - - std::cout<< "use_can_less_mode " << _use_can_less_mode << std::endl; - std::cout << "stop_judgment_threshold " << _rolling_parameter.stop_judgment_threshold << std::endl; - std::cout << "filter_process_noise " << _rolling_parameter.filter_process_noise << std::endl; - std::cout << "filter_observation_noise " << _rolling_parameter.filter_observation_noise << std::endl; + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + rolling_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + rolling_parameter_.filter_process_noise = + conf["/**"]["ros__parameters"]["rolling"]["filter_process_noise"].as(); + rolling_parameter_.filter_observation_noise = + conf["/**"]["ros__parameters"]["rolling"]["filter_observation_noise"].as(); + + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "stop_judgment_threshold " << rolling_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "filter_process_noise " << rolling_parameter_.filter_process_noise << std::endl; + std::cout << "filter_observation_noise " << rolling_parameter_.filter_observation_noise + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mrolling Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&RollingNode::imuCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&RollingNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&RollingNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", 1000, + std::bind(&RollingNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", 1000, + std::bind(&RollingNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("rolling", 1000); } - catch (YAML::Exception& e) + +private: + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + sensor_msgs::msg::Imu imu_; + eagleye_msgs::msg::Rolling rolling_; + RollingParameter rolling_parameter_; + RollingStatus rolling_status_ = {}; + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - std::cerr << "\033[1;31mrolling Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + velocity_ = *msg; } -} -void rolling_node(rclcpp::Node::SharedPtr node) -{ - setParam(node); + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) + { + velocity_status_ = *msg; + } - auto imu_sub = - node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto velocity_sub = - node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto velocity_status_sub = - node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto yaw_rate_offset_2nd_sub = - node->create_subscription("yaw_rate_offset_2nd", 1000, yaw_rate_offset_2nd_callback); - auto yaw_rate_offset_stop_sub = - node->create_subscription("yaw_rate_offset_stop", 1000, yaw_rate_offset_stop_callback); + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; + } - _rolling_pub = node->create_publisher("rolling", 1000); + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_2nd_ = *msg; + } - rclcpp::spin(node); -} + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + imu_ = *msg; + rolling_estimate( + imu_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_2nd_, rolling_parameter_, + &rolling_status_, &rolling_); + pub_->publish(rolling_); + } +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_rolling"); - - rolling_node(node); - + rclcpp::spin(std::make_shared()); return 0; -} \ No newline at end of file +} diff --git a/eagleye_rt/src/rtk_dead_reckoning_node.cpp b/eagleye_rt/src/rtk_dead_reckoning_node.cpp index c8b8d18b..1a931f1a 100644 --- a/eagleye_rt/src/rtk_dead_reckoning_node.cpp +++ b/eagleye_rt/src/rtk_dead_reckoning_node.cpp @@ -28,9 +28,9 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" #include #include @@ -40,158 +40,181 @@ #include #endif -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static nmea_msgs::msg::Gpgga gga; -static geometry_msgs::msg::Vector3Stamped enu_vel; - -static eagleye_msgs::msg::Position enu_absolute_rtk_dead_reckoning; -static sensor_msgs::msg::NavSatFix eagleye_fix; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; - -rclcpp::Publisher::SharedPtr pub1; -rclcpp::Publisher::SharedPtr pub2; - -struct RtkDeadreckoningParameter rtk_dead_reckoning_parameter; -struct RtkDeadreckoningStatus rtk_dead_reckoning_status; - -std::string use_gnss_mode; - -rclcpp::Clock clock_(RCL_ROS_TIME); -tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - rtklib_nav = *msg; -} - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga = *msg; -} - -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) +class RtkDeadReckoningNode : public rclcpp::Node { - heading_interpolate_3rd = *msg; -} - +public: + RtkDeadReckoningNode() + : Node("eagleye_rtk_dead_reckoning"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) + { + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_gga_topic_name = "gnss/gga"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_gnss_mode_ = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); + rtk_dead_reckoning_parameter_.ecef_base_pos_x = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); + rtk_dead_reckoning_parameter_.ecef_base_pos_y = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); + rtk_dead_reckoning_parameter_.ecef_base_pos_z = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); + rtk_dead_reckoning_parameter_.use_ecef_base_position = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["use_ecef_base_position"].as(); + rtk_dead_reckoning_parameter_.tf_gnss_parent_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); + rtk_dead_reckoning_parameter_.tf_gnss_child_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); + rtk_dead_reckoning_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + rtk_dead_reckoning_parameter_.rtk_fix_STD = + conf["/**"]["ros__parameters"]["rtk_dead_reckoning"]["rtk_fix_STD"].as(); + rtk_dead_reckoning_parameter_.proc_noise = + conf["/**"]["ros__parameters"]["rtk_dead_reckoning"]["proc_noise"].as(); + + std::cout << "use_gnss_mode " << use_gnss_mode_ << std::endl; + std::cout << "ecef_base_pos_x " << rtk_dead_reckoning_parameter_.ecef_base_pos_x + << std::endl; + std::cout << "ecef_base_pos_y " << rtk_dead_reckoning_parameter_.ecef_base_pos_y + << std::endl; + std::cout << "ecef_base_pos_z " << rtk_dead_reckoning_parameter_.ecef_base_pos_z + << std::endl; + std::cout << "use_ecef_base_position " + << rtk_dead_reckoning_parameter_.use_ecef_base_position << std::endl; + std::cout << "tf_gnss_frame/parent " << rtk_dead_reckoning_parameter_.tf_gnss_parent_frame + << std::endl; + std::cout << "tf_gnss_frame/child " << rtk_dead_reckoning_parameter_.tf_gnss_child_frame + << std::endl; + std::cout << "stop_judgment_threshold " + << rtk_dead_reckoning_parameter_.stop_judgment_threshold << std::endl; + std::cout << "rtk_fix_STD " << rtk_dead_reckoning_parameter_.rtk_fix_STD << std::endl; + std::cout << "proc_noise " << rtk_dead_reckoning_parameter_.proc_noise << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mrtk_dead_reckoning Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&RtkDeadReckoningNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_enu_vel_ = this->create_subscription( + "enu_vel", 1000, + std::bind(&RtkDeadReckoningNode::enuVelCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, 1000, + std::bind(&RtkDeadReckoningNode::ggaCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", 1000, + std::bind( + &RtkDeadReckoningNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); + pub1_ = this->create_publisher("enu_absolute_pos_interpolate", + 1000); + pub2_ = this->create_publisher("fix", 1000); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), std::bind(&RtkDeadReckoningNode::onTimer, this)); + } -void on_timer() -{ - geometry_msgs::msg::TransformStamped transformStamped; - try +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + nmea_msgs::msg::Gpgga gga_; + geometry_msgs::msg::Vector3Stamped enu_vel_; + eagleye_msgs::msg::Position enu_absolute_rtk_dead_reckoning_; + sensor_msgs::msg::NavSatFix eagleye_fix_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + RtkDeadreckoningParameter rtk_dead_reckoning_parameter_; + RtkDeadreckoningStatus rtk_dead_reckoning_status_ = {}; + std::string use_gnss_mode_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub1_; + rclcpp::Publisher::SharedPtr pub2_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_enu_vel_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - transformStamped = tfBuffer_.lookupTransform(rtk_dead_reckoning_parameter.tf_gnss_parent_frame, rtk_dead_reckoning_parameter.tf_gnss_child_frame, tf2::TimePointZero); - - rtk_dead_reckoning_parameter.tf_gnss_translation_x = transformStamped.transform.translation.x; - rtk_dead_reckoning_parameter.tf_gnss_translation_y = transformStamped.transform.translation.y; - rtk_dead_reckoning_parameter.tf_gnss_translation_z = transformStamped.transform.translation.z; - rtk_dead_reckoning_parameter.tf_gnss_rotation_x = transformStamped.transform.rotation.x; - rtk_dead_reckoning_parameter.tf_gnss_rotation_y = transformStamped.transform.rotation.y; - rtk_dead_reckoning_parameter.tf_gnss_rotation_z = transformStamped.transform.rotation.z; - rtk_dead_reckoning_parameter.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + rtklib_nav_ = *msg; } - catch (tf2::TransformException& ex) + + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_ = *msg; } + + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - // RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - return; + heading_interpolate_3rd_ = *msg; } -} -void enu_vel_callback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) -{ - rclcpp::Time ros_clock(gga.header.stamp); - auto gga_time = ros_clock.seconds(); - - enu_vel.header = msg->header; - enu_vel.vector = msg->vector; - enu_absolute_rtk_dead_reckoning.header = msg->header; - enu_absolute_rtk_dead_reckoning.header.frame_id = "base_link"; - eagleye_fix.header = msg->header; - eagleye_fix.header.frame_id = "gnss"; - if (use_gnss_mode == "rtklib" || use_gnss_mode == "RTKLIB") // use RTKLIB mode - rtk_dead_reckoning_estimate(rtklib_nav,enu_vel,gga,heading_interpolate_3rd,rtk_dead_reckoning_parameter,&rtk_dead_reckoning_status,&enu_absolute_rtk_dead_reckoning,&eagleye_fix); - else if (use_gnss_mode == "nmea" || use_gnss_mode == "NMEA") // use NMEA mode - rtk_dead_reckoning_estimate(enu_vel,gga,heading_interpolate_3rd,rtk_dead_reckoning_parameter,&rtk_dead_reckoning_status,&enu_absolute_rtk_dead_reckoning,&eagleye_fix); - if (enu_absolute_rtk_dead_reckoning.status.enabled_status == true) + void onTimer() { - pub1->publish(enu_absolute_rtk_dead_reckoning); - pub2->publish(eagleye_fix); + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tf_buffer_.lookupTransform( + rtk_dead_reckoning_parameter_.tf_gnss_parent_frame, + rtk_dead_reckoning_parameter_.tf_gnss_child_frame, tf2::TimePointZero); + + rtk_dead_reckoning_parameter_.tf_gnss_translation_x = + transformStamped.transform.translation.x; + rtk_dead_reckoning_parameter_.tf_gnss_translation_y = + transformStamped.transform.translation.y; + rtk_dead_reckoning_parameter_.tf_gnss_translation_z = + transformStamped.transform.translation.z; + rtk_dead_reckoning_parameter_.tf_gnss_rotation_x = transformStamped.transform.rotation.x; + rtk_dead_reckoning_parameter_.tf_gnss_rotation_y = transformStamped.transform.rotation.y; + rtk_dead_reckoning_parameter_.tf_gnss_rotation_z = transformStamped.transform.rotation.z; + rtk_dead_reckoning_parameter_.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + } catch (tf2::TransformException& ex) { + return; + } } - else if (gga_time != 0) + + void enuVelCallback(const geometry_msgs::msg::Vector3Stamped::ConstSharedPtr msg) { - sensor_msgs::msg::NavSatFix fix; - fix.header = gga.header; - fix.latitude = gga.lat; - fix.longitude = gga.lon; - fix.altitude = gga.alt + gga.undulation; - pub2->publish(fix); + rclcpp::Time ros_clock(gga_.header.stamp); + auto gga_time = ros_clock.seconds(); + + enu_vel_.header = msg->header; + enu_vel_.vector = msg->vector; + enu_absolute_rtk_dead_reckoning_.header = msg->header; + enu_absolute_rtk_dead_reckoning_.header.frame_id = "base_link"; + eagleye_fix_.header = msg->header; + eagleye_fix_.header.frame_id = "gnss"; + if (use_gnss_mode_ == "rtklib" || use_gnss_mode_ == "RTKLIB") + rtk_dead_reckoning_estimate( + rtklib_nav_, enu_vel_, gga_, heading_interpolate_3rd_, rtk_dead_reckoning_parameter_, + &rtk_dead_reckoning_status_, &enu_absolute_rtk_dead_reckoning_, &eagleye_fix_); + else if (use_gnss_mode_ == "nmea" || use_gnss_mode_ == "NMEA") + rtk_dead_reckoning_estimate( + enu_vel_, gga_, heading_interpolate_3rd_, rtk_dead_reckoning_parameter_, + &rtk_dead_reckoning_status_, &enu_absolute_rtk_dead_reckoning_, &eagleye_fix_); + if (enu_absolute_rtk_dead_reckoning_.status.enabled_status == true) { + pub1_->publish(enu_absolute_rtk_dead_reckoning_); + pub2_->publish(eagleye_fix_); + } else if (gga_time != 0) { + sensor_msgs::msg::NavSatFix fix; + fix.header = gga_.header; + fix.latitude = gga_.lat; + fix.longitude = gga_.lon; + fix.altitude = gga_.alt + gga_.undulation; + pub2_->publish(fix); + } } -} +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_rtk_dead_reckoning"); - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - std::string subscribe_gga_topic_name = "gnss/gga"; - - 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; - - try - { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); - - rtk_dead_reckoning_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); - rtk_dead_reckoning_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); - rtk_dead_reckoning_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); - rtk_dead_reckoning_parameter.use_ecef_base_position = conf["/**"]["ros__parameters"]["ecef_base_pos"]["use_ecef_base_position"].as(); - rtk_dead_reckoning_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); - rtk_dead_reckoning_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); - rtk_dead_reckoning_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - rtk_dead_reckoning_parameter.rtk_fix_STD = conf["/**"]["ros__parameters"]["rtk_dead_reckoning"]["rtk_fix_STD"].as(); - rtk_dead_reckoning_parameter.proc_noise = conf["/**"]["ros__parameters"]["rtk_dead_reckoning"]["proc_noise"].as(); - - std::cout << "use_gnss_mode " << use_gnss_mode << std::endl; - - std::cout << "ecef_base_pos_x " << rtk_dead_reckoning_parameter.ecef_base_pos_x << std::endl; - std::cout << "ecef_base_pos_y " << rtk_dead_reckoning_parameter.ecef_base_pos_y << std::endl; - std::cout << "ecef_base_pos_z " << rtk_dead_reckoning_parameter.ecef_base_pos_z << std::endl; - std::cout << "use_ecef_base_position " << rtk_dead_reckoning_parameter.use_ecef_base_position << std::endl; - std::cout << "tf_gnss_frame/parent " << rtk_dead_reckoning_parameter.tf_gnss_parent_frame << std::endl; - std::cout << "tf_gnss_frame/child " << rtk_dead_reckoning_parameter.tf_gnss_child_frame << std::endl; - std::cout << "stop_judgment_threshold " << rtk_dead_reckoning_parameter.stop_judgment_threshold << std::endl; - std::cout << "rtk_fix_STD " << rtk_dead_reckoning_parameter.rtk_fix_STD << std::endl; - std::cout << "proc_noise " << rtk_dead_reckoning_parameter.proc_noise << std::endl; - } - catch (YAML::Exception& e) - { - std::cerr << "\033[1;31mrtk_dead_reckoning Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); - } - - auto sub1 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub2 = node->create_subscription("enu_vel", 1000, enu_vel_callback); - auto sub3 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - auto sub4 = node->create_subscription("heading_interpolate_3rd", 1000, heading_interpolate_3rd_callback); - - pub1 = node->create_publisher("enu_absolute_pos_interpolate", 1000); - pub2 = node->create_publisher("fix", 1000); - - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(0.5)); - auto timer_callback = std::bind(on_timer); - auto timer = std::make_shared>( - node->get_clock(), period_ns, std::move(timer_callback), - node->get_node_base_interface()->get_context()); - node->get_node_timers_interface()->add_timer(timer, nullptr); - - rclcpp::spin(node); - + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/rtk_heading_node.cpp b/eagleye_rt/src/rtk_heading_node.cpp index bb0abfdb..8215ba10 100644 --- a/eagleye_rt/src/rtk_heading_node.cpp +++ b/eagleye_rt/src/rtk_heading_node.cpp @@ -28,187 +28,214 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static nmea_msgs::msg::Gpgga gga; -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::Distance distance; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset; -static eagleye_msgs::msg::SlipAngle slip_angle; -static eagleye_msgs::msg::Heading heading_interpolate; - -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::Heading heading; - -struct RtkHeadingParameter heading_parameter; -struct RtkHeadingStatus heading_status; - -static bool use_can_less_mode; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset = *msg; -} - -void slip_angle_callback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) -{ - slip_angle = *msg; -} - -void heading_interpolate_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate = *msg; -} - -void distance_callback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) +class RtkHeadingNode : public rclcpp::Node { - distance = *msg; -} +public: + RtkHeadingNode(int argc, char** argv) : Node("eagleye_rtk_heading") + { + std::string subscribe_gga_topic_name = "gnss/gga"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + heading_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + heading_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + heading_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + heading_parameter_.slow_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["slow_judgment_threshold"].as(); + heading_parameter_.update_distance = + conf["/**"]["ros__parameters"]["rtk_heading"]["update_distance"].as(); + heading_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["rtk_heading"]["estimated_minimum_interval"].as(); + heading_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["rtk_heading"]["estimated_maximum_interval"].as(); + heading_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["rtk_heading"]["gnss_receiving_threshold"].as(); + heading_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["rtk_heading"]["outlier_threshold"].as(); + heading_parameter_.outlier_ratio_threshold = + conf["/**"]["ros__parameters"]["rtk_heading"]["outlier_ratio_threshold"].as(); + heading_parameter_.curve_judgment_threshold = + conf["/**"]["ros__parameters"]["rtk_heading"]["curve_judgment_threshold"].as(); + + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; + std::cout << "imu_rate " << heading_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << heading_parameter_.gnss_rate << std::endl; + std::cout << "stop_judgment_threshold " << heading_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "slow_judgment_threshold " << heading_parameter_.slow_judgment_threshold + << std::endl; + std::cout << "update_distance " << heading_parameter_.update_distance << std::endl; + std::cout << "estimated_minimum_interval " << heading_parameter_.estimated_minimum_interval + << std::endl; + std::cout << "estimated_maximum_interval " << heading_parameter_.estimated_maximum_interval + << std::endl; + std::cout << "gnss_receiving_threshold " << heading_parameter_.gnss_receiving_threshold + << std::endl; + std::cout << "outlier_threshold " << heading_parameter_.outlier_threshold << std::endl; + std::cout << "outlier_ratio_threshold " << heading_parameter_.outlier_ratio_threshold + << std::endl; + std::cout << "curve_judgment_threshold " << heading_parameter_.curve_judgment_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mrtk_heading Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if(use_can_less_mode && !velocity_status.status.enabled_status) return; + std::string publish_topic_name = "/publish_topic_name/invalid"; + std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; + std::string subscribe_topic_name2 = "/subscribe_topic_name2/invalid"; + + if (argc > 2) { + if (strcmp(argv[1], "1st") == 0) { + publish_topic_name = "heading_1st"; + subscribe_topic_name = "yaw_rate_offset_stop"; + subscribe_topic_name2 = "heading_interpolate_1st"; + } else if (strcmp(argv[1], "2nd") == 0) { + publish_topic_name = "heading_2nd"; + subscribe_topic_name = "yaw_rate_offset_1st"; + subscribe_topic_name2 = "heading_interpolate_2nd"; + } else if (strcmp(argv[1], "3rd") == 0) { + publish_topic_name = "heading_3rd"; + subscribe_topic_name = "yaw_rate_offset_2nd"; + subscribe_topic_name2 = "heading_interpolate_3rd"; + } else { + RCLCPP_ERROR(this->get_logger(), "Invalid argument"); + rclcpp::shutdown(); + } + } else { + RCLCPP_ERROR(this->get_logger(), "No arguments"); + rclcpp::shutdown(); + } - imu = *msg; - heading.header = msg->header; - heading.header.frame_id = "base_link"; - rtk_heading_estimate(gga,imu,velocity,distance,yaw_rate_offset_stop,yaw_rate_offset,slip_angle,heading_interpolate,heading_parameter,&heading_status,&heading); + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&RtkHeadingNode::imuCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + subscribe_gga_topic_name, 1000, + std::bind(&RtkHeadingNode::ggaCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&RtkHeadingNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&RtkHeadingNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", 1000, + std::bind(&RtkHeadingNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_ = this->create_subscription( + subscribe_topic_name, 1000, + std::bind(&RtkHeadingNode::yawRateOffsetCallback, this, std::placeholders::_1)); + sub_slip_angle_ = this->create_subscription( + "slip_angle", 1000, + std::bind(&RtkHeadingNode::slipAngleCallback, this, std::placeholders::_1)); + sub_heading_interpolate_ = this->create_subscription( + subscribe_topic_name2, 1000, + std::bind(&RtkHeadingNode::headingInterpolateCallback, this, std::placeholders::_1)); + sub_distance_ = this->create_subscription( + "distance", 1000, + std::bind(&RtkHeadingNode::distanceCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher(publish_topic_name, 1000); + } - if (heading.status.estimate_status == true) +private: + nmea_msgs::msg::Gpgga gga_; + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::Distance distance_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_; + eagleye_msgs::msg::SlipAngle slip_angle_; + eagleye_msgs::msg::Heading heading_interpolate_; + eagleye_msgs::msg::Heading heading_; + RtkHeadingParameter heading_parameter_; + RtkHeadingStatus heading_status_ = {}; + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_; + rclcpp::Subscription::SharedPtr sub_slip_angle_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_; + rclcpp::Subscription::SharedPtr sub_distance_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - pub->publish(heading); + velocity_ = *msg; } - heading.status.estimate_status = false; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_rtk_heading"); + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_ = *msg; } - std::string subscribe_gga_topic_name = "gnss/gga"; + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) + { + velocity_status_ = *msg; + } - 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; - try + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - - heading_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - heading_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - heading_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - heading_parameter.slow_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["slow_judgment_threshold"].as(); - - heading_parameter.update_distance = conf["/**"]["ros__parameters"]["rtk_heading"]["update_distance"].as(); - heading_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["rtk_heading"]["estimated_minimum_interval"].as(); - heading_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["rtk_heading"]["estimated_maximum_interval"].as(); - heading_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["rtk_heading"]["gnss_receiving_threshold"].as(); - heading_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["rtk_heading"]["outlier_threshold"].as(); - heading_parameter.outlier_ratio_threshold = conf["/**"]["ros__parameters"]["rtk_heading"]["outlier_ratio_threshold"].as(); - heading_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["rtk_heading"]["curve_judgment_threshold"].as(); - - std::cout<< "use_can_less_mode " << use_can_less_mode << std::endl; - - std::cout<< "subscribe_gga_topic_name " << subscribe_gga_topic_name << std::endl; - - std::cout << "imu_rate " << heading_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << heading_parameter.gnss_rate << std::endl; - std::cout << "stop_judgment_threshold " << heading_parameter.stop_judgment_threshold << std::endl; - std::cout << "slow_judgment_threshold " << heading_parameter.slow_judgment_threshold << std::endl; - - std::cout << "update_distance " << heading_parameter.update_distance << std::endl; - std::cout << "estimated_minimum_interval " << heading_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << heading_parameter.estimated_maximum_interval << std::endl; - std::cout << "gnss_receiving_threshold " << heading_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_threshold " << heading_parameter.outlier_threshold << std::endl; - std::cout << "outlier_ratio_threshold " << heading_parameter.outlier_ratio_threshold << std::endl; - std::cout << "curve_judgment_threshold " << heading_parameter.curve_judgment_threshold << std::endl; + yaw_rate_offset_stop_ = *msg; } - catch (YAML::Exception& e) + + void yawRateOffsetCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - std::cerr << "\033[1;31mrtk_heading Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + yaw_rate_offset_ = *msg; } - std::string publish_topic_name = "/publish_topic_name/invalid"; - std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; - std::string subscribe_topic_name2 = "/subscribe_topic_name2/invalid"; + void slipAngleCallback(const eagleye_msgs::msg::SlipAngle::ConstSharedPtr msg) + { + slip_angle_ = *msg; + } - if (argc > 2) + void headingInterpolateCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - if (strcmp(argv[1], "1st") == 0) - { - publish_topic_name = "heading_1st"; - subscribe_topic_name = "yaw_rate_offset_stop"; - subscribe_topic_name2 = "heading_interpolate_1st"; - } - else if (strcmp(argv[1], "2nd") == 0) - { - publish_topic_name = "heading_2nd"; - subscribe_topic_name = "yaw_rate_offset_1st"; - subscribe_topic_name2 = "heading_interpolate_2nd"; - } - else if (strcmp(argv[1], "3rd") == 0) - { - publish_topic_name = "heading_3rd"; - subscribe_topic_name = "yaw_rate_offset_2nd"; - subscribe_topic_name2 = "heading_interpolate_3rd"; - } - else - { - RCLCPP_ERROR(node->get_logger(),"Invalid argument"); - rclcpp::shutdown(); - } + heading_interpolate_ = *msg; } - else + + void distanceCallback(const eagleye_msgs::msg::Distance::ConstSharedPtr msg) { - RCLCPP_ERROR(node->get_logger(),"No arguments"); - rclcpp::shutdown(); + distance_ = *msg; } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub5 = node->create_subscription("yaw_rate_offset_stop", 1000, yaw_rate_offset_stop_callback); - auto sub6 = node->create_subscription(subscribe_topic_name, 1000, yaw_rate_offset_callback); - auto sub7 = node->create_subscription("slip_angle", 1000, slip_angle_callback); - auto sub8 = node->create_subscription(subscribe_topic_name2, 1000, heading_interpolate_callback); - auto sub9 = node->create_subscription("distance", 1000, distance_callback); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; - pub = node->create_publisher(publish_topic_name, 1000); + imu_ = *msg; + heading_.header = msg->header; + heading_.header.frame_id = "base_link"; + rtk_heading_estimate( + gga_, imu_, velocity_, distance_, yaw_rate_offset_stop_, yaw_rate_offset_, slip_angle_, + heading_interpolate_, heading_parameter_, &heading_status_, &heading_); - rclcpp::spin(node); + if (heading_.status.estimate_status == true) { + pub_->publish(heading_); + } + heading_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(argc, argv)); return 0; } diff --git a/eagleye_rt/src/slip_angle_node.cpp b/eagleye_rt/src/slip_angle_node.cpp index ed4374f6..fcc85b5e 100644 --- a/eagleye_rt/src/slip_angle_node.cpp +++ b/eagleye_rt/src/slip_angle_node.cpp @@ -28,107 +28,127 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd; - -rclcpp::Publisher::SharedPtr pub; -static eagleye_msgs::msg::SlipAngle slip_angle; - -struct SlipangleParameter slip_angle_parameter; - -static bool use_can_less_mode; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) -{ - velocity_scale_factor = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_2nd = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class SlipAngleNode : public rclcpp::Node { - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - - eagleye_msgs::msg::StatusStamped velocity_enable_status; - if(use_can_less_mode) +public: + SlipAngleNode() : Node("eagleye_slip_angle") { - velocity_enable_status = velocity_status; + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + slip_angle_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + slip_angle_parameter_.manual_coefficient = + conf["/**"]["ros__parameters"]["slip_angle"]["manual_coefficient"].as(); + + std::cout << "stop_judgment_threshold " << slip_angle_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "manual_coefficient " << slip_angle_parameter_.manual_coefficient << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mslip_angle Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", rclcpp::QoS(10), + std::bind(&SlipAngleNode::imuCallback, this, std::placeholders::_1)); + sub_velocity_scale_factor_ = this->create_subscription( + "velocity_scale_factor", rclcpp::QoS(10), + std::bind(&SlipAngleNode::velocityScaleFactorCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&SlipAngleNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", rclcpp::QoS(10), + std::bind(&SlipAngleNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&SlipAngleNode::velocityCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher("slip_angle", rclcpp::QoS(10)); } - else + +private: + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::SlipAngle slip_angle_; + SlipangleParameter slip_angle_parameter_; + bool use_can_less_mode_ = false; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr + sub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - velocity_enable_status.header = velocity_scale_factor.header; - velocity_enable_status.status = velocity_scale_factor.status; + velocity_ = *msg; } - imu = *msg; - slip_angle.header = msg->header; - slip_angle.header.frame_id = "base_link"; - slip_angle_estimate(imu,velocity,velocity_enable_status,yaw_rate_offset_stop,yaw_rate_offset_2nd,slip_angle_parameter,&slip_angle); - pub->publish(slip_angle); - slip_angle.status.estimate_status = false; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_slip_angle"); - - 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; - - try + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - slip_angle_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - slip_angle_parameter.manual_coefficient = conf["/**"]["ros__parameters"]["slip_angle"]["manual_coefficient"].as(); - - std::cout << "stop_judgment_threshold " << slip_angle_parameter.stop_judgment_threshold << std::endl; - std::cout << "manual_coefficient " << slip_angle_parameter.manual_coefficient << std::endl; + velocity_status_ = *msg; } - catch (YAML::Exception& e) + + void velocityScaleFactorCallback( + const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { - std::cerr << "\033[1;31mslip_angle Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + velocity_scale_factor_ = *msg; } - auto sub1 = node->create_subscription("imu/data_tf_converted", rclcpp::QoS(10), imu_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback); //ros::TransportHints().tcpNoDelay() - auto sub5 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); //ros::TransportHints().tcpNoDelay() - pub = node->create_publisher("slip_angle", rclcpp::QoS(10)); + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; + } - rclcpp::spin(node); + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_2nd_ = *msg; + } + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if (use_can_less_mode_) { + velocity_enable_status = velocity_status_; + } else { + velocity_enable_status.header = velocity_scale_factor_.header; + velocity_enable_status.status = velocity_scale_factor_.status; + } + + imu_ = *msg; + slip_angle_.header = msg->header; + slip_angle_.header.frame_id = "base_link"; + slip_angle_estimate( + imu_, velocity_, velocity_enable_status, yaw_rate_offset_stop_, yaw_rate_offset_2nd_, + slip_angle_parameter_, &slip_angle_); + pub_->publish(slip_angle_); + slip_angle_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/slip_coefficient_node.cpp b/eagleye_rt/src/slip_coefficient_node.cpp index fb8b9612..a25ca4d9 100644 --- a/eagleye_rt/src/slip_coefficient_node.cpp +++ b/eagleye_rt/src/slip_coefficient_node.cpp @@ -28,135 +28,178 @@ * Author MapIV Takanose */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -#include #include #include +#include -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; - -struct SlipCoefficientParameter slip_coefficient_parameter; -struct SlipCoefficientStatus slip_coefficient_status; - -static double estimate_coefficient; - -bool is_first_correction_velocity = false; -static bool use_can_less_mode; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - rtklib_nav = *msg; -} - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +class SlipCoefficientNode : public rclcpp::Node { - velocity = *msg; - if (is_first_correction_velocity == false && msg->twist.linear.x > slip_coefficient_parameter.moving_judgment_threshold) +public: + SlipCoefficientNode() : Node("eagleye_slip_coefficient") { - is_first_correction_velocity = true; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + slip_coefficient_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + slip_coefficient_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + slip_coefficient_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + slip_coefficient_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["slip_coefficient"]["estimated_minimum_interval"] + .as(); + slip_coefficient_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["slip_coefficient"]["estimated_maximum_interval"] + .as(); + slip_coefficient_parameter_.curve_judgment_threshold = + conf["/**"]["ros__parameters"]["slip_coefficient"]["curve_judgment_threshold"].as(); + slip_coefficient_parameter_.lever_arm = + conf["/**"]["ros__parameters"]["slip_coefficient"]["lever_arm"].as(); + + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "imu_rate " << slip_coefficient_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " + << slip_coefficient_parameter_.stop_judgment_threshold << std::endl; + std::cout << "moving_judgment_threshold " + << slip_coefficient_parameter_.moving_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " + << slip_coefficient_parameter_.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " + << slip_coefficient_parameter_.estimated_maximum_interval << std::endl; + std::cout << "curve_judgment_threshold " + << slip_coefficient_parameter_.curve_judgment_threshold << std::endl; + std::cout << "lever_arm " << slip_coefficient_parameter_.lever_arm << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mslip_coefficient Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&SlipCoefficientNode::imuCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&SlipCoefficientNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&SlipCoefficientNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&SlipCoefficientNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&SlipCoefficientNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", rclcpp::QoS(10), + std::bind(&SlipCoefficientNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", rclcpp::QoS(10), + std::bind( + &SlipCoefficientNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); } -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_2nd = *msg; -} - -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate_3rd = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if (is_first_correction_velocity == false) return; - if (use_can_less_mode && !velocity_status.status.enabled_status) return; - imu = *msg; - slip_coefficient_estimate(imu,rtklib_nav,velocity,yaw_rate_offset_stop,yaw_rate_offset_2nd,heading_interpolate_3rd,slip_coefficient_parameter,&slip_coefficient_status,&estimate_coefficient); - - std::cout << "--- \033[1;34m slip_coefficient \033[m ------------------------------"<< std::endl; - std::cout<<"\033[1m estimate_coefficient \033[m "<declare_parameter("yaml_file",yaml_file); - node->get_parameter("yaml_file",yaml_file); - std::cout << "yaml_file: " << yaml_file << std::endl; - - try + double getEstimateCoefficient() const { return estimate_coefficient_; } + +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + SlipCoefficientParameter slip_coefficient_parameter_; + SlipCoefficientStatus slip_coefficient_status_ = {}; + double estimate_coefficient_ = 0.0; + bool is_first_correction_velocity_ = false; + bool use_can_less_mode_ = false; + + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - - slip_coefficient_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - slip_coefficient_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - slip_coefficient_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + rtklib_nav_ = *msg; + } - slip_coefficient_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["slip_coefficient"]["estimated_minimum_interval"].as(); - slip_coefficient_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["slip_coefficient"]["estimated_maximum_interval"].as(); - slip_coefficient_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["slip_coefficient"]["curve_judgment_threshold"].as(); - slip_coefficient_parameter.lever_arm = conf["/**"]["ros__parameters"]["slip_coefficient"]["lever_arm"].as(); + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + velocity_ = *msg; + if ( + is_first_correction_velocity_ == false && + msg->twist.linear.x > slip_coefficient_parameter_.moving_judgment_threshold) { + is_first_correction_velocity_ = true; + } + } - std::cout<< "use_can_less_mode " << use_can_less_mode << std::endl; + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) + { + velocity_status_ = *msg; + } - std::cout << "imu_rate " << slip_coefficient_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << slip_coefficient_parameter.stop_judgment_threshold << std::endl; - std::cout << "moving_judgment_threshold " << slip_coefficient_parameter.moving_judgment_threshold << std::endl; + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_stop_ = *msg; + } - std::cout << "estimated_minimum_interval " << slip_coefficient_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << slip_coefficient_parameter.estimated_maximum_interval << std::endl; - std::cout << "curve_judgment_threshold " << slip_coefficient_parameter.curve_judgment_threshold << std::endl; - std::cout << "lever_arm " << slip_coefficient_parameter.lever_arm << std::endl; + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) + { + yaw_rate_offset_2nd_ = *msg; } - catch (YAML::Exception& e) + + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - std::cerr << "\033[1;31mslip_coefficient Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + heading_interpolate_3rd_ = *msg; } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub5 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); - auto sub6 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback); - auto sub7 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (is_first_correction_velocity_ == false) return; + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + imu_ = *msg; + slip_coefficient_estimate( + imu_, rtklib_nav_, velocity_, yaw_rate_offset_stop_, yaw_rate_offset_2nd_, + heading_interpolate_3rd_, slip_coefficient_parameter_, &slip_coefficient_status_, + &estimate_coefficient_); + + std::cout << "--- \033[1;34m slip_coefficient \033[m ------------------------------" + << std::endl; + std::cout << "\033[1m estimate_coefficient \033[m " << estimate_coefficient_ << std::endl; + std::cout << std::endl; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); rclcpp::spin(node); std::string str; node->declare_parameter(str, "output_dir"); std::ofstream ofs(str, std::ios_base::trunc | std::ios_base::out); - ofs << "slip_coefficient" << " : " << estimate_coefficient << std::endl; + ofs << "slip_coefficient" + << " : " << node->getEstimateCoefficient() << std::endl; ofs.close(); return 0; diff --git a/eagleye_rt/src/smoothing_node.cpp b/eagleye_rt/src/smoothing_node.cpp index 3e5767e5..4bb322c6 100644 --- a/eagleye_rt/src/smoothing_node.cpp +++ b/eagleye_rt/src/smoothing_node.cpp @@ -28,137 +28,147 @@ * Author MapIV Takanose */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static rtklib_msgs::msg::RtklibNav rtklib_nav; -static eagleye_msgs::msg::Position enu_absolute_pos,gnss_smooth_pos_enu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -rclcpp::Publisher::SharedPtr pub; - -struct PositionParameter position_parameter; -struct SmoothingParameter smoothing_parameter; -struct SmoothingStatus smoothing_status; - -static bool use_can_less_mode; - -rclcpp::Clock clock_(RCL_ROS_TIME); -tf2_ros::Buffer tfBuffer_(std::make_shared(clock_)); - -const std::string node_name = "eagleye_smoothing"; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - - rtklib_nav = *msg; - gnss_smooth_pos_enu.header = msg->header; - gnss_smooth_pos_enu.header.frame_id = "base_link"; - smoothing_estimate(rtklib_nav,velocity,smoothing_parameter,&smoothing_status,&gnss_smooth_pos_enu); - gnss_smooth_pos_enu.enu_pos.z -= position_parameter.tf_gnss_translation_z; - pub->publish(gnss_smooth_pos_enu); -} - -void on_timer() +class SmoothingNode : public rclcpp::Node { - geometry_msgs::msg::TransformStamped transformStamped; - try +public: + SmoothingNode() + : Node("eagleye_smoothing"), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) { - transformStamped = tfBuffer_.lookupTransform(position_parameter.tf_gnss_parent_frame, position_parameter.tf_gnss_child_frame, tf2::TimePointZero); - - position_parameter.tf_gnss_translation_x = transformStamped.transform.translation.x; - position_parameter.tf_gnss_translation_y = transformStamped.transform.translation.y; - position_parameter.tf_gnss_translation_z = transformStamped.transform.translation.z; - position_parameter.tf_gnss_rotation_x = transformStamped.transform.rotation.x; - position_parameter.tf_gnss_rotation_y = transformStamped.transform.rotation.y; - position_parameter.tf_gnss_rotation_z = transformStamped.transform.rotation.z; - position_parameter.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + position_parameter_.tf_gnss_parent_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); + position_parameter_.tf_gnss_child_frame = + conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); + smoothing_parameter_.ecef_base_pos_x = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); + smoothing_parameter_.ecef_base_pos_y = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); + smoothing_parameter_.ecef_base_pos_z = + conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); + smoothing_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + smoothing_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + smoothing_parameter_.moving_average_time = + conf["/**"]["ros__parameters"]["smoothing"]["moving_average_time"].as(); + smoothing_parameter_.moving_ratio_threshold = + conf["/**"]["ros__parameters"]["smoothing"]["moving_ratio_threshold"].as(); + + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name + << std::endl; + std::cout << "ecef_base_pos_x " << smoothing_parameter_.ecef_base_pos_x << std::endl; + std::cout << "ecef_base_pos_y " << smoothing_parameter_.ecef_base_pos_y << std::endl; + std::cout << "ecef_base_pos_z " << smoothing_parameter_.ecef_base_pos_z << std::endl; + std::cout << "gnss_rate " << smoothing_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " << smoothing_parameter_.moving_judgment_threshold + << std::endl; + std::cout << "moving_average_time " << smoothing_parameter_.moving_average_time << std::endl; + std::cout << "moving_ratio_threshold " << smoothing_parameter_.moving_ratio_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31msmoothing Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } + + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&SmoothingNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&SmoothingNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&SmoothingNode::rtklibNavCallback, this, std::placeholders::_1)); + pub_ = + this->create_publisher("gnss_smooth_pos_enu", rclcpp::QoS(10)); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), std::bind(&SmoothingNode::onTimer, this)); } - catch (tf2::TransformException& ex) + +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + eagleye_msgs::msg::Position gnss_smooth_pos_enu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + PositionParameter position_parameter_; + SmoothingParameter smoothing_parameter_; + SmoothingStatus smoothing_status_ = {}; + bool use_can_less_mode_ = false; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - RCLCPP_WARN(rclcpp::get_logger(node_name), "%s", ex.what()); - return; + velocity_ = *msg; } -} - - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(node_name); - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - 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; - - try + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - - position_parameter.tf_gnss_parent_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["parent"].as(); - position_parameter.tf_gnss_child_frame = conf["/**"]["ros__parameters"]["tf_gnss_frame"]["child"].as(); - - smoothing_parameter.ecef_base_pos_x = conf["/**"]["ros__parameters"]["ecef_base_pos"]["x"].as(); - smoothing_parameter.ecef_base_pos_y = conf["/**"]["ros__parameters"]["ecef_base_pos"]["y"].as(); - smoothing_parameter.ecef_base_pos_z = conf["/**"]["ros__parameters"]["ecef_base_pos"]["z"].as(); - - smoothing_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - smoothing_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - smoothing_parameter.moving_average_time = conf["/**"]["ros__parameters"]["smoothing"]["moving_average_time"].as(); - smoothing_parameter.moving_ratio_threshold = conf["/**"]["ros__parameters"]["smoothing"]["moving_ratio_threshold"].as(); - - std::cout<< "use_can_less_mode " << use_can_less_mode << std::endl; - - std::cout<< "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; - - std::cout<< "ecef_base_pos_x " << smoothing_parameter.ecef_base_pos_x << std::endl; - std::cout<< "ecef_base_pos_y " << smoothing_parameter.ecef_base_pos_y << std::endl; - std::cout<< "ecef_base_pos_z " << smoothing_parameter.ecef_base_pos_z << std::endl; - - std::cout << "gnss_rate " << smoothing_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << smoothing_parameter.moving_judgment_threshold << std::endl; - std::cout << "moving_average_time " << smoothing_parameter.moving_average_time << std::endl; - std::cout << "moving_ratio_threshold " << smoothing_parameter.moving_ratio_threshold << std::endl; + velocity_status_ = *msg; } - catch (YAML::Exception& e) + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - std::cerr << "\033[1;31msmoothing Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + rtklib_nav_ = *msg; + gnss_smooth_pos_enu_.header = msg->header; + gnss_smooth_pos_enu_.header.frame_id = "base_link"; + smoothing_estimate( + rtklib_nav_, velocity_, smoothing_parameter_, &smoothing_status_, &gnss_smooth_pos_enu_); + gnss_smooth_pos_enu_.enu_pos.z -= position_parameter_.tf_gnss_translation_z; + pub_->publish(gnss_smooth_pos_enu_); } - auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); - auto sub2 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub3 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - pub = node->create_publisher("gnss_smooth_pos_enu", rclcpp::QoS(10)); - - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(0.5)); - auto timer_callback = std::bind(on_timer); - auto timer = std::make_shared>( - node->get_clock(), period_ns, std::move(timer_callback), - node->get_node_base_interface()->get_context()); - node->get_node_timers_interface()->add_timer(timer, nullptr); - - tf2_ros::TransformListener listener(tfBuffer_); - - rclcpp::spin(node); + void onTimer() + { + geometry_msgs::msg::TransformStamped transformStamped; + try { + transformStamped = tf_buffer_.lookupTransform( + position_parameter_.tf_gnss_parent_frame, position_parameter_.tf_gnss_child_frame, + tf2::TimePointZero); + + position_parameter_.tf_gnss_translation_x = transformStamped.transform.translation.x; + position_parameter_.tf_gnss_translation_y = transformStamped.transform.translation.y; + position_parameter_.tf_gnss_translation_z = transformStamped.transform.translation.z; + position_parameter_.tf_gnss_rotation_x = transformStamped.transform.rotation.x; + position_parameter_.tf_gnss_rotation_y = transformStamped.transform.rotation.y; + position_parameter_.tf_gnss_rotation_z = transformStamped.transform.rotation.z; + position_parameter_.tf_gnss_rotation_w = transformStamped.transform.rotation.w; + } catch (tf2::TransformException& ex) { + RCLCPP_WARN(this->get_logger(), "%s", ex.what()); + return; + } + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/trajectory_node.cpp b/eagleye_rt/src/trajectory_node.cpp index 966835a4..0d54b927 100644 --- a/eagleye_rt/src/trajectory_node.cpp +++ b/eagleye_rt/src/trajectory_node.cpp @@ -28,215 +28,254 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static sensor_msgs::msg::Imu imu; -static geometry_msgs::msg::TwistStamped velocity; -static eagleye_msgs::msg::StatusStamped velocity_status; -static geometry_msgs::msg::TwistStamped correction_velocity; -static eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor; -static eagleye_msgs::msg::Heading heading_interpolate_3rd; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop; -static eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd; -static eagleye_msgs::msg::Pitching pitching; - -static geometry_msgs::msg::Vector3Stamped enu_vel; -static eagleye_msgs::msg::Position enu_relative_pos; -static geometry_msgs::msg::TwistStamped eagleye_twist; -static geometry_msgs::msg::TwistWithCovarianceStamped eagleye_twist_with_covariance; -rclcpp::Publisher::SharedPtr pub1; -rclcpp::Publisher::SharedPtr pub2; -rclcpp::Publisher::SharedPtr pub3; -rclcpp::Publisher::SharedPtr pub4; - -struct TrajectoryParameter trajectory_parameter; -struct TrajectoryStatus trajectory_status; - -static double timer_update_rate = 10; -static double th_deadlock_time = 1; - -static double imu_time_last,velocity_time_last; -static bool input_status; - -static bool use_can_less_mode; - -static std::string node_name = "eagleye_trajectory"; - -void correction_velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - correction_velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) -{ - velocity_status = *msg; -} - -void velocity_scale_factor_callback(const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) -{ - velocity_scale_factor = *msg; -} - -void heading_interpolate_3rd_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - heading_interpolate_3rd = *msg; -} - -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_stop = *msg; -} - -void yaw_rate_offset_2nd_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - yaw_rate_offset_2nd = *msg; -} - -void pitching_callback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) +class TrajectoryNode : public rclcpp::Node { - pitching = *msg; -} +public: + TrajectoryNode() : Node("eagleye_trajectory") + { + std::string subscribe_twist_topic_name = "vehicle/twist"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + double timer_update_rate = 10; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_can_less_mode_ = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); + trajectory_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + trajectory_parameter_.curve_judgment_threshold = + conf["/**"]["ros__parameters"]["trajectory"]["curve_judgment_threshold"].as(); + trajectory_parameter_.sensor_noise_velocity = + conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_velocity"].as(); + trajectory_parameter_.sensor_scale_noise_velocity = + conf["/**"]["ros__parameters"]["trajectory"]["sensor_scale_noise_velocity"] + .as(); + trajectory_parameter_.sensor_noise_yaw_rate = + conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_yaw_rate"].as(); + trajectory_parameter_.sensor_bias_noise_yaw_rate = + conf["/**"]["ros__parameters"]["trajectory"]["sensor_bias_noise_yaw_rate"].as(); + timer_update_rate = + conf["/**"]["ros__parameters"]["trajectory"]["timer_update_rate"].as(); + + std::cout << "use_can_less_mode " << use_can_less_mode_ << std::endl; + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "stop_judgment_threshold " << trajectory_parameter_.stop_judgment_threshold + << std::endl; + std::cout << "curve_judgment_threshold " << trajectory_parameter_.curve_judgment_threshold + << std::endl; + std::cout << "sensor_noise_velocity " << trajectory_parameter_.sensor_noise_velocity + << std::endl; + std::cout << "sensor_scale_noise_velocity " + << trajectory_parameter_.sensor_scale_noise_velocity << std::endl; + std::cout << "sensor_noise_yaw_rate " << trajectory_parameter_.sensor_noise_yaw_rate + << std::endl; + std::cout << "sensor_bias_noise_yaw_rate " + << trajectory_parameter_.sensor_bias_noise_yaw_rate << std::endl; + std::cout << "timer_update_rate " << timer_update_rate << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mtrajectory Node YAML Error: " << e.msg << "\033[0m" << std::endl; + exit(3); + } -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - velocity = *msg; -} + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&TrajectoryNode::imuCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + subscribe_twist_topic_name, rclcpp::QoS(10), + std::bind(&TrajectoryNode::velocityCallback, this, std::placeholders::_1)); + sub_correction_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&TrajectoryNode::correctionVelocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&TrajectoryNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_velocity_scale_factor_ = + this->create_subscription( + "velocity_scale_factor", rclcpp::QoS(10), + std::bind( + &TrajectoryNode::velocityScaleFactorCallback, this, std::placeholders::_1)); + sub_heading_interpolate_3rd_ = this->create_subscription( + "heading_interpolate_3rd", rclcpp::QoS(10), + std::bind( + &TrajectoryNode::headingInterpolate3rdCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&TrajectoryNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_2nd_ = this->create_subscription( + "yaw_rate_offset_2nd", rclcpp::QoS(10), + std::bind(&TrajectoryNode::yawRateOffset2ndCallback, this, std::placeholders::_1)); + sub_pitching_ = this->create_subscription( + "pitching", rclcpp::QoS(10), + std::bind(&TrajectoryNode::pitchingCallback, this, std::placeholders::_1)); + + pub_enu_vel_ = + this->create_publisher("enu_vel", 1000); + pub_enu_relative_pos_ = + this->create_publisher("enu_relative_pos", 1000); + pub_twist_ = this->create_publisher("twist", 1000); + pub_twist_with_covariance_ = + this->create_publisher( + "twist_with_covariance", 1000); + + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(1.0 / timer_update_rate)); + timer_ = this->create_wall_timer( + period_ns, std::bind(&TrajectoryNode::onTimer, this)); + } -void on_timer() -{ - rclcpp::Time imu_clock(imu.header.stamp); - double imu_time = imu_clock.seconds(); - rclcpp::Time velocity_clock(velocity.header.stamp); - double velocity_time = velocity_clock.seconds(); - if (std::abs(imu_time - imu_time_last) < th_deadlock_time && - std::abs(velocity_time - velocity_time_last) < th_deadlock_time && - std::abs(velocity_time - imu_time) < th_deadlock_time) +private: + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + geometry_msgs::msg::TwistStamped correction_velocity_; + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + eagleye_msgs::msg::Heading heading_interpolate_3rd_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_2nd_; + eagleye_msgs::msg::Pitching pitching_; + + geometry_msgs::msg::Vector3Stamped enu_vel_; + eagleye_msgs::msg::Position enu_relative_pos_; + geometry_msgs::msg::TwistStamped eagleye_twist_; + geometry_msgs::msg::TwistWithCovarianceStamped eagleye_twist_with_covariance_; + + TrajectoryParameter trajectory_parameter_; + TrajectoryStatus trajectory_status_ = {}; + + double th_deadlock_time_ = 1; + double imu_time_last_ = 0; + double velocity_time_last_ = 0; + bool input_status_ = false; + bool use_can_less_mode_ = false; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_enu_vel_; + rclcpp::Publisher::SharedPtr pub_enu_relative_pos_; + rclcpp::Publisher::SharedPtr pub_twist_; + rclcpp::Publisher::SharedPtr + pub_twist_with_covariance_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_correction_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr + sub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_3rd_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_2nd_; + rclcpp::Subscription::SharedPtr sub_pitching_; + + void correctionVelocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - input_status = true; + correction_velocity_ = *msg; } - else + + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - input_status = false; - RCLCPP_WARN(rclcpp::get_logger(node_name), "Twist is missing the required input topics."); + velocity_status_ = *msg; } - if (imu_time != imu_time_last) imu_time_last = imu_time; - if (velocity_time != velocity_time_last) velocity_time_last = velocity_time; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if(use_can_less_mode && !velocity_status.status.enabled_status) return; - - eagleye_msgs::msg::StatusStamped velocity_enable_status; - if(use_can_less_mode) + void velocityScaleFactorCallback( + const eagleye_msgs::msg::VelocityScaleFactor::ConstSharedPtr msg) { - velocity_enable_status = velocity_status; + velocity_scale_factor_ = *msg; } - else + + void headingInterpolate3rdCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - velocity_enable_status.header = velocity_scale_factor.header; - velocity_enable_status.status = velocity_scale_factor.status; + heading_interpolate_3rd_ = *msg; } - imu = *msg; - if(input_status) + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - enu_vel.header = msg->header; - enu_vel.header.frame_id = "gnss"; - enu_relative_pos.header = msg->header; - enu_relative_pos.header.frame_id = "base_link"; - eagleye_twist.header = msg->header; - eagleye_twist.header.frame_id = "base_link"; - eagleye_twist_with_covariance.header = msg->header; - eagleye_twist_with_covariance.header.frame_id = "base_link"; - trajectory3d_estimate(imu,correction_velocity,velocity_enable_status,heading_interpolate_3rd,yaw_rate_offset_stop,yaw_rate_offset_2nd,pitching, - trajectory_parameter,&trajectory_status,&enu_vel,&enu_relative_pos,&eagleye_twist, &eagleye_twist_with_covariance); - - if (heading_interpolate_3rd.status.enabled_status) - { - pub1->publish(enu_vel); - pub2->publish(enu_relative_pos); - } - pub3->publish(eagleye_twist); - pub4->publish(eagleye_twist_with_covariance); + yaw_rate_offset_stop_ = *msg; } -} -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared(node_name); - - std::string subscribe_twist_topic_name = "vehicle/twist"; - - 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; - - try + void yawRateOffset2ndCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - use_can_less_mode = conf["/**"]["ros__parameters"]["use_can_less_mode"].as(); - trajectory_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - trajectory_parameter.curve_judgment_threshold = conf["/**"]["ros__parameters"]["trajectory"]["curve_judgment_threshold"].as(); - trajectory_parameter.sensor_noise_velocity = conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_velocity"].as(); - trajectory_parameter.sensor_scale_noise_velocity = conf["/**"]["ros__parameters"]["trajectory"]["sensor_scale_noise_velocity"].as(); - trajectory_parameter.sensor_noise_yaw_rate = conf["/**"]["ros__parameters"]["trajectory"]["sensor_noise_yaw_rate"].as(); - trajectory_parameter.sensor_bias_noise_yaw_rate = conf["/**"]["ros__parameters"]["trajectory"]["sensor_bias_noise_yaw_rate"].as(); - timer_update_rate = conf["/**"]["ros__parameters"]["trajectory"]["timer_update_rate"].as(); - // deadlock_threshold = conf["/**"]["ros__parameters"]["trajectory"]["deadlock_threshold"].as(); - - std::cout<< "use_can_less_mode " << use_can_less_mode << std::endl; - - std::cout<< "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + yaw_rate_offset_2nd_ = *msg; + } - std::cout << "stop_judgment_threshold " << trajectory_parameter.stop_judgment_threshold << std::endl; + void pitchingCallback(const eagleye_msgs::msg::Pitching::ConstSharedPtr msg) + { + pitching_ = *msg; + } - std::cout << "curve_judgment_threshold " << trajectory_parameter.curve_judgment_threshold << std::endl; + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) + { + velocity_ = *msg; + } - std::cout << "sensor_noise_velocity " << trajectory_parameter.sensor_noise_velocity << std::endl; - std::cout << "sensor_scale_noise_velocity " << trajectory_parameter.sensor_scale_noise_velocity << std::endl; - std::cout << "sensor_noise_yaw_rate " << trajectory_parameter.sensor_noise_yaw_rate << std::endl; - std::cout << "sensor_bias_noise_yaw_rate " << trajectory_parameter.sensor_bias_noise_yaw_rate << std::endl; + void onTimer() + { + rclcpp::Time imu_clock(imu_.header.stamp); + double imu_time = imu_clock.seconds(); + rclcpp::Time velocity_clock(velocity_.header.stamp); + double velocity_time = velocity_clock.seconds(); + if ( + std::abs(imu_time - imu_time_last_) < th_deadlock_time_ && + std::abs(velocity_time - velocity_time_last_) < th_deadlock_time_ && + std::abs(velocity_time - imu_time) < th_deadlock_time_) { + input_status_ = true; + } else { + input_status_ = false; + RCLCPP_WARN(this->get_logger(), "Twist is missing the required input topics."); + } - std::cout << "timer_update_rate " << timer_update_rate << std::endl; - // std::cout << "deadlock_threshold " << deadlock_threshold << std::endl; + if (imu_time != imu_time_last_) imu_time_last_ = imu_time; + if (velocity_time != velocity_time_last_) velocity_time_last_ = velocity_time; } - catch (YAML::Exception& e) + + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - std::cerr << "\033[1;31mtrajectory Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); - } + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + eagleye_msgs::msg::StatusStamped velocity_enable_status; + if (use_can_less_mode_) { + velocity_enable_status = velocity_status_; + } else { + velocity_enable_status.header = velocity_scale_factor_.header; + velocity_enable_status.status = velocity_scale_factor_.status; + } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_twist_topic_name, rclcpp::QoS(10), velocity_callback); - auto sub3 = node->create_subscription("velocity", rclcpp::QoS(10), correction_velocity_callback); - auto sub4 = node->create_subscription("velocity_status", rclcpp::QoS(10), velocity_status_callback); - auto sub5 = node->create_subscription("velocity_scale_factor", rclcpp::QoS(10), velocity_scale_factor_callback); - auto sub6 = node->create_subscription("heading_interpolate_3rd", rclcpp::QoS(10), heading_interpolate_3rd_callback); - auto sub7 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); - auto sub8 = node->create_subscription("yaw_rate_offset_2nd", rclcpp::QoS(10), yaw_rate_offset_2nd_callback); - auto sub9 = node->create_subscription("pitching", rclcpp::QoS(10), pitching_callback); - pub1 = node->create_publisher("enu_vel", 1000); - pub2 = node->create_publisher("enu_relative_pos", 1000); - pub3 = node->create_publisher("twist", 1000); - pub4 = node->create_publisher("twist_with_covariance", 1000); - - double delta_time = 1.0 / static_cast(timer_update_rate); - auto timer_callback = std::bind(on_timer); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(delta_time)); - auto timer = std::make_shared>( - node->get_clock(), period_ns, std::move(timer_callback), - node->get_node_base_interface()->get_context()); - node->get_node_timers_interface()->add_timer(timer, nullptr); - - rclcpp::spin(node); + imu_ = *msg; + if (input_status_) { + enu_vel_.header = msg->header; + enu_vel_.header.frame_id = "gnss"; + enu_relative_pos_.header = msg->header; + enu_relative_pos_.header.frame_id = "base_link"; + eagleye_twist_.header = msg->header; + eagleye_twist_.header.frame_id = "base_link"; + eagleye_twist_with_covariance_.header = msg->header; + eagleye_twist_with_covariance_.header.frame_id = "base_link"; + trajectory3d_estimate( + imu_, correction_velocity_, velocity_enable_status, heading_interpolate_3rd_, + yaw_rate_offset_stop_, yaw_rate_offset_2nd_, pitching_, trajectory_parameter_, + &trajectory_status_, &enu_vel_, &enu_relative_pos_, &eagleye_twist_, + &eagleye_twist_with_covariance_); + + if (heading_interpolate_3rd_.status.enabled_status) { + pub_enu_vel_->publish(enu_vel_); + pub_enu_relative_pos_->publish(enu_relative_pos_); + } + pub_twist_->publish(eagleye_twist_); + pub_twist_with_covariance_->publish(eagleye_twist_with_covariance_); + } + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/velocity_estimator_node.cpp b/eagleye_rt/src/velocity_estimator_node.cpp index 6e64d6c9..9d77e95f 100644 --- a/eagleye_rt/src/velocity_estimator_node.cpp +++ b/eagleye_rt/src/velocity_estimator_node.cpp @@ -28,80 +28,79 @@ * Author MapIV Takanose */ -#include "rclcpp/rclcpp.hpp" -#include - #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -rclcpp::Publisher::SharedPtr velocity_pub; -rclcpp::Publisher::SharedPtr velocity_status_pub; - -static rtklib_msgs::msg::RtklibNav rtklib_nav_msg; -static nmea_msgs::msg::Gpgga gga_msg; -static sensor_msgs::msg::Imu imu_msg; - -VelocityEstimator velocity_estimator; -static geometry_msgs::msg::TwistStamped velocity_msg; - -static std::string yaml_file; -static std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; -static std::string subscribe_gga_topic_name = "gnss/gga"; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - rtklib_nav_msg = *msg; -} - -void gga_callback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) -{ - gga_msg = *msg; -} +#include -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class VelocityEstimatorNode : public rclcpp::Node { - imu_msg = *msg; +public: + VelocityEstimatorNode() : Node("eagleye_velocity_estimator") + { + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + + velocity_estimator_.setParam(yaml_file); + + sub_rtklib_nav_ = this->create_subscription( + "gnss/rtklib_nav", 1000, + std::bind(&VelocityEstimatorNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_gga_ = this->create_subscription( + "gnss/gga", 1000, + std::bind(&VelocityEstimatorNode::ggaCallback, this, std::placeholders::_1)); + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&VelocityEstimatorNode::imuCallback, this, std::placeholders::_1)); + + pub_velocity_ = + this->create_publisher("velocity", 1000); + pub_velocity_status_ = + this->create_publisher("velocity_status", 1000); + } - velocity_estimator.VelocityEstimate(imu_msg, rtklib_nav_msg, gga_msg, &velocity_msg); +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_msg_; + nmea_msgs::msg::Gpgga gga_msg_; + sensor_msgs::msg::Imu imu_msg_; + geometry_msgs::msg::TwistStamped velocity_msg_; + VelocityEstimator velocity_estimator_; - eagleye_msgs::msg::StatusStamped velocity_status; - velocity_status.header = msg->header; - velocity_status.status = velocity_estimator.getStatus(); - velocity_status_pub->publish(velocity_status); + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_gga_; + rclcpp::Subscription::SharedPtr sub_imu_; - if(velocity_status.status.enabled_status) + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - velocity_pub->publish(velocity_msg); + rtklib_nav_msg_ = *msg; } -} - -void velocity_estimator_node(rclcpp::Node::SharedPtr node) -{ - node->declare_parameter("yaml_file",yaml_file); - node->get_parameter("yaml_file",yaml_file); + void ggaCallback(const nmea_msgs::msg::Gpgga::ConstSharedPtr msg) { gga_msg_ = *msg; } - velocity_estimator.setParam(yaml_file); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + imu_msg_ = *msg; - auto rtklib_sub = - node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto gga_sub = - node->create_subscription(subscribe_gga_topic_name, 1000, gga_callback); - auto imu_sub = - node->create_subscription("imu/data_tf_converted", 1000, imu_callback); + velocity_estimator_.VelocityEstimate(imu_msg_, rtklib_nav_msg_, gga_msg_, &velocity_msg_); - velocity_pub = node->create_publisher("velocity", 1000); - velocity_status_pub = node->create_publisher("velocity_status", 1000); + eagleye_msgs::msg::StatusStamped velocity_status; + velocity_status.header = msg->header; + velocity_status.status = velocity_estimator_.getStatus(); + pub_velocity_status_->publish(velocity_status); - rclcpp::spin(node); -} + if (velocity_status.status.enabled_status) { + pub_velocity_->publish(velocity_msg_); + } + } +}; int main(int argc, char** argv) { rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_velocity_estimator"); - - velocity_estimator_node(node); - + rclcpp::spin(std::make_shared()); return 0; -} \ No newline at end of file +} diff --git a/eagleye_rt/src/velocity_scale_factor_node.cpp b/eagleye_rt/src/velocity_scale_factor_node.cpp index 17005487..93563ae2 100644 --- a/eagleye_rt/src/velocity_scale_factor_node.cpp +++ b/eagleye_rt/src/velocity_scale_factor_node.cpp @@ -28,249 +28,280 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static rtklib_msgs::msg::RtklibNav _rtklib_nav; -static nmea_msgs::msg::Gprmc _nmea_rmc; -static geometry_msgs::msg::TwistStamped _velocity; -static sensor_msgs::msg::Imu _imu; -static geometry_msgs::msg::TwistStamped _correction_velocity; - -rclcpp::Publisher::SharedPtr _pub1; -rclcpp::Publisher::SharedPtr _pub2; -static eagleye_msgs::msg::VelocityScaleFactor _velocity_scale_factor; - -struct VelocityScaleFactorParameter _velocity_scale_factor_parameter; -struct VelocityScaleFactorStatus _velocity_scale_factor_status; - -std::string _use_gnss_mode; - -bool _is_first_move = false; - -std::string _velocity_scale_factor_save_str; -double _saved_vsf_estimater_number; -double _saved_velocity_scale_factor = 1.0; -double _previous_velocity_scale_factor = 1.0; -double _th_velocity_scale_factor_percent = 20; - -void rtklib_nav_callback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) -{ - _rtklib_nav = *msg; -} - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +class VelocityScaleFactorNode : public rclcpp::Node { - _velocity = *msg; - - if (_is_first_move == false && msg->twist.linear.x > _velocity_scale_factor_parameter.moving_judgment_threshold) +public: + VelocityScaleFactorNode() : Node("eagleye_velocity_scale_factor") { - _is_first_move = true; - } -} - -void rmc_callback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) -{ - _nmea_rmc = *msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - double initial_velocity_scale_factor = _saved_velocity_scale_factor; - - _imu = *msg; - _velocity_scale_factor.header = msg->header; - _velocity_scale_factor.header.frame_id = "base_link"; - - _correction_velocity.header = msg->header; - _correction_velocity.header.frame_id = "base_link"; + std::string subscribe_twist_topic_name = "vehicle/twist"; + std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; + std::string subscribe_rmc_topic_name = "gnss/rmc"; + + double velocity_scale_factor_save_duration = 100.0; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + use_gnss_mode_ = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); + + velocity_scale_factor_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + velocity_scale_factor_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + velocity_scale_factor_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + + velocity_scale_factor_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_minimum_interval"] + .as(); + velocity_scale_factor_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_maximum_interval"] + .as(); + velocity_scale_factor_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["velocity_scale_factor"]["gnss_receiving_threshold"] + .as(); + + this->declare_parameter( + "velocity_scale_factor_save_str", velocity_scale_factor_save_str_); + this->declare_parameter( + "velocity_scale_factor.save_velocity_scale_factor", + velocity_scale_factor_parameter_.save_velocity_scale_factor); + this->declare_parameter( + "velocity_scale_factor.velocity_scale_factor_save_duration", + velocity_scale_factor_save_duration); + this->declare_parameter( + "velocity_scale_factor.th_velocity_scale_factor_percent", + th_velocity_scale_factor_percent_); + + this->get_parameter( + "velocity_scale_factor_save_str", velocity_scale_factor_save_str_); + this->get_parameter( + "velocity_scale_factor.save_velocity_scale_factor", + velocity_scale_factor_parameter_.save_velocity_scale_factor); + this->get_parameter( + "velocity_scale_factor.velocity_scale_factor_save_duration", + velocity_scale_factor_save_duration); + this->get_parameter( + "velocity_scale_factor.th_velocity_scale_factor_percent", + th_velocity_scale_factor_percent_); + + std::cout << "use_gnss_mode " << use_gnss_mode_ << std::endl; + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name + << std::endl; + std::cout << "gnss_rate " << velocity_scale_factor_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " + << velocity_scale_factor_parameter_.moving_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " + << velocity_scale_factor_parameter_.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " + << velocity_scale_factor_parameter_.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " + << velocity_scale_factor_parameter_.gnss_receiving_threshold << std::endl; + std::cout << "velocity_scale_factor_save_str " << velocity_scale_factor_save_str_ + << std::endl; + std::cout << "save_velocity_scale_factor " + << velocity_scale_factor_parameter_.save_velocity_scale_factor << std::endl; + std::cout << "velocity_scale_factor_save_duration " << velocity_scale_factor_save_duration + << std::endl; + std::cout << "th_velocity_scale_factor_percent " << th_velocity_scale_factor_percent_ + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31mvelocity_scale_factor Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } - if (_is_first_move == false) - { - _velocity_scale_factor.scale_factor = initial_velocity_scale_factor; - _correction_velocity.twist = _velocity.twist; - _pub1->publish(_correction_velocity); - _pub2->publish(_velocity_scale_factor); - return; + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&VelocityScaleFactorNode::imuCallback, this, std::placeholders::_1)); + sub_velocity_ = this->create_subscription( + subscribe_twist_topic_name, 1000, + std::bind(&VelocityScaleFactorNode::velocityCallback, this, std::placeholders::_1)); + sub_rtklib_nav_ = this->create_subscription( + subscribe_rtklib_nav_topic_name, 1000, + std::bind(&VelocityScaleFactorNode::rtklibNavCallback, this, std::placeholders::_1)); + sub_rmc_ = this->create_subscription( + subscribe_rmc_topic_name, 1000, + std::bind(&VelocityScaleFactorNode::rmcCallback, this, std::placeholders::_1)); + pub_correction_velocity_ = + this->create_publisher("velocity", rclcpp::QoS(10)); + pub_velocity_scale_factor_ = this->create_publisher( + "velocity_scale_factor", rclcpp::QoS(10)); + + if (velocity_scale_factor_parameter_.save_velocity_scale_factor) { + const auto period_ns = std::chrono::duration_cast( + std::chrono::duration(velocity_scale_factor_save_duration)); + timer_ = this->create_wall_timer( + period_ns, std::bind(&VelocityScaleFactorNode::onTimer, this)); + loadVelocityScaleFactor(velocity_scale_factor_save_str_); + } } - if (_use_gnss_mode == "rtklib" || _use_gnss_mode == "RTKLIB") // use RTKLIB mode - { - velocity_scale_factor_estimate(_rtklib_nav,_velocity,_velocity_scale_factor_parameter, - &_velocity_scale_factor_status,&_correction_velocity,&_velocity_scale_factor); - } - else if (_use_gnss_mode == "nmea" || _use_gnss_mode == "NMEA") // use NMEA mode +private: + rtklib_msgs::msg::RtklibNav rtklib_nav_; + nmea_msgs::msg::Gprmc nmea_rmc_; + geometry_msgs::msg::TwistStamped velocity_; + sensor_msgs::msg::Imu imu_; + geometry_msgs::msg::TwistStamped correction_velocity_; + eagleye_msgs::msg::VelocityScaleFactor velocity_scale_factor_; + + VelocityScaleFactorParameter velocity_scale_factor_parameter_; + VelocityScaleFactorStatus velocity_scale_factor_status_ = {}; + + std::string use_gnss_mode_; + bool is_first_move_ = false; + std::string velocity_scale_factor_save_str_; + double saved_vsf_estimater_number_ = 0; + double saved_velocity_scale_factor_ = 1.0; + double previous_velocity_scale_factor_ = 1.0; + double th_velocity_scale_factor_percent_ = 20; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr pub_correction_velocity_; + rclcpp::Publisher::SharedPtr + pub_velocity_scale_factor_; + rclcpp::Subscription::SharedPtr sub_imu_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_rtklib_nav_; + rclcpp::Subscription::SharedPtr sub_rmc_; + + void rtklibNavCallback(const rtklib_msgs::msg::RtklibNav::ConstSharedPtr msg) { - velocity_scale_factor_estimate(_nmea_rmc,_velocity,_velocity_scale_factor_parameter, - &_velocity_scale_factor_status,&_correction_velocity,&_velocity_scale_factor); + rtklib_nav_ = *msg; } - _velocity_scale_factor.status.is_abnormal = false; - if (!std::isfinite(_velocity_scale_factor.scale_factor)) { - _correction_velocity.twist.linear.x = _velocity.twist.linear.x * _previous_velocity_scale_factor; - _velocity_scale_factor.scale_factor = _previous_velocity_scale_factor; - _velocity_scale_factor.status.is_abnormal = true; - _velocity_scale_factor.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; - } - else if (_th_velocity_scale_factor_percent / 100 < std::abs(1.0 - _velocity_scale_factor.scale_factor)) + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - _correction_velocity.twist.linear.x = _velocity.twist.linear.x * _previous_velocity_scale_factor; - _velocity_scale_factor.scale_factor = _previous_velocity_scale_factor; - _velocity_scale_factor.status.is_abnormal = true; - _velocity_scale_factor.status.error_code = eagleye_msgs::msg::Status::TOO_LARGE_OR_SMALL; - } - else - { - _previous_velocity_scale_factor = _velocity_scale_factor.scale_factor; - } - - _pub1->publish(_correction_velocity); - _pub2->publish(_velocity_scale_factor); -} + velocity_ = *msg; -void load_velocity_scale_factor(std::string txt_path) -{ - std::ifstream ifs(txt_path); - if (!ifs) - { - std::cout << "Initial VelocityScaleFactor file not found!" << std::endl; - } - else - { - std::cout << "Loaded the saved velocity scale factor!" << std::endl; - int count = 0; - std::string row; - while (getline(ifs, row)) - { - if(count == 1) - { - _saved_vsf_estimater_number = std::stod(row); - std::cout<< "saved_vsf_estimater_number " << _saved_vsf_estimater_number << std::endl; - } - if(count == 3) - { - _saved_velocity_scale_factor = std::stod(row); - _velocity_scale_factor_status.estimate_start_status = true; - _velocity_scale_factor_status.velocity_scale_factor_last = _saved_velocity_scale_factor; - _velocity_scale_factor.status.enabled_status = true; - _velocity_scale_factor.scale_factor = _saved_velocity_scale_factor; - std::cout<< "saved_velocity_scale_factor " << _saved_velocity_scale_factor << std::endl; - } - count++; + if ( + is_first_move_ == false && + msg->twist.linear.x > velocity_scale_factor_parameter_.moving_judgment_threshold) { + is_first_move_ = true; } } - ifs.close(); -} - -void on_timer() -{ - if(!_velocity_scale_factor.status.enabled_status && _saved_vsf_estimater_number >= _velocity_scale_factor_status.estimated_number) - { - std::ofstream csv_file(_velocity_scale_factor_save_str); - return; - } - - std::ofstream csv_file(_velocity_scale_factor_save_str); - csv_file << "estimated_number"; - csv_file << "\n"; - csv_file << _velocity_scale_factor_status.estimated_number; - csv_file << "\n"; - csv_file << "velocity_scale_factor"; - csv_file << "\n"; - csv_file << _velocity_scale_factor_status.velocity_scale_factor_last; - csv_file << "\n"; - csv_file.close(); - - _saved_vsf_estimater_number = _velocity_scale_factor_status.estimated_number; - - return; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_velocity_scale_factor"); - - double velocity_scale_factor_save_duration = 100.0; - - std::string subscribe_twist_topic_name = "vehicle/twist"; - - std::string subscribe_rtklib_nav_topic_name = "gnss/rtklib_nav"; - std::string subscribe_rmc_topic_name = "gnss/rmc"; - 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; + void rmcCallback(const nmea_msgs::msg::Gprmc::ConstSharedPtr msg) { nmea_rmc_ = *msg; } - try + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); + double initial_velocity_scale_factor = saved_velocity_scale_factor_; - _use_gnss_mode = conf["/**"]["ros__parameters"]["use_gnss_mode"].as(); + imu_ = *msg; + velocity_scale_factor_.header = msg->header; + velocity_scale_factor_.header.frame_id = "base_link"; - _velocity_scale_factor_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - _velocity_scale_factor_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - _velocity_scale_factor_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + correction_velocity_.header = msg->header; + correction_velocity_.header.frame_id = "base_link"; - _velocity_scale_factor_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_minimum_interval"].as(); - _velocity_scale_factor_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["estimated_maximum_interval"].as(); - _velocity_scale_factor_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["velocity_scale_factor"]["gnss_receiving_threshold"].as(); - - node->declare_parameter("velocity_scale_factor_save_str",_velocity_scale_factor_save_str); - node->declare_parameter("velocity_scale_factor.save_velocity_scale_factor",_velocity_scale_factor_parameter.save_velocity_scale_factor); - node->declare_parameter("velocity_scale_factor.velocity_scale_factor_save_duration",velocity_scale_factor_save_duration); - node->declare_parameter("velocity_scale_factor.th_velocity_scale_factor_percent",_th_velocity_scale_factor_percent); - - node->get_parameter("velocity_scale_factor_save_str",_velocity_scale_factor_save_str); - node->get_parameter("velocity_scale_factor.save_velocity_scale_factor",_velocity_scale_factor_parameter.save_velocity_scale_factor); - node->get_parameter("velocity_scale_factor.velocity_scale_factor_save_duration",velocity_scale_factor_save_duration); - node->get_parameter("velocity_scale_factor.th_velocity_scale_factor_percent",_th_velocity_scale_factor_percent); - - std::cout << "use_gnss_mode " << _use_gnss_mode << std::endl; - - std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; - std::cout << "subscribe_rtklib_nav_topic_name " << subscribe_rtklib_nav_topic_name << std::endl; + if (is_first_move_ == false) { + velocity_scale_factor_.scale_factor = initial_velocity_scale_factor; + correction_velocity_.twist = velocity_.twist; + pub_correction_velocity_->publish(correction_velocity_); + pub_velocity_scale_factor_->publish(velocity_scale_factor_); + return; + } - std::cout << "gnss_rate " << _velocity_scale_factor_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << _velocity_scale_factor_parameter.moving_judgment_threshold << std::endl; + if (use_gnss_mode_ == "rtklib" || use_gnss_mode_ == "RTKLIB") { + velocity_scale_factor_estimate( + rtklib_nav_, velocity_, velocity_scale_factor_parameter_, + &velocity_scale_factor_status_, &correction_velocity_, &velocity_scale_factor_); + } else if (use_gnss_mode_ == "nmea" || use_gnss_mode_ == "NMEA") { + velocity_scale_factor_estimate( + nmea_rmc_, velocity_, velocity_scale_factor_parameter_, &velocity_scale_factor_status_, + &correction_velocity_, &velocity_scale_factor_); + } - std::cout << "estimated_minimum_interval " << _velocity_scale_factor_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << _velocity_scale_factor_parameter.estimated_maximum_interval << std::endl; - std::cout << "gnss_receiving_threshold " << _velocity_scale_factor_parameter.gnss_receiving_threshold << std::endl; + velocity_scale_factor_.status.is_abnormal = false; + if (!std::isfinite(velocity_scale_factor_.scale_factor)) { + correction_velocity_.twist.linear.x = + velocity_.twist.linear.x * previous_velocity_scale_factor_; + velocity_scale_factor_.scale_factor = previous_velocity_scale_factor_; + velocity_scale_factor_.status.is_abnormal = true; + velocity_scale_factor_.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; + } else if ( + th_velocity_scale_factor_percent_ / 100 < + std::abs(1.0 - velocity_scale_factor_.scale_factor)) { + correction_velocity_.twist.linear.x = + velocity_.twist.linear.x * previous_velocity_scale_factor_; + velocity_scale_factor_.scale_factor = previous_velocity_scale_factor_; + velocity_scale_factor_.status.is_abnormal = true; + velocity_scale_factor_.status.error_code = eagleye_msgs::msg::Status::TOO_LARGE_OR_SMALL; + } else { + previous_velocity_scale_factor_ = velocity_scale_factor_.scale_factor; + } - std::cout<< "velocity_scale_factor_save_str " << _velocity_scale_factor_save_str << std::endl; - std::cout<< "save_velocity_scale_factor " << _velocity_scale_factor_parameter.save_velocity_scale_factor << std::endl; - std::cout<< "velocity_scale_factor_save_duration " << velocity_scale_factor_save_duration << std::endl; - std::cout<< "th_velocity_scale_factor_percent "<<_th_velocity_scale_factor_percent<publish(correction_velocity_); + pub_velocity_scale_factor_->publish(velocity_scale_factor_); } - catch (YAML::Exception& e) + + void loadVelocityScaleFactor(std::string txt_path) { - std::cerr << "\033[1;31mvelocity_scale_factor Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + std::ifstream ifs(txt_path); + if (!ifs) { + std::cout << "Initial VelocityScaleFactor file not found!" << std::endl; + } else { + std::cout << "Loaded the saved velocity scale factor!" << std::endl; + int count = 0; + std::string row; + while (getline(ifs, row)) { + if (count == 1) { + saved_vsf_estimater_number_ = std::stod(row); + std::cout << "saved_vsf_estimater_number " << saved_vsf_estimater_number_ << std::endl; + } + if (count == 3) { + saved_velocity_scale_factor_ = std::stod(row); + velocity_scale_factor_status_.estimate_start_status = true; + velocity_scale_factor_status_.velocity_scale_factor_last = + saved_velocity_scale_factor_; + velocity_scale_factor_.status.enabled_status = true; + velocity_scale_factor_.scale_factor = saved_velocity_scale_factor_; + std::cout << "saved_velocity_scale_factor " << saved_velocity_scale_factor_ + << std::endl; + } + count++; + } + } + ifs.close(); } - auto sub1 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); - auto sub2 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); - auto sub3 = node->create_subscription(subscribe_rtklib_nav_topic_name, 1000, rtklib_nav_callback); - auto sub4 = node->create_subscription(subscribe_rmc_topic_name, 1000, rmc_callback); - _pub1 = node->create_publisher("velocity", rclcpp::QoS(10)); - _pub2 = node->create_publisher("velocity_scale_factor", rclcpp::QoS(10)); - - double delta_time = static_cast(velocity_scale_factor_save_duration); - auto timer_callback = std::bind(on_timer); - const auto period_ns = - std::chrono::duration_cast(std::chrono::duration(delta_time)); - auto timer = std::make_shared>( - node->get_clock(), period_ns, std::move(timer_callback), - node->get_node_base_interface()->get_context()); - if(_velocity_scale_factor_parameter.save_velocity_scale_factor) + + void onTimer() { - node->get_node_timers_interface()->add_timer(timer, nullptr); - load_velocity_scale_factor(_velocity_scale_factor_save_str); - } + if ( + !velocity_scale_factor_.status.enabled_status && + saved_vsf_estimater_number_ >= velocity_scale_factor_status_.estimated_number) { + std::ofstream csv_file(velocity_scale_factor_save_str_); + return; + } - rclcpp::spin(node); + std::ofstream csv_file(velocity_scale_factor_save_str_); + csv_file << "estimated_number"; + csv_file << "\n"; + csv_file << velocity_scale_factor_status_.estimated_number; + csv_file << "\n"; + csv_file << "velocity_scale_factor"; + csv_file << "\n"; + csv_file << velocity_scale_factor_status_.velocity_scale_factor_last; + csv_file << "\n"; + csv_file.close(); + + saved_vsf_estimater_number_ = velocity_scale_factor_status_.estimated_number; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; } diff --git a/eagleye_rt/src/yaw_rate_offset_node.cpp b/eagleye_rt/src/yaw_rate_offset_node.cpp index 9dc521e0..b7a0e59a 100644 --- a/eagleye_rt/src/yaw_rate_offset_node.cpp +++ b/eagleye_rt/src/yaw_rate_offset_node.cpp @@ -28,176 +28,207 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static geometry_msgs::msg::TwistStamped _velocity; -static eagleye_msgs::msg::StatusStamped _velocity_status; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_stop; -static eagleye_msgs::msg::Heading _heading_interpolate; -static sensor_msgs::msg::Imu _imu; -rclcpp::Publisher::SharedPtr _pub; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset; - -struct YawrateOffsetParameter _yaw_rate_offset_parameter; -struct YawrateOffsetStatus _yaw_rate_offset_status; - -bool _is_first_heading = false; -static bool _use_can_less_mode = false; - -double _previous_yaw_rate_offset = 0.0; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _velocity = *msg; -} - -void velocity_status_callback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) +class YawRateOffsetNode : public rclcpp::Node { - _velocity_status = *msg; -} +public: + YawRateOffsetNode(int argc, char** argv) : Node("eagleye_yaw_rate_offset") + { + std::string publish_topic_name = "/publish_topic_name/invalid"; + std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + if (argc > 2) { + if (strcmp(argv[1], "1st") == 0) { + publish_topic_name = "yaw_rate_offset_1st"; + subscribe_topic_name = "heading_interpolate_1st"; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + yaw_rate_offset_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + yaw_rate_offset_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + yaw_rate_offset_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + yaw_rate_offset_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["estimated_minimum_interval"] + .as(); + yaw_rate_offset_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["1st"]["estimated_maximum_interval"] + .as(); + yaw_rate_offset_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["gnss_receiving_threshold"] + .as(); + yaw_rate_offset_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "imu_rate " << yaw_rate_offset_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << yaw_rate_offset_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " + << yaw_rate_offset_parameter_.moving_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " + << yaw_rate_offset_parameter_.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " + << yaw_rate_offset_parameter_.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " + << yaw_rate_offset_parameter_.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << yaw_rate_offset_parameter_.outlier_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;yaw_rate_offset_1st Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + } else if (strcmp(argv[1], "2nd") == 0) { + publish_topic_name = "yaw_rate_offset_2nd"; + subscribe_topic_name = "heading_interpolate_2nd"; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + yaw_rate_offset_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + yaw_rate_offset_parameter_.gnss_rate = + conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); + yaw_rate_offset_parameter_.moving_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); + yaw_rate_offset_parameter_.estimated_minimum_interval = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["estimated_minimum_interval"] + .as(); + yaw_rate_offset_parameter_.estimated_maximum_interval = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["2nd"]["estimated_maximum_interval"] + .as(); + yaw_rate_offset_parameter_.gnss_receiving_threshold = + conf["/**"]["ros__parameters"]["yaw_rate_offset"]["gnss_receiving_threshold"] + .as(); + yaw_rate_offset_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "imu_rate " << yaw_rate_offset_parameter_.imu_rate << std::endl; + std::cout << "gnss_rate " << yaw_rate_offset_parameter_.gnss_rate << std::endl; + std::cout << "moving_judgment_threshold " + << yaw_rate_offset_parameter_.moving_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " + << yaw_rate_offset_parameter_.estimated_minimum_interval << std::endl; + std::cout << "estimated_maximum_interval " + << yaw_rate_offset_parameter_.estimated_maximum_interval << std::endl; + std::cout << "gnss_receiving_threshold " + << yaw_rate_offset_parameter_.gnss_receiving_threshold << std::endl; + std::cout << "outlier_threshold " << yaw_rate_offset_parameter_.outlier_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;yaw_rate_offset_2nd Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + } else { + RCLCPP_ERROR(this->get_logger(), "No arguments"); + rclcpp::shutdown(); + } + } else { + RCLCPP_ERROR(this->get_logger(), "No arguments"); + rclcpp::shutdown(); + } -void yaw_rate_offset_stop_callback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) -{ - _yaw_rate_offset_stop = *msg; -} + sub_velocity_ = this->create_subscription( + "velocity", rclcpp::QoS(10), + std::bind(&YawRateOffsetNode::velocityCallback, this, std::placeholders::_1)); + sub_velocity_status_ = this->create_subscription( + "velocity_status", rclcpp::QoS(10), + std::bind(&YawRateOffsetNode::velocityStatusCallback, this, std::placeholders::_1)); + sub_yaw_rate_offset_stop_ = this->create_subscription( + "yaw_rate_offset_stop", rclcpp::QoS(10), + std::bind(&YawRateOffsetNode::yawRateOffsetStopCallback, this, std::placeholders::_1)); + sub_heading_interpolate_ = this->create_subscription( + subscribe_topic_name, 1000, + std::bind(&YawRateOffsetNode::headingInterpolateCallback, this, std::placeholders::_1)); + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&YawRateOffsetNode::imuCallback, this, std::placeholders::_1)); + pub_ = + this->create_publisher(publish_topic_name, rclcpp::QoS(10)); + } -void heading_interpolate_callback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) -{ - _heading_interpolate = *msg; - if (_is_first_heading == false && _heading_interpolate.status.enabled_status == true) +private: + geometry_msgs::msg::TwistStamped velocity_; + eagleye_msgs::msg::StatusStamped velocity_status_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + eagleye_msgs::msg::Heading heading_interpolate_; + sensor_msgs::msg::Imu imu_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_; + YawrateOffsetParameter yaw_rate_offset_parameter_; + YawrateOffsetStatus yaw_rate_offset_status_ = {}; + bool is_first_heading_ = false; + bool use_can_less_mode_ = false; + double previous_yaw_rate_offset_ = 0.0; + + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_velocity_status_; + rclcpp::Subscription::SharedPtr sub_yaw_rate_offset_stop_; + rclcpp::Subscription::SharedPtr sub_heading_interpolate_; + rclcpp::Subscription::SharedPtr sub_imu_; + + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - _is_first_heading = true; + velocity_ = *msg; } -} -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) -{ - if (_is_first_heading == false) return; - if(_use_can_less_mode && !_velocity_status.status.enabled_status) return; - - _imu = *msg; - _yaw_rate_offset.header = msg->header; - yaw_rate_offset_estimate(_velocity,_yaw_rate_offset_stop,_heading_interpolate,_imu,_yaw_rate_offset_parameter, &_yaw_rate_offset_status, &_yaw_rate_offset); - - _yaw_rate_offset.status.is_abnormal = false; - if (!std::isfinite(_yaw_rate_offset_stop.yaw_rate_offset)) { - _yaw_rate_offset_stop.yaw_rate_offset =_previous_yaw_rate_offset; - _yaw_rate_offset.status.is_abnormal = true; - _yaw_rate_offset.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; - } - else + void velocityStatusCallback(const eagleye_msgs::msg::StatusStamped::ConstSharedPtr msg) { - _previous_yaw_rate_offset = _yaw_rate_offset_stop.yaw_rate_offset; + velocity_status_ = *msg; } - _pub->publish(_yaw_rate_offset); - _yaw_rate_offset.status.estimate_status = false; -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_yaw_rate_offset"); - - std::string publish_topic_name = "/publish_topic_name/invalid"; - std::string subscribe_topic_name = "/subscribe_topic_name/invalid"; - - 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; - - if (argc > 2) + void yawRateOffsetStopCallback(const eagleye_msgs::msg::YawrateOffset::ConstSharedPtr msg) { - if (strcmp(argv[1], "1st") == 0) - { - publish_topic_name = "yaw_rate_offset_1st"; - subscribe_topic_name = "heading_interpolate_1st"; - - try - { - YAML::Node conf = YAML::LoadFile(yaml_file); - - _yaw_rate_offset_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - _yaw_rate_offset_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - _yaw_rate_offset_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - - _yaw_rate_offset_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["estimated_minimum_interval"].as(); - _yaw_rate_offset_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["1st"]["estimated_maximum_interval"].as(); - _yaw_rate_offset_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["gnss_receiving_threshold"].as(); - _yaw_rate_offset_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); - - std::cout << "imu_rate " << _yaw_rate_offset_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << _yaw_rate_offset_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << _yaw_rate_offset_parameter.moving_judgment_threshold << std::endl; - - std::cout << "estimated_minimum_interval " << _yaw_rate_offset_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << _yaw_rate_offset_parameter.estimated_maximum_interval << std::endl; - std::cout << "gnss_receiving_threshold " << _yaw_rate_offset_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_threshold " << _yaw_rate_offset_parameter.outlier_threshold << std::endl; - } - catch (YAML::Exception& e) - { - std::cerr << "\033[1;yaw_rate_offset_1st Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); - } - } - else if (strcmp(argv[1], "2nd") == 0) - { - publish_topic_name = "yaw_rate_offset_2nd"; - subscribe_topic_name = "heading_interpolate_2nd"; - - try - { - YAML::Node conf = YAML::LoadFile(yaml_file); - - _yaw_rate_offset_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - _yaw_rate_offset_parameter.gnss_rate = conf["/**"]["ros__parameters"]["common"]["gnss_rate"].as(); - _yaw_rate_offset_parameter.moving_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["moving_judgment_threshold"].as(); - - _yaw_rate_offset_parameter.estimated_minimum_interval = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["estimated_minimum_interval"].as(); - _yaw_rate_offset_parameter.estimated_maximum_interval = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["2nd"]["estimated_maximum_interval"].as(); - _yaw_rate_offset_parameter.gnss_receiving_threshold = conf["/**"]["ros__parameters"]["yaw_rate_offset"]["gnss_receiving_threshold"].as(); - _yaw_rate_offset_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); - - std::cout << "imu_rate " << _yaw_rate_offset_parameter.imu_rate << std::endl; - std::cout << "gnss_rate " << _yaw_rate_offset_parameter.gnss_rate << std::endl; - std::cout << "moving_judgment_threshold " << _yaw_rate_offset_parameter.moving_judgment_threshold << std::endl; - - std::cout << "estimated_minimum_interval " << _yaw_rate_offset_parameter.estimated_minimum_interval << std::endl; - std::cout << "estimated_maximum_interval " << _yaw_rate_offset_parameter.estimated_maximum_interval << std::endl; - std::cout << "gnss_receiving_threshold " << _yaw_rate_offset_parameter.gnss_receiving_threshold << std::endl; - std::cout << "outlier_threshold " << _yaw_rate_offset_parameter.outlier_threshold << std::endl; - } - catch (YAML::Exception& e) - { - std::cerr << "\033[1;yaw_rate_offset_2nd Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); - } - } - else - { - // RCLCPP_ERROR(node->get_logger(),"Invalid argument"); - RCLCPP_ERROR(node->get_logger(), "No arguments"); - rclcpp::shutdown(); - } + yaw_rate_offset_stop_ = *msg; } - else + + void headingInterpolateCallback(const eagleye_msgs::msg::Heading::ConstSharedPtr msg) { - RCLCPP_ERROR(node->get_logger(), "No arguments"); - rclcpp::shutdown(); + heading_interpolate_ = *msg; + if (is_first_heading_ == false && heading_interpolate_.status.enabled_status == true) { + is_first_heading_ = true; + } } - auto sub1 = node->create_subscription("velocity", rclcpp::QoS(10), velocity_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("yaw_rate_offset_stop", rclcpp::QoS(10), yaw_rate_offset_stop_callback); //ros::TransportHints().tcpNoDelay() - auto sub3 = node->create_subscription(subscribe_topic_name, 1000, heading_interpolate_callback); //ros::TransportHints().tcpNoDelay() - auto sub4 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - _pub = node->create_publisher(publish_topic_name, rclcpp::QoS(10)); - - rclcpp::spin(node); + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) + { + if (is_first_heading_ == false) return; + if (use_can_less_mode_ && !velocity_status_.status.enabled_status) return; + + imu_ = *msg; + yaw_rate_offset_.header = msg->header; + yaw_rate_offset_estimate( + velocity_, yaw_rate_offset_stop_, heading_interpolate_, imu_, yaw_rate_offset_parameter_, + &yaw_rate_offset_status_, &yaw_rate_offset_); + + yaw_rate_offset_.status.is_abnormal = false; + if (!std::isfinite(yaw_rate_offset_stop_.yaw_rate_offset)) { + yaw_rate_offset_stop_.yaw_rate_offset = previous_yaw_rate_offset_; + yaw_rate_offset_.status.is_abnormal = true; + yaw_rate_offset_.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; + } else { + previous_yaw_rate_offset_ = yaw_rate_offset_stop_.yaw_rate_offset; + } + pub_->publish(yaw_rate_offset_); + yaw_rate_offset_.status.estimate_status = false; + } +}; +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(argc, argv)); return 0; } diff --git a/eagleye_rt/src/yaw_rate_offset_stop_node.cpp b/eagleye_rt/src/yaw_rate_offset_stop_node.cpp index a7ea53dc..a3ae50ca 100644 --- a/eagleye_rt/src/yaw_rate_offset_stop_node.cpp +++ b/eagleye_rt/src/yaw_rate_offset_stop_node.cpp @@ -28,86 +28,100 @@ * Author MapIV Sekino */ -#include "rclcpp/rclcpp.hpp" #include "eagleye_coordinate/eagleye_coordinate.hpp" #include "eagleye_navigation/eagleye_navigation.hpp" +#include "rclcpp/rclcpp.hpp" -static geometry_msgs::msg::TwistStamped::ConstSharedPtr _velocity_ptr ; -rclcpp::Publisher::SharedPtr _pub; -static eagleye_msgs::msg::YawrateOffset _yaw_rate_offset_stop; -static sensor_msgs::msg::Imu _imu; - -struct YawrateOffsetStopParameter _yaw_rate_offset_stop_parameter; -struct YawrateOffsetStopStatus _yaw_rate_offset_stop_status; - -double _previous_yaw_rate_offset_stop = 0.0; - -void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - _velocity_ptr = msg; -} - -void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) +class YawRateOffsetStopNode : public rclcpp::Node { - if (_velocity_ptr == nullptr) return; - _imu = *msg; - _yaw_rate_offset_stop.header = msg->header; - yaw_rate_offset_stop_estimate(*_velocity_ptr, _imu, _yaw_rate_offset_stop_parameter, &_yaw_rate_offset_stop_status, &_yaw_rate_offset_stop); - - _yaw_rate_offset_stop.status.is_abnormal = false; - if (!std::isfinite(_yaw_rate_offset_stop.yaw_rate_offset)) { - _yaw_rate_offset_stop.yaw_rate_offset =_previous_yaw_rate_offset_stop; - _yaw_rate_offset_stop.status.is_abnormal = true; - _yaw_rate_offset_stop.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; - } - else +public: + YawRateOffsetStopNode() : Node("eagleye_yaw_rate_offset_stop") { - _previous_yaw_rate_offset_stop = _yaw_rate_offset_stop.yaw_rate_offset; + std::string subscribe_twist_topic_name = "vehicle/twist"; + + std::string yaml_file; + this->declare_parameter("yaml_file", yaml_file); + this->get_parameter("yaml_file", yaml_file); + std::cout << "yaml_file: " << yaml_file << std::endl; + + try { + YAML::Node conf = YAML::LoadFile(yaml_file); + + yaw_rate_offset_stop_parameter_.imu_rate = + conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); + yaw_rate_offset_stop_parameter_.stop_judgment_threshold = + conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); + yaw_rate_offset_stop_parameter_.estimated_interval = + conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["estimated_interval"].as(); + yaw_rate_offset_stop_parameter_.outlier_threshold = + conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); + + std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; + std::cout << "imu_rate " << yaw_rate_offset_stop_parameter_.imu_rate << std::endl; + std::cout << "stop_judgment_threshold " + << yaw_rate_offset_stop_parameter_.stop_judgment_threshold << std::endl; + std::cout << "estimated_minimum_interval " + << yaw_rate_offset_stop_parameter_.estimated_interval << std::endl; + std::cout << "outlier_threshold " << yaw_rate_offset_stop_parameter_.outlier_threshold + << std::endl; + } catch (YAML::Exception& e) { + std::cerr << "\033[1;31myaw_rate_offset_stop Node YAML Error: " << e.msg << "\033[0m" + << std::endl; + exit(3); + } + + sub_velocity_ = this->create_subscription( + subscribe_twist_topic_name, 1000, + std::bind(&YawRateOffsetStopNode::velocityCallback, this, std::placeholders::_1)); + sub_imu_ = this->create_subscription( + "imu/data_tf_converted", 1000, + std::bind(&YawRateOffsetStopNode::imuCallback, this, std::placeholders::_1)); + pub_ = this->create_publisher( + "yaw_rate_offset_stop", rclcpp::QoS(10)); } - _pub->publish(_yaw_rate_offset_stop); -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("eagleye_yaw_rate_offset_stop"); - std::string subscribe_twist_topic_name = "vehicle/twist"; +private: + geometry_msgs::msg::TwistStamped::ConstSharedPtr velocity_ptr_; + eagleye_msgs::msg::YawrateOffset yaw_rate_offset_stop_; + sensor_msgs::msg::Imu imu_; + YawrateOffsetStopParameter yaw_rate_offset_stop_parameter_; + YawrateOffsetStopStatus yaw_rate_offset_stop_status_ = {}; + double previous_yaw_rate_offset_stop_ = 0.0; - 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; + rclcpp::Publisher::SharedPtr pub_; + rclcpp::Subscription::SharedPtr sub_velocity_; + rclcpp::Subscription::SharedPtr sub_imu_; - try + void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - YAML::Node conf = YAML::LoadFile(yaml_file); - - _yaw_rate_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as(); - _yaw_rate_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as(); - - _yaw_rate_offset_stop_parameter.estimated_interval = conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["estimated_interval"].as(); - _yaw_rate_offset_stop_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["yaw_rate_offset_stop"]["outlier_threshold"].as(); - - std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl; - - std::cout << "imu_rate " << _yaw_rate_offset_stop_parameter.imu_rate << std::endl; - std::cout << "stop_judgment_threshold " << _yaw_rate_offset_stop_parameter.stop_judgment_threshold << std::endl; - - std::cout << "estimated_minimum_interval " << _yaw_rate_offset_stop_parameter.estimated_interval << std::endl; - std::cout << "outlier_threshold " << _yaw_rate_offset_stop_parameter.outlier_threshold << std::endl; + velocity_ptr_ = msg; } - catch (YAML::Exception& e) + + void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg) { - std::cerr << "\033[1;31myaw_rate_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl; - exit(3); + if (velocity_ptr_ == nullptr) return; + imu_ = *msg; + yaw_rate_offset_stop_.header = msg->header; + yaw_rate_offset_stop_estimate( + *velocity_ptr_, imu_, yaw_rate_offset_stop_parameter_, &yaw_rate_offset_stop_status_, + &yaw_rate_offset_stop_); + + yaw_rate_offset_stop_.status.is_abnormal = false; + if (!std::isfinite(yaw_rate_offset_stop_.yaw_rate_offset)) { + yaw_rate_offset_stop_.yaw_rate_offset = previous_yaw_rate_offset_stop_; + yaw_rate_offset_stop_.status.is_abnormal = true; + yaw_rate_offset_stop_.status.error_code = eagleye_msgs::msg::Status::NAN_OR_INFINITE; + } else { + previous_yaw_rate_offset_stop_ = yaw_rate_offset_stop_.yaw_rate_offset; + } + + pub_->publish(yaw_rate_offset_stop_); } +}; - auto sub1 = node->create_subscription(subscribe_twist_topic_name, 1000, velocity_callback); //ros::TransportHints().tcpNoDelay() - auto sub2 = node->create_subscription("imu/data_tf_converted", 1000, imu_callback); //ros::TransportHints().tcpNoDelay() - _pub = node->create_publisher("yaw_rate_offset_stop", rclcpp::QoS(10)); - - rclcpp::spin(node); - +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); return 0; }