Skip to content

Commit

Permalink
[ros-o] boost::make_shared -> std::make_shared, use .makeShared()
Browse files Browse the repository at this point in the history
  • Loading branch information
k-okada committed Dec 27, 2024
1 parent 93e11f8 commit f9dc100
Show file tree
Hide file tree
Showing 16 changed files with 108 additions and 19 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,17 @@ namespace jsk_pcl_ros{
tf2_ros::TransformListener* tf_listener_;
sensor_msgs::PointCloud2::Ptr transformed_points_msg_ =
boost::shared_ptr<sensor_msgs::PointCloud2>(new sensor_msgs::PointCloud2);
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PCLPointCloud2Ptr pcl_pc2_ptr_ =
std::shared_ptr<pcl::PCLPointCloud2>(new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_xyz_ptr_ =
std::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
#else
pcl::PCLPointCloud2Ptr pcl_pc2_ptr_ =
boost::shared_ptr<pcl::PCLPointCloud2>(new pcl::PCLPointCloud2);
pcl::PointCloud<pcl::PointXYZ>::Ptr pcl_xyz_ptr_ =
boost::shared_ptr<pcl::PointCloud<pcl::PointXYZ>>(new pcl::PointCloud<pcl::PointXYZ>);
#endif

////////////////////////////////////////////////////////
// Diagnostics Variables
Expand Down
2 changes: 1 addition & 1 deletion jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h
Original file line number Diff line number Diff line change
Expand Up @@ -186,7 +186,7 @@ namespace pcl
// }

if (!change_detector_)
change_detector_ = boost::shared_ptr<pcl::octree::OctreePointCloudChangeDetector<PointInT> >(new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_));
pcl::octree::OctreePointCloudChangeDetector<PointInT> *change_detector_ = new pcl::octree::OctreePointCloudChangeDetector<PointInT> (change_detector_resolution_);

if (!particles_ || particles_->points.empty ())
initParticles (true);
Expand Down
1 change: 1 addition & 0 deletions jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <pcl/features/gfpfh.h>
#include <pcl/features/pfh.h>
#include <pcl/features/cvfh.h>
#include <pcl/features/pfh_tools.h> // for computePairFeatures
#include <pcl/features/normal_3d_omp.h>
#include <pcl/tracking/tracking.h>
#include <pcl/common/common.h>
Expand Down
4 changes: 4 additions & 0 deletions jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -756,7 +756,11 @@ namespace jsk_pcl_ros
*points_on_edges = *points_on_edges + *points_on_edge;
cubes.push_back(cube);
}
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pub_debug_filtered_cloud_.publish(*points_on_edges);
#else
pub_debug_filtered_cloud_.publish(points_on_edges);
#endif
}
}

Expand Down
15 changes: 13 additions & 2 deletions jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,9 @@ namespace jsk_pcl_ros
EuclideanClusterExtraction<pcl::PointXYZ> impl;
if (filtered_cloud->points.size() > 0) {
jsk_topic_tools::ScopedTimer timer = kdtree_acc_.scopedTimer();
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>());
#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
Expand Down Expand Up @@ -332,7 +334,9 @@ namespace jsk_pcl_ros
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(req.input, *cloud);

