From 19ea1014b65bd3aad71f612a508fd5d42cf0b8b1 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 3 Jun 2024 21:16:53 +0900 Subject: [PATCH] feat!: remove autoware_auto_tf2 package (#7218) * feat!: remove autoware_auto_geometry package Signed-off-by: Ryohsuke Mitsudome * docs: remove autoware_auto_geometry package from docs Signed-off-by: Ryohsuke Mitsudome * feat!: remove autoware_auto_tf2 package Signed-off-by: Ryohsuke Mitsudome * fix: remove from autoware_auto_tf2 packages from docs page Signed-off-by: Ryohsuke Mitsudome --------- Signed-off-by: Ryohsuke Mitsudome --- .github/CODEOWNERS | 1 - common/.pages | 1 - common/autoware_auto_tf2/CMakeLists.txt | 22 -- .../design/autoware-auto-tf2-design.md | 183 ---------- .../tf2_autoware_auto_msgs.hpp | 147 -------- common/autoware_auto_tf2/package.xml | 32 -- .../test/test_tf2_autoware_auto_msgs.cpp | 318 ------------------ .../package.xml | 1 - planning/behavior_path_planner/package.xml | 1 - .../behavior_path_planner_common/package.xml | 1 - .../package.xml | 1 - .../package.xml | 1 - .../src/scene_crosswalk.cpp | 1 - .../package.xml | 1 - .../surround_obstacle_checker/package.xml | 1 - .../surround_obstacle_checker/src/node.cpp | 1 - .../sim_model_delay_steer_map_acc_geared.hpp | 2 - .../simple_planning_simulator/package.xml | 1 - .../simple_planning_simulator_core.cpp | 1 - 19 files changed, 717 deletions(-) delete mode 100755 common/autoware_auto_tf2/CMakeLists.txt delete mode 100644 common/autoware_auto_tf2/design/autoware-auto-tf2-design.md delete mode 100644 common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp delete mode 100644 common/autoware_auto_tf2/package.xml delete mode 100644 common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp diff --git a/.github/CODEOWNERS b/.github/CODEOWNERS index b7fc70f042251..dd73e823b6c05 100644 --- a/.github/CODEOWNERS +++ b/.github/CODEOWNERS @@ -2,7 +2,6 @@ common/autoware_ad_api_specs/** isamu.takagi@tier4.jp ryohsuke.mitsudome@tier4.jp common/autoware_auto_common/** opensource@apex.ai satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_perception_rviz_plugin/** opensource@apex.ai satoshi.tanaka@tier4.jp shunsuke.miura@tier4.jp taiki.tanaka@tier4.jp takeshi.miura@tier4.jp yoshi.ri@tier4.jp -common/autoware_auto_tf2/** jit.ray.c@gmail.com satoshi.ota@tier4.jp shumpei.wakabayashi@tier4.jp tomoya.kimura@tier4.jp common/autoware_overlay_rviz_plugin/autoware_mission_details_overlay_rviz_plugin/** ahmed.ebrahim@leodrive.ai common/autoware_overlay_rviz_plugin/autoware_overlay_rviz_plugin/** khalil@leodrive.ai common/autoware_point_types/** taichi.higashide@tier4.jp diff --git a/common/.pages b/common/.pages index 105466bcba763..38cffcb1dc027 100644 --- a/common/.pages +++ b/common/.pages @@ -6,7 +6,6 @@ nav: - 'Common Libraries': - 'autoware_auto_common': - 'comparisons': common/autoware_auto_common/design/comparisons - - 'autoware_auto_tf2': common/autoware_auto_tf2/design/autoware-auto-tf2-design - 'autoware_point_types': common/autoware_point_types - 'Cuda Utils': common/cuda_utils - 'Geography Utils': common/geography_utils diff --git a/common/autoware_auto_tf2/CMakeLists.txt b/common/autoware_auto_tf2/CMakeLists.txt deleted file mode 100755 index a8ae9ec2d962e..0000000000000 --- a/common/autoware_auto_tf2/CMakeLists.txt +++ /dev/null @@ -1,22 +0,0 @@ -cmake_minimum_required(VERSION 3.14) -project(autoware_auto_tf2) - -find_package(autoware_cmake REQUIRED) -autoware_package() - -if(BUILD_TESTING) - ament_add_ros_isolated_gtest(test_tf2_autoware_auto_msgs test/test_tf2_autoware_auto_msgs.cpp) - target_include_directories(test_tf2_autoware_auto_msgs PRIVATE "include") - ament_target_dependencies(test_tf2_autoware_auto_msgs - "autoware_auto_common" - "autoware_auto_perception_msgs" - "autoware_auto_system_msgs" - "autoware_auto_geometry_msgs" - "geometry_msgs" - "tf2" - "tf2_geometry_msgs" - "tf2_ros" - ) -endif() - -ament_auto_package() diff --git a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md b/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md deleted file mode 100644 index aaa719d617373..0000000000000 --- a/common/autoware_auto_tf2/design/autoware-auto-tf2-design.md +++ /dev/null @@ -1,183 +0,0 @@ -# autoware_auto_tf2 - -This is the design document for the `autoware_auto_tf2` package. - -## Purpose / Use cases - -In general, users of ROS rely on tf (and its successor, tf2) for publishing and utilizing coordinate -frame transforms. This is true even to the extent that the tf2 contains the packages -`tf2_geometry_msgs` and `tf2_sensor_msgs` which allow for easy conversion to and from the message -types defined in `geometry_msgs` and `sensor_msgs`, respectively. However, AutowareAuto contains -some specialized message types which are not transformable between frames using the ROS 2 library. -The `autoware_auto_tf2` package aims to provide developers with tools to transform applicable -`autoware_auto_msgs` types. In addition to this, this package also provides transform tools for -messages types in `geometry_msgs` missing in `tf2_geometry_msgs`. - -## Design - -While writing `tf2_some_msgs` or contributing to `tf2_geometry_msgs`, compatibility and design -intent was ensured with the following files in the existing tf2 framework: - -- `tf2/convert.h` -- `tf2_ros/buffer_interface.h` - -For example: - -```cpp -void tf2::convert( const A & a,B & b) -``` - -The method `tf2::convert` is dependent on the following: - -```cpp -template - B tf2::toMsg(const A& a); -template - void tf2::fromMsg(const A&, B& b); - -// New way to transform instead of using tf2::doTransform() directly -tf2_ros::BufferInterface::transform(...) -``` - -Which, in turn, is dependent on the following: - -```cpp -void tf2::convert( const A & a,B & b) -const std::string& tf2::getFrameId(const T& t) -const ros::Time& tf2::getTimestamp(const T& t); -``` - -## Current Implementation of tf2_geometry_msgs - -In both ROS 1 and ROS 2 stamped msgs like `Vector3Stamped`, `QuaternionStamped` have associated -functions like: - -- `getTimestamp` -- `getFrameId` -- `doTransform` -- `toMsg` -- `fromMsg` - -In ROS 1, to support `tf2::convert` and need in `doTransform` of the stamped data, non-stamped -underlying data like `Vector3`, `Point`, have implementations of the following functions: - -- `toMsg` -- `fromMsg` - -In ROS 2, much of the `doTransform` method is not using `toMsg` and `fromMsg` as data types from tf2 -are not used. Instead `doTransform` is done using `KDL`, thus functions relating to underlying data -were not added; such as `Vector3`, `Point`, or ported in this commit ros/geometry2/commit/6f2a82. -The non-stamped data with `toMsg` and `fromMsg` are `Quaternion`, `Transform`. `Pose` has the -modified `toMsg` and not used by `PoseStamped`. - -## Plan for autoware_auto_perception_msgs::msg::BoundingBoxArray - -The initial rough plan was to implement some of the common tf2 functions like `toMsg`, `fromMsg`, -and `doTransform`, as needed for all the underlying data types in `BoundingBoxArray`. Examples -of the data types include: `BoundingBox`, `Quaternion32`, and `Point32`. In addition, the -implementation should be done such that upstream contributions could also be made to `geometry_msgs`. - -## Assumptions / Known limits - -Due to conflicts in a function signatures, the predefined template of `convert.h`/ -`transform_functions.h` is not followed and compatibility with `tf2::convert(..)` is broken and -`toMsg` is written differently. - -```cpp -// Old style -geometry_msgs::Vector3 toMsg(const tf2::Vector3& in) -geometry_msgs::Point& toMsg(const tf2::Vector3& in) - -// New style -geometry_msgs::Point& toMsg(const tf2::Vector3& in, geometry_msgs::Point& out) -``` - -## Inputs / Outputs / API - - - -The library provides API `doTransform` for the following data-types that are either not available -in `tf2_geometry_msgs` or the messages types are part of `autoware_auto_msgs` and are therefore -custom and not inherently supported by any of the tf2 libraries. The following APIs are provided -for the following data types: - -- `Point32` - -```cpp -inline void doTransform( - const geometry_msgs::msg::Point32 & t_in, - geometry_msgs::msg::Point32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `Quaternion32` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBox` (`autoware_auto_msgs`) - -```cpp -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -- `BoundingBoxArray` - -```cpp -inline void doTransform( - const BoundingBoxArray & t_in, - BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -``` - -In addition, the following helper methods are also added: - -- `BoundingBoxArray` - -```cpp -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) - -inline std::string getFrameId(const BoundingBoxArray & t) -``` - - - - - - - - - - - - - - -## Future extensions / Unimplemented parts - -## Challenges - -- `tf2_geometry_msgs` does not implement `doTransform` for any non-stamped data types, but it is - possible with the same function template. It is needed when transforming sub-data, with main data - that does have a stamp and can call doTransform on the sub-data with the same transform. Is this a useful upstream contribution? -- `tf2_geometry_msgs` does not have `Point`, `Point32`, does not seem it needs one, also the - implementation of non-standard `toMsg` would not help the convert. -- `BoundingBox` uses 32-bit float like `Quaternion32` and `Point32` to save space, as they are used - repeatedly in `BoundingBoxArray`. While transforming is it better to convert to 64-bit `Quaternion`, - `Point`, or `PoseStamped`, to re-use existing implementation of `doTransform`, or does it need to be - implemented? It may not be simple to template. - - - diff --git a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp b/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp deleted file mode 100644 index 50c16ba8eaf7d..0000000000000 --- a/common/autoware_auto_tf2/include/autoware_auto_tf2/tf2_autoware_auto_msgs.hpp +++ /dev/null @@ -1,147 +0,0 @@ -// Copyright 2020, 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. -/// \file -/// \brief This file includes common transform functionality for autoware_auto_msgs - -#ifndef AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ -#define AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ - -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include - -using autoware::common::types::float32_t; -using autoware::common::types::float64_t; -using BoundingBoxArray = autoware_auto_perception_msgs::msg::BoundingBoxArray; -using BoundingBox = autoware_auto_perception_msgs::msg::BoundingBox; - -namespace tf2 -{ -/******************/ -/** Quaternion32 **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs Quaternion32 type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The Quaternion32 message to transform. - * \param t_out The transformed Quaternion32 message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const autoware_auto_geometry_msgs::msg::Quaternion32 & t_in, - autoware_auto_geometry_msgs::msg::Quaternion32 & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - KDL::Rotation r_in = KDL::Rotation::Quaternion(t_in.x, t_in.y, t_in.z, t_in.w); - KDL::Rotation out = gmTransformToKDL(transform).M * r_in; - - double qx, qy, qz, qw; - out.GetQuaternion(qx, qy, qz, qw); - t_out.x = static_cast(qx); - t_out.y = static_cast(qy); - t_out.z = static_cast(qz); - t_out.w = static_cast(qw); -} - -/******************/ -/** BoundingBox **/ -/******************/ - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBox type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBox message to transform. - * \param t_out The transformed BoundingBox message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBox & t_in, BoundingBox & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - doTransform(t_in.orientation, t_out.orientation, transform); - doTransform(t_in.centroid, t_out.centroid, transform); - doTransform(t_in.corners[0], t_out.corners[0], transform); - doTransform(t_in.corners[1], t_out.corners[1], transform); - doTransform(t_in.corners[2], t_out.corners[2], transform); - doTransform(t_in.corners[3], t_out.corners[3], transform); - // TODO(jitrc): add conversion for other fields of BoundingBox, such as heading, variance, size -} - -/**********************/ -/** BoundingBoxArray **/ -/**********************/ - -/** \brief Extract a timestamp from the header of a BoundingBoxArray message. - * This function is a specialization of the getTimestamp template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the timestamp from. - * \return The timestamp of the message. - */ -template <> -inline tf2::TimePoint getTimestamp(const BoundingBoxArray & t) -{ - return tf2_ros::fromMsg(t.header.stamp); -} - -/** \brief Extract a frame ID from the header of a BoundingBoxArray message. - * This function is a specialization of the getFrameId template defined in tf2/convert.h. - * \param t A timestamped BoundingBoxArray message to extract the frame ID from. - * \return A string containing the frame ID of the message. - */ -template <> -inline std::string getFrameId(const BoundingBoxArray & t) -{ - return t.header.frame_id; -} - -/** \brief Apply a geometry_msgs TransformStamped to an autoware_auto_msgs BoundingBoxArray type. - * This function is a specialization of the doTransform template defined in tf2/convert.h. - * \param t_in The BoundingBoxArray to transform, as a timestamped BoundingBoxArray message. - * \param t_out The transformed BoundingBoxArray, as a timestamped BoundingBoxArray message. - * \param transform The timestamped transform to apply, as a TransformStamped message. - */ -template <> -inline void doTransform( - const BoundingBoxArray & t_in, BoundingBoxArray & t_out, - const geometry_msgs::msg::TransformStamped & transform) -{ - t_out = t_in; - for (auto idx = 0U; idx < t_in.boxes.size(); idx++) { - doTransform(t_out.boxes[idx], t_out.boxes[idx], transform); - } - t_out.header.stamp = transform.header.stamp; - t_out.header.frame_id = transform.header.frame_id; -} - -} // namespace tf2 - -#endif // AUTOWARE_AUTO_TF2__TF2_AUTOWARE_AUTO_MSGS_HPP_ diff --git a/common/autoware_auto_tf2/package.xml b/common/autoware_auto_tf2/package.xml deleted file mode 100644 index b84e2d77befda..0000000000000 --- a/common/autoware_auto_tf2/package.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - autoware_auto_tf2 - 1.0.0 - Transform related utilities for different msg types - Jit Ray Chowdhury - Tomoya Kimura - Shumpei Wakabayashi - Satoshi Ota - Apache License 2.0 - - ament_cmake - autoware_cmake - - autoware_auto_common - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_system_msgs - geometry_msgs - tf2 - tf2_geometry_msgs - tf2_ros - - ament_cmake_ros - ament_lint_auto - autoware_lint_common - - - ament_cmake - - diff --git a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp b/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp deleted file mode 100644 index ce81dd2276870..0000000000000 --- a/common/autoware_auto_tf2/test/test_tf2_autoware_auto_msgs.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2020, 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. -/// \file -/// \brief This file includes common transform functionally for autoware_auto_msgs - -#include -#include - -#include -#include -#include - -#include - -std::unique_ptr tf_buffer = nullptr; -constexpr double EPS = 1e-3; - -geometry_msgs::msg::TransformStamped filled_transform() -{ - geometry_msgs::msg::TransformStamped t; - t.transform.translation.x = 10; - t.transform.translation.y = 20; - t.transform.translation.z = 30; - t.transform.rotation.w = 0; - t.transform.rotation.x = 1; - t.transform.rotation.y = 0; - t.transform.rotation.z = 0; - t.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - t.header.frame_id = "A"; - t.child_frame_id = "B"; - - return t; -} - -TEST(Tf2AutowareAuto, DoTransformPoint32) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - - // doTransform - geometry_msgs::msg::Point32 p_out; - tf2::doTransform(p1, p_out, trans); - - EXPECT_NEAR(p_out.x, 11, EPS); - EXPECT_NEAR(p_out.y, 18, EPS); - EXPECT_NEAR(p_out.z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformPolygon) -{ - const auto trans = filled_transform(); - geometry_msgs::msg::Polygon poly; - geometry_msgs::msg::Point32 p1; - p1.x = 1; - p1.y = 2; - p1.z = 3; - poly.points.push_back(p1); - // doTransform - geometry_msgs::msg::Polygon poly_out; - tf2::doTransform(poly, poly_out, trans); - - EXPECT_NEAR(poly_out.points[0].x, 11, EPS); - EXPECT_NEAR(poly_out.points[0].y, 18, EPS); - EXPECT_NEAR(poly_out.points[0].z, 27, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformQuaternion32) -{ - const auto trans = filled_transform(); - autoware_auto_geometry_msgs::msg::Quaternion32 q1; - q1.w = 0; - q1.x = 0; - q1.y = 0; - q1.z = 1; - - // doTransform - autoware_auto_geometry_msgs::msg::Quaternion32 q_out; - tf2::doTransform(q1, q_out, trans); - - EXPECT_NEAR(q_out.x, 0.0, EPS); - EXPECT_NEAR(q_out.y, 1.0, EPS); - EXPECT_NEAR(q_out.z, 0.0, EPS); - EXPECT_NEAR(q_out.w, 0.0, EPS); -} - -TEST(Tf2AutowareAuto, DoTransformBoundingBox) -{ - const auto trans = filled_transform(); - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 1; - bb1.centroid.y = 2; - bb1.centroid.z = 3; - bb1.corners[0].x = 4; - bb1.corners[0].y = 5; - bb1.corners[0].z = 6; - bb1.corners[1].x = 7; - bb1.corners[1].y = 8; - bb1.corners[1].z = 9; - bb1.corners[2].x = 10; - bb1.corners[2].y = 11; - bb1.corners[2].z = 12; - bb1.corners[3].x = 13; - bb1.corners[3].y = 14; - bb1.corners[3].z = 15; - - // doTransform - BoundingBox bb_out; - tf2::doTransform(bb1, bb_out, trans); - - EXPECT_NEAR(bb_out.orientation.x, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.y, 1.0, EPS); - EXPECT_NEAR(bb_out.orientation.z, 0.0, EPS); - EXPECT_NEAR(bb_out.orientation.w, 0.0, EPS); - EXPECT_NEAR(bb_out.centroid.x, 11, EPS); - EXPECT_NEAR(bb_out.centroid.y, 18, EPS); - EXPECT_NEAR(bb_out.centroid.z, 27, EPS); - EXPECT_NEAR(bb_out.corners[0].x, 14, EPS); - EXPECT_NEAR(bb_out.corners[0].y, 15, EPS); - EXPECT_NEAR(bb_out.corners[0].z, 24, EPS); - EXPECT_NEAR(bb_out.corners[1].x, 17, EPS); - EXPECT_NEAR(bb_out.corners[1].y, 12, EPS); - EXPECT_NEAR(bb_out.corners[1].z, 21, EPS); - EXPECT_NEAR(bb_out.corners[2].x, 20, EPS); - EXPECT_NEAR(bb_out.corners[2].y, 9, EPS); - EXPECT_NEAR(bb_out.corners[2].z, 18, EPS); - EXPECT_NEAR(bb_out.corners[3].x, 23, EPS); - EXPECT_NEAR(bb_out.corners[3].y, 6, EPS); - EXPECT_NEAR(bb_out.corners[3].z, 15, EPS); - - // Testing unused fields are unmodified - EXPECT_EQ(bb_out.size, bb1.size); - EXPECT_EQ(bb_out.heading, bb1.heading); - EXPECT_EQ(bb_out.heading_rate, bb1.heading_rate); - EXPECT_EQ(bb_out.variance, bb1.variance); - EXPECT_EQ(bb_out.vehicle_label, bb1.vehicle_label); - EXPECT_EQ(bb_out.signal_label, bb1.signal_label); - EXPECT_EQ(bb_out.class_likelihood, bb1.class_likelihood); -} - -TEST(Tf2AutowareAuto, TransformBoundingBoxArray) -{ - BoundingBox bb1; - bb1.orientation.w = 0; - bb1.orientation.x = 0; - bb1.orientation.y = 0; - bb1.orientation.z = 1; - bb1.centroid.x = 20; - bb1.centroid.y = 21; - bb1.centroid.z = 22; - bb1.corners[0].x = 23; - bb1.corners[0].y = 24; - bb1.corners[0].z = 25; - bb1.corners[1].x = 26; - bb1.corners[1].y = 27; - bb1.corners[1].z = 28; - bb1.corners[2].x = 29; - bb1.corners[2].y = 30; - bb1.corners[2].z = 31; - bb1.corners[3].x = 32; - bb1.corners[3].y = 33; - bb1.corners[3].z = 34; - - BoundingBox bb2; - bb2.orientation.w = 0.707f; - bb2.orientation.x = -0.706f; - bb2.orientation.y = 0; - bb2.orientation.z = 0; - bb2.centroid.x = 50; - bb2.centroid.y = 51; - bb2.centroid.z = 52; - bb2.corners[0].x = 53; - bb2.corners[0].y = 54; - bb2.corners[0].z = 55; - bb2.corners[1].x = 56; - bb2.corners[1].y = 57; - bb2.corners[1].z = 58; - bb2.corners[2].x = 59; - bb2.corners[2].y = 50; - bb2.corners[2].z = 51; - bb2.corners[3].x = 52; - bb2.corners[3].y = 53; - bb2.corners[3].z = 54; - - BoundingBoxArray bba1; - bba1.header.stamp = tf2_ros::toMsg(tf2::timeFromSec(2)); - bba1.header.frame_id = "A"; - bba1.boxes.push_back(bb1); - bba1.boxes.push_back(bb2); - - // simple api - const auto bba_simple = tf_buffer->transform(bba1, "B", tf2::durationFromSec(2.0)); - - EXPECT_EQ(bba_simple.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_simple.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_simple.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_simple.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_simple.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_simple.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_simple.boxes[1].corners[3].z, -24, EPS); - - // advanced api - const auto bba_advanced = - tf_buffer->transform(bba1, "B", tf2::timeFromSec(2.0), "A", tf2::durationFromSec(3.0)); - - EXPECT_EQ(bba_advanced.header.frame_id, "B"); - - // checking boxes[0] - EXPECT_NEAR(bba_advanced.boxes[0].orientation.x, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.y, 1.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].orientation.w, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.x, 10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.y, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].centroid.z, 8, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].x, 13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].y, -4, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[0].z, 5, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].x, 16, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].y, -7, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[1].z, 2, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].x, 19, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].y, -10, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[2].z, -1, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].x, 22, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].y, -13, EPS); - EXPECT_NEAR(bba_advanced.boxes[0].corners[3].z, -4, EPS); - - // checking boxes[1] - EXPECT_NEAR(bba_advanced.boxes[1].orientation.x, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.y, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.z, 0.0, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].orientation.w, 0.707, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.x, 40, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.y, -31, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].centroid.z, -22, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].x, 43, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].y, -34, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[0].z, -25, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].x, 46, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].y, -37, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[1].z, -28, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].x, 49, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].y, -30, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[2].z, -21, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].x, 42, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].y, -33, EPS); - EXPECT_NEAR(bba_advanced.boxes[1].corners[3].z, -24, EPS); -} - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - rclcpp::Clock::SharedPtr clock = std::make_shared(RCL_SYSTEM_TIME); - tf_buffer = std::make_unique(clock); - tf_buffer->setUsingDedicatedThread(true); - - // populate buffer - const geometry_msgs::msg::TransformStamped t = filled_transform(); - tf_buffer->setTransform(t, "test"); - - int ret = RUN_ALL_TESTS(); - return ret; -} diff --git a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml index 3643257d243b4..f243e166e6790 100644 --- a/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml +++ b/planning/autoware_behavior_path_static_obstacle_avoidance_module/package.xml @@ -23,7 +23,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs behavior_path_planner behavior_path_planner_common diff --git a/planning/behavior_path_planner/package.xml b/planning/behavior_path_planner/package.xml index 14220c09880de..91a2ef5951bda 100644 --- a/planning/behavior_path_planner/package.xml +++ b/planning/behavior_path_planner/package.xml @@ -39,7 +39,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs autoware_perception_msgs autoware_planning_test_manager diff --git a/planning/behavior_path_planner_common/package.xml b/planning/behavior_path_planner_common/package.xml index 7bc40974b9a62..cc6bb451719c8 100644 --- a/planning/behavior_path_planner_common/package.xml +++ b/planning/behavior_path_planner_common/package.xml @@ -45,7 +45,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs freespace_planning_algorithms geometry_msgs diff --git a/planning/behavior_path_sampling_planner_module/package.xml b/planning/behavior_path_sampling_planner_module/package.xml index f016cb6de1e7c..c3c5624f0d182 100644 --- a/planning/behavior_path_sampling_planner_module/package.xml +++ b/planning/behavior_path_sampling_planner_module/package.xml @@ -15,7 +15,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs autoware_perception_msgs autoware_planning_test_manager diff --git a/planning/behavior_velocity_crosswalk_module/package.xml b/planning/behavior_velocity_crosswalk_module/package.xml index 2c38a9836e0af..90b5592bb3ddb 100644 --- a/planning/behavior_velocity_crosswalk_module/package.xml +++ b/planning/behavior_velocity_crosswalk_module/package.xml @@ -23,7 +23,6 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs behavior_velocity_planner_common eigen diff --git a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp index 0f9c753523f13..721af25e75ece 100644 --- a/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp +++ b/planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp @@ -16,7 +16,6 @@ #include "occluded_crosswalk.hpp" -#include #include #include #include diff --git a/planning/behavior_velocity_planner_common/package.xml b/planning/behavior_velocity_planner_common/package.xml index ee3e33f6e2987..30916709c82f7 100644 --- a/planning/behavior_velocity_planner_common/package.xml +++ b/planning/behavior_velocity_planner_common/package.xml @@ -22,7 +22,6 @@ autoware_adapi_v1_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_perception_msgs autoware_velocity_smoother diagnostic_msgs diff --git a/planning/surround_obstacle_checker/package.xml b/planning/surround_obstacle_checker/package.xml index 1f99ba8d2fcba..49165f5475ad2 100644 --- a/planning/surround_obstacle_checker/package.xml +++ b/planning/surround_obstacle_checker/package.xml @@ -16,7 +16,6 @@ autoware_adapi_v1_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs - autoware_auto_tf2 diagnostic_msgs eigen libpcl-all-dev diff --git a/planning/surround_obstacle_checker/src/node.cpp b/planning/surround_obstacle_checker/src/node.cpp index 4c7d0589f843b..e499d6b43dea3 100644 --- a/planning/surround_obstacle_checker/src/node.cpp +++ b/planning/surround_obstacle_checker/src/node.cpp @@ -14,7 +14,6 @@ #include "surround_obstacle_checker/node.hpp" -#include #include #include #include diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp index d7ca032aa6c48..4ac91eadc0593 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_map_acc_geared.hpp @@ -21,8 +21,6 @@ #include "simple_planning_simulator/utils/csv_loader.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include - #include #include #include diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 7618ef4204335..ef80cea466277 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -20,7 +20,6 @@ autoware_auto_control_msgs autoware_auto_mapping_msgs autoware_auto_planning_msgs - autoware_auto_tf2 autoware_auto_vehicle_msgs geometry_msgs lanelet2_core diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 7fba0431706f0..2d7325d90eb89 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -14,7 +14,6 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "rclcpp_components/register_node_macro.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp"