Skip to content

Commit

Permalink
Merge pull request #126 from OctoMap/topic/fix-ros2-pcl
Browse files Browse the repository at this point in the history
[ROS2] Fix deprecated PCL API: setFilterLimitsNegative -> setNegative
  • Loading branch information
wxmerkt authored Apr 17, 2024
2 parents 72141b9 + 8829049 commit 99e9f64
Show file tree
Hide file tree
Showing 2 changed files with 7 additions and 25 deletions.
13 changes: 2 additions & 11 deletions .github/workflows/industrial_ci_build_and_test.yml
Original file line number Diff line number Diff line change
Expand Up @@ -6,20 +6,11 @@ jobs:
strategy:
matrix:
env:
- {ROS_DISTRO: rolling}
- {ROS_DISTRO: rolling, PRERELEASE: true}
# - {ROS_DISTRO: rolling}
- {ROS_DISTRO: humble, PRERELEASE: true}
- {ROS_DISTRO: iron, PRERELEASE: true}
# env:
# CCACHE_DIR: /github/home/.ccache # Enable ccache
runs-on: ubuntu-latest
steps:
- uses: actions/checkout@v3
# # This step will fetch/store the directory used by ccache before/after the ci run
# - uses: actions/cache@v3
# with:
# path: ${{ env.CCACHE_DIR }}
# key: ccache-${{ matrix.env.ROS_DISTRO }}-${{ matrix.env.ROS_REPO }}
# Run industrial_ci
- uses: actions/checkout@v4
- uses: 'ros-industrial/industrial_ci@master'
env: ${{ matrix.env }}
19 changes: 5 additions & 14 deletions octomap_server/src/octomap_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -413,9 +413,6 @@ void OctomapServer::insertCloudCallback(const PointCloud2::ConstSharedPtr cloud)
return;
}

Eigen::Matrix4f sensor_to_world =
tf2::transformToEigen(sensor_to_world_transform_stamped.transform).matrix().cast<float>();

// set up filter for height range, also removes NANs:
pcl::PassThrough<PCLPoint> pass_x;
pass_x.setFilterFieldName("x");
Expand Down Expand Up @@ -450,14 +447,8 @@ void OctomapServer::insertCloudCallback(const PointCloud2::ConstSharedPtr cloud)
"You need to set the base_frame_id or disable filter_ground.");
}


Eigen::Matrix4f sensor_to_base =
tf2::transformToEigen(sensor_to_base_transform_stamped.transform).matrix().cast<float>();
Eigen::Matrix4f base_to_world =
tf2::transformToEigen(base_to_world_transform_stamped.transform).matrix().cast<float>();

// transform pointcloud from sensor frame to fixed robot frame
pcl::transformPointCloud(pc, pc, sensor_to_base);
pcl_ros::transformPointCloud(pc, pc, sensor_to_base_transform_stamped);
pass_x.setInputCloud(pc.makeShared());
pass_x.filter(pc);
pass_y.setInputCloud(pc.makeShared());
Expand All @@ -467,11 +458,11 @@ void OctomapServer::insertCloudCallback(const PointCloud2::ConstSharedPtr cloud)
filterGroundPlane(pc, pc_ground, pc_nonground);

// transform clouds to world frame for insertion
pcl::transformPointCloud(pc_ground, pc_ground, base_to_world);
pcl::transformPointCloud(pc_nonground, pc_nonground, base_to_world);
pcl_ros::transformPointCloud(pc_ground, pc_ground, base_to_world_transform_stamped);
pcl_ros::transformPointCloud(pc_nonground, pc_nonground, base_to_world_transform_stamped);
} else {
// directly transform to map frame:
pcl::transformPointCloud(pc, pc, sensor_to_world);
pcl_ros::transformPointCloud(pc, pc, sensor_to_world_transform_stamped);

// just filter height range:
pass_x.setInputCloud(pc.makeShared());
Expand Down Expand Up @@ -1110,7 +1101,7 @@ void OctomapServer::filterGroundPlane(
second_pass.setFilterLimits(-ground_filter_plane_distance_, ground_filter_plane_distance_);
second_pass.setInputCloud(pc.makeShared());
second_pass.filter(ground);
second_pass.setFilterLimitsNegative(true);
second_pass.setNegative(true);
second_pass.filter(nonground);
}
// debug:
Expand Down

0 comments on commit 99e9f64

Please sign in to comment.