feat: add twist_estimator packages (autowarefoundation#57)

* release v0.4.0 * Avoid setting CMAKE_BUILD_TYPE=Release in each CMakeLists.txt (autowarefoundation#720) * remove set CMAKE_BUILD_TYPE Release in each CMakeLists.txt * remove set CMAKE_BUILD_TYPE Release in ndt_pcl_modified * set compile options for debug in ndt_omp * Fix indent * add warning if -DCMAKE_BUILD_TYPE=Release is not set in ndt_omp Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> * remove ROS1 packages temporarily Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "remove ROS1 packages temporarily" This reverts commit 2ba159d0875d817ae73a0df4ba12c4660c86acfe. Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Rename launch files to launch.xml (autowarefoundation#28) * ROS2 Porting: gyro_odometer (autowarefoundation#39) * Make compile - Fix cmake and package.xml - Update tf and msgs * Add publishers, subscribers and loggers - Fix launch file and Cmake to generate executable * Address PR comments: - Remove launch file xml line (not needed) - Convert Buffer and TransformListener to plain objects - Fix some typos * ported pose2twist from ROS1 to ROS2 (autowarefoundation#19) * ported pose2twist from ROS1 to ROS2 * Update CMakeLists.txt updated CMakeLists to be consise using ament_auto * Update pose2twist_core.cpp updated the ~Pose2Twist() = default in header * Update package.xml sorted dependencies alphabetically * Update pose2twist_core.h added ~Pose2Twist() = default; * Update pose2twist_core.h Include guards replaced * edited pose2twist launch file to ROS2 * fixed package XML * Add geometry2 to repos (autowarefoundation#76) * add geometry2 package temporarily until new release Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * trigger-ci Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * add tf2 dependency to the packages that use tf2_geometry_msgs Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> * Revert "Add geometry2 to repos (autowarefoundation#76)" (autowarefoundation#96) * Revert "Add geometry2 to repos (autowarefoundation#76)" This reverts commit 2e5ba3e86d0597920c0adf23f066521ea8a1a5b6. * Re-add tf2 dependencies * Revert "Re-add tf2 dependencies" This reverts commit e23b0c8b0826cf9518924d33349f9de34b4975df. * Debug CI pipeline * Revert "Debug CI pipeline" This reverts commit 58f1eba550360d82c08230552abfb64b33b23e0f. * Explicitly install known versions of the geometry packages * No need to skip tf2 packages anymore Co-authored-by: Esteve Fernandez <esteve@apache.org> * Rename h files to hpp (autowarefoundation#142) * Change includes * Rename files * Adjustments to make things compile * Other packages * Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143) * Use quotes for includes where appropriate (autowarefoundation#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully * Run uncrustify on the entire Pilot.Auto codebase (autowarefoundation#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs * adding linters to pose2twist (autowarefoundation#193) * adding linters to gyro_odometer (autowarefoundation#192) * apply env_var to use_sim_time (autowarefoundation#222) * Fix dt in pose2twist (autowarefoundation#289) * fix dt Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * apply format Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp> * fix covariance (autowarefoundation#875) (autowarefoundation#267) Signed-off-by: Yamato Ando <yamato.ando@gmail.com> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> * rm std_msgs (autowarefoundation#359) * rm_std_msgs (autowarefoundation#402) * Fix rolling build errors (autowarefoundation#1225) * Add missing include files Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace rclcpp::Duration Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use reference for exceptions Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Use from_seconds Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Sync public repo (autowarefoundation#1228) * [simple_planning_simulator] add readme (autowarefoundation#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (autowarefoundation#460) * Fix pose2twist (autowarefoundation#462) Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Ros2 vehicle info param server (autowarefoundation#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (autowarefoundation#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu <isamu.takagi@tier4.jp> * Fix issues in hdd_reader (autowarefoundation#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (autowarefoundation#463) * Rviz overlay render fix (autowarefoundation#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * super class now back to MFD Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * uncrustified Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * acquire data in mutex Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * back to RTD as superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * Rviz overlay render in update (autowarefoundation#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * super class now back to MFD Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * uncrustified Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * acquire data in mutex Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> * restored RTD superclass Signed-off-by: Adam Dabrowski <adam.dabrowski@robotec.ai> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com> Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai> * Fix for rolling (autowarefoundation#1226) * Replace doc by description Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Replace ns by push-ros-namespace Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Unify Apache-2.0 license name (autowarefoundation#1242) * Remove use_sim_time for set_parameter (autowarefoundation#1260) Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * Feature/gyro odometer add twist with cov input (autowarefoundation#1586) * add description for gyro_odometer package autowarefoundation#1819 * Fix -Wunused-parameter (autowarefoundation#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix mistake Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * fix spell * Fix lint issues Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Ignore flake8 warnings Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> * Invoke code formatter at pre-commit (autowarefoundation#1935) * Run ament_uncrustify at pre-commit * Reformat existing files * Fix copyright and cpplint errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> * Feature/add stop filter ros2 (autowarefoundation#1575) * revert imu yaw bias (autowarefoundation#2040) (autowarefoundation#2043) Co-authored-by: YamatoAndo <yamato.ando@gmail.com> * add sort-package-xml hook in pre-commit (autowarefoundation#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit * Change formatter to clang-format and black (autowarefoundation#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply Black Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Apply clang-format Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Fix build errors Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Add COLCON_IGNORE (autowarefoundation#500) Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * Remove COLCON_IGNORE under localization (autowarefoundation#553) * [gyro_odometer]add readme (autowarefoundation#625) * add readme * update * update * Update localization/twist_estimator/gyro_odometer/README.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * add discription Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * add readme in pose2twist (autowarefoundation#662) * add readme Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * Update localization/twist_estimator/pose2twist/src/README.md Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> * fix readme (autowarefoundation#669) Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com> * convert VehicleReport to Twist msgs (autowarefoundation#657) * add vehicle velocity converter package * clang format * add README * add heading_rate value * remove twist msg * clang format * set covariance value * fixed bug * [gyro_odometer] remove input twist (autowarefoundation#706) * remove input twist * update README * fix: move README.md directory Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp> Co-authored-by: Daichi Murakami <harihitode@gmail.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Nikolai Morin <nnmmgit@gmail.com> Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com> Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com> Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Co-authored-by: Esteve Fernandez <esteve@apache.org> Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: YamatoAndo <yamato.ando@gmail.com> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Takamasa Horibe <horibe.takamasa@gmail.com> Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp> Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara <kmiya@outlook.com> Co-authored-by: Makoto Tokunaga <vios-fish@users.noreply.github.com> Co-authored-by: Adam Dąbrowski <adam.dabrowski@robotec.ai> Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp> Co-authored-by: Takeshi Ishita <ishitah.takeshi@gmail.com> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: autoware-iv-sync-ci[bot] <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Co-authored-by: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Co-authored-by: RyuYamamoto <ryu.yamamoto@tier4.jp>
badai-nguyen · Dec 3, 2021 · 3c6e4ba · 3c6e4ba
1 parent f5aeb92
commit 3c6e4ba
Show file tree

Hide file tree

Showing 22 changed files with 894 additions and 0 deletions.
diff --git a/localization/gyro_odometer/CMakeLists.txt b/localization/gyro_odometer/CMakeLists.txt
@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 3.5)
+project(gyro_odometer)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+set(GYRO_ODOMETER_SRC
+  src/gyro_odometer_core.cpp)
+
+set(GYRO_ODOMETER_HEADERS
+  include/gyro_odometer/gyro_odometer_core.hpp)
+
+ament_auto_add_executable(${PROJECT_NAME}
+  src/gyro_odometer_node.cpp
+  ${GYRO_ODOMETER_SRC}
+  ${GYRO_ODOMETER_HEADERS}
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)
diff --git a/localization/gyro_odometer/README.md b/localization/gyro_odometer/README.md
@@ -0,0 +1,31 @@
+# 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`                 | `geometry_msgs::msg::TwistStamped`               | estimated twist                 |
+| `twist_with_covariance` | `geometry_msgs::msg::TwistWithCovarianceStamped` | estimated twist with covariance |
+
+## Parameters
+
+| Parameter      | Type   | Description       |
+| -------------- | ------ | ----------------- |
+| `output_frame` | String | output's frame id |
+
+## Assumptions / Known limits
+
+TBD.
diff --git a/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp b/localization/gyro_odometer/include/gyro_odometer/gyro_odometer_core.hpp
@@ -0,0 +1,60 @@
+// 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 <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>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+
+#include <string>
+
+class GyroOdometer : public rclcpp::Node
+{
+public:
+  GyroOdometer();
+  ~GyroOdometer();
+
+private:
+  void callbackTwistWithCovariance(
+    const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr);
+  void callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr);
+  bool getTransform(
+    const std::string & target_frame, const std::string & source_frame,
+    const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr);
+
+  rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    vehicle_twist_with_cov_sub_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_sub_;
+
+  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
+  rclcpp::Publisher<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr
+    twist_with_covariance_pub_;
+
+  tf2_ros::Buffer tf_buffer_;
+  tf2_ros::TransformListener tf_listener_;
+
+  std::string output_frame_;
+  geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr_;
+};
+
+#endif  // GYRO_ODOMETER__GYRO_ODOMETER_CORE_HPP_
diff --git a/localization/gyro_odometer/launch/gyro_odometer.launch.xml b/localization/gyro_odometer/launch/gyro_odometer.launch.xml
@@ -0,0 +1,22 @@
+<launch>
+  <arg name="input_vehicle_twist_with_covariance_topic" default="/vehicle/status/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_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"/>
+
+  <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" 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)" />
+  </node>
+</launch>
diff --git a/localization/gyro_odometer/package.xml b/localization/gyro_odometer/package.xml
@@ -0,0 +1,27 @@
+<?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@gmail.com">Yamato Ando</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_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>
+
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>rclcpp_components</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>autoware_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
diff --git a/localization/gyro_odometer/src/gyro_odometer_core.cpp b/localization/gyro_odometer/src/gyro_odometer_core.cpp
@@ -0,0 +1,145 @@
+// 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.
+
+#include "gyro_odometer/gyro_odometer_core.hpp"
+
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+
+#include <cmath>
+#include <memory>
+#include <string>
+
+GyroOdometer::GyroOdometer()
+: Node("gyro_odometer"),
+  tf_buffer_(this->get_clock()),
+  tf_listener_(tf_buffer_),
+  output_frame_(declare_parameter("base_link", "base_link"))
+{
+  vehicle_twist_with_cov_sub_ = create_subscription<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "vehicle/twist_with_covariance", rclcpp::QoS{100},
+    std::bind(&GyroOdometer::callbackTwistWithCovariance, this, std::placeholders::_1));
+
+  imu_sub_ = create_subscription<sensor_msgs::msg::Imu>(
+    "imu", rclcpp::QoS{100}, std::bind(&GyroOdometer::callbackImu, this, std::placeholders::_1));
+
+  twist_pub_ = create_publisher<geometry_msgs::msg::TwistStamped>("twist", rclcpp::QoS{10});
+  twist_with_covariance_pub_ = create_publisher<geometry_msgs::msg::TwistWithCovarianceStamped>(
+    "twist_with_covariance", rclcpp::QoS{10});
+
+  // TODO(YamatoAndo) createTimer
+}
+
+GyroOdometer::~GyroOdometer() {}
+
+void GyroOdometer::callbackTwistWithCovariance(
+  const geometry_msgs::msg::TwistWithCovarianceStamped::ConstSharedPtr twist_with_cov_msg_ptr)
+{
+  // TODO(YamatoAndo) trans from twist_with_cov_msg_ptr->header to base_frame
+  twist_with_cov_msg_ptr_ = twist_with_cov_msg_ptr;
+}
+
+void GyroOdometer::callbackImu(const sensor_msgs::msg::Imu::ConstSharedPtr imu_msg_ptr)
+{
+  if (!twist_with_cov_msg_ptr_) {
+    return;
+  }
+
+  geometry_msgs::msg::TransformStamped::SharedPtr tf_base2imu_ptr =
+    std::make_shared<geometry_msgs::msg::TransformStamped>();
+  getTransform(output_frame_, imu_msg_ptr->header.frame_id, tf_base2imu_ptr);
+
+  geometry_msgs::msg::Vector3Stamped angular_velocity;
+  angular_velocity.header = imu_msg_ptr->header;
+  angular_velocity.vector = imu_msg_ptr->angular_velocity;
+
+  geometry_msgs::msg::Vector3Stamped transformed_angular_velocity;
+  transformed_angular_velocity.header = tf_base2imu_ptr->header;
+
+  tf2::doTransform(angular_velocity, transformed_angular_velocity, *tf_base2imu_ptr);
+
+  // clear imu yaw bias if vehicle is stopped
+  if (
+    std::fabs(transformed_angular_velocity.vector.z) < 0.01 &&
+    std::fabs(twist_with_cov_msg_ptr_->twist.twist.linear.x) < 0.01) {
+    transformed_angular_velocity.vector.z = 0.0;
+  }
+
+  // TODO(YamatoAndo) move code
+  geometry_msgs::msg::TwistStamped twist;
+  twist.header.stamp = imu_msg_ptr->header.stamp;
+  twist.header.frame_id = output_frame_;
+  twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear;
+  twist.twist.angular.z = transformed_angular_velocity.vector.z;  // TODO(YamatoAndo) yaw_rate only
+  twist_pub_->publish(twist);
+
+  geometry_msgs::msg::TwistWithCovarianceStamped twist_with_covariance;
+  twist_with_covariance.header.stamp = imu_msg_ptr->header.stamp;
+  twist_with_covariance.header.frame_id = output_frame_;
+  twist_with_covariance.twist.twist.linear = twist_with_cov_msg_ptr_->twist.twist.linear;
+  twist_with_covariance.twist.twist.angular.z =
+    transformed_angular_velocity.vector.z;  // TODO(YamatoAndo) yaw_rate only
+
+  // NOTE
+  // linear.y, linear.z, angular.x, and angular.y are not measured values.
+  // Therefore, they should be assigned large variance values.
+  twist_with_covariance.twist.covariance[0] = twist_with_cov_msg_ptr_->twist.covariance[0];
+  twist_with_covariance.twist.covariance[7] = 10000.0;
+  twist_with_covariance.twist.covariance[14] = 10000.0;
+  twist_with_covariance.twist.covariance[21] = 10000.0;
+  twist_with_covariance.twist.covariance[28] = 10000.0;
+  twist_with_covariance.twist.covariance[35] = imu_msg_ptr->angular_velocity_covariance[8];
+
+  twist_with_covariance_pub_->publish(twist_with_covariance);
+}
+
+bool GyroOdometer::getTransform(
+  const std::string & target_frame, const std::string & source_frame,
+  const geometry_msgs::msg::TransformStamped::SharedPtr transform_stamped_ptr)
+{
+  if (target_frame == source_frame) {
+    transform_stamped_ptr->header.stamp = this->get_clock()->now();
+    transform_stamped_ptr->header.frame_id = target_frame;
+    transform_stamped_ptr->child_frame_id = source_frame;
+    transform_stamped_ptr->transform.translation.x = 0.0;
+    transform_stamped_ptr->transform.translation.y = 0.0;
+    transform_stamped_ptr->transform.translation.z = 0.0;
+    transform_stamped_ptr->transform.rotation.x = 0.0;
+    transform_stamped_ptr->transform.rotation.y = 0.0;
+    transform_stamped_ptr->transform.rotation.z = 0.0;
+    transform_stamped_ptr->transform.rotation.w = 1.0;
+    return true;
+  }
+
+  try {
+    *transform_stamped_ptr =
+      tf_buffer_.lookupTransform(target_frame, source_frame, tf2::TimePointZero);
+  } catch (tf2::TransformException & ex) {
+    RCLCPP_WARN(this->get_logger(), "%s", ex.what());
+    RCLCPP_ERROR(
+      this->get_logger(), "Please publish TF %s to %s", target_frame.c_str(), source_frame.c_str());
+
+    transform_stamped_ptr->header.stamp = this->get_clock()->now();
+    transform_stamped_ptr->header.frame_id = target_frame;
+    transform_stamped_ptr->child_frame_id = source_frame;
+    transform_stamped_ptr->transform.translation.x = 0.0;
+    transform_stamped_ptr->transform.translation.y = 0.0;
+    transform_stamped_ptr->transform.translation.z = 0.0;
+    transform_stamped_ptr->transform.rotation.x = 0.0;
+    transform_stamped_ptr->transform.rotation.y = 0.0;
+    transform_stamped_ptr->transform.rotation.z = 0.0;
+    transform_stamped_ptr->transform.rotation.w = 1.0;
+    return false;
+  }
+  return true;
+}
diff --git a/localization/gyro_odometer/src/gyro_odometer_node.cpp b/localization/gyro_odometer/src/gyro_odometer_node.cpp
@@ -0,0 +1,29 @@
+// 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.
+
+#include "gyro_odometer/gyro_odometer_core.hpp"
+
+#include <rclcpp/rclcpp.hpp>
+
+#include <memory>
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<GyroOdometer>();
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+
+  return 0;
+}
diff --git a/localization/pose2twist/CMakeLists.txt b/localization/pose2twist/CMakeLists.txt
@@ -0,0 +1,26 @@
+cmake_minimum_required(VERSION 3.5)
+project(pose2twist)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+  set(CMAKE_CXX_STANDARD_REQUIRED ON)
+  set(CMAKE_CXX_EXTENSIONS OFF)
+endif()
+
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_executable(pose2twist
+  src/pose2twist_node.cpp
+  src/pose2twist_core.cpp
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package(
+  INSTALL_TO_SHARE
+  launch
+)