Merge pull request #58 from h-wata/feature/downsampleCloudMsg_function

Add `downsampleCloudMsg` function to `noetic-devel`
Alpaca-zip · Feb 14, 2024 · ce0951f · ce0951f
2 parents 7d61af8 + 4c8a92f
commit ce0951f
Show file tree

Hide file tree

Showing 4 changed files with 23 additions and 51 deletions.
diff --git a/.github/workflows/noetic-ci.yml b/.github/workflows/noetic-ci.yml
@@ -8,8 +8,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env:
@@ -22,8 +20,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env:
@@ -36,8 +32,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env:
@@ -49,8 +43,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env:
@@ -64,8 +56,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env:
@@ -92,8 +82,6 @@ jobs:
     steps:
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: run industrial_ci
         uses: 'ros-industrial/industrial_ci@master'
         env: ${{matrix.env}}
diff --git a/.github/workflows/noetic-docker-build-check.yml b/.github/workflows/noetic-docker-build-check.yml
@@ -13,17 +13,10 @@ jobs:
         run: rm -rf /opt/hostedtoolcache
       - name: checkout
         uses: actions/checkout@v2
-        with:
-          ref: ${{ github.head_ref }}
       - name: set up QEMU
         uses: docker/setup-qemu-action@v1
       - name: set up Docker Buildx
         uses: docker/setup-buildx-action@v1
-      - name: login to DockerHub
-        uses: docker/login-action@v1
-        with:
-          username: ${{ secrets.DOCKERHUB_USERNAME }}
-          password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: build check
         uses: docker/build-push-action@v2
         with:

diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h
@@ -93,9 +93,10 @@ class TrackerWithCloudNode
                              pcl::PointCloud<pcl::PointXYZ>::Ptr& detection_cloud_raw);
   void processPointsWithMask(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const sensor_msgs::Image& mask,
                              pcl::PointCloud<pcl::PointXYZ>::Ptr& detection_cloud_raw);
-  pcl::PointCloud<pcl::PointXYZ>::Ptr msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg);
   pcl::PointCloud<pcl::PointXYZ>::Ptr cloud2TransformedCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
-                                                             const std_msgs::Header& header);
+                                                             const std::string& source_frame,
+                                                             const std::string& target_frame, const ros::Time& stamp);
   pcl::PointCloud<pcl::PointXYZ>::Ptr euclideanClusterExtraction(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud);
   void createBoundingBox(vision_msgs::Detection3DArray& detections3d_msg,
                          const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,

diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp
@@ -50,14 +50,17 @@ void TrackerWithCloudNode::syncCallback(const sensor_msgs::CameraInfo::ConstPtr&
   ros::Time current_call_time = ros::Time::now();
   ros::Duration callback_interval = current_call_time - last_call_time_;
   last_call_time_ = current_call_time;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  downsampled_cloud = downsampleCloudMsg(cloud_msg);
 
   pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud;
+  transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cloud_msg->header.frame_id, cam_model_.tfFrame(),
+                                             cloud_msg->header.stamp);
   vision_msgs::Detection3DArray detections3d_msg;
   sensor_msgs::PointCloud2 detection_cloud_msg;
   visualization_msgs::MarkerArray marker_array_msg;
 
   cam_model_.fromCameraInfo(camera_info_msg);
-  transformed_cloud = msg2TransformedCloud(cloud_msg);
   projectCloud(transformed_cloud, yolo_result_msg, cloud_msg->header, detections3d_msg, detection_cloud_msg);
   marker_array_msg = createMarkerArray(detections3d_msg, callback_interval.toSec());
 
@@ -90,7 +93,8 @@ void TrackerWithCloudNode::projectCloud(const pcl::PointCloud<pcl::PointXYZ>::Pt
 
     if (!detection_cloud_raw->points.empty())
     {
-      pcl::PointCloud<pcl::PointXYZ>::Ptr detection_cloud = cloud2TransformedCloud(detection_cloud_raw, header);
+      pcl::PointCloud<pcl::PointXYZ>::Ptr detection_cloud =
+          cloud2TransformedCloud(detection_cloud_raw, cam_model_.tfFrame(), header.frame_id, header.stamp);
       pcl::PointCloud<pcl::PointXYZ>::Ptr closest_detection_cloud = euclideanClusterExtraction(detection_cloud);
       createBoundingBox(detections3d_msg, closest_detection_cloud, yolo_result_msg->detections.detections[i].results);
       combine_detection_cloud += *closest_detection_cloud;
@@ -151,36 +155,28 @@ void TrackerWithCloudNode::processPointsWithMask(const pcl::PointCloud<pcl::Poin
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr
-TrackerWithCloudNode::msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
+TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
 {
-  pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-  sensor_msgs::PointCloud2 transformed_cloud_msg;
-
-  try
-  {
-    geometry_msgs::TransformStamped tf =
-        tf_buffer_->lookupTransform(cam_model_.tfFrame(), cloud_msg->header.frame_id, cloud_msg->header.stamp);
-    pcl_ros::transformPointCloud(cam_model_.tfFrame(), *cloud_msg, transformed_cloud_msg, *tf_buffer_);
-    pcl::fromROSMsg(transformed_cloud_msg, *transformed_cloud);
-  }
-  catch (tf2::TransformException& e)
-  {
-    ROS_WARN("%s", e.what());
-  }
-
-  return transformed_cloud;
+  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>());
+  pcl::fromROSMsg(*cloud_msg, *cloud);
+  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);
+  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
+  voxel_grid.setInputCloud(cloud);
+  voxel_grid.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
+  voxel_grid.filter(*downsampled_cloud);
+  return downsampled_cloud;
 }
 
 pcl::PointCloud<pcl::PointXYZ>::Ptr
 TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud,
-                                             const std_msgs::Header& header)
+                                             const std::string& source_frame, const std::string& target_frame,
+                                             const ros::Time& stamp)
 {
   pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
 
   try
   {
-    geometry_msgs::TransformStamped tf =
-        tf_buffer_->lookupTransform(header.frame_id, cam_model_.tfFrame(), header.stamp);
+    geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform(target_frame, source_frame, stamp);
     pcl_ros::transformPointCloud(*cloud, *transformed_cloud, tf.transform);
   }
   catch (tf2::TransformException& e)
@@ -194,12 +190,6 @@ TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud<pcl::PointXYZ
 pcl::PointCloud<pcl::PointXYZ>::Ptr
 TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud)
 {
-  pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled_cloud(new pcl::PointCloud<pcl::PointXYZ>);
-  pcl::VoxelGrid<pcl::PointXYZ> voxel_grid;
-  voxel_grid.setInputCloud(cloud);
-  voxel_grid.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, voxel_leaf_size_);
-  voxel_grid.filter(*downsampled_cloud);
-
   pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
   std::vector<pcl::PointIndices> cluster_indices;
   pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
@@ -208,7 +198,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud<pcl::Poin
   ec.setMinClusterSize(min_cluster_size_);
   ec.setMaxClusterSize(max_cluster_size_);
   ec.setSearchMethod(tree);
-  ec.setInputCloud(downsampled_cloud);
+  ec.setInputCloud(cloud);
   ec.extract(cluster_indices);
 
   float min_distance = std::numeric_limits<float>::max();
@@ -219,7 +209,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud<pcl::Poin
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);
     for (const auto& indice : cluster.indices)
     {
-      cloud_cluster->push_back((*downsampled_cloud)[indice]);
+      cloud_cluster->push_back((*cloud)[indice]);
     }
 
     Eigen::Vector4f centroid;