Feat/gnss poser (#14)

* Change topic name * Add params file * Add files for gnss_poser
CIT-Autonomous-Robot-Lab · Aug 22, 2024 · 6608ec1 · 6608ec1
1 parent d2db3e2
commit 6608ec1
Show file tree

Hide file tree

Showing 5 changed files with 148 additions and 1 deletion.
diff --git 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/config/params/iscas_museum.para.yaml b/config/params/iscas_museum.para.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    p0: [-0.04613610518881555, 0.10713541606642694, -0.07801946013724306]
+    gnss0: [-0.000135655462704628, -0.0001347502293765223, 0.21616219822317362]
+    p1: [5.5663636794726115, 8.885238326740312]
+    gnss1: [-0.00021460116061412394, -0.00018572578442608106]
+    a: 6378137.0
+    F: 298.257222
+    m0: 0.9999
+    ignore_th_cov: 1000.0
+    # range_limit: [0., 0., 272.9515075683594, 172.54762268066406]
diff --git a/include/gnss2map/GnssPoser.hpp 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_
diff --git a/src/GaussKruger.cpp b/src/GaussKruger.cpp
@@ -48,7 +48,7 @@ namespace gnss2map
     {
         sub_gnss_ = this->create_subscription<sensor_msgs::msg::NavSatFix>("gnss/fix", 2, std::bind(&GaussKruger::cbGnss, this, std::placeholders::_1));
         // pub_odom_gnss_ = this->create_publisher<nav_msgs::msg::Odometry>("odom/gnss", 2);
-        pub_gnss_pose_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("gnss_pose", 2);
+        pub_gnss_pose_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("gnss_pose_with_covariance", 2);
     }
 
     void GaussKruger::cbGnss(sensor_msgs::msg::NavSatFix::ConstSharedPtr msg)

diff --git a/src/GnssPoser.cpp 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;
+}