feat: add true gps module and edit reference launch (#27)

* feat: add true gps module and edit reference launch

* Update aichallenge/workspace/src/aichallenge_submit/gps_module/src/true_gps_module.cpp

---------

Co-authored-by: knorrrr <99851410+knorrrr@users.noreply.github.com>

aichallenge/workspace/src/aichallenge_submit/aichallenge_submit_launch/launch/reference.launch.xml

@@ -51,6 +51,11 @@
         <arg name="param_file" value="$(var imu_corrector_param_file)"/>
       </include>
     </group>
+
+    <group>
+      <push-ros-namespace namespace="gnss"/>
+      <include file="$(find-pkg-share gps_module)/launch/gps_module.launch.xml"/>
+    </group>
   </group>
 
     <!-- Localization -->

aichallenge/workspace/src/aichallenge_submit/gps_module/CMakeLists.txt

@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.8)
+project(gps_module)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+
+add_executable(true_gps_module src/true_gps_module.cpp)
+
+ament_target_dependencies(true_gps_module rclcpp geometry_msgs)
+
+install(TARGETS
+  true_gps_module
+  DESTINATION lib/${PROJECT_NAME})
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

aichallenge/workspace/src/aichallenge_submit/gps_module/launch/gps_module.launch.xml

@@ -0,0 +1,10 @@
+<launch>
+  <node pkg="gps_module" exec="true_gps_module" name="true_gps_module" output="screen">
+
+    <param name="debug" value="false"/>
+    <remap from="~/input/pose_with_covariance" to="/sensing/gnss/pose_with_covariance"/>
+    <remap from="~/output/pose_with_covariance" to="/sensing/gnss/gps_module/pose_with_covariance"/>
+    <remap from="~/output/pose" to="/sensing/gnss/gps_module/pose"/>
+
+  </node>
+</launch>

aichallenge/workspace/src/aichallenge_submit/gps_module/package.xml

@@ -0,0 +1,17 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>gps_module</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="arata22@todo.todo">arata22</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>geometry_msgs</depend>
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aichallenge/workspace/src/aichallenge_submit/gps_module/src/true_gps_module.cpp

@@ -0,0 +1,95 @@
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "geometry_msgs/msg/pose.hpp"
+#include <deque>
+
+class PoseComparisonNode : public rclcpp::Node
+{
+public:
+    PoseComparisonNode()
+    : Node("pose_comparison_node")
+    {
+        // パラメータの宣言とデフォルト値
+        this->declare_parameter<bool>("debug", false);
+        debug_ = this->get_parameter("debug").as_bool();
+
+        // サブスクリプションの設定
+        subscription_ = this->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
+            "~/input/pose_with_covariance", 10, std::bind(&PoseComparisonNode::pose_callback, this, std::placeholders::_1));
+
+        // PoseWithCovarianceStampedメッセージ用のパブリッシャー
+        publisher_pose_with_cov_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("~/output/pose_with_covariance", 10);
+
+        // Poseメッセージ用のパブリッシャー
+        publisher_pose_ = this->create_publisher<geometry_msgs::msg::Pose>("~/output/pose", 10);
+    }
+
+private:
+    // 最新の2つのメッセージを保持するためのキュー
+    std::deque<geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr> pose_queue_;
+    bool debug_;  // デバッグフラグ
+
+    rclcpp::Publisher<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr publisher_pose_with_cov_;
+    rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr publisher_pose_;
+
+    void pose_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
+    {
+        // キューに新しいメッセージを追加し、2つを超える場合は古いメッセージを削除
+        if (pose_queue_.size() >= 2) {
+            pose_queue_.pop_front();
+        }
+        pose_queue_.push_back(msg);
+
+        // キューに2つのメッセージが揃ったら比較を行う
+        if (pose_queue_.size() == 2) {
+            compare_poses(pose_queue_[0], pose_queue_[1]);
+        }
+    }
+
+    void compare_poses(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& old_pose,
+                       const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr& new_pose)
+    {
+        // 位置の変化を確認
+        bool position_change = 
+            old_pose->pose.pose.position.x != new_pose->pose.pose.position.x ||
+            old_pose->pose.pose.position.y != new_pose->pose.pose.position.y ||
+            old_pose->pose.pose.position.z != new_pose->pose.pose.position.z;
+
+        // 向き（四元数）の変化を確認
+        bool orientation_change = 
+            old_pose->pose.pose.orientation.x != new_pose->pose.pose.orientation.x ||
+            old_pose->pose.pose.orientation.y != new_pose->pose.pose.orientation.y ||
+            old_pose->pose.pose.orientation.z != new_pose->pose.pose.orientation.z ||
+            old_pose->pose.pose.orientation.w != new_pose->pose.pose.orientation.w;
+
+
+        // 位置または向きが変化した場合にイベントをトリガー
+        if (position_change || orientation_change) {
+            if (debug_) {
+                RCLCPP_INFO(this->get_logger(), "Pose has changed!");
+            }
+
+            // PoseWithCovarianceStampedメッセージのパブリッシュ
+            publisher_pose_with_cov_->publish(*new_pose);
+
+            // Poseメッセージのパブリッシュ
+            geometry_msgs::msg::Pose pose_msg = new_pose->pose.pose;
+            publisher_pose_->publish(pose_msg);
+        } else {
+            if (debug_) {
+                RCLCPP_INFO(this->get_logger(), "Pose not changed!");
+            }
+        }
+    }
+
+    rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr subscription_;
+};
+
+int main(int argc, char * argv[])
+{
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<PoseComparisonNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}