Merge pull request #13 from iASL-Gifu/fix/pose_cov_node

fix(pose_cov_node): fix culculation

aichallenge/workspace/src/aichallenge_submit/pose_cov_transformer/CMakeLists.txt → aichallenge/workspace/src/aichallenge_submit/pose_transformer/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.8)
-project(pose_cov_transformer)
+project(pose_transformer)
 
 if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
@@ -10,8 +10,8 @@ find_package(ament_cmake_auto REQUIRED)
 
 ament_auto_find_build_dependencies()
 
-ament_auto_add_executable(pose_cov_transformer
-  src/pose_cov_transformer.cpp
+ament_auto_add_executable(pose_transformer
+  src/pose_transformer.cpp
 )
 
 ament_auto_package(

aichallenge/workspace/src/aichallenge_submit/pose_cov_transformer/package.xml → aichallenge/workspace/src/aichallenge_submit/pose_transformer/package.xml

@@ -1,6 +1,6 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>pose_cov_transformer</name>
+  <name>pose_transformer</name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="yoshi-22@todo.todo">yoshi-22</maintainer>

aichallenge/workspace/src/aichallenge_submit/pose_transformer/src/pose_transformer.cpp

@@ -0,0 +1,42 @@
+#include "pose_transformer.hpp"
+
+namespace pose_transformer
+{
+
+Pose_transformer::Pose_transformer() : Node("pose_trasnformer"),
+tf_buffer_(get_clock()),
+tf_listener_(tf_buffer_, this, false)
+{
+    tf_buffer_.setUsingDedicatedThread(true);
+    using std::placeholders::_1;
+
+    // pub, subの初期化
+    sub_gnss_pose_ = create_subscription<geometry_msgs::msg::PoseStamped>("/sensing/gnss/pose", 1, std::bind(&Pose_transformer::on_gnss_pose, this, _1));
+    pub_gnss_pose_ = create_publisher<geometry_msgs::msg::PoseStamped>("/sensing/gnss/base_link/pose", 1);
+    convert_frame_id_ = this->declare_parameter<std::string>("frame_id", "base_link");
+}
+
+void Pose_transformer::on_gnss_pose(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg)
+{
+    geometry_msgs::msg::TransformStamped transform_stamped;
+    try {
+        transform_stamped = tf_buffer_.lookupTransform(convert_frame_id_, msg->header.frame_id, rclcpp::Time(0), rclcpp::Duration(1, 0));
+    } 
+    catch (tf2::TransformException & ex) {
+        RCLCPP_WARN(get_logger(), "Could not find transformation: %s", ex.what());
+        return;
+    }
+    auto pose_cov = std::make_shared<geometry_msgs::msg::PoseStamped>();
+    tf2::doTransform(*msg, *pose_cov, transform_stamped);
+    pose_cov->header.stamp = msg->header.stamp;
+    pose_cov->header.frame_id = convert_frame_id_;
+
+    pub_gnss_pose_->publish(*pose_cov);
+}
+}// namespace pose_transformer
+int main(int argc, char **argv){
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<pose_transformer::Pose_transformer>());
+    rclcpp::shutdown();
+    return 0;
+}

aichallenge/workspace/src/aichallenge_submit/pose_transformer/src/pose_transformer.hpp

@@ -0,0 +1,39 @@
+#ifndef POSE_COV_TRANSFORMER_HPP_
+#define POSE_COV_TRANSFORMER_HPP_
+
+#include <rclcpp/rclcpp.hpp>
+#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#include <thread>
+#include "tf2/utils.h"
+#include <cmath>
+#include <utility>
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+
+namespace pose_transformer
+{
+
+class Pose_transformer : public rclcpp::Node
+{
+
+public:
+    Pose_transformer();
+
+private:
+    // Publish
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pub_gnss_pose_;
+
+    // Subscription
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_gnss_pose_;
+    void on_gnss_pose(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg);
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::string convert_frame_id_;
+};
+
+}
+
+#endif