From 4dfd07be633ad692b20d0d20b5b80a9327097be0 Mon Sep 17 00:00:00 2001
From: makotoyoshigoe <myoshigo0127@gmail.com>
Date: Thu, 22 Aug 2024 21:24:19 +0900
Subject: [PATCH] Add files for gnss_poser

---
 CMakeLists.txt                 |  4 ++
 include/gnss2map/GnssPoser.hpp | 41 +++++++++++++++
 src/GnssPoser.cpp              | 91 ++++++++++++++++++++++++++++++++++
 3 files changed, 136 insertions(+)
 create mode 100644 include/gnss2map/GnssPoser.hpp
 create mode 100644 src/GnssPoser.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 0e3be1f..e207bf7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -20,6 +20,10 @@ ament_auto_add_executable(gauss_kruger_node
   src/GaussKruger.cpp
 )
 
+ament_auto_add_executable(gnss_poser
+  src/GnssPoser.cpp
+)
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   set(ament_cmake_copyright_FOUND TRUE)
diff --git a/include/gnss2map/GnssPoser.hpp b/include/gnss2map/GnssPoser.hpp
new file mode 100644
index 0000000..fc30261
--- /dev/null
+++ b/include/gnss2map/GnssPoser.hpp
@@ -0,0 +1,41 @@
+// SPDX-FileCopyrightText: 2024 MakotoYoshigoe
+// SPDX-License-Identifier: Apache-2.0
+
+#ifndef GNSS2MAP__GNSSPOSER_HPP_
+#define GNSS2MAP__GNSSPOSER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <string>
+
+namespace gnss2map
+{
+class GnssPoser : public rclcpp::Node
+{
+        public:
+    GnssPoser();
+    ~GnssPoser();
+    void gnss_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
+    void ekf_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
+    void loop(void);
+    void declare_params(void);
+    void init_pub_sub(void);
+    void set_gnss_info(void);
+    void set_ekf_info(void);
+    double get_pub_rate(void);
+
+        private:
+    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pub_gnss_ekf_pose_with_covariance_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_ekf_pose_with_covariance_;
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr sub_gnss_pose_with_covariance_;
+    bool ekf_first_receive_;
+    geometry_msgs::msg::PoseWithCovarianceStamped gnss_ekf_pose_with_covariance_;
+    geometry_msgs::msg::PoseWithCovarianceStamped ekf_pose_with_covariance_;
+    geometry_msgs::msg::PoseWithCovarianceStamped gnss_pose_with_covariance_;
+    std::string frame_id_;
+    double pub_rate_;
+};
+}
+
+#endif // GNSS2MAP__GNSSPOSER_HPP_
\ No newline at end of file
diff --git a/src/GnssPoser.cpp b/src/GnssPoser.cpp
new file mode 100644
index 0000000..d91f95d
--- /dev/null
+++ b/src/GnssPoser.cpp
@@ -0,0 +1,91 @@
+// SPDX-FileCopyrightText: 2024 MakotoYoshigoe
+// SPDX-License-Identifier: Apache-2.0
+
+#include "gnss2map/GnssPoser.hpp"
+
+namespace gnss2map
+{
+    GnssPoser::GnssPoser() : Node("gnss_poser")
+    {
+        declare_params();
+        init_pub_sub();
+    }
+
+    GnssPoser::~GnssPoser(){}
+
+    void GnssPoser::declare_params(void)
+    {
+        declare_parameter("frame_id", "map");
+        get_parameter("frame_id", frame_id_);
+        declare_parameter("pub_rate", 10.0);
+        get_parameter("pub_rate", pub_rate_);
+        ekf_first_receive_ = false;
+    }
+
+    void GnssPoser::init_pub_sub(void)
+    {
+        pub_gnss_ekf_pose_with_covariance_ = create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("gnss_ekf_pose_with_covariance", 2);
+        sub_gnss_pose_with_covariance_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "gnss_pose_with_covariance", 2, std::bind(&GnssPoser::gnss_pose_callback, this, std::placeholders::_1)
+        );
+        sub_ekf_pose_with_covariance_ = create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "ekf_pose_with_covariance", 2, std::bind(&GnssPoser::ekf_pose_callback, this, std::placeholders::_1)
+        );
+
+    }
+
+    void GnssPoser::gnss_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
+    {
+        gnss_pose_with_covariance_ = *msg;
+    }
+
+    void GnssPoser::ekf_pose_callback(geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg)
+    {
+        if(!ekf_first_receive_) ekf_first_receive_ = true;
+        ekf_pose_with_covariance_ = *msg;
+    }
+
+    void GnssPoser::set_gnss_info()
+    {
+        gnss_ekf_pose_with_covariance_.pose.pose.position = gnss_pose_with_covariance_.pose.pose.position;
+        gnss_ekf_pose_with_covariance_.pose.covariance[0] = gnss_pose_with_covariance_.pose.covariance[0];
+        gnss_ekf_pose_with_covariance_.pose.covariance[7] = gnss_pose_with_covariance_.pose.covariance[7];
+        gnss_ekf_pose_with_covariance_.pose.covariance[14] = gnss_pose_with_covariance_.pose.covariance[14];
+    }
+
+    void GnssPoser::set_ekf_info()
+    {
+        gnss_ekf_pose_with_covariance_.pose.pose.orientation = ekf_pose_with_covariance_.pose.pose.orientation;
+        gnss_ekf_pose_with_covariance_.pose.covariance[21] = ekf_pose_with_covariance_.pose.covariance[21];
+        gnss_ekf_pose_with_covariance_.pose.covariance[28] = ekf_pose_with_covariance_.pose.covariance[28];
+        gnss_ekf_pose_with_covariance_.pose.covariance[35] = ekf_pose_with_covariance_.pose.covariance[35];
+    }
+
+    double GnssPoser::get_pub_rate(){
+        return pub_rate_;
+    }
+
+    void GnssPoser::loop(void)
+    {
+        if(!ekf_first_receive_) return;
+        gnss_ekf_pose_with_covariance_.header.frame_id = frame_id_;
+        gnss_ekf_pose_with_covariance_.header.stamp = now();
+        set_gnss_info();
+        set_ekf_info();
+        pub_gnss_ekf_pose_with_covariance_->publish(gnss_ekf_pose_with_covariance_);
+    }
+}
+
+int main(int argc, char ** argv)
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<gnss2map::GnssPoser>();
+    rclcpp::Rate rate(node->get_pub_rate());
+    while(rclcpp::ok()){
+        node->loop();
+        rclcpp::spin_some(node);
+        rate.sleep();
+    }
+    rclcpp::shutdown();
+    return 0;
+}
\ No newline at end of file