Skip to content

Commit

Permalink
fix: replace boost::mutex::scoped_lock to std::scoped_lock (tier4#907)
Browse files Browse the repository at this point in the history
* fix: replace boost::mutex::scoped_lock to std::scoped_lock

* fix: replace boost::mutex to std::mutex
  • Loading branch information
RyuYamamoto authored and boyali committed Oct 19, 2022
1 parent 6987cf7 commit 6f5ce8d
Show file tree
Hide file tree
Showing 20 changed files with 43 additions and 82 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void DistanceBasedCompareMapFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
if (map_ptr_ == NULL || tree_ == NULL) {
output = *input;
return;
Expand All @@ -65,7 +65,7 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl
pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);

boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
map_ptr_ = map_pcl_ptr;
tf_input_frame_ = map_ptr_->header.frame_id;
if (!tree_) {
Expand All @@ -81,7 +81,7 @@ void DistanceBasedCompareMapFilterComponent::input_target_callback(const PointCl
rcl_interfaces::msg::SetParametersResult DistanceBasedCompareMapFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "distance_threshold", distance_threshold_)) {
RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", distance_threshold_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
if (voxel_map_ptr_ == NULL) {
output = *input;
return;
Expand Down Expand Up @@ -86,7 +86,7 @@ void VoxelBasedApproximateCompareMapFilterComponent::input_target_callback(
pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);

boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
set_map_in_voxel_grid_ = true;
tf_input_frame_ = map_pcl_ptr->header.frame_id;
voxel_map_ptr_.reset(new pcl::PointCloud<pcl::PointXYZ>);
Expand All @@ -109,7 +109,7 @@ rcl_interfaces::msg::SetParametersResult
VoxelBasedApproximateCompareMapFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "distance_threshold", distance_threshold_)) {
voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void VoxelBasedCompareMapFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
if (voxel_map_ptr_ == NULL) {
output = *input;
return;
Expand Down Expand Up @@ -252,7 +252,7 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud
pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);

boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
set_map_in_voxel_grid_ = true;
tf_input_frame_ = map_pcl_ptr->header.frame_id;
voxel_map_ptr_.reset(new pcl::PointCloud<pcl::PointXYZ>);
Expand All @@ -275,7 +275,7 @@ void VoxelBasedCompareMapFilterComponent::input_target_callback(const PointCloud
rcl_interfaces::msg::SetParametersResult VoxelBasedCompareMapFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "distance_threshold", distance_threshold_)) {
voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void VoxelDistanceBasedCompareMapFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
if (voxel_map_ptr_ == NULL || map_ptr_ == NULL || tree_ == NULL) {
output = *input;
return;
Expand Down Expand Up @@ -76,7 +76,7 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback(
pcl::fromROSMsg<pcl::PointXYZ>(*map, map_pcl);
const auto map_pcl_ptr = pcl::make_shared<pcl::PointCloud<pcl::PointXYZ>>(map_pcl);

boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
tf_input_frame_ = map_pcl_ptr->header.frame_id;
// voxel
voxel_map_ptr_.reset(new pcl::PointCloud<pcl::PointXYZ>);
Expand All @@ -99,7 +99,7 @@ void VoxelDistanceBasedCompareMapFilterComponent::input_target_callback(
rcl_interfaces::msg::SetParametersResult VoxelDistanceBasedCompareMapFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "distance_threshold", distance_threshold_)) {
voxel_grid_.setLeafSize(distance_threshold_, distance_threshold_, distance_threshold_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -231,7 +231,7 @@ void RANSACGroundFilterComponent::filter(
const PointCloud2::ConstSharedPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
sensor_msgs::msg::PointCloud2::SharedPtr input_transformed_ptr(new sensor_msgs::msg::PointCloud2);
if (!transformPointCloud(base_frame_, input, input_transformed_ptr)) {
RCLCPP_ERROR_STREAM_THROTTLE(
Expand Down Expand Up @@ -322,7 +322,7 @@ void RANSACGroundFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult RANSACGroundFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "base_frame", base_frame_)) {
RCLCPP_DEBUG(get_logger(), "Setting base_frame to: %s.", base_frame_.c_str());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -310,7 +310,7 @@ void RayGroundFilterComponent::filter(
const PointCloud2::ConstSharedPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

sensor_msgs::msg::PointCloud2::SharedPtr input_transformed_ptr(new sensor_msgs::msg::PointCloud2);
bool succeeded = TransformPointCloud(base_frame_, input, input_transformed_ptr);
Expand Down Expand Up @@ -366,7 +366,7 @@ void RayGroundFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult RayGroundFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "min_x", min_x_)) {
RCLCPP_DEBUG(get_logger(), "Setting min_x to: %f.", min_x_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,7 @@ class Filter : public rclcpp::Node
std::string tf_output_frame_;

/** \brief Internal mutex. */
boost::mutex mutex_;
std::mutex mutex_;

/** \brief processing time publisher. **/
std::unique_ptr<tier4_autoware_utils::StopWatch<std::chrono::milliseconds>> stop_watch_ptr_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -106,7 +106,7 @@ void BlockageDiagComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
uint horizontal_bins = static_cast<uint>((angle_range_deg_[1] - angle_range_deg_[0]));
uint vertical_bins = vertical_bins_;
pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_input(new pcl::PointCloud<PointXYZIRADRT>);
Expand Down Expand Up @@ -224,7 +224,7 @@ void BlockageDiagComponent::filter(
rcl_interfaces::msg::SetParametersResult BlockageDiagComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
if (get_param(p, "blockage_ratio_threshold", blockage_ratio_threshold_)) {
RCLCPP_DEBUG(
get_logger(), "Setting new blockage_ratio_threshold to: %f.", blockage_ratio_threshold_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -60,16 +60,6 @@ namespace pointcloud_preprocessor
CropBoxFilterComponent::CropBoxFilterComponent(const rclcpp::NodeOptions & options)
: Filter("CropBoxFilter", options)
{
// initialize debug tool
{
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
debug_publisher_ = std::make_unique<DebugPublisher>(this, "crop_box_filter");
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
}

// set initial parameters
{
auto & p = param_;
Expand Down Expand Up @@ -100,8 +90,8 @@ void CropBoxFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);
std::scoped_lock lock(mutex_);

output.data.resize(input->data.size());
Eigen::Vector3f pt(Eigen::Vector3f::Zero());
size_t j = 0;
Expand Down Expand Up @@ -142,15 +132,6 @@ void CropBoxFilterComponent::filter(
output.row_step = static_cast<uint32_t>(output.data.size() / output.height);

publishCropBoxPolygon();
// add processing time for debug
if (debug_publisher_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
}
}

void CropBoxFilterComponent::publishCropBoxPolygon()
Expand Down Expand Up @@ -207,7 +188,7 @@ void CropBoxFilterComponent::publishCropBoxPolygon()
rcl_interfaces::msg::SetParametersResult CropBoxFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

CropBoxParam new_param{};

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ ApproximateDownsampleFilterComponent::ApproximateDownsampleFilterComponent(
void ApproximateDownsampleFilterComponent::filter(
const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *pcl_input);
Expand All @@ -94,7 +94,7 @@ void ApproximateDownsampleFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult ApproximateDownsampleFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "voxel_size_x", voxel_size_x_)) {
RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ RandomDownsampleFilterComponent::RandomDownsampleFilterComponent(
void RandomDownsampleFilterComponent::filter(
const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *pcl_input);
Expand All @@ -87,7 +87,7 @@ void RandomDownsampleFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult RandomDownsampleFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "sample_num", sample_num_)) {
RCLCPP_DEBUG(get_logger(), "Setting new sample num to: %zu.", sample_num_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -77,7 +77,7 @@ VoxelGridDownsampleFilterComponent::VoxelGridDownsampleFilterComponent(
void VoxelGridDownsampleFilterComponent::filter(
const PointCloud2ConstPtr & input, const IndicesPtr & /*indices*/, PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *pcl_input);
Expand All @@ -95,7 +95,7 @@ void VoxelGridDownsampleFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult VoxelGridDownsampleFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "voxel_size_x", voxel_size_x_)) {
RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_);
Expand Down
2 changes: 1 addition & 1 deletion sensing/pointcloud_preprocessor/src/filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -210,7 +210,7 @@ void pointcloud_preprocessor::Filter::computePublish(
rcl_interfaces::msg::SetParametersResult pointcloud_preprocessor::Filter::filterParamCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "input_frame", tf_input_frame_)) {
RCLCPP_DEBUG(get_logger(), "Setting the input TF frame to: %s.", tf_input_frame_.c_str());
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -115,7 +115,7 @@ void DualReturnOutlierFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<PointXYZIRADRT>::Ptr pcl_input(new pcl::PointCloud<PointXYZIRADRT>);
pcl::fromROSMsg(*input, *pcl_input);

Expand Down Expand Up @@ -349,7 +349,7 @@ void DualReturnOutlierFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult DualReturnOutlierFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "weak_first_distance_ratio", weak_first_distance_ratio_)) {
RCLCPP_DEBUG(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ void RadiusSearch2DOutlierFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(*input, *xyz_cloud);

Expand Down Expand Up @@ -71,7 +71,7 @@ void RadiusSearch2DOutlierFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult RadiusSearch2DOutlierFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "min_neighbors", min_neighbors_)) {
RCLCPP_DEBUG(get_logger(), "Setting new min neighbors to: %zu.", min_neighbors_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,16 +21,6 @@ namespace pointcloud_preprocessor
RingOutlierFilterComponent::RingOutlierFilterComponent(const rclcpp::NodeOptions & options)
: Filter("RingOutlierFilter", options)
{
// initialize debug tool
{
using tier4_autoware_utils::DebugPublisher;
using tier4_autoware_utils::StopWatch;
stop_watch_ptr_ = std::make_unique<StopWatch<std::chrono::milliseconds>>();
debug_publisher_ = std::make_unique<DebugPublisher>(this, "ring_outlier_filter");
stop_watch_ptr_->tic("cyclic_time");
stop_watch_ptr_->tic("processing_time");
}

// set initial parameters
{
distance_ratio_ = static_cast<double>(declare_parameter("distance_ratio", 1.03));
Expand All @@ -48,8 +38,7 @@ void RingOutlierFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
stop_watch_ptr_->toc("processing_time", true);
std::scoped_lock lock(mutex_);
std::unordered_map<uint16_t, std::vector<std::size_t>> input_ring_map;
input_ring_map.reserve(128);
sensor_msgs::msg::PointCloud2::SharedPtr input_ptr =
Expand Down Expand Up @@ -120,21 +109,12 @@ void RingOutlierFilterComponent::filter(
}
tmp_indices.clear();
}
// add processing time for debug
if (debug_publisher_) {
const double cyclic_time_ms = stop_watch_ptr_->toc("cyclic_time", true);
const double processing_time_ms = stop_watch_ptr_->toc("processing_time", true);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/cyclic_time_ms", cyclic_time_ms);
debug_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
"debug/processing_time_ms", processing_time_ms);
}
}

rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "distance_ratio", distance_ratio_)) {
RCLCPP_DEBUG(get_logger(), "Setting new distance ratio to: %f.", distance_ratio_);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ void VoxelGridOutlierFilterComponent::filter(
const PointCloud2ConstPtr & input, [[maybe_unused]] const IndicesPtr & indices,
PointCloud2 & output)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_voxelized_input(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_output(new pcl::PointCloud<pcl::PointXYZ>);
Expand Down Expand Up @@ -70,7 +70,7 @@ void VoxelGridOutlierFilterComponent::filter(
rcl_interfaces::msg::SetParametersResult VoxelGridOutlierFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & p)
{
boost::mutex::scoped_lock lock(mutex_);
std::scoped_lock lock(mutex_);

if (get_param(p, "voxel_size_x", voxel_size_x_)) {
RCLCPP_DEBUG(get_logger(), "Setting new distance threshold to: %f.", voxel_size_x_);
Expand Down
Loading

0 comments on commit 6f5ce8d

Please sign in to comment.