Skip to content

Commit

Permalink
Merge branch 'master' into pub-compressed-tile-image-github
Browse files Browse the repository at this point in the history
  • Loading branch information
k-okada authored Dec 29, 2024
2 parents 4aa35af + 8245f4b commit c518162
Show file tree
Hide file tree
Showing 34 changed files with 270 additions and 46 deletions.
82 changes: 82 additions & 0 deletions .github/workflows/config.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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

2 changes: 1 addition & 1 deletion audio_to_spectrogram/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@ install(DIRECTORY launch sample test
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS
)
if(NOT $ENV{ROS_DISTRO} STREQUAL "indigo") # on noetic it needs catkin_install_python to support Python3 and it does not work on indigo for some reason...
if($ENV{ROS_DISTRO} STREQUAL "indigo") # on noetic it needs catkin_install_python to support Python3 and it does not work on indigo for some reason...
install(DIRECTORY scripts
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
USE_SOURCE_PERMISSIONS
Expand Down
14 changes: 12 additions & 2 deletions jsk_pcl_ros/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand Down Expand Up @@ -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}")
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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)
Expand Down
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
5 changes: 5 additions & 0 deletions jsk_pcl_ros/include/jsk_pcl_ros/organized_pass_through.h
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
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
5 changes: 5 additions & 0 deletions jsk_pcl_ros/src/attention_clipper_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
5 changes: 5 additions & 0 deletions jsk_pcl_ros/src/cluster_point_indices_decomposer_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down
8 changes: 7 additions & 1 deletion jsk_pcl_ros/src/colorize_random_points_RF_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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> ());
Expand Down
5 changes: 5 additions & 0 deletions jsk_pcl_ros/src/colorize_segmented_RF_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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> ());
Expand Down
11 changes: 10 additions & 1 deletion jsk_pcl_ros/src/edgebased_cube_finder_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
}
}

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
5 changes: 5 additions & 0 deletions jsk_pcl_ros/src/geometric_consistency_grouping_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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)
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
5 changes: 5 additions & 0 deletions jsk_pcl_ros/src/linemod_nodelet.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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,
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
Loading

0 comments on commit c518162

Please sign in to comment.