fix: correct localization

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

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.14)
+project(gyro_odometer)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+ament_auto_add_executable(${PROJECT_NAME}
+  src/gyro_odometer_node.cpp
+  src/gyro_odometer_core.cpp
+)
+
+target_link_libraries(${PROJECT_NAME} fmt)
+
+ament_auto_add_library(gyro_odometer_node SHARED
+  src/gyro_odometer_core.cpp
+)
+
+if(BUILD_TESTING)
+  ament_add_ros_isolated_gtest(test_gyro_odometer
+    test/test_main.cpp
+    test/test_gyro_odometer_pubsub.cpp
+    test/test_gyro_odometer_helper.cpp
+  )
+  ament_target_dependencies(test_gyro_odometer
+    rclcpp
+  )
+  target_link_libraries(test_gyro_odometer
+    gyro_odometer_node
+  )
+endif()
+
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

aichallenge/workspace/src/aichallenge_submit/gyro_odometer/README.md

@@ -0,0 +1,39 @@
+# gyro_odometer
+
+## Purpose
+
+`gyro_odometer` is the package to estimate twist by combining imu and vehicle speed.
+
+## Inputs / Outputs
+
+### Input
+
+| Name                            | Type                                             | Description                        |
+| ------------------------------- | ------------------------------------------------ | ---------------------------------- |
+| `vehicle/twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | twist with covariance from vehicle |
+| `imu`                           | `sensor_msgs::msg::Imu`                          | imu from sensor                    |
+
+### Output
+
+| Name                    | Type                                             | Description                     |
+| ----------------------- | ------------------------------------------------ | ------------------------------- |
+| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance |
+
+## Parameters
+
+| Parameter             | Type   | Description                      |
+| --------------------- | ------ | -------------------------------- |
+| `output_frame`        | String | output's frame id                |
+| `message_timeout_sec` | Double | delay tolerance time for message |
+
+## Assumptions / Known limits
+
+- [Assumption] The frame_id of input twist message must be set to base_link.
+
+- [Assumption] The covariance in the input messages must be properly assigned.
+
+- [Assumption] The angular velocity is set to zero if both the longitudinal vehicle velocity and the angular velocity around the yaw axis are sufficiently small. This is for suppression of the IMU angular velocity bias. Without this process, we misestimate the vehicle status when stationary.
+
+- [Limitation] The frequency of the output messages depends on the frequency of the input IMU message.
+
+- [Limitation] We cannot produce reliable values for the lateral and vertical velocities. Therefore we assign large values to the corresponding elements in the output covariance matrix.

aichallenge/workspace/src/aichallenge_submit/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp

@@ -0,0 +1,76 @@
+// Copyright 2015-2019 Autoware Foundation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+#define GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
+
+#include "tier4_autoware_utils/ros/transform_listener.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <geometry_msgs/msg/twist_stamped.hpp>
+#include <geometry_msgs/msg/twist_with_covariance_stamped.hpp>
+#include <sensor_msgs/msg/imu.hpp>
+
+#include <tf2/transform_datatypes.h>
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <deque>
+#include <memory>
+#include <string>
+
+class GyroOdometer : public rclcpp::Node
+{
+private:
+  using COV_IDX = tier4_autoware_utils::xyz_covariance_index::XYZ_COV_IDX;
+
+public:
+  explicit GyroOdometer(const rclcpp::NodeOptions & options);
+  ~GyroOdometer();
+
+private:
+  void callbackVehicleTwist(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr vehicle_twist_msg_ptr);
+  void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+  void publishData(const geometry_msgs::msg::TwistWithCovarianceStamped & twist_with_cov_raw);
+
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    vehicle_twist_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_raw_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    twist_with_covariance_raw_pub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    twist_with_covariance_pub_;
+
+  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
+
+  std::string output_frame_;
+  double message_timeout_sec_;
+
+  bool vehicle_twist_arrived_;
+  bool imu_arrived_;
+  std::deque<geometry_msgs::msg::TwistWithCovarianceStamped> vehicle_twist_queue_;
+  std::deque<sensor_msgs::msg::Imu> gyro_queue_;
+};
+
+#endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_

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

@@ -0,0 +1,29 @@
+<launch>
+  <arg name="input_vehicle_twist_with_covariance_topic" default="/sensing/vehicle_velocity_converter/twist_with_covariance" description="input twist with covariance topic name from vehicle"/>
+
+  <arg name="input_imu_topic" default="/sensing/imu/imu_data" description="input imu topic name"/>
+
+  <arg name="output_twist_raw_topic" default="gyro_twist_raw" description="output raw twist topic name"/>
+  <arg name="output_twist_with_covariance_raw_topic" default="gyro_twist_with_covariance_raw" description="output raw twist with covariance topic name"/>
+
+  <arg name="output_twist_topic" default="gyro_twist" description="output twist topic name"/>
+  <arg name="output_twist_with_covariance_topic" default="gyro_twist_with_covariance" description="output twist with covariance topic name"/>
+
+  <arg name="output_frame" default="base_link" description="output frame id"/>
+  <arg name="message_timeout_sec" default="0.2" description="delay tolerance time for message"/>
+
+  <node pkg="gyro_odometer" exec="gyro_odometer" name="gyro_odometer" output="screen">
+    <remap from="vehicle/twist_with_covariance" to="$(var input_vehicle_twist_with_covariance_topic)"/>
+
+    <remap from="imu" to="$(var input_imu_topic)"/>
+
+    <remap from="twist_raw" to="$(var output_twist_raw_topic)"/>
+    <remap from="twist_with_covariance_raw" to="$(var output_twist_with_covariance_raw_topic)"/>
+
+    <remap from="twist" to="$(var output_twist_topic)"/>
+    <remap from="twist_with_covariance" to="$(var output_twist_with_covariance_topic)"/>
+
+    <param name="output_frame" value="$(var output_frame)"/>
+    <param name="message_timeout_sec" value="$(var message_timeout_sec)"/>
+  </node>
+</launch>

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

@@ -0,0 +1,34 @@
+<?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>gyro_odometer</name>
+  <version>0.1.0</version>
+  <description>The gyro_odometer package as a ROS2 node</description>
+  <maintainer email="yamato.ando@tier4.jp">Yamato Ando</maintainer>
+  <license>Apache License 2.0</license>
+  <author email="yamato.ando@tier4.jp">Yamato Ando</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>autoware_cmake</build_depend>
+
+  <depend>fmt</depend>
+  <depend>geometry_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>std_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>tf2_ros</depend>
+  <depend>tier4_autoware_utils</depend>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+
+  <test_depend>ament_cmake_ros</test_depend>
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>