From e28c1698420c4b13b836f1a04f96591342e24685 Mon Sep 17 00:00:00 2001
From: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com>
Date: Tue, 13 Sep 2022 00:04:52 +0900
Subject: [PATCH] feat(tier4_autoware_utils): add covariance util (#1840)

* feature(tier4_autoware_utils): add covariance util

Signed-off-by: scepter914 <scepter914@gmail.com>

* add include file

Signed-off-by: scepter914 <scepter914@gmail.com>

* fix typo

Signed-off-by: scepter914 <scepter914@gmail.com>

* change enum to enum class

Signed-off-by: scepter914 <scepter914@gmail.com>

* add namespace

Signed-off-by: scepter914 <scepter914@gmail.com>

* change enum class to enum

Signed-off-by: scepter914 <scepter914@gmail.com>

* apply pre commit

Signed-off-by: scepter914 <scepter914@gmail.com>

Signed-off-by: scepter914 <scepter914@gmail.com>
---
 .../ros/msg_covariance.hpp                    | 120 ++++++++++++++++++
 .../tier4_autoware_utils.hpp                  |   1 +
 2 files changed, 121 insertions(+)
 create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp

diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp
new file mode 100644
index 0000000000000..fdaaf84d8efbb
--- /dev/null
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_covariance.hpp
@@ -0,0 +1,120 @@
+// Copyright 2022 TIER IV, Inc.
+//
+// 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 TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_
+
+namespace tier4_autoware_utils
+{
+namespace xyz_covariance_index
+{
+/// Covariance for x-y-z.
+/// Used at
+/// - sensor_msgs/msg/Imu.msg: msg.linear_acceleration_covariance
+enum XYZ_COV_IDX {
+  X_X = 0,
+  X_Y = 1,
+  X_Z = 2,
+  Y_X = 3,
+  Y_Y = 4,
+  Y_Z = 5,
+  Z_X = 6,
+  Z_Y = 7,
+  Z_Z = 8,
+};
+}  // namespace xyz_covariance_index
+
+namespace rpy_covariance_index
+{
+/// Covariance for roll-pitch-yaw.
+/// Used at
+/// - sensor_msgs/msg/Imu.msg: msg.angular_velocity_covariance
+/// - sensor_msgs/msg/Imu.msg: msg.orientation_covariance
+enum RPY_COV_IDX {
+  ROLL_ROLL = 0,
+  ROLL_PITCH = 1,
+  ROLL_YAW = 2,
+  PITCH_ROLL = 3,
+  PITCH_PITCH = 4,
+  PITCH_YAW = 5,
+  YAW_ROLL = 6,
+  YAW_PITCH = 7,
+  YAW_YAW = 8
+};
+}  // namespace rpy_covariance_index
+
+namespace pose_covariance_index
+{
+/// Covariance for 6-DOF pose.
+/// Used at
+/// - geometry_msgs/msg/AccelWithCovariance.msg: msg.covariance
+/// - geometry_msgs/msg/TwistWithCovariance.msg: msg.covariance
+/// - geometry_msgs/msg/PoseWithCovariance.msg: msg.covariance
+enum POSE_COV_IDX {
+  X_X = 0,
+  X_Y = 1,
+  X_Z = 2,
+  X_ROLL = 3,
+  X_PITCH = 4,
+  X_YAW = 5,
+  Y_X = 6,
+  Y_Y = 7,
+  Y_Z = 8,
+  Y_ROLL = 9,
+  Y_PITCH = 10,
+  Y_YAW = 11,
+  Z_X = 12,
+  Z_Y = 13,
+  Z_Z = 14,
+  Z_ROLL = 15,
+  Z_PITCH = 16,
+  Z_YAW = 17,
+  ROLL_X = 18,
+  ROLL_Y = 19,
+  ROLL_Z = 20,
+  ROLL_ROLL = 21,
+  ROLL_PITCH = 22,
+  ROLL_YAW = 23,
+  PITCH_X = 24,
+  PITCH_Y = 25,
+  PITCH_Z = 26,
+  PITCH_ROLL = 27,
+  PITCH_PITCH = 28,
+  PITCH_YAW = 29,
+  YAW_X = 30,
+  YAW_Y = 31,
+  YAW_Z = 32,
+  YAW_ROLL = 33,
+  YAW_PITCH = 34,
+  YAW_YAW = 35
+};
+}  // namespace pose_covariance_index
+
+namespace xyz_upper_covariance_index
+{
+/// Upper-triangle covariance about the x, y, z axes
+/// Used at
+/// - radar_msgs/msg/RadarTrack.msg: msg.{position, velocity, acceleration}_covariance
+enum XYZ_UPPER_COV_IDX {
+  X_X = 0,
+  X_Y = 1,
+  X_Z = 2,
+  Y_Y = 3,
+  Y_Z = 4,
+  Z_Z = 5,
+};
+}  // namespace xyz_upper_covariance_index
+}  // namespace tier4_autoware_utils
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__MSG_COVARIANCE_HPP_
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp
index 3f2fed24b7e0b..4f989b7f29bce 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp
@@ -27,6 +27,7 @@
 #include "tier4_autoware_utils/ros/debug_publisher.hpp"
 #include "tier4_autoware_utils/ros/debug_traits.hpp"
 #include "tier4_autoware_utils/ros/marker_helper.hpp"
+#include "tier4_autoware_utils/ros/msg_covariance.hpp"
 #include "tier4_autoware_utils/ros/processing_time_publisher.hpp"
 #include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "tier4_autoware_utils/ros/transform_listener.hpp"