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;