feat(tier4_autoware_utils): add quaternion operation (#2244)

* fet(tier4_autoware_utils): add quaternion operation

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix file name

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

common/tier4_autoware_utils/CMakeLists.txt

@@ -9,6 +9,7 @@ find_package(Boost REQUIRED)
 ament_auto_add_library(tier4_autoware_utils SHARED
   src/tier4_autoware_utils.cpp
   src/geometry/boost_polygon_utils.cpp
+  src/ros/msg_operation.cpp
 )
 
 if(BUILD_TESTING)

common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp

@@ -0,0 +1,31 @@
+// 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_OPERATION_HPP_
+#define TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_
+
+#include "geometry_msgs/msg/quaternion.hpp"
+
+// NOTE: Do not use tier4_autoware_utils namespace
+namespace geometry_msgs
+{
+namespace msg
+{
+Quaternion operator+(Quaternion a, Quaternion b) noexcept;
+Quaternion operator-(Quaternion a) noexcept;
+Quaternion operator-(Quaternion a, Quaternion b) noexcept;
+}  // namespace msg
+}  // namespace geometry_msgs
+
+#endif  // TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_

common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp

@@ -28,6 +28,7 @@
 #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/msg_operation.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"

common/tier4_autoware_utils/src/ros/msg_operation.cpp

@@ -0,0 +1,55 @@
+// 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.
+
+#include "tier4_autoware_utils/ros/msg_operation.hpp"
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+// NOTE: Do not use tier4_autoware_utils namespace
+namespace geometry_msgs
+{
+namespace msg
+{
+Quaternion operator+(Quaternion a, Quaternion b) noexcept
+{
+  tf2::Quaternion quat_a;
+  tf2::Quaternion quat_b;
+  tf2::fromMsg(a, quat_a);
+  tf2::fromMsg(b, quat_b);
+  return tf2::toMsg(quat_a + quat_b);
+}
+
+Quaternion operator-(Quaternion a) noexcept
+{
+  tf2::Quaternion quat_a;
+  tf2::fromMsg(a, quat_a);
+  return tf2::toMsg(quat_a * -1.0);
+}
+
+Quaternion operator-(Quaternion a, Quaternion b) noexcept
+{
+  tf2::Quaternion quat_a;
+  tf2::Quaternion quat_b;
+  tf2::fromMsg(a, quat_a);
+  tf2::fromMsg(b, quat_b);
+  return tf2::toMsg(quat_a * quat_b.inverse());
+}
+}  // namespace msg
+}  // namespace geometry_msgs