Skip to content

Commit

Permalink
feat: add node param to filter by z
Browse files Browse the repository at this point in the history
Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
YoshiRi committed Jun 6, 2024
1 parent 0323f33 commit 63cea6d
Show file tree
Hide file tree
Showing 2 changed files with 28 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,8 @@ class VectorMapInsideAreaFilterComponent : public pointcloud_preprocessor::Filte

// parameter
std::string polygon_type_;
bool use_z_filter_ = false;
float z_threshold_;

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,8 @@ lanelet::ConstPolygons3d calcIntersectedPolygons(
}

pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons)
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud_in, const lanelet::ConstPolygons3d & polygons,
const std::optional<float> & z_threshold_)
{
std::vector<PolygonCgal> cgal_polys;

Expand All @@ -53,7 +54,7 @@ pcl::PointCloud<pcl::PointXYZ> removePointsWithinPolygons(

pcl::PointCloud<pcl::PointXYZ> filtered_cloud;
pointcloud_preprocessor::utils::remove_polygon_cgal_from_cloud(
*cloud_in, cgal_polys, filtered_cloud);
*cloud_in, cgal_polys, filtered_cloud, z_threshold_);

return filtered_cloud;
}
Expand All @@ -74,6 +75,10 @@ VectorMapInsideAreaFilterComponent::VectorMapInsideAreaFilterComponent(
map_sub_ = this->create_subscription<autoware_auto_mapping_msgs::msg::HADMapBin>(
"input/vector_map", rclcpp::QoS{1}.transient_local(),
std::bind(&VectorMapInsideAreaFilterComponent::mapCallback, this, _1));

// Set parameters
use_z_filter_ = declare_parameter<bool>("use_z_filter", false);
z_threshold_ = declare_parameter<float>("z_threshold", 0.0f); // defined in the base_link frame
}

void VectorMapInsideAreaFilterComponent::filter(
Expand All @@ -99,7 +104,25 @@ void VectorMapInsideAreaFilterComponent::filter(
const auto intersected_lanelets = calcIntersectedPolygons(bounding_box, polygon_lanelets_);

// filter pointcloud by lanelet
const auto filtered_pc = removePointsWithinPolygons(pc_input, intersected_lanelets);
std::optional<float> z_threshold_in_base_link = std::nullopt;
if (use_z_filter_) {
// assume z_max is defined in the base_link frame
const std::string base_link_frame = "base_link";
z_threshold_in_base_link = z_threshold_;
if (input->header.frame_id != base_link_frame) {
try {
// get z difference from baselink to input frame
const auto transform =
tf_buffer_->lookupTransform(input->header.frame_id, base_link_frame, input->header.stamp);
*z_threshold_in_base_link += transform.transform.translation.z;
} catch (const tf2::TransformException & e) {
RCLCPP_WARN(get_logger(), "Failed to transform z_threshold to base_link frame");
z_threshold_in_base_link = std::nullopt;
}
}
}
const auto filtered_pc =
removePointsWithinPolygons(pc_input, intersected_lanelets, z_threshold_in_base_link);

// convert to ROS message
pcl::toROSMsg(filtered_pc, output);
Expand Down

0 comments on commit 63cea6d

Please sign in to comment.