#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
tree = boost::make_shared< pcl::search::KdTree<pcl::PointXYZ> > ();
#else
Expand Down Expand Up @@ -361,12 +365,19 @@ namespace jsk_pcl_ros
pcl_conversions::toPCL(req.input, *pcl_cloud);
pcl::ExtractIndices<pcl::PCLPointCloud2> ex;
ex.setInputCloud(pcl_cloud);
#elif ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::ExtractIndices<sensor_msgs::PointCloud2> ex;
ex.setInputCloud ( req.input.makeShared() );
#else
pcl::ExtractIndices<sensor_msgs::PointCloud2> ex;
ex.setInputCloud ( boost::make_shared< sensor_msgs::PointCloud2 > (req.input) );
#endif
for ( size_t i = 0; i < cluster_indices.size(); i++ ) {
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
ex.setIndices ( std::make_shared< pcl::PointIndices > (cluster_indices[i]) );
#else
ex.setIndices ( boost::make_shared< pcl::PointIndices > (cluster_indices[i]) );
#endif
ex.setNegative ( false );
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 7 )
pcl::PCLPointCloud2 output_cloud;
Expand Down
14 changes: 14 additions & 0 deletions jsk_pcl_ros/src/icp_registration_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -592,6 +592,12 @@ namespace jsk_pcl_ros
pcl::PointCloud<PointT>::Ptr& output_cloud,
Eigen::Affine3d& output_transform)
{
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::GeneralizedIterativeClosestPoint<PointT, PointT> icp;
icp.setRotationEpsilon(rotation_epsilon_);
icp.setCorrespondenceRandomness(correspondence_randomness_);
icp.setMaximumOptimizerIterations(maximum_optimizer_iterations_);
#else
pcl::IterativeClosestPoint<PointT, PointT> icp;
if (use_normal_) {
pcl::IterativeClosestPointWithNormals<PointT, PointT> icp_with_normal;
Expand All @@ -606,6 +612,7 @@ namespace jsk_pcl_ros
gicp.setMaximumOptimizerIterations(maximum_optimizer_iterations_);
icp = gicp;
}
#endif
if (correspondence_algorithm_ == 1) { // Projective
if (!camera_info_msg_) {
NODELET_ERROR("no camera info is available yet");
Expand Down Expand Up @@ -636,10 +643,17 @@ namespace jsk_pcl_ros
icp.setInputTarget(cloud);

if (transform_3dof_) {
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::registration::WarpPointRigid3D<PointT, PointT>::Ptr warp_func
(new pcl::registration::WarpPointRigid3D<PointT, PointT>);
pcl::registration::TransformationEstimationLM<PointT, PointT>::Ptr te
(new pcl::registration::TransformationEstimationLM<PointT, PointT>);
#else
boost::shared_ptr<pcl::registration::WarpPointRigid3D<PointT, PointT> > warp_func
(new pcl::registration::WarpPointRigid3D<PointT, PointT>);
boost::shared_ptr<pcl::registration::TransformationEstimationLM<PointT, PointT> > te
(new pcl::registration::TransformationEstimationLM<PointT, PointT>);
#endif
te->setWarpFunction(warp_func);
icp.setTransformationEstimation(te);
}
Expand Down
3 changes: 3 additions & 0 deletions jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <geometry_msgs/PointStamped.h>

#include "jsk_recognition_utils/pcl_conversion_util.h"
#include <pcl/search/kdtree.h> // for KdTree

namespace jsk_pcl_ros
{
Expand All @@ -56,7 +57,9 @@ namespace jsk_pcl_ros
pcl::MovingLeastSquares<pcl::PointXYZRGB, pcl::PointXYZRGB> smoother;
smoother.setSearchRadius (search_radius_);
if (gauss_param_set_) smoother.setSqrGaussParam (gauss_param_set_);
#if PCL_VERSION_COMPARE (<, 1, 9, 0) // https://github.com/PointCloudLibrary/pcl/blob/cc08f815a9a5f2a1d3f897fb2bc9d541635792c9/CHANGES.md?plain=1#L1633
smoother.setPolynomialFit (use_polynomial_fit_);
#endif
smoother.setPolynomialOrder (polynomial_order_);
smoother.setComputeNormals (calc_normal_);

Expand Down
28 changes: 28 additions & 0 deletions jsk_pcl_ros/src/organized_multi_plane_segmentation_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -246,10 +246,17 @@ namespace jsk_pcl_ros

// compute the distance between two boundaries.
// if they are near enough, we can regard these two map should connect
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr a_indices
= std::make_shared<pcl::PointIndices>(boundary_indices[i]);
pcl::PointIndices::Ptr b_indices
= std::make_shared<pcl::PointIndices>(boundary_indices[j]);
#else
pcl::PointIndices::Ptr a_indices
= boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
pcl::PointIndices::Ptr b_indices
= boost::make_shared<pcl::PointIndices>(boundary_indices[j]);
#endif
pcl::PointCloud<PointT> a_cloud, b_cloud;
extract.setIndices(a_indices);
extract.filter(a_cloud);
Expand Down Expand Up @@ -364,10 +371,17 @@ namespace jsk_pcl_ros
pcl::ModelCoefficients pcl_new_coefficients;
pcl_new_coefficients.values = new_coefficients;
// estimate concave hull
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr indices_ptr
= std::make_shared<pcl::PointIndices>(one_boundaries);
pcl::ModelCoefficients::Ptr coefficients_ptr
= std::make_shared<pcl::ModelCoefficients>(pcl_new_coefficients);
#else
pcl::PointIndices::Ptr indices_ptr
= boost::make_shared<pcl::PointIndices>(one_boundaries);
pcl::ModelCoefficients::Ptr coefficients_ptr
= boost::make_shared<pcl::ModelCoefficients>(pcl_new_coefficients);
#endif
jsk_recognition_utils::ConvexPolygon::Ptr convex
= jsk_recognition_utils::convexFromCoefficientsAndInliers<PointT>(
input, indices_ptr, coefficients_ptr);
Expand Down Expand Up @@ -478,7 +492,11 @@ namespace jsk_pcl_ros
for (size_t i = 0; i < boundary_indices.size(); i++) {
pcl::PointCloud<PointT> boundary_cloud;
pcl::PointIndices boundary_one_indices = boundary_indices[i];
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr indices_ptr = std::make_shared<pcl::PointIndices>(boundary_indices[i]);
#else
pcl::PointIndices::Ptr indices_ptr = boost::make_shared<pcl::PointIndices>(boundary_indices[i]);
#endif
extract.setIndices(indices_ptr);
extract.filter(boundary_cloud);
boundaries.push_back(boundary_cloud);
Expand Down Expand Up @@ -510,8 +528,13 @@ namespace jsk_pcl_ros
////////////////////////////////////////////////////////
jsk_recognition_utils::Vertices centroids;
for (size_t i = 0; i < inliers.size(); i++) {
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr target_inliers
= std::make_shared<pcl::PointIndices>(inliers[i]);
#else
pcl::PointIndices::Ptr target_inliers
= boost::make_shared<pcl::PointIndices>(inliers[i]);
#endif
pcl::PointCloud<PointT>::Ptr target_cloud (new pcl::PointCloud<PointT>);
Eigen::Vector4f centroid;
pcl::ExtractIndices<PointT> ex;
Expand Down Expand Up @@ -640,8 +663,13 @@ namespace jsk_pcl_ros
jsk_topic_tools::ScopedTimer timer
= ransac_refinement_time_acc_.scopedTimer();
for (size_t i = 0; i < input_indices.size(); i++) {
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr input_indices_ptr
= std::make_shared<pcl::PointIndices>(input_indices[i]);
#else
pcl::PointIndices::Ptr input_indices_ptr
= boost::make_shared<pcl::PointIndices>(input_indices[i]);
#endif
Eigen::Vector3f normal(input_coefficients[i].values[0],
input_coefficients[i].values[1],
input_coefficients[i].values[2]);
Expand Down
9 changes: 3 additions & 6 deletions jsk_pcl_ros/src/particle_filter_tracking_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -145,19 +145,16 @@ namespace jsk_pcl_ros
}


boost::shared_ptr<DistanceCoherence<PointT> >
distance_coherence(new DistanceCoherence<PointT>);
DistanceCoherence<PointT>::Ptr distance_coherence(new DistanceCoherence<PointT>);
coherence->addPointCoherence(distance_coherence);

//add HSV coherence
if (use_hsv) {
boost::shared_ptr<HSVColorCoherence<PointT> > hsv_color_coherence
= boost::shared_ptr<HSVColorCoherence<PointT> >(new HSVColorCoherence<PointT>());
HSVColorCoherence<PointT>::Ptr hsv_color_coherence(new HSVColorCoherence<PointT>());
coherence->addPointCoherence(hsv_color_coherence);
}

boost::shared_ptr<pcl::search::Octree<PointT> > search
(new pcl::search::Octree<PointT>(octree_resolution));
pcl::search::Octree<PointT>::Ptr search(new pcl::search::Octree<PointT>(octree_resolution));
//boost::shared_ptr<pcl::search::KdTree<PointT> > search(new pcl::search::KdTree<PointT>());
coherence->setSearchMethod(search);
double max_distance;
Expand Down
1 change: 1 addition & 0 deletions jsk_pcl_ros/src/pointcloud_database_server_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,7 @@
#include <pcl/io/pcd_io.h>
#include <pcl/io/vtk_lib_io.h>
#include "jsk_recognition_utils/pcl_conversion_util.h"
#include <boost/filesystem.hpp>

namespace jsk_pcl_ros
{
Expand Down
7 changes: 3 additions & 4 deletions jsk_pcl_ros/src/pointcloud_screenpoint_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ void PointcloudScreenpoint::onInit()
ConnectionBasedNodelet::onInit();


normals_tree_ = boost::make_shared< pcl::search::KdTree< pcl::PointXYZ > > ();
pcl::search::KdTree<pcl::PointXYZ>::Ptr normals_tree_(new pcl::search::KdTree<pcl::PointXYZ>);
n3d_.setSearchMethod (normals_tree_);

dyn_srv_ = boost::make_shared< dynamic_reconfigure::Server< Config > >(*pnh_);
Expand Down Expand Up @@ -257,8 +257,7 @@ bool PointcloudScreenpoint::screenpoint_cb (
}

// search normal
n3d_.setSearchSurface(
boost::make_shared<pcl::PointCloud<pcl::PointXYZ > > (latest_cloud_));
n3d_.setSearchSurface(latest_cloud_.makeShared());

pcl::PointCloud<pcl::PointXYZ> cloud_;
pcl::PointXYZ pt;
Expand All @@ -268,7 +267,7 @@ bool PointcloudScreenpoint::screenpoint_cb (
cloud_.points.resize(0);
cloud_.points.push_back(pt);

n3d_.setInputCloud (boost::make_shared<pcl::PointCloud<pcl::PointXYZ > > (cloud_));
n3d_.setInputCloud (cloud_.makeShared());
pcl::PointCloud<pcl::Normal> cloud_normals;
n3d_.compute (cloud_normals);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,11 @@ namespace jsk_pcl_ros
for (size_t i = 0; i < clusters->size(); i++) {
pcl::PointIndices::Ptr plane_inliers(new pcl::PointIndices);
pcl::ModelCoefficients::Ptr plane_coefficients(new pcl::ModelCoefficients);
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::PointIndices::Ptr cluster = std::make_shared<pcl::PointIndices>((*clusters)[i]);
#else
pcl::PointIndices::Ptr cluster = boost::make_shared<pcl::PointIndices>((*clusters)[i]);
#endif
ransacEstimation(cloud, cluster,
*plane_inliers, *plane_coefficients);
if (plane_inliers->indices.size() > 0) {
Expand Down
4 changes: 4 additions & 0 deletions jsk_pcl_ros/src/region_growing_segmentation_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,11 @@ namespace jsk_pcl_ros
void RegionGrowingSegmentation::segment(const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
pcl::search::Search<pcl::PointNormal>::Ptr tree(new pcl::search::KdTree<pcl::PointNormal>());
#else
pcl::search::Search<pcl::PointNormal>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointNormal> > (new pcl::search::KdTree<pcl::PointNormal>);
#endif
pcl::PointCloud<pcl::PointNormal> cloud;
pcl::fromROSMsg(*msg, cloud);

Expand Down
4 changes: 4 additions & 0 deletions jsk_pcl_ros/src/resize_points_publisher_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -184,7 +184,11 @@ namespace jsk_pcl_ros
}
pcl::ExtractIndices<T> extract;
extract.setInputCloud (pcl_input_cloud.makeShared());
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
extract.setIndices (std::make_shared <std::vector<int> > (ex_indices));
#else
extract.setIndices (boost::make_shared <std::vector<int> > (ex_indices));
#endif
extract.setNegative (false);
extract.filter (output);

Expand Down
8 changes: 6 additions & 2 deletions jsk_pcl_ros/src/supervoxel_segmentation_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -72,8 +72,12 @@ namespace jsk_pcl_ros
pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);
pcl::fromROSMsg(*cloud_msg, *cloud);
pcl::SupervoxelClustering<PointT> super(voxel_resolution_,
seed_resolution_,
use_transform_);
seed_resolution_
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
);
#else
, use_transform_);
#endif
super.setInputCloud(cloud);
super.setColorImportance(color_importance_);
super.setSpatialImportance(spatial_importance_);
Expand Down
16 changes: 12 additions & 4 deletions jsk_pcl_ros/src/target_adaptive_tracking_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1782,8 +1782,12 @@ namespace jsk_pcl_ros
boost::mutex::scoped_lock lock(mutex_);
pcl::SupervoxelClustering<PointT> super(
voxel_resolution_,
static_cast<double>(seed_resolution),
use_transform_);
static_cast<double>(seed_resolution)
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
);
#else
, use_transform_);
#endif
super.setInputCloud(cloud);
super.setColorImportance(color_importance_);
super.setSpatialImportance(spatial_importance_);
Expand All @@ -1804,8 +1808,12 @@ namespace jsk_pcl_ros
}
boost::mutex::scoped_lock lock(mutex_);
pcl::SupervoxelClustering<PointT> super(voxel_resolution_,
seed_resolution_,
use_transform_);
seed_resolution_
#if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 12 )
);
#else
, use_transform_);
#endif
super.setInputCloud(cloud);
super.setColorImportance(color_importance_);
super.setSpatialImportance(spatial_importance_);
Expand Down

0 comments on commit f9dc100

Please sign in to comment.