From d9569988fb1ed1f5d5c90bddfc1d280f47def90b Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Nov 2022 15:15:25 +0900 Subject: [PATCH 1/3] fet(tier4_autoware_utils): add quaternion operation Signed-off-by: Takayuki Murooka --- common/tier4_autoware_utils/CMakeLists.txt | 1 + .../tier4_autoware_utils/ros/operation.hpp | 31 +++++++++++ .../tier4_autoware_utils.hpp | 1 + .../src/ros/operation.cpp | 55 +++++++++++++++++++ 4 files changed, 88 insertions(+) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp create mode 100644 common/tier4_autoware_utils/src/ros/operation.cpp diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index 41497402e2d48..cf2bbf653ff84 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/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/operation.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp new file mode 100644 index 0000000000000..6f26fd91af7a6 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/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__OPERATION_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__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__OPERATION_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 4f989b7f29bce..654145b6efc3a 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 @@ -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/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" diff --git a/common/tier4_autoware_utils/src/ros/operation.cpp b/common/tier4_autoware_utils/src/ros/operation.cpp new file mode 100644 index 0000000000000..26e22c453e870 --- /dev/null +++ b/common/tier4_autoware_utils/src/ros/operation.cpp @@ -0,0 +1,55 @@ +// Copyright 2019-2021 the 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 "tier4_autoware_utils/ros/operation.hpp" + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#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 From fe891f57b748c989ca3fc7e5e5ec65f20552bd8c Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Nov 2022 15:22:18 +0900 Subject: [PATCH 2/3] fix Signed-off-by: Takayuki Murooka --- common/tier4_autoware_utils/src/ros/operation.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/common/tier4_autoware_utils/src/ros/operation.cpp b/common/tier4_autoware_utils/src/ros/operation.cpp index 26e22c453e870..6de13324bdb45 100644 --- a/common/tier4_autoware_utils/src/ros/operation.cpp +++ b/common/tier4_autoware_utils/src/ros/operation.cpp @@ -1,4 +1,4 @@ -// Copyright 2019-2021 the Autoware Foundation +// 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. From 6a310b7138c36c1d011a4458e1bf31dbc770d141 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 8 Nov 2022 15:33:55 +0900 Subject: [PATCH 3/3] fix file name Signed-off-by: Takayuki Murooka --- common/tier4_autoware_utils/CMakeLists.txt | 2 +- .../ros/{operation.hpp => msg_operation.hpp} | 6 +++--- .../include/tier4_autoware_utils/tier4_autoware_utils.hpp | 2 +- .../src/ros/{operation.cpp => msg_operation.cpp} | 2 +- 4 files changed, 6 insertions(+), 6 deletions(-) rename common/tier4_autoware_utils/include/tier4_autoware_utils/ros/{operation.hpp => msg_operation.hpp} (85%) rename common/tier4_autoware_utils/src/ros/{operation.cpp => msg_operation.cpp} (96%) diff --git a/common/tier4_autoware_utils/CMakeLists.txt b/common/tier4_autoware_utils/CMakeLists.txt index cf2bbf653ff84..f889e067ea5b6 100644 --- a/common/tier4_autoware_utils/CMakeLists.txt +++ b/common/tier4_autoware_utils/CMakeLists.txt @@ -9,7 +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/operation.cpp + src/ros/msg_operation.cpp ) if(BUILD_TESTING) diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp similarity index 85% rename from common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp rename to common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp index 6f26fd91af7a6..4ac5dc05204f9 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/operation.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/msg_operation.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef TIER4_AUTOWARE_UTILS__ROS__OPERATION_HPP_ -#define TIER4_AUTOWARE_UTILS__ROS__OPERATION_HPP_ +#ifndef TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_HPP_ #include "geometry_msgs/msg/quaternion.hpp" @@ -28,4 +28,4 @@ Quaternion operator-(Quaternion a, Quaternion b) noexcept; } // namespace msg } // namespace geometry_msgs -#endif // TIER4_AUTOWARE_UTILS__ROS__OPERATION_HPP_ +#endif // TIER4_AUTOWARE_UTILS__ROS__MSG_OPERATION_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 654145b6efc3a..35b4c7b836b46 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 @@ -28,7 +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/operation.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" diff --git a/common/tier4_autoware_utils/src/ros/operation.cpp b/common/tier4_autoware_utils/src/ros/msg_operation.cpp similarity index 96% rename from common/tier4_autoware_utils/src/ros/operation.cpp rename to common/tier4_autoware_utils/src/ros/msg_operation.cpp index 6de13324bdb45..cc1a59317a8e0 100644 --- a/common/tier4_autoware_utils/src/ros/operation.cpp +++ b/common/tier4_autoware_utils/src/ros/msg_operation.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "tier4_autoware_utils/ros/operation.hpp" +#include "tier4_autoware_utils/ros/msg_operation.hpp" #include