feat: add twist_estimator packages

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
+)

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.

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_

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>

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>

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;
+}

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;
+}

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
+)