[ROS-O] patches

.github/workflows/config.yml

@@ -141,3 +141,85 @@ jobs:
           USE_JENKINS : ${{ matrix.USE_JENKINS }}
           DOCKER_IMAGE_JENKINS : ${{ matrix.DOCKER_IMAGE_JENKINS }}
           TIMEOUT_JENKINS : 180
+
+# ROS-O setup https://github.com/v4hn/ros-o-builder/blob/jammy-one/README.md#install-instructions
+  ros-o:
+    runs-on: ubuntu-latest
+
+    strategy:
+      fail-fast: false
+      matrix:
+        include:
+          - DISTRO: ubuntu:22.04
+            ROS_REPOSITORY_URL: https://raw.githubusercontent.com/v4hn/ros-o-builder/jammy-one/repository
+
+    container: ${{ matrix.DISTRO }}
+
+    env:
+      DEBIAN_FRONTEND : noninteractive
+
+    steps:
+      - name: Install git on container
+        run: |
+          apt update && apt install -y -q -qq git
+
+      - name: Chcekout Source
+        uses: actions/[email protected]
+        with:
+          submodules: recursive
+
+      - name: Setup ROS-O deb repository
+        run: |
+          set -x
+          apt update && apt install -qq -y ca-certificates
+          echo "deb [trusted=yes] ${{ matrix.ROS_REPOSITORY_URL }}/ ./" | tee /etc/apt/sources.list.d/ros-o-builder.list
+          apt update
+          apt install -qq -y python3-rosdep2
+          echo "yaml ${{ matrix.ROS_REPOSITORY_URL }}/local.yaml debian" | tee /etc/ros/rosdep/sources.list.d/1-ros-o-builder.list
+          rosdep update
+
+      - name: Setup catkin-tools
+        run: |
+          set -x
+          # setup catkin tools
+          apt install -qq -y python3-pip
+          pip3 install catkin-tools
+          # setup build tools
+          apt install -qq -y cmake build-essential catkin ros-one-rosbash
+
+      - name: Download unreleased files
+        run: |
+          source /opt/ros/one/setup.bash
+          set -x
+          apt install -qq -y git wget
+          mkdir -p ~/ws/src	  
+          cd ~/ws/src
+          # rosinstall_generator libsiftfast --rosdistro noetic
+          git clone https://github.com/tork-a/jsk_3rdparty-release.git -b release/noetic/libsiftfast/2.1.28-1
+          wget https://github.com/jsk-ros-pkg/jsk_3rdparty/commit/cafbff6dd2bd7869eb4f989bedd0a322a7c35d50.diff -O 1.patch
+          wget https://github.com/jsk-ros-pkg/jsk_3rdparty/commit/c8eb21e211d1a8f803cd55549a5b2bdc87e6ff9f.diff -O 2.patch
+          patch -d jsk_3rdparty-release -p3 < 1.patch
+          patch -d jsk_3rdparty-release -p3 < 2.patch
+          # rosinstall_generator jsk_topic_tools --rosdistro noetic
+          git clone https://github.com/tork-a/jsk_common-release.git -b release/noetic/jsk_topic_tools/2.2.15-4
+        shell: bash
+
+      - name: Setup Workspace
+        run: |
+          source /opt/ros/one/setup.bash
+          set -x
+          # setup workspace
+          mkdir -p ~/ws/src
+          cd ~/ws/src
+          ln -sf $GITHUB_WORKSPACE .
+          rosdep install -qq -r -y --from-path . --ignore-src || echo "OK"
+        shell: bash
+
+      - name: Compile Packages
+        run: |
+          source /opt/ros/one/setup.bash
+          set -x
+          cd ~/ws/
+          catkin build --no-status -sv ${{ matrix.CATKIN_OPTIONS }} --cmake-args -DCATKIN_ENABLE_TESTING=OFF -DCMAKE_VERBOSE_MAKEFILE:BOOL=ON ${{ matrix.CMAKE_OPTIONS }}
+        shell: bash
+

