From 041b6cdba9c5a5d9efceafe9e296e3c970ee4f8c Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 6 Oct 2022 13:39:18 +0900 Subject: [PATCH 1/5] fix include path Signed-off-by: scepter914 --- .../perception_utils/include/perception_utils/conversion.hpp | 4 ++-- .../include/perception_utils/predicted_path_utils.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/common/perception_utils/include/perception_utils/conversion.hpp b/common/perception_utils/include/perception_utils/conversion.hpp index 1497114eee532..8aef31a92928e 100644 --- a/common/perception_utils/include/perception_utils/conversion.hpp +++ b/common/perception_utils/include/perception_utils/conversion.hpp @@ -15,8 +15,8 @@ #ifndef PERCEPTION_UTILS__CONVERSION_HPP_ #define PERCEPTION_UTILS__CONVERSION_HPP_ -#include "autoware_auto_perception_msgs/msg/detected_objects.hpp" -#include "autoware_auto_perception_msgs/msg/tracked_objects.hpp" +#include +#include namespace perception_utils { diff --git a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp index c02282f9efeea..cb8ec6bf5ff3e 100644 --- a/common/perception_utils/include/perception_utils/predicted_path_utils.hpp +++ b/common/perception_utils/include/perception_utils/predicted_path_utils.hpp @@ -17,7 +17,7 @@ #include "tier4_autoware_utils/geometry/geometry.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" +#include #include #include From eb4d67a61955cae80e701a227ded9ade3f222d85 Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 6 Oct 2022 13:39:57 +0900 Subject: [PATCH 2/5] move unnamed namespace to src file Signed-off-by: scepter914 --- common/perception_utils/CMakeLists.txt | 1 + .../include/perception_utils/transform.hpp | 57 +------------- common/perception_utils/src/transform.cpp | 78 +++++++++++++++++++ 3 files changed, 81 insertions(+), 55 deletions(-) create mode 100644 common/perception_utils/src/transform.cpp diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index 338874a41a3f7..0c5e3b9c27455 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -9,6 +9,7 @@ find_package(Boost REQUIRED) ament_auto_add_library(perception_utils SHARED src/predicted_path_utils.cpp src/conversion.cpp + src/transform.cpp ) if(BUILD_TESTING) diff --git a/common/perception_utils/include/perception_utils/transform.hpp b/common/perception_utils/include/perception_utils/transform.hpp index 54af7c04a006e..f4991aa901495 100644 --- a/common/perception_utils/include/perception_utils/transform.hpp +++ b/common/perception_utils/include/perception_utils/transform.hpp @@ -15,71 +15,18 @@ #ifndef PERCEPTION_UTILS__TRANSFORM_HPP_ #define PERCEPTION_UTILS__TRANSFORM_HPP_ -#include "geometry_msgs/msg/transform.hpp" - #include #include -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - -#include "boost/optional.hpp" - #include -namespace -{ -[[maybe_unused]] boost::optional getTransform( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); - return boost::none; - } -} -} // namespace - namespace perception_utils { + template bool transformObjects( const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - T & output_msg) -{ - output_msg = input_msg; + T & output_msg); - // transform to world coordinate - if (input_msg.header.frame_id != target_frame_id) { - output_msg.header.frame_id = target_frame_id; - tf2::Transform tf_target2objects_world; - tf2::Transform tf_target2objects; - tf2::Transform tf_objects_world2objects; - { - const auto ros_target2objects_world = - getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); - if (!ros_target2objects_world) { - return false; - } - tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); - } - for (auto & object : output_msg.objects) { - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); - tf_target2objects = tf_target2objects_world * tf_objects_world2objects; - tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); - // TODO(yukkysaito) transform covariance - } - } - return true; } -} // namespace perception_utils - #endif // PERCEPTION_UTILS__TRANSFORM_HPP_ diff --git a/common/perception_utils/src/transform.cpp b/common/perception_utils/src/transform.cpp new file mode 100644 index 0000000000000..fd908ad1685d6 --- /dev/null +++ b/common/perception_utils/src/transform.cpp @@ -0,0 +1,78 @@ + +// 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 "perception_utils/transform.hpp" + +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace +{ +[[maybe_unused]] boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); + return boost::none; + } +} +} // namespace + +namespace perception_utils +{ +template +bool transformObjects( + const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, + T & output_msg) +{ + output_msg = input_msg; + + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); + // TODO(yukkysaito) transform covariance + } + } + return true; +} +} // namespace perception_utils From d7f668c0459506d35409eb4edd26f0dabfde62ac Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 6 Oct 2022 13:52:32 +0900 Subject: [PATCH 3/5] fix template function Signed-off-by: scepter914 --- .../include/perception_utils/transform.hpp | 36 +++++++++++++++- common/perception_utils/src/transform.cpp | 42 ------------------- 2 files changed, 34 insertions(+), 44 deletions(-) diff --git a/common/perception_utils/include/perception_utils/transform.hpp b/common/perception_utils/include/perception_utils/transform.hpp index f4991aa901495..856fa73b02757 100644 --- a/common/perception_utils/include/perception_utils/transform.hpp +++ b/common/perception_utils/include/perception_utils/transform.hpp @@ -15,18 +15,50 @@ #ifndef PERCEPTION_UTILS__TRANSFORM_HPP_ #define PERCEPTION_UTILS__TRANSFORM_HPP_ +#include + #include #include +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + #include namespace perception_utils { - template bool transformObjects( const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - T & output_msg); + T & output_msg) +{ + output_msg = input_msg; + // transform to world coordinate + if (input_msg.header.frame_id != target_frame_id) { + output_msg.header.frame_id = target_frame_id; + tf2::Transform tf_target2objects_world; + tf2::Transform tf_target2objects; + tf2::Transform tf_objects_world2objects; + { + const auto ros_target2objects_world = + getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + if (!ros_target2objects_world) { + return false; + } + tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); + } + for (auto & object : output_msg.objects) { + tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); + tf_target2objects = tf_target2objects_world * tf_objects_world2objects; + tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); + // TODO(yukkysaito) transform covariance + } + } + return true; } +} // namespace perception_utils #endif // PERCEPTION_UTILS__TRANSFORM_HPP_ diff --git a/common/perception_utils/src/transform.cpp b/common/perception_utils/src/transform.cpp index fd908ad1685d6..c6cb3f1bee746 100644 --- a/common/perception_utils/src/transform.cpp +++ b/common/perception_utils/src/transform.cpp @@ -15,14 +15,6 @@ #include "perception_utils/transform.hpp" -#include - -#ifdef ROS_DISTRO_GALACTIC -#include -#else -#include -#endif - #include namespace @@ -42,37 +34,3 @@ namespace } } } // namespace - -namespace perception_utils -{ -template -bool transformObjects( - const T & input_msg, const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer, - T & output_msg) -{ - output_msg = input_msg; - - // transform to world coordinate - if (input_msg.header.frame_id != target_frame_id) { - output_msg.header.frame_id = target_frame_id; - tf2::Transform tf_target2objects_world; - tf2::Transform tf_target2objects; - tf2::Transform tf_objects_world2objects; - { - const auto ros_target2objects_world = - getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); - if (!ros_target2objects_world) { - return false; - } - tf2::fromMsg(*ros_target2objects_world, tf_target2objects_world); - } - for (auto & object : output_msg.objects) { - tf2::fromMsg(object.kinematics.pose_with_covariance.pose, tf_objects_world2objects); - tf_target2objects = tf_target2objects_world * tf_objects_world2objects; - tf2::toMsg(tf_target2objects, object.kinematics.pose_with_covariance.pose); - // TODO(yukkysaito) transform covariance - } - } - return true; -} -} // namespace perception_utils From d4aa0fd73ddd94c358bca163a8ecb39a736b51ea Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 6 Oct 2022 14:11:12 +0900 Subject: [PATCH 4/5] fix unnamed namespace Signed-off-by: scepter914 --- common/perception_utils/CMakeLists.txt | 1 - .../include/perception_utils/transform.hpp | 29 +++++++++++++-- common/perception_utils/src/transform.cpp | 36 ------------------- 3 files changed, 26 insertions(+), 40 deletions(-) delete mode 100644 common/perception_utils/src/transform.cpp diff --git a/common/perception_utils/CMakeLists.txt b/common/perception_utils/CMakeLists.txt index 0c5e3b9c27455..338874a41a3f7 100644 --- a/common/perception_utils/CMakeLists.txt +++ b/common/perception_utils/CMakeLists.txt @@ -9,7 +9,6 @@ find_package(Boost REQUIRED) ament_auto_add_library(perception_utils SHARED src/predicted_path_utils.cpp src/conversion.cpp - src/transform.cpp ) if(BUILD_TESTING) diff --git a/common/perception_utils/include/perception_utils/transform.hpp b/common/perception_utils/include/perception_utils/transform.hpp index 856fa73b02757..b55a101c9ea5b 100644 --- a/common/perception_utils/include/perception_utils/transform.hpp +++ b/common/perception_utils/include/perception_utils/transform.hpp @@ -17,9 +17,10 @@ #include +#include + #include #include - #ifdef ROS_DISTRO_GALACTIC #include #else @@ -28,6 +29,27 @@ #include +// To avoid unnamed namespace function in include file and to write template function in include +// file, avoid unnamed namespace +// "detail" name is used in boost library as same reason. +namespace detail +{ +[[maybe_unused]] inline boost::optional getTransform( + const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, + const std::string & target_frame_id, const rclcpp::Time & time) +{ + try { + geometry_msgs::msg::TransformStamped self_transform_stamped; + self_transform_stamped = tf_buffer.lookupTransform( + target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); + return self_transform_stamped.transform; + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); + return boost::none; + } +} +} // namespace detail + namespace perception_utils { template @@ -44,8 +66,8 @@ bool transformObjects( tf2::Transform tf_target2objects; tf2::Transform tf_objects_world2objects; { - const auto ros_target2objects_world = - getTransform(tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); + const auto ros_target2objects_world = detail::getTransform( + tf_buffer, input_msg.header.frame_id, target_frame_id, input_msg.header.stamp); if (!ros_target2objects_world) { return false; } @@ -61,4 +83,5 @@ bool transformObjects( return true; } } // namespace perception_utils + #endif // PERCEPTION_UTILS__TRANSFORM_HPP_ diff --git a/common/perception_utils/src/transform.cpp b/common/perception_utils/src/transform.cpp deleted file mode 100644 index c6cb3f1bee746..0000000000000 --- a/common/perception_utils/src/transform.cpp +++ /dev/null @@ -1,36 +0,0 @@ - -// 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 "perception_utils/transform.hpp" - -#include - -namespace -{ -[[maybe_unused]] boost::optional getTransform( - const tf2_ros::Buffer & tf_buffer, const std::string & source_frame_id, - const std::string & target_frame_id, const rclcpp::Time & time) -{ - try { - geometry_msgs::msg::TransformStamped self_transform_stamped; - self_transform_stamped = tf_buffer.lookupTransform( - target_frame_id, source_frame_id, time, rclcpp::Duration::from_seconds(0.5)); - return self_transform_stamped.transform; - } catch (tf2::TransformException & ex) { - RCLCPP_WARN_STREAM(rclcpp::get_logger("perception_utils"), ex.what()); - return boost::none; - } -} -} // namespace From 61f3767523b4060d1cafbd34657685be9f8e2f9a Mon Sep 17 00:00:00 2001 From: scepter914 Date: Thu, 6 Oct 2022 15:44:20 +0900 Subject: [PATCH 5/5] delete comment Signed-off-by: scepter914 --- common/perception_utils/include/perception_utils/transform.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/common/perception_utils/include/perception_utils/transform.hpp b/common/perception_utils/include/perception_utils/transform.hpp index b55a101c9ea5b..fe4fced16120f 100644 --- a/common/perception_utils/include/perception_utils/transform.hpp +++ b/common/perception_utils/include/perception_utils/transform.hpp @@ -29,9 +29,6 @@ #include -// To avoid unnamed namespace function in include file and to write template function in include -// file, avoid unnamed namespace -// "detail" name is used in boost library as same reason. namespace detail { [[maybe_unused]] inline boost::optional getTransform(