From f8b0302d22eaacef51a49401e27fbec4a336f370 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 6 Jul 2021 16:54:22 +0900 Subject: [PATCH 1/6] Migrate to Github Actions --- .github/workflows/build.yml | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) create mode 100644 .github/workflows/build.yml diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 00000000..1106f11e --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,33 @@ +name: Build + +on: + push: + branches: [ master ] + pull_request: + branches: [ master ] + + # Allows you to run this workflow manually from the Actions tab + workflow_dispatch: + +jobs: + build: + runs-on: ubuntu-latest + strategy: + matrix: + ROS_DISTRO: [melodic, melodic_llvm, noetic, noetic_llvm] + + steps: + - uses: actions/checkout@v2 + + - name: Docker login + uses: docker/login-action@v1 + with: + username: ${{ secrets.DOCKER_USERNAME }} + password: ${{ secrets.DOCKER_TOKEN }} + + - name: Docker build + uses: docker/build-push-action@v2 + with: + file: ${{github.workspace}}/docker/${{ matrix.ROS_DISTRO }}/Dockerfile + context: . + push: false From 6ed6b7da9adf8be764bb9ad0340f4aca9d420ed2 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 6 Jul 2021 16:54:43 +0900 Subject: [PATCH 2/6] Delete .travis.yml --- .travis.yml | 26 -------------------------- 1 file changed, 26 deletions(-) delete mode 100644 .travis.yml diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 11e3dd6c..00000000 --- a/.travis.yml +++ /dev/null @@ -1,26 +0,0 @@ -sudo: required -language: generic -dist: focal - -services: -- docker - -matrix: - include: - - name: Bionic melodic - env: ROS_DISTRO=melodic - - name: Bionic melodic_llvm - env: ROS_DISTRO=melodic_llvm - - name: Focal Fossa noetic - env: ROS_DISTRO=noetic - - name: Focal Fossa noetic_llvm - env: ROS_DISTRO=noetic_llvm - -script: - - echo "$DOCKER_PASSWORD" | docker login --username "$DOCKER_USERNAME" --password-stdin || true - - docker build -f ./docker/$ROS_DISTRO/Dockerfile . - -env: - global: - - secure: xMQFPf2nogRIc7wn6/aHq7B5++gf1aMQggL2R+xfL5rayCRNPgYZrIvZTS8sIey7aatxM3nWGd3YuTbFQpiNU4W8+8s1RiFvxrARFa9KpW5uPXno48dbxyTFKIrhFqw7QWI412r5UwznfblYl47UUsjXnj8v6E8okVR47JYe1WNu6LfSNm59sD4GMv96YAq6eV2ZFb+M4cO3d2uBDe8An8GQeVZJjsXV57i75QVOKze/7+nLgikmOXUUaWHOZRlbXhaz0JbiIX+Canqeq3tBxLTDkNfYAkAfCAeyf1RpsJLnsegV3m79fqoyOPaFgnUSa+75g6W4zJ1TH8SpmfrLv2xZTZ621ljV9h4mRYIiJpc6YAxYm6fk9M4b1w0irm+kpI8AmJcs74hQtB28sUUNdLOp2r41yAITfE24k+QZ/PoftYdQgyeRf7WEHJLYYxbUIzkfVfiBmVbkeLZJGY5251AOuIHWjkVc6Z/yMSU2aAY1Wue1cqbqpWsnPclgvHgDKhq3dVY6H9rCIPgTZiztp43ZpzuEWY4LxJxrJKX3ZGNAzIVwka1uSXbu4W9x7HAoyA4dqtG+FGC9Jm+yRLSHo4lIkTFBANzuSt5z0NxNCwaml4OO9lApZ1AIn3cKhHUXG/u+CGhVSEvQuHYTpL2tMxRuxiUVD0cxmy48fttnGUA= - - secure: QlqYV2mR30pjTLSSfg+Px+fYl8jPRSD4cBssIk/m9ATTG8ewXrtnfAvVfmlhNWEwXRO4ZPM/unNNbZf4CxGmJs6zgTK5wonyoIjOjghEPMdYYPsGxNO2Avw+q/7DL/DwLAbjDZN3Cdx+zj86Ufh2RwKzdJ9K2JbyluWSU3dsXYX9V1tbntk5nUgya0MQbpO9min42PeqG1oBBsD7n2F/X7iVBtE0nxvRg8jBmX/QlBNIO9qLip/3m/arUUGa95Nb8dNR85H+kIlgQQ2K94aWXbLHgoznnPYBpOfCSvnY4fZXNH8e64kC8kuCC0LnDnpk5aWMKqJZZll2kqmkg1Acaz1hgwzxdzpyX1ssrTFsmCZR9vJq3063veXOHAZItTTD59UKiS0beolf087l3kU8JsWjaLKho7vXGDtTBtWDUWaUSksR/+NU7/sciLhomiZRWJzEi+3uiAfHyT2kLQZO57f9LH3sMMOVh3zUPnMESkp7ewMK4lEjjOXYasDEehMLVaoSvwr8Fh/ZbqvzvECgDh5vD54zjdfHLhjkRB9eITWZWYJyKeSMqFpTFnZvZEH8bdMwh40XMe1Zqd/3ZRClKv4TIAg0W5GMQ/bE8HfVoTZj8TIOYgmyt01+ka9imzu+A7YV77Wui2wzNg/y/4w69bd84PLVt7gtEZ3oz8Kddms= From 53831b2f28629ebd7e2b2c6cc40a39268ee01156 Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 6 Jul 2021 17:04:08 +0900 Subject: [PATCH 3/6] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 7d850f02..77e5a8b8 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ [video](https://drive.google.com/open?id=0B9f5zFkpn4soSG96Tkt4SFFTbms) -[![Codacy Badge](https://api.codacy.com/project/badge/Grade/1175635f00394e789b457b44690ce72c)](https://app.codacy.com/app/koide3/hdl_graph_slam?utm_source=github.com&utm_medium=referral&utm_content=koide3/hdl_graph_slam&utm_campaign=Badge_Grade_Dashboard) [![Build Status](https://travis-ci.org/koide3/hdl_graph_slam.svg?branch=master)](https://travis-ci.org/koide3/hdl_graph_slam) on melodic & noetic +[![Codacy Badge](https://api.codacy.com/project/badge/Grade/1175635f00394e789b457b44690ce72c)](https://app.codacy.com/app/koide3/hdl_graph_slam?utm_source=github.com&utm_medium=referral&utm_content=koide3/hdl_graph_slam&utm_campaign=Badge_Grade_Dashboard) [![Build](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml) on melodic & noetic ## Nodelets ***hdl_graph_slam*** consists of four nodelets. From 14150a0ec42b58f41ea1aa6011fa389399803b6b Mon Sep 17 00:00:00 2001 From: koide3 <31344317+koide3@users.noreply.github.com> Date: Tue, 6 Jul 2021 17:04:23 +0900 Subject: [PATCH 4/6] Update README.md --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 77e5a8b8..0c33b95e 100644 --- a/README.md +++ b/README.md @@ -5,7 +5,7 @@ [video](https://drive.google.com/open?id=0B9f5zFkpn4soSG96Tkt4SFFTbms) -[![Codacy Badge](https://api.codacy.com/project/badge/Grade/1175635f00394e789b457b44690ce72c)](https://app.codacy.com/app/koide3/hdl_graph_slam?utm_source=github.com&utm_medium=referral&utm_content=koide3/hdl_graph_slam&utm_campaign=Badge_Grade_Dashboard) [![Build](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml) on melodic & noetic +[![Build](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml/badge.svg)](https://github.com/koide3/hdl_graph_slam/actions/workflows/build.yml) on melodic & noetic ## Nodelets ***hdl_graph_slam*** consists of four nodelets. From 7f163ac75e556920b0b036751e71a15090428b30 Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Tue, 6 Jul 2021 17:12:21 +0900 Subject: [PATCH 5/6] replace boost::shared_ptr with ::Ptr --- .gitignore | 1 + apps/prefiltering_nodelet.cpp | 4 ++-- apps/scan_matching_odometry_nodelet.cpp | 6 +++--- docker/melodic/Dockerfile | 2 +- include/hdl_graph_slam/registrations.hpp | 2 +- src/hdl_graph_slam/registrations.cpp | 18 +++++++++--------- 6 files changed, 17 insertions(+), 16 deletions(-) diff --git a/.gitignore b/.gitignore index 6c0cf880..0238ce87 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ +.vscode/* imgui.ini rviz/* diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp index 268c319c..2f1c958c 100644 --- a/apps/prefiltering_nodelet.cpp +++ b/apps/prefiltering_nodelet.cpp @@ -53,12 +53,12 @@ class PrefilteringNodelet : public nodelet::Nodelet { if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; - boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + pcl::VoxelGrid::Ptr voxelgrid(new pcl::VoxelGrid()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = voxelgrid; } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; - boost::shared_ptr> approx_voxelgrid(new pcl::ApproximateVoxelGrid()); + pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = approx_voxelgrid; } else { diff --git a/apps/scan_matching_odometry_nodelet.cpp b/apps/scan_matching_odometry_nodelet.cpp index d3dff75c..4cc1efd4 100644 --- a/apps/scan_matching_odometry_nodelet.cpp +++ b/apps/scan_matching_odometry_nodelet.cpp @@ -84,12 +84,12 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet { double downsample_resolution = pnh.param("downsample_resolution", 0.1); if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; - boost::shared_ptr> voxelgrid(new pcl::VoxelGrid()); + pcl::VoxelGrid::Ptr voxelgrid(new pcl::VoxelGrid()); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = voxelgrid; } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; - boost::shared_ptr> approx_voxelgrid(new pcl::ApproximateVoxelGrid()); + pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); approx_voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); downsample_filter = approx_voxelgrid; } else { @@ -98,7 +98,7 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet { std::cerr << " : use passthrough filter" << std::endl; } std::cout << "downsample: NONE" << std::endl; - boost::shared_ptr> passthrough(new pcl::PassThrough()); + pcl::PassThrough::Ptr passthrough(new pcl::PassThrough()); downsample_filter = passthrough; } diff --git a/docker/melodic/Dockerfile b/docker/melodic/Dockerfile index 63c5a5d3..0c2237b3 100644 --- a/docker/melodic/Dockerfile +++ b/docker/melodic/Dockerfile @@ -11,7 +11,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ RUN mkdir -p /root/catkin_ws/src WORKDIR /root/catkin_ws/src RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_init_workspace' -RUN git clone -b melodic https://github.com/koide3/ndt_omp.git +RUN git clone https://github.com/koide3/ndt_omp.git RUN git clone https://github.com/SMRT-AIST/fast_gicp.git --recursive COPY . /root/catkin_ws/src/hdl_graph_slam/ diff --git a/include/hdl_graph_slam/registrations.hpp b/include/hdl_graph_slam/registrations.hpp index afbc5ffb..07af3bfa 100644 --- a/include/hdl_graph_slam/registrations.hpp +++ b/include/hdl_graph_slam/registrations.hpp @@ -14,7 +14,7 @@ namespace hdl_graph_slam { * @param pnh * @return selected scan matching */ -boost::shared_ptr> select_registration_method(ros::NodeHandle& pnh); +pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh); } // namespace hdl_graph_slam diff --git a/src/hdl_graph_slam/registrations.cpp b/src/hdl_graph_slam/registrations.cpp index 8e7ad893..daaf0c0a 100644 --- a/src/hdl_graph_slam/registrations.cpp +++ b/src/hdl_graph_slam/registrations.cpp @@ -19,14 +19,14 @@ namespace hdl_graph_slam { -boost::shared_ptr> select_registration_method(ros::NodeHandle& pnh) { +pcl::Registration::Ptr select_registration_method(ros::NodeHandle& pnh) { using PointT = pcl::PointXYZI; // select a registration method (ICP, GICP, NDT) std::string registration_method = pnh.param("registration_method", "NDT_OMP"); if(registration_method == "FAST_GICP") { std::cout << "registration: FAST_GICP" << std::endl; - boost::shared_ptr> gicp(new fast_gicp::FastGICP()); + fast_gicp::FastGICP::Ptr gicp(new fast_gicp::FastGICP()); gicp->setNumThreads(pnh.param("reg_num_threads", 0)); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); @@ -37,7 +37,7 @@ boost::shared_ptr> select_regi #ifdef USE_VGICP_CUDA else if(registration_method == "FAST_VGICP_CUDA") { std::cout << "registration: FAST_VGICP_CUDA" << std::endl; - boost::shared_ptr> vgicp(new fast_gicp::FastVGICPCuda()); + fast_gicp::FastVGICPCuda::Ptr vgicp(new fast_gicp::FastVGICPCuda()); vgicp->setResolution(pnh.param("reg_resolution", 1.0)); vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); vgicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); @@ -47,7 +47,7 @@ boost::shared_ptr> select_regi #endif else if(registration_method == "FAST_VGICP") { std::cout << "registration: FAST_VGICP" << std::endl; - boost::shared_ptr> vgicp(new fast_gicp::FastVGICP()); + fast_gicp::FastVGICP::Ptr vgicp(new fast_gicp::FastVGICP()); vgicp->setNumThreads(pnh.param("reg_num_threads", 0)); vgicp->setResolution(pnh.param("reg_resolution", 1.0)); vgicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); @@ -56,7 +56,7 @@ boost::shared_ptr> select_regi return vgicp; } else if(registration_method == "ICP") { std::cout << "registration: ICP" << std::endl; - boost::shared_ptr> icp(new pcl::IterativeClosestPoint()); + pcl::IterativeClosestPoint::Ptr icp(new pcl::IterativeClosestPoint()); icp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); icp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); icp->setMaxCorrespondenceDistance(pnh.param("reg_max_correspondence_distance", 2.5)); @@ -65,7 +65,7 @@ boost::shared_ptr> select_regi } else if(registration_method.find("GICP") != std::string::npos) { if(registration_method.find("OMP") == std::string::npos) { std::cout << "registration: GICP" << std::endl; - boost::shared_ptr> gicp(new pcl::GeneralizedIterativeClosestPoint()); + pcl::GeneralizedIterativeClosestPoint::Ptr gicp(new pcl::GeneralizedIterativeClosestPoint()); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); @@ -75,7 +75,7 @@ boost::shared_ptr> select_regi return gicp; } else { std::cout << "registration: GICP_OMP" << std::endl; - boost::shared_ptr> gicp(new pclomp::GeneralizedIterativeClosestPoint()); + pclomp::GeneralizedIterativeClosestPoint::Ptr gicp(new pclomp::GeneralizedIterativeClosestPoint()); gicp->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); gicp->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); gicp->setUseReciprocalCorrespondences(pnh.param("reg_use_reciprocal_correspondences", false)); @@ -93,7 +93,7 @@ boost::shared_ptr> select_regi double ndt_resolution = pnh.param("reg_resolution", 0.5); if(registration_method.find("OMP") == std::string::npos) { std::cout << "registration: NDT " << ndt_resolution << std::endl; - boost::shared_ptr> ndt(new pcl::NormalDistributionsTransform()); + pcl::NormalDistributionsTransform::Ptr ndt(new pcl::NormalDistributionsTransform()); ndt->setTransformationEpsilon(pnh.param("reg_transformation_epsilon", 0.01)); ndt->setMaximumIterations(pnh.param("reg_maximum_iterations", 64)); ndt->setResolution(ndt_resolution); @@ -102,7 +102,7 @@ boost::shared_ptr> select_regi int num_threads = pnh.param("reg_num_threads", 0); std::string nn_search_method = pnh.param("reg_nn_search_method", "DIRECT7"); std::cout << "registration: NDT_OMP " << nn_search_method << " " << ndt_resolution << " (" << num_threads << " threads)" << std::endl; - boost::shared_ptr> ndt(new pclomp::NormalDistributionsTransform()); + pclomp::NormalDistributionsTransform::Ptr ndt(new pclomp::NormalDistributionsTransform()); if(num_threads > 0) { ndt->setNumThreads(num_threads); } From fb55b1d70153be7fefe53b7e6fc0f616dd07446c Mon Sep 17 00:00:00 2001 From: "k.koide" Date: Tue, 6 Jul 2021 17:18:30 +0900 Subject: [PATCH 6/6] avoid missing VoxelGrid::Ptr --- apps/prefiltering_nodelet.cpp | 4 ++-- apps/scan_matching_odometry_nodelet.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/prefiltering_nodelet.cpp b/apps/prefiltering_nodelet.cpp index 2f1c958c..80773352 100644 --- a/apps/prefiltering_nodelet.cpp +++ b/apps/prefiltering_nodelet.cpp @@ -53,9 +53,9 @@ class PrefilteringNodelet : public nodelet::Nodelet { if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; - pcl::VoxelGrid::Ptr voxelgrid(new pcl::VoxelGrid()); + auto voxelgrid = new pcl::VoxelGrid(); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - downsample_filter = voxelgrid; + downsample_filter.reset(voxelgrid); } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid()); diff --git a/apps/scan_matching_odometry_nodelet.cpp b/apps/scan_matching_odometry_nodelet.cpp index 4cc1efd4..05566ee0 100644 --- a/apps/scan_matching_odometry_nodelet.cpp +++ b/apps/scan_matching_odometry_nodelet.cpp @@ -84,9 +84,9 @@ class ScanMatchingOdometryNodelet : public nodelet::Nodelet { double downsample_resolution = pnh.param("downsample_resolution", 0.1); if(downsample_method == "VOXELGRID") { std::cout << "downsample: VOXELGRID " << downsample_resolution << std::endl; - pcl::VoxelGrid::Ptr voxelgrid(new pcl::VoxelGrid()); + auto voxelgrid = new pcl::VoxelGrid(); voxelgrid->setLeafSize(downsample_resolution, downsample_resolution, downsample_resolution); - downsample_filter = voxelgrid; + downsample_filter.reset(voxelgrid); } else if(downsample_method == "APPROX_VOXELGRID") { std::cout << "downsample: APPROX_VOXELGRID " << downsample_resolution << std::endl; pcl::ApproximateVoxelGrid::Ptr approx_voxelgrid(new pcl::ApproximateVoxelGrid());