From d18a16aa0e2a048585c6c82d5ae3ce6923ad5d0d Mon Sep 17 00:00:00 2001 From: h-wata Date: Wed, 7 Feb 2024 17:31:29 +0900 Subject: [PATCH 1/9] Downsample point cloud function in syncCallback --- .../tracker_with_cloud_node.h | 1 + src/tracker_with_cloud_node.cpp | 27 ++++++++++++------- 2 files changed, 19 insertions(+), 9 deletions(-) diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h index f9f8d7e..312e905 100644 --- a/include/tracker_with_cloud_node/tracker_with_cloud_node.h +++ b/include/tracker_with_cloud_node/tracker_with_cloud_node.h @@ -94,6 +94,7 @@ class TrackerWithCloudNode void processPointsWithMask(const pcl::PointCloud::Ptr& cloud, const sensor_msgs::Image& mask, pcl::PointCloud::Ptr& detection_cloud_raw); pcl::PointCloud::Ptr msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); + pcl::PointCloud::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, const std_msgs::Header& header); pcl::PointCloud::Ptr euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud); diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index 891e07d..a29e0b8 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -50,14 +50,16 @@ 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::Ptr downsampled_cloud(new pcl::PointCloud); + downsampled_cloud = downsampleCloudMsg(cloud_msg); pcl::PointCloud::Ptr transformed_cloud; + transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cloud_msg->header); 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()); @@ -171,6 +173,19 @@ TrackerWithCloudNode::msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPt return transformed_cloud; } +pcl::PointCloud::Ptr +TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) +{ + pcl::PointCloud::Ptr cloud(new pcl::PointCloud()); + pcl::fromROSMsg(*cloud_msg, *cloud); + pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); + pcl::VoxelGrid 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::Ptr TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, const std_msgs::Header& header) @@ -194,12 +209,6 @@ TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud) { - pcl::PointCloud::Ptr downsampled_cloud(new pcl::PointCloud); - pcl::VoxelGrid 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::Ptr tree(new pcl::search::KdTree); std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; @@ -208,7 +217,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::max(); @@ -219,7 +228,7 @@ TrackerWithCloudNode::euclideanClusterExtraction(const pcl::PointCloud::Ptr cloud_cluster(new pcl::PointCloud); for (const auto& indice : cluster.indices) { - cloud_cluster->push_back((*downsampled_cloud)[indice]); + cloud_cluster->push_back((*cloud)[indice]); } Eigen::Vector4f centroid; From fa334b001ddab9ce179a3292d9d50a543e51b03d Mon Sep 17 00:00:00 2001 From: h-wata Date: Fri, 9 Feb 2024 16:21:16 +0900 Subject: [PATCH 2/9] Remove unused function msg2TransformedCloud --- .../tracker_with_cloud_node.h | 1 - src/tracker_with_cloud_node.cpp | 21 ------------------- 2 files changed, 22 deletions(-) diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h index 312e905..72fbe01 100644 --- a/include/tracker_with_cloud_node/tracker_with_cloud_node.h +++ b/include/tracker_with_cloud_node/tracker_with_cloud_node.h @@ -93,7 +93,6 @@ class TrackerWithCloudNode pcl::PointCloud::Ptr& detection_cloud_raw); void processPointsWithMask(const pcl::PointCloud::Ptr& cloud, const sensor_msgs::Image& mask, pcl::PointCloud::Ptr& detection_cloud_raw); - pcl::PointCloud::Ptr msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, const std_msgs::Header& header); diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index a29e0b8..25dc8ba 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -152,27 +152,6 @@ void TrackerWithCloudNode::processPointsWithMask(const pcl::PointCloud::Ptr -TrackerWithCloudNode::msg2TransformedCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) -{ - pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); - 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::Ptr TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg) { From 53d86161ad0f08d9fd9fbba1e36adaa376ffebbf Mon Sep 17 00:00:00 2001 From: h-wata Date: Fri, 9 Feb 2024 16:22:22 +0900 Subject: [PATCH 3/9] Transform cloud to target frame in cloud2TransformedCloud() --- .../tracker_with_cloud_node/tracker_with_cloud_node.h | 1 + src/tracker_with_cloud_node.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h index 72fbe01..0ba2d45 100644 --- a/include/tracker_with_cloud_node/tracker_with_cloud_node.h +++ b/include/tracker_with_cloud_node/tracker_with_cloud_node.h @@ -95,6 +95,7 @@ class TrackerWithCloudNode pcl::PointCloud::Ptr& detection_cloud_raw); pcl::PointCloud::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, + const std::string& target_frame, const std_msgs::Header& header); pcl::PointCloud::Ptr euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud); void createBoundingBox(vision_msgs::Detection3DArray& detections3d_msg, diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index 25dc8ba..7b766cc 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -54,7 +54,7 @@ void TrackerWithCloudNode::syncCallback(const sensor_msgs::CameraInfo::ConstPtr& downsampled_cloud = downsampleCloudMsg(cloud_msg); pcl::PointCloud::Ptr transformed_cloud; - transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cloud_msg->header); + transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cam_model_.tfFrame(), cloud_msg->header); vision_msgs::Detection3DArray detections3d_msg; sensor_msgs::PointCloud2 detection_cloud_msg; visualization_msgs::MarkerArray marker_array_msg; @@ -92,7 +92,8 @@ void TrackerWithCloudNode::projectCloud(const pcl::PointCloud::Pt if (!detection_cloud_raw->points.empty()) { - pcl::PointCloud::Ptr detection_cloud = cloud2TransformedCloud(detection_cloud_raw, header); + pcl::PointCloud::Ptr detection_cloud = + cloud2TransformedCloud(detection_cloud_raw, cam_model_.tfFrame(), header); pcl::PointCloud::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; @@ -167,14 +168,13 @@ TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& pcl::PointCloud::Ptr TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, - const std_msgs::Header& header) + const std::string& target_frame, const std_msgs::Header& header) { pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); try { - geometry_msgs::TransformStamped tf = - tf_buffer_->lookupTransform(header.frame_id, cam_model_.tfFrame(), header.stamp); + geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform(header.frame_id, target_frame, header.stamp); pcl_ros::transformPointCloud(*cloud, *transformed_cloud, tf.transform); } catch (tf2::TransformException& e) From 41753e2308560601cfd88674032b5de8d1310e1c Mon Sep 17 00:00:00 2001 From: h-wata Date: Fri, 9 Feb 2024 16:25:32 +0900 Subject: [PATCH 4/9] Remove unnecessary checkout step in noetic-ci.yml --- .github/workflows/noetic-ci.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/noetic-ci.yml b/.github/workflows/noetic-ci.yml index 983526f..f2b021f 100644 --- 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: From 1aca79ed403912fd8519a80854f4984a72781a7f Mon Sep 17 00:00:00 2001 From: h-wata Date: Tue, 13 Feb 2024 13:38:18 +0900 Subject: [PATCH 5/9] Refactor cloud2TransformedCloud function signature --- .../tracker_with_cloud_node/tracker_with_cloud_node.h | 4 ++-- src/tracker_with_cloud_node.cpp | 10 ++++++---- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/tracker_with_cloud_node/tracker_with_cloud_node.h b/include/tracker_with_cloud_node/tracker_with_cloud_node.h index 0ba2d45..6b98ae5 100644 --- a/include/tracker_with_cloud_node/tracker_with_cloud_node.h +++ b/include/tracker_with_cloud_node/tracker_with_cloud_node.h @@ -95,8 +95,8 @@ class TrackerWithCloudNode pcl::PointCloud::Ptr& detection_cloud_raw); pcl::PointCloud::Ptr downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& cloud_msg); pcl::PointCloud::Ptr cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, - const std::string& target_frame, - const std_msgs::Header& header); + const std::string& source_frame, + const std::string& target_frame, const ros::Time& stamp); pcl::PointCloud::Ptr euclideanClusterExtraction(const pcl::PointCloud::Ptr& cloud); void createBoundingBox(vision_msgs::Detection3DArray& detections3d_msg, const pcl::PointCloud::Ptr& cloud, diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index 7b766cc..8b1a51b 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -54,7 +54,8 @@ void TrackerWithCloudNode::syncCallback(const sensor_msgs::CameraInfo::ConstPtr& downsampled_cloud = downsampleCloudMsg(cloud_msg); pcl::PointCloud::Ptr transformed_cloud; - transformed_cloud = cloud2TransformedCloud(downsampled_cloud, cam_model_.tfFrame(), cloud_msg->header); + 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; @@ -93,7 +94,7 @@ void TrackerWithCloudNode::projectCloud(const pcl::PointCloud::Pt if (!detection_cloud_raw->points.empty()) { pcl::PointCloud::Ptr detection_cloud = - cloud2TransformedCloud(detection_cloud_raw, cam_model_.tfFrame(), header); + cloud2TransformedCloud(detection_cloud_raw, cam_model_.tfFrame(), header.frame_id, header.stamp); pcl::PointCloud::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; @@ -168,13 +169,14 @@ TrackerWithCloudNode::downsampleCloudMsg(const sensor_msgs::PointCloud2ConstPtr& pcl::PointCloud::Ptr TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloud::Ptr& cloud, - const std::string& target_frame, const std_msgs::Header& header) + const std::string& source_frame, const std::string& target_frame, + const ros::Time& stamp) { pcl::PointCloud::Ptr transformed_cloud(new pcl::PointCloud); try { - geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform(header.frame_id, target_frame, header.stamp); + geometry_msgs::TransformStamped tf = tf_buffer_->lookupTransform(source_frame, target_frame, stamp); pcl_ros::transformPointCloud(*cloud, *transformed_cloud, tf.transform); } catch (tf2::TransformException& e) From c23d77fde3fab75b5c0a10e8996f884b3dc5fe6d Mon Sep 17 00:00:00 2001 From: h-wata Date: Tue, 13 Feb 2024 13:45:55 +0900 Subject: [PATCH 6/9] fix noetic-ci --- .github/workflows/noetic-ci.yml | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/.github/workflows/noetic-ci.yml b/.github/workflows/noetic-ci.yml index f2b021f..873efb4 100644 --- a/.github/workflows/noetic-ci.yml +++ b/.github/workflows/noetic-ci.yml @@ -20,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: @@ -34,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: @@ -47,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: @@ -62,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: @@ -90,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}} From 2c82be9df24328ca0e8abb0ed9b99845e12e4f83 Mon Sep 17 00:00:00 2001 From: h-wata Date: Tue, 13 Feb 2024 16:44:45 +0900 Subject: [PATCH 7/9] Remove unnecessary checkout step in workflow --- .github/workflows/noetic-docker-build-check.yml | 2 -- 1 file changed, 2 deletions(-) diff --git a/.github/workflows/noetic-docker-build-check.yml b/.github/workflows/noetic-docker-build-check.yml index 88fd007..0eed57a 100644 --- a/.github/workflows/noetic-docker-build-check.yml +++ b/.github/workflows/noetic-docker-build-check.yml @@ -13,8 +13,6 @@ 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 From dfbdb2351d81e53724e49568cc32368d2414d9c8 Mon Sep 17 00:00:00 2001 From: h-wata Date: Tue, 13 Feb 2024 16:53:46 +0900 Subject: [PATCH 8/9] Fix transform lookup order in cloud2TransformedCloud function --- src/tracker_with_cloud_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tracker_with_cloud_node.cpp b/src/tracker_with_cloud_node.cpp index 8b1a51b..c8f5668 100644 --- a/src/tracker_with_cloud_node.cpp +++ b/src/tracker_with_cloud_node.cpp @@ -176,7 +176,7 @@ TrackerWithCloudNode::cloud2TransformedCloud(const pcl::PointCloudlookupTransform(source_frame, target_frame, 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) From 4c8a92f5102912460ec749474f4bff9b29251c95 Mon Sep 17 00:00:00 2001 From: h-wata Date: Wed, 14 Feb 2024 10:52:32 +0900 Subject: [PATCH 9/9] Remove 'login to Dockerhub' task from ci --- .github/workflows/noetic-docker-build-check.yml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/.github/workflows/noetic-docker-build-check.yml b/.github/workflows/noetic-docker-build-check.yml index 0eed57a..4482fa0 100644 --- a/.github/workflows/noetic-docker-build-check.yml +++ b/.github/workflows/noetic-docker-build-check.yml @@ -17,11 +17,6 @@ jobs: 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: