diff --git a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp
index f4f3cd48c1187..1f63ff28401f6 100644
--- a/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp
+++ b/perception/detected_object_validation/include/detected_object_filter/object_lanelet_filter.hpp
@@ -68,9 +68,6 @@ class ObjectLaneletFilterNode : public rclcpp::Node
Filter_target_label filter_target_;
- bool transformDetectedObjects(
- const autoware_auto_perception_msgs::msg::DetectedObjects &, const std::string &,
- const tf2_ros::Buffer &, autoware_auto_perception_msgs::msg::DetectedObjects &);
LinearRing2d getConvexHull(const autoware_auto_perception_msgs::msg::DetectedObjects &);
lanelet::ConstLanelets getIntersectedLanelets(
const LinearRing2d &, const lanelet::ConstLanelets &);
diff --git a/perception/detected_object_validation/package.xml b/perception/detected_object_validation/package.xml
index afba244eb3cb7..b1317d9dcb90d 100644
--- a/perception/detected_object_validation/package.xml
+++ b/perception/detected_object_validation/package.xml
@@ -19,6 +19,7 @@
message_filters
nav_msgs
pcl_conversions
+ perception_utils
rclcpp
rclcpp_components
tf2
diff --git a/perception/detected_object_validation/src/object_lanelet_filter.cpp b/perception/detected_object_validation/src/object_lanelet_filter.cpp
index 4d5f794324195..0c9cacf178292 100644
--- a/perception/detected_object_validation/src/object_lanelet_filter.cpp
+++ b/perception/detected_object_validation/src/object_lanelet_filter.cpp
@@ -14,6 +14,7 @@
#include "detected_object_filter/object_lanelet_filter.hpp"
+#include
#include
#include
@@ -22,21 +23,6 @@
#include
-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*/ target_frame_id, /*src*/ 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("object_lanelet_filter"), ex.what());
- return boost::none;
- }
-}
namespace object_lanelet_filter
{
ObjectLaneletFilterNode::ObjectLaneletFilterNode(const rclcpp::NodeOptions & node_options)
@@ -91,7 +77,7 @@ void ObjectLaneletFilterNode::objectCallback(
return;
}
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
- if (!transformDetectedObjects(*input_msg, "map", tf_buffer_, transformed_objects)) {
+ if (!perception_utils::transformObjects(*input_msg, "map", tf_buffer_, transformed_objects)) {
RCLCPP_ERROR(get_logger(), "Failed transform to map.");
return;
}
@@ -131,36 +117,6 @@ void ObjectLaneletFilterNode::objectCallback(
object_pub_->publish(output_object_msg);
}
-bool ObjectLaneletFilterNode::transformDetectedObjects(
- const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
- const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
- autoware_auto_perception_msgs::msg::DetectedObjects & 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);
- }
- }
- return true;
-}
-
LinearRing2d ObjectLaneletFilterNode::getConvexHull(
const autoware_auto_perception_msgs::msg::DetectedObjects & detected_objects)
{
diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
index 68c4d20010916..5789b1de23339 100644
--- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
+++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
@@ -14,6 +14,7 @@
#include "obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp"
+#include
#include
#include
@@ -34,54 +35,6 @@
namespace
{
-std::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*/ target_frame_id, /*src*/ 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("obstacle_pointcloud_based_validator"), ex.what());
- return std::nullopt;
- }
-}
-
-bool transformDetectedObjects(
- const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
- const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
- autoware_auto_perception_msgs::msg::DetectedObjects & 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 (size_t i = 0; i < output_msg.objects.size(); ++i) {
- tf2::fromMsg(
- output_msg.objects.at(i).kinematics.pose_with_covariance.pose, tf_objects_world2objects);
- tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
- tf2::toMsg(tf_target2objects, output_msg.objects.at(i).kinematics.pose_with_covariance.pose);
- // Note: Covariance is not transformed.
- }
- }
- return true;
-}
-
inline pcl::PointXY toPCL(const double x, const double y)
{
pcl::PointXY pcl_point;
@@ -152,7 +105,7 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
// Transform to pointcloud frame
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
- if (!transformDetectedObjects(
+ if (!perception_utils::transformObjects(
*input_objects, input_obstacle_pointcloud->header.frame_id, tf_buffer_,
transformed_objects)) {
// objects_pub_->publish(*input_objects);
diff --git a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp
index e66070613a98a..dcaf137a15c16 100644
--- a/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp
+++ b/perception/detected_object_validation/src/occupancy_grid_based_validator.cpp
@@ -14,6 +14,7 @@
#include "occupancy_grid_based_validator/occupancy_grid_based_validator.hpp"
+#include
#include
#include
@@ -28,58 +29,6 @@
#include
#include
-namespace
-{
-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*/ target_frame_id, /*src*/ 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("multi_object_tracker"), ex.what());
- return boost::none;
- }
-}
-
-bool transformDetectedObjects(
- const autoware_auto_perception_msgs::msg::DetectedObjects & input_msg,
- const std::string & target_frame_id, const tf2_ros::Buffer & tf_buffer,
- autoware_auto_perception_msgs::msg::DetectedObjects & 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 (size_t i = 0; i < output_msg.objects.size(); ++i) {
- tf2::fromMsg(
- output_msg.objects.at(i).kinematics.pose_with_covariance.pose, tf_objects_world2objects);
- tf_target2objects = tf_target2objects_world * tf_objects_world2objects;
- tf2::toMsg(tf_target2objects, output_msg.objects.at(i).kinematics.pose_with_covariance.pose);
- // Note: Covariance is not transformed.
- }
- }
- return true;
-}
-
-} // namespace
-
namespace occupancy_grid_based_validator
{
using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
@@ -113,7 +62,7 @@ void OccupancyGridBasedValidator::onObjectsAndOccGrid(
// Transform to occ grid frame
autoware_auto_perception_msgs::msg::DetectedObjects transformed_objects;
- if (!transformDetectedObjects(
+ if (!perception_utils::transformObjects(
*input_objects, input_occ_grid->header.frame_id, tf_buffer_, transformed_objects))
return;