jsk_pcl_ros/CMakeLists.txt

@@ -11,10 +11,13 @@ endif()
 
 # check c++11 / c++0x
 include(CheckCXXCompilerFlag)
+check_cxx_compiler_flag("-std=c++17" COMPILER_SUPPORTS_CXX17)
 check_cxx_compiler_flag("-std=c++14" COMPILER_SUPPORTS_CXX14)
 check_cxx_compiler_flag("-std=c++11" COMPILER_SUPPORTS_CXX11)
 check_cxx_compiler_flag("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
-if(COMPILER_SUPPORTS_CXX14)
+if(COMPILER_SUPPORTS_CXX17)
+  set(CMAKE_CXX_FLAGS "-std=c++17")
+elseif(COMPILER_SUPPORTS_CXX14)
   set(CMAKE_CXX_FLAGS "-std=c++14")
 elseif(COMPILER_SUPPORTS_CXX11)
   set(CMAKE_CXX_FLAGS "-std=c++11")
@@ -57,6 +60,7 @@ find_package(PkgConfig)
 pkg_check_modules(ml_classifiers ml_classifiers QUIET)
 # only run in hydro
 find_package(PCL REQUIRED)
+message(STATUS "PCL_VERSION : ${PCL_VERSION}")
 find_package(OpenMP)
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
@@ -180,8 +184,12 @@ jsk_pcl_nodelet(src/moving_least_square_smoothing_nodelet.cpp "jsk_pcl/MovingLea
 jsk_pcl_nodelet(src/fisheye_sphere_publisher_nodelet.cpp "jsk_pcl/FisheyeSpherePublisher" "fisheye_sphere_publisher")
 jsk_pcl_nodelet(src/plane_supported_cuboid_estimator_nodelet.cpp
   "jsk_pcl/PlaneSupportedCuboidEstimator" "plane_supported_cuboid_estimator")
+if(${PCL_VERSION} VERSION_LESS "1.12.0")
+  # Could not compile on PCL-1.12.0
+  # undefined reference to `pcl::FilterIndices<pcl::tracking::ParticleCuboid>::applyFilter(pcl::PointCloud<pcl::tracking::ParticleCuboid>&)'
 jsk_pcl_nodelet(src/extract_cuboid_particles_top_n_nodelet.cpp
   "jsk_pcl/ExtractCuboidParticlesTopN" "extract_cuboid_particles_top_n")
+endif()
 jsk_pcl_nodelet(src/interactive_cuboid_likelihood_nodelet.cpp
   "jsk_pcl/InteractiveCuboidLikelihood" "interactive_cuboid_likelihood")
 jsk_pcl_nodelet(src/image_rotate_nodelet.cpp
@@ -577,7 +585,9 @@ if (CATKIN_ENABLE_TESTING)
     add_rostest(test/test_edgebased_cube_finder.test)
     add_rostest(test/test_environment_plane_modeling.test)
     add_rostest(test/test_euclidean_segmentation.test)
-    add_rostest(test/test_extract_cuboid_particles_top_n.test)
+    if(${PCL_VERSION} VERSION_LESS "1.12.0")
+      add_rostest(test/test_extract_cuboid_particles_top_n.test)
+    endif()
     add_rostest(test/test_extract_top_polygon_likelihood.test)
     add_rostest(test/test_feature_registration.test)
     add_rostest(test/test_find_object_on_plane.test)

jsk_pcl_ros/include/jsk_pcl_ros/container_occupancy_detector.h

@@ -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

jsk_pcl_ros/include/jsk_pcl_ros/organized_pass_through.h

@@ -70,6 +70,11 @@ namespace jsk_pcl_ros
     virtual void updateDiagnostic(
       diagnostic_updater::DiagnosticStatusWrapper &stat);
 
+      // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
     bool isPointNaN(const PointT& p) {
       return (!pcl_isfinite(p.x) || !pcl_isfinite(p.y) || !pcl_isfinite(p.z));
     }

jsk_pcl_ros/include/jsk_pcl_ros/particle_filter_tracking.h

@@ -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);

jsk_pcl_ros/include/jsk_pcl_ros/target_adaptive_tracking.h

@@ -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>

jsk_pcl_ros/src/attention_clipper_nodelet.cpp

@@ -378,6 +378,11 @@ namespace jsk_pcl_ros
     cv::rectangle(mask, roi_rect, white, CV_FILLED);
   }
 
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
   void AttentionClipper::clipPointcloud(
     const sensor_msgs::PointCloud2::ConstPtr& msg)
   {

jsk_pcl_ros/src/cluster_point_indices_decomposer_nodelet.cpp

@@ -432,6 +432,11 @@ namespace jsk_pcl_ros
     return true;
   }
 
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
   bool ClusterPointIndicesDecomposer::computeCenterAndBoundingBox
   (const pcl::PointCloud<pcl::PointXYZ>::Ptr segmented_cloud,
    const std_msgs::Header header,

jsk_pcl_ros/src/colorize_random_points_RF_nodelet.cpp

@@ -34,6 +34,7 @@
 #include "jsk_pcl_ros/colorize_random_points_RF.h"
 #include <pluginlib/class_list_macros.h>
 #include <pcl/common/centroid.h>
+#include <pcl/octree/octree_search.h>
 
 namespace jsk_pcl_ros
 {
@@ -86,7 +87,12 @@ namespace jsk_pcl_ros
   {
     sub_input_.shutdown();
   }
-
+
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isnan(x) std::isnan(x)
+#endif
+
   void ColorizeMapRandomForest::extract(const sensor_msgs::PointCloud2 pc)
   {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());

jsk_pcl_ros/src/colorize_segmented_RF_nodelet.cpp

@@ -72,6 +72,11 @@ namespace jsk_pcl_ros
     pass_offset2_ = tmp_pass2;
   }
 
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isnan(x) std::isnan(x)
+#endif
+
   void ColorizeRandomForest::extract(const sensor_msgs::PointCloud2 pc)
   {
     pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB> ());

jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp

@@ -115,7 +115,12 @@ namespace jsk_pcl_ros
     jsk_recognition_utils::ConvexPolygon::Ptr convex (new jsk_recognition_utils::ConvexPolygon(vertices));
     return convex;
   }
-
+
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
   double CubeHypothesis::evaluatePointOnPlanes(
     const pcl::PointCloud<pcl::PointXYZRGB>& cloud,
     jsk_recognition_utils::ConvexPolygon& polygon_a,
@@ -751,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
        }
      }
 

