diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp index 677977ecdfa49..29f5b372a46c6 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.cpp @@ -20,16 +20,26 @@ #include #include #include - +#ifdef ROS_DISTRO_GALACTIC +#include +#include +#else +#include +#include +#endif #include #include +#include +#include namespace autoware::compare_map_segmentation { VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( const rclcpp::NodeOptions & options) -: Filter("VoxelBasedCompareMapFilter", options) +: Filter("VoxelBasedCompareMapFilter", options), + tf_buffer_(this->get_clock()), + tf_listener_(tf_buffer_) { // initialize debug tool { @@ -62,6 +72,111 @@ VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent( RCLCPP_INFO(this->get_logger(), "tf_map_input_frame: %s", tf_input_frame_.c_str()); } +// TODO(badai-nguyen): Temporary Implementation of input_indices_callback and convert_output_costly +// functions; Delete this override function when autoware_utils refactor +// (https://github.com/autowarefoundation/autoware_utils/pull/50) or new ManagedTransformBuffer lib +// is deployed for autoware +void VoxelBasedCompareMapFilterComponent::input_indices_callback( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr cloud, const PointIndicesConstPtr indices) +{ + // If cloud is given, check if it's valid + if (!isValid(cloud)) { + RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid input!"); + return; + } + // If indices are given, check if they are valid + if (indices && !isValid(indices)) { + RCLCPP_ERROR(this->get_logger(), "[input_indices_callback] Invalid indices!"); + return; + } + + // Check whether the user has given a different input TF frame + tf_input_orig_frame_ = cloud->header.frame_id; + PointCloud2ConstPtr cloud_tf; + if (cloud->header.frame_id != tf_input_frame_) { + RCLCPP_DEBUG( + this->get_logger(), "[input_indices_callback] Transforming input dataset from %s to %s.", + cloud->header.frame_id.c_str(), tf_input_frame_.c_str()); + // Save the original frame ID + // Convert the cloud into the different frame + PointCloud2 cloud_transformed; + + try { + // Lookup the transform from input frame to "map" + geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform( + tf_input_frame_, tf_input_orig_frame_, rclcpp::Time(cloud->header.stamp), + rclcpp::Duration::from_seconds(0.0)); + + // Transform the point cloud + tf2::doTransform(*cloud, cloud_transformed, transform_stamped); + cloud_tf = std::make_shared(cloud_transformed); + } catch (tf2::TransformException & ex) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s", + ex.what()); + cloud_tf = cloud; // Fallback to original data + } + } else { + cloud_tf = cloud; + } + // Need setInputCloud () here because we have to extract x/y/z + IndicesPtr vindices; + if (indices) { + vindices.reset(new std::vector(indices->indices)); + } + + computePublish(cloud_tf, vindices); +} + +bool VoxelBasedCompareMapFilterComponent::convert_output_costly( + std::unique_ptr & output) +{ + if (!output || output->data.empty() || output->fields.empty()) { + RCLCPP_ERROR(this->get_logger(), "Invalid output point cloud!"); + return false; + } + if ( + pcl::getFieldIndex(*output, "x") == -1 || pcl::getFieldIndex(*output, "y") == -1 || + pcl::getFieldIndex(*output, "z") == -1) { + RCLCPP_ERROR(this->get_logger(), "Input pointcloud does not have xyz fields"); + return false; + } + if (!tf_output_frame_.empty() && output->header.frame_id != tf_output_frame_) { + auto cloud_transformed = std::make_unique(); + try { + geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform( + tf_output_frame_, output->header.frame_id, rclcpp::Time(output->header.stamp), + rclcpp::Duration::from_seconds(0.0)); + tf2::doTransform(*output, *cloud_transformed, transform_stamped); + cloud_transformed->header.frame_id = tf_output_frame_; + output = std::move(cloud_transformed); + } catch (tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s", + e.what()); + return false; + } + } + + if (tf_output_frame_.empty() && output->header.frame_id != tf_input_orig_frame_) { + auto cloud_transformed = std::make_unique(); + try { + geometry_msgs::msg::TransformStamped transform_stamped = tf_buffer_.lookupTransform( + tf_input_orig_frame_, output->header.frame_id, rclcpp::Time(output->header.stamp), + rclcpp::Duration::from_seconds(0.0)); + tf2::doTransform(*output, *cloud_transformed, transform_stamped); + cloud_transformed->header.frame_id = tf_input_orig_frame_; + output = std::move(cloud_transformed); + } catch (tf2::TransformException & e) { + RCLCPP_WARN_THROTTLE( + this->get_logger(), *this->get_clock(), 5000, "Could not transform pointcloud: %s", + e.what()); + return false; + } + } + return true; +} + void VoxelBasedCompareMapFilterComponent::filter( const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices, PointCloud2 & output) diff --git a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp index ba29f9ac25af0..e867a7d24a2d6 100644 --- a/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp +++ b/perception/autoware_compare_map_segmentation/src/voxel_based_compare_map_filter/node.hpp @@ -27,9 +27,20 @@ namespace autoware::compare_map_segmentation { class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter { + using PointCloud2 = sensor_msgs::msg::PointCloud2; + using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr; + + using PointIndices = pcl_msgs::msg::PointIndices; + using PointIndicesPtr = PointIndices::SharedPtr; + using PointIndicesConstPtr = PointIndices::ConstSharedPtr; + protected: void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override; + void input_indices_callback( + const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices) override; + + bool convert_output_costly(std::unique_ptr & output) override; private: // pcl::SegmentDifferences impl_; @@ -37,6 +48,8 @@ class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preproce rclcpp::Subscription::SharedPtr sub_map_; double distance_threshold_; bool set_map_in_voxel_grid_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp index f43db051f3d67..036648c311153 100644 --- a/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp +++ b/sensing/autoware_pointcloud_preprocessor/include/autoware/pointcloud_preprocessor/filter.hpp @@ -206,7 +206,11 @@ class Filter : public rclcpp::Node * \param input the input point cloud dataset. * \param indices a pointer to the vector of point indices to use. */ - void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices); + virtual void computePublish(const PointCloud2ConstPtr & input, const IndicesPtr & indices); + /** \brief PointCloud2 + Indices data callback. */ + virtual void input_indices_callback( + const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); + virtual bool convert_output_costly(std::unique_ptr & output); ////////////////////// // from PCLNodelet // @@ -279,16 +283,11 @@ class Filter : public rclcpp::Node std::shared_ptr sync_input_indices_e_; std::shared_ptr sync_input_indices_a_; - /** \brief PointCloud2 + Indices data callback. */ - void input_indices_callback(const PointCloud2ConstPtr cloud, const PointIndicesConstPtr indices); - /** \brief Get a matrix for conversion from the original frame to the target frame */ bool calculate_transform_matrix( const std::string & target_frame, const sensor_msgs::msg::PointCloud2 & from, TransformInfo & transform_info /*output*/); - bool convert_output_costly(std::unique_ptr & output); - // TODO(sykwer): Temporary Implementation: Remove this interface when all the filter nodes conform // to new API. void faster_input_indices_callback(