From 6608ec1775d329ac07865c3233ecd952de46d565 Mon Sep 17 00:00:00 2001 From: makotoyoshigoe <91446273+makotoyoshigoe@users.noreply.github.com> Date: Thu, 22 Aug 2024 21:39:53 +0900 Subject: [PATCH] Feat/gnss poser (#14) * Change topic name * Add params file * Add files for gnss_poser --- CMakeLists.txt | 4 ++ config/params/iscas_museum.para.yaml | 11 ++++ include/gnss2map/GnssPoser.hpp | 41 +++++++++++++ src/GaussKruger.cpp | 2 +- src/GnssPoser.cpp | 91 ++++++++++++++++++++++++++++ 5 files changed, 148 insertions(+), 1 deletion(-) create mode 100644 config/params/iscas_museum.para.yaml 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/config/params/iscas_museum.para.yaml b/config/params/iscas_museum.para.yaml new file mode 100644 index 0000000..2e1d867 --- /dev/null +++ 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 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 +#include +#include +#include + +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::SharedPtr pub_gnss_ekf_pose_with_covariance_; + rclcpp::Subscription::SharedPtr sub_ekf_pose_with_covariance_; + rclcpp::Subscription::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/GaussKruger.cpp b/src/GaussKruger.cpp index 44dc403..a84e3a1 100644 --- a/src/GaussKruger.cpp +++ b/src/GaussKruger.cpp @@ -48,7 +48,7 @@ namespace gnss2map { sub_gnss_ = this->create_subscription("gnss/fix", 2, std::bind(&GaussKruger::cbGnss, this, std::placeholders::_1)); // pub_odom_gnss_ = this->create_publisher("odom/gnss", 2); - pub_gnss_pose_ = this->create_publisher("gnss_pose", 2); + pub_gnss_pose_ = this->create_publisher("gnss_pose_with_covariance", 2); } void GaussKruger::cbGnss(sensor_msgs::msg::NavSatFix::ConstSharedPtr msg) 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("gnss_ekf_pose_with_covariance", 2); + sub_gnss_pose_with_covariance_ = create_subscription( + "gnss_pose_with_covariance", 2, std::bind(&GnssPoser::gnss_pose_callback, this, std::placeholders::_1) + ); + sub_ekf_pose_with_covariance_ = create_subscription( + "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(); + 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