jsk_pcl_ros/src/euclidean_cluster_extraction_nodelet.cpp

@@ -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
@@ -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
@@ -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;

jsk_pcl_ros/src/geometric_consistency_grouping_nodelet.cpp

@@ -110,6 +110,11 @@ namespace jsk_pcl_ros
     pcl::fromROSMsg(*model_feature_msg, *reference_feature_);
   }
 
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
   void GeometricConsistencyGrouping::recognize(
     const sensor_msgs::PointCloud2::ConstPtr& scene_cloud_msg,
     const sensor_msgs::PointCloud2::ConstPtr& scene_feature_msg)

jsk_pcl_ros/src/icp_registration_nodelet.cpp

@@ -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;
@@ -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");
@@ -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);
     }

jsk_pcl_ros/src/linemod_nodelet.cpp

@@ -646,6 +646,11 @@ namespace jsk_pcl_ros
     linemod_in.close();
   }
 
+    // pcl removed the method by 1.13, no harm in defining it ourselves to use below
+#if __cplusplus >= 201103L
+#define pcl_isfinite(x) std::isfinite(x)
+#endif
+
   void LINEMODDetector::computeCenterOfTemplate(
     pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud,
     const pcl::SparseQuantizedMultiModTemplate& linemod_template,

jsk_pcl_ros/src/moving_least_square_smoothing_nodelet.cpp

@@ -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
 {
@@ -